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

added disengage_threshold #4690

Closed
wants to merge 2 commits into from
Closed

added disengage_threshold #4690

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3dd2ec6
added disengage_threshold
sandeepdutta Sep 14, 2024
3118e91
added global costmap clearing once it reaches goal
sandeepdutta Sep 27, 2024
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
9 changes: 6 additions & 3 deletions nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,10 @@
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<Sequence>
<FollowPath path="{path}" controller_id="FollowPath"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</PipelineSequence>
Expand All @@ -26,9 +29,9 @@
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Spin spin_dist="1.57"/>
<Wait wait_duration="5"/>
<BackUp backup_dist="0.30" backup_speed="0.05"/>
<!--Spin spin_dist="1.57"/-->
<Wait wait_duration="5"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
Expand Down
3 changes: 2 additions & 1 deletion ...n_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,8 @@ class RotationShimController : public nav2_core::Controller
nav2_core::Controller::Ptr primary_controller_;
bool path_updated_;
nav_msgs::msg::Path current_path_;
double forward_sampling_distance_, angular_dist_threshold_;
double forward_sampling_distance_, angular_dist_threshold_, angular_dist_threshold_param_;
double angular_disengage_threshold_;
double rotate_to_heading_angular_vel_, max_angular_accel_;
double control_duration_, simulate_ahead_time_;
bool rotate_to_goal_heading_;
Expand Down
7 changes: 7 additions & 0 deletions nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,8 @@ void RotationShimController::configure(
double control_frequency;
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue(0.785)); // 45 deg
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".angular_disengage_threshold", rclcpp::ParameterValue(0.785/3.0)); // 15 deg
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5));
nav2_util::declare_parameter_if_not_declared(
Expand All @@ -66,6 +68,9 @@ void RotationShimController::configure(
node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false));

node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
angular_dist_threshold_param_ = angular_dist_threshold_;
node->get_parameter(
plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_);
node->get_parameter(
plugin_name_ + ".rotate_to_heading_angular_vel",
Expand Down Expand Up @@ -191,11 +196,13 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
double angular_distance_to_heading =
std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x);
if (fabs(angular_distance_to_heading) > angular_dist_threshold_) {
angular_dist_threshold_ = angular_disengage_threshold_;
RCLCPP_DEBUG(
logger_,
"Robot is not within the new path's rough heading, rotating to heading...");
return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity);
} else {
angular_dist_threshold_ = angular_dist_threshold_param_;
RCLCPP_DEBUG(
logger_,
"Robot is at the new path's rough heading, passing to controller");
Expand Down