Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

add footprint inflation for higher speeds #32

Merged
merged 10 commits into from
May 10, 2022
Merged

add footprint inflation for higher speeds #32

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
1ba9f2e
only update max_vel_x once per call
mikeferguson May 6, 2022
e3958e8
add ability to inflate footprint
mikeferguson May 9, 2022
5b61e92
naive footprint expansion
mikeferguson May 9, 2022
365f747
refactor so we can be less naive
mikeferguson May 9, 2022
12ccbab
iteratively reduce speed when using footprint scaling
mikeferguson May 9, 2022
a4b2ca1
protect against getting stuck
mikeferguson May 10, 2022
58abc6c
velocity and factor should never dip below 0
mikeferguson May 10, 2022
8a6562f
protect against div by zero
mikeferguson May 10, 2022
f64a715
additional comments
mikeferguson May 10, 2022
961e59c
review comments
mikeferguson May 10, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions graceful_controller_ros/cfg/GracefulController.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,4 +38,9 @@ gen.add("yaw_gap_tolerance", double_t, 0, "Maximum distance between poses in the
# Goal tolerance latch
gen.add("latch_xy_goal_tolerance", bool_t, 0, "When goal has been reached, just fix heading", False)

# Footprint scaling at higher speeds
gen.add("scaling_vel_x", double_t, 0, "Above this velocity, the footprint will be scaled up", 0.5, 0.0);
gen.add("scaling_factor", double_t, 0, "Amount to scale footprint when at max velocity", 0.0, 0.0);
gen.add("scaling_step", double_t, 0, "Amount to reduce x velocity when iteratively reducing velocity", 0.1, 0.01, 1.0);

exit(gen.generate("graceful_controller", "graceful_controller", "GracefulController"))
12 changes: 12 additions & 0 deletions graceful_controller_ros/include/graceful_controller_ros/graceful_controller_ros.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,14 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
private:
void velocityCallback(const std_msgs::Float32::ConstPtr& max_vel_x);

/**
* @brief Simulate a path.
* @param target_pose Pose to simulate towards.
* @param cmd_vel The returned command to execute.
* @returns True if the path is valid.
*/
bool simulate(const geometry_msgs::PoseStamped& target_pose, geometry_msgs::Twist& cmd_vel);

ros::Publisher global_plan_pub_, local_plan_pub_, target_pose_pub_;
ros::Subscriber max_vel_sub_;

Expand All @@ -119,6 +127,7 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner

tf2_ros::Buffer* buffer_;
costmap_2d::Costmap2DROS* costmap_ros_;
geometry_msgs::TransformStamped robot_to_costmap_transform_;
base_local_planner::LocalPlannerUtil planner_util_;
base_local_planner::OdometryHelperRos odom_helper_;

Expand All @@ -131,6 +140,9 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
double min_in_place_vel_theta_;
double acc_lim_x_;
double acc_lim_theta_;
double scaling_vel_x_;
double scaling_factor_;
double scaling_step_;
double xy_goal_tolerance_;
double yaw_goal_tolerance_;
double min_lookahead_;
Expand Down
Loading