Add acc limit consideration to avoid overshoot in RotationShimController

Signed-off-by: RBT22 <[email protected]>
ros-navigation · Jan 17, 2025 · 3e9b083 · 3e9b083
1 parent a33e8d2
commit 3e9b083
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
@@ -300,6 +300,12 @@ RotationShimController::computeRotateToHeadingCommand(
   cmd_vel.twist.angular.z =
     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 * max_angular_accel_ * fabs(angular_distance_to_heading));
+  if (fabs(cmd_vel.twist.angular.z) > max_vel_to_stop) {
+    cmd_vel.twist.angular.z = sign * max_vel_to_stop;
+  }
+
   isCollisionFree(cmd_vel, angular_distance_to_heading, pose);
   return cmd_vel;
 }