From fc7e08611a453889feb112bc4c2160687d320a91 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Oct 2024 11:25:37 -0700 Subject: [PATCH] Adding disengagement threshold to rotation shim controller (#4699) * 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 --- .../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 f0da5e0fba2..d3807eeb952 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 @@ -168,10 +168,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 4e111edfda5..5947f5e36e4 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -34,7 +34,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) { } @@ -56,6 +57,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( @@ -70,6 +73,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", @@ -111,6 +115,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( @@ -197,10 +202,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( @@ -219,6 +228,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); }