Conserve curvature with LIMIT action (#5255)

* Conserve curvature with LIMIT action

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix format

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix test

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

Author: tonynajjar

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -548,13 +548,17 @@ bool CollisionMonitor::processStopSlowdownLimit(
       const double linear_vel = std::hypot(velocity.x, velocity.y);  // absolute
       Velocity safe_vel;
       double ratio = 1.0;
+
+      // Calculate the most restrictive ratio to preserve curvature
       if (linear_vel != 0.0) {
-        ratio = std::clamp(polygon->getLinearLimit() / linear_vel, 0.0, 1.0);
+        ratio = std::min(ratio, polygon->getLinearLimit() / linear_vel);
+      }
+      if (velocity.tw != 0.0) {
+        ratio = std::min(ratio, polygon->getAngularLimit() / std::abs(velocity.tw));
       }
-      safe_vel.x = velocity.x * ratio;
-      safe_vel.y = velocity.y * ratio;
-      safe_vel.tw = std::clamp(
-        velocity.tw, -polygon->getAngularLimit(), polygon->getAngularLimit());
+      ratio = std::clamp(ratio, 0.0, 1.0);
+      // Apply the same ratio to all components to preserve curvature
+      safe_vel = velocity * ratio;
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
       if (safe_vel < robot_action.req_vel) {

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -821,10 +821,12 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   publishCmdVel(0.5, 0.2, 0.1);
   ASSERT_TRUE(waitCmdVel(500ms));
   const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2);
-  const double ratio = LINEAR_LIMIT / speed;
+  const double linear_ratio = LINEAR_LIMIT / speed;
+  const double angular_ratio = ANGULAR_LIMIT / 0.1;
+  const double ratio = std::min(linear_ratio, angular_ratio);
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON);
-  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON);
   ASSERT_TRUE(waitActionState(500ms));
   ASSERT_EQ(action_state_->action_type, LIMIT);
   ASSERT_EQ(action_state_->polygon_name, "Limit");
@@ -906,10 +908,12 @@ TEST_F(Tester, testPolygonSource)
   publishCmdVel(0.5, 0.2, 0.1);
   EXPECT_TRUE(waitCmdVel(500ms));
   const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2);
-  const double ratio = LINEAR_LIMIT / speed;
+  const double linear_ratio = LINEAR_LIMIT / speed;
+  const double angular_ratio = ANGULAR_LIMIT / 0.1;
+  const double ratio = std::min(linear_ratio, angular_ratio);
   EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON);
   EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON);
-  EXPECT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON);
+  EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * ratio, EPSILON);
   EXPECT_TRUE(waitActionState(500ms));
   EXPECT_EQ(action_state_->action_type, LIMIT);
   EXPECT_EQ(action_state_->polygon_name, "Limit");