Adding slow down at target heading to RPP Controller (#5361)

* Adding slow down at target heading to RPP

Signed-off-by: SteveMacenski <[email protected]>

* Update test_regulated_pp.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: SteveMacenski <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>

Author: SteveMacenski

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -350,6 +350,12 @@ void RegulatedPurePursuitController::rotateToHeading(
   const double min_feasible_angular_speed = curr_speed.angular.z - params_->max_angular_accel * dt;
   const double max_feasible_angular_speed = curr_speed.angular.z + params_->max_angular_accel * dt;
   angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
+
+  // Check if we need to slow down to avoid overshooting
+  double max_vel_to_stop = std::sqrt(2 * params_->max_angular_accel * fabs(angle_to_path));
+  if (fabs(angular_vel) > max_vel_to_stop) {
+    angular_vel = sign * max_vel_to_stop;
+  }
 }
 
 geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection(

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -549,13 +549,13 @@ TEST(RegulatedPurePursuitTest, rotateTests)
   // basic full speed at a speed
   ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed);
   EXPECT_EQ(lin_v, 0.0);
-  EXPECT_EQ(ang_v, 1.8);
+  EXPECT_EQ(ang_v, 1.6);  // hit slow down limit
 
   // negative direction
   angle_to_path = -0.4;
   curr_speed.angular.z = -1.75;
   ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed);
-  EXPECT_EQ(ang_v, -1.8);
+  EXPECT_EQ(ang_v, -1.6);  // hit slow down limit
 
   // kinematic clamping, no speed, some speed accelerating, some speed decelerating
   angle_to_path = 0.4;