merged with origin

Author: sandeepdutta

nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp

@@ -172,11 +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_, angular_dist_threshold_param_;
-  double angular_disengage_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)
 {
 }
 
@@ -53,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/3.0));  // 15 deg
+    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(
@@ -68,9 +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_);
-  angular_dist_threshold_param_ = angular_dist_threshold_;
-  node->get_parameter(
-    plugin_name_ + ".angular_disengage_threshold", angular_disengage_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",
@@ -112,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(
@@ -195,11 +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_) {
-        angular_dist_threshold_ = angular_disengage_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 {
         angular_dist_threshold_ = angular_dist_threshold_param_;
@@ -219,6 +222,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);
 }