From b9be009b92d07546da9672e31f67e73ad5576f92 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Oct 2024 11:25:37 -0700 Subject: [PATCH 1/2] 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 (cherry picked from commit fc7e08611a453889feb112bc4c2160687d320a91) --- .../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 e5ecbad9e5..fcdc7efaf9 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 9e2485af7c..2ecc74e34d 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -30,7 +30,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) { } @@ -52,6 +53,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( @@ -66,6 +69,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", @@ -107,6 +111,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( @@ -190,10 +195,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( @@ -212,6 +221,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); } From 02633bf38e85ecd150ab4764b1f451550d289d6e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Oct 2024 11:29:02 -0700 Subject: [PATCH 2/2] Update nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp Signed-off-by: Steve Macenski --- .../src/nav2_rotation_shim_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2ecc74e34d..de8ce424e2 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -54,7 +54,7 @@ void RotationShimController::configure( 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)); + node, plugin_name_ + ".angular_disengage_threshold", rclcpp::ParameterValue(0.785)); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared(