From 1de7d2f25db3ffb73351d15527c2d69fe2fc7be8 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 2 Oct 2024 11:28:34 -0700 Subject: [PATCH] Adding disengagement threshold to rotation shim controller (#4699) (#4700) * adding disengagement threshold to rotation shim controller Signed-off-by: Steve Macenski * change default to 22.5 deg Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski (cherry picked from commit fc7e08611a453889feb112bc4c2160687d320a91) Co-authored-by: Steve Macenski --- .../nav2_rotation_shim_controller.hpp | 4 ++-- .../src/nav2_rotation_shim_controller.cpp | 14 ++++++++++++-- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 6d4e8b86b33..cb1b8ad7ed9 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -172,10 +172,10 @@ 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_disengage_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; - bool rotate_to_goal_heading_; + bool rotate_to_goal_heading_, in_rotation_; // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index afb74c357eb..4704c0b8ebe 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -29,7 +29,8 @@ namespace nav2_rotation_shim_controller RotationShimController::RotationShimController() : lp_loader_("nav2_core", "nav2_core::Controller"), primary_controller_(nullptr), - path_updated_(false) + path_updated_(false), + in_rotation_(false) { } @@ -51,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 / 2.0)); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( @@ -65,6 +68,7 @@ void RotationShimController::configure( node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", 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", @@ -106,6 +110,7 @@ void RotationShimController::activate() plugin_name_.c_str()); primary_controller_->activate(); + in_rotation_ = false; auto node = node_.lock(); dyn_params_handler_ = node->add_on_set_parameters_callback( @@ -192,10 +197,14 @@ 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_) { + + double angular_thresh = + in_rotation_ ? angular_disengage_threshold_ : angular_dist_threshold_; + if (abs(angular_distance_to_heading) > angular_thresh) { RCLCPP_DEBUG( logger_, "Robot is not within the new path's rough heading, rotating to heading..."); + in_rotation_ = true; return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); } else { RCLCPP_DEBUG( @@ -214,6 +223,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands } // If at this point, use the primary controller to path track + in_rotation_ = false; return primary_controller_->computeVelocityCommands(pose, velocity, goal_checker); }