Use angular acceleration te acelerate / decelerate

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

Author: ajtudela

nav2_docking/opennav_docking/src/controller.cpp

@@ -139,6 +139,14 @@ geometry_msgs::msg::Twist Controller::computeRotateToHeadingCommand(
     current_velocity.angular.z + rotate_to_heading_max_angular_accel_ * dt;
   cmd_vel.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 * rotate_to_heading_max_angular_accel_ * fabs(angular_distance_to_heading));
+  if (fabs(cmd_vel.angular.z) > max_vel_to_stop) {
+    cmd_vel.angular.z = sign * max_vel_to_stop;
+  }
+
   return cmd_vel;
 }
 

nav2_docking/opennav_docking/test/test_controller.cpp

@@ -603,7 +603,7 @@ TEST(ControllerTests, RotateToHeading) {
   cmd_vel =
     controller->computeRotateToHeadingCommand(angular_distance_to_heading, current_velocity, dt);
   EXPECT_DOUBLE_EQ(cmd_vel.linear.x, 0.0);
-  EXPECT_DOUBLE_EQ(cmd_vel.angular.z, -rotate_to_heading_max_angular_accel * dt);
+  EXPECT_DOUBLE_EQ(cmd_vel.angular.z, 0.0);
 
   controller.reset();
 }