Adding disengagement threshold to rotation shim controller (backport #…

…4699) (#4702)

* Adding disengagement threshold to rotation shim controller (#4699)

* adding disengagement threshold to rotation shim controller

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* change default to 22.5 deg

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
(cherry picked from commit fc7e086)

* Update nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

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_;

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));
  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);
 }