From e5cc81df94c748c9203dbdb6ac010c0d21dbe4b0 Mon Sep 17 00:00:00 2001
From: Brice <brice.renaudeau@gmail.com>
Date: Thu, 30 Mar 2023 19:27:04 +0200
Subject: [PATCH 01/11] add LIMIT polygon type

---
 nav2_collision_monitor/README.md              |  3 +-
 .../include/nav2_collision_monitor/circle.hpp |  2 +-
 .../collision_monitor_node.hpp                |  4 +-
 .../nav2_collision_monitor/polygon.hpp        | 26 ++++++++-
 .../include/nav2_collision_monitor/types.hpp  |  3 +-
 .../params/collision_monitor_params.yaml      | 12 +++-
 .../src/collision_monitor_node.cpp            | 25 +++++++--
 nav2_collision_monitor/src/polygon.cpp        | 34 +++++++++++-
 .../test/collision_monitor_node_test.cpp      | 55 +++++++++++++------
 nav2_collision_monitor/test/polygons_test.cpp | 33 +++++++++++
 10 files changed, 166 insertions(+), 31 deletions(-)

diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md
index 788a77ebd4..134a52cff6 100644
--- a/nav2_collision_monitor/README.md
+++ b/nav2_collision_monitor/README.md
@@ -23,6 +23,7 @@ The following models of safety behaviors are employed by Collision Monitor:
 
 * **Stop model**: Define a zone and a point threshold. If more that `N` obstacle points appear inside this area, stop the robot until the obstacles will disappear.
 * **Slowdown model**: Define a zone around the robot and slow the maximum speed for a `%S` percent, if more than `N` points will appear inside the area.
+* **Limit model**: Define a zone around the robot and clamp the maximum speed below a fixed value, if more than `N` points will appear inside the area.
 * **Approach model**: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than `M` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least `M` seconds to collision. The effect here would be to keep the robot always `M` seconds from any collision.
 
 The zones around the robot can take the following shapes:
@@ -56,7 +57,7 @@ Designed to be used in wide variety of robots (incl. moving fast) and have a hig
 Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points).
 The table below represents the details of operating times for different behavior models and shapes:
 
-| | Stop/Slowdown model, Polygon area | Stop/Slowdown model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint |
+| | Stop/Slowdown/Limit model, Polygon area | Stop/Slowdown/Limit model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint |
 |-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------|
 | LaserScan (360 points) processing time, ms  | 4.45 | 4.45 | 4.93  | 4.86  |
 | PointCloud (24K points) processing time, ms | 4.94 | 4.06 | 20.67 | 10.87 |
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
index 9cab7485be..c440032701 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
@@ -26,7 +26,7 @@ namespace nav2_collision_monitor
 
 /**
  * @brief Circle shape implementaiton.
- * For STOP/SLOWDOWN model it represents zone around the robot
+ * For STOP/SLOWDOWN/LIMIT model it represents zone around the robot
  * while for APPROACH model it represents robot footprint.
  */
 class Circle : public Polygon
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
index adeab2daee..52934fc12d 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
@@ -147,14 +147,14 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   void process(const Velocity & cmd_vel_in);
 
   /**
-   * @brief Processes the polygon of STOP and SLOWDOWN action type
+   * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type
    * @param polygon Polygon to process
    * @param collision_points Array of 2D obstacle points
    * @param velocity Desired robot velocity
    * @param robot_action Output processed robot action
    * @return True if returned action is caused by current polygon, otherwise false
    */
-  bool processStopSlowdown(
+  bool processStopSlowdownLimit(
     const std::shared_ptr<Polygon> polygon,
     const std::vector<Point> & collision_points,
     const Velocity & velocity,
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
index 3580a45245..ed0dbfc3fa 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -35,7 +35,7 @@ namespace nav2_collision_monitor
 
 /**
  * @brief Basic polygon shape class.
- * For STOP/SLOWDOWN model it represents zone around the robot
+ * For STOP/SLOWDOWN/LIMIT model it represents zone around the robot
  * while for APPROACH model it represents robot footprint.
  */
 class Polygon
@@ -96,6 +96,24 @@ class Polygon
    * @return Speed slowdown ratio
    */
   double getSlowdownRatio() const;
+  /**
+   * @brief Obtains speed linear x limit for current polygon.
+   * Applicable for LIMIT model.
+   * @return Speed limit x
+   */
+  double getLimitX() const;
+  /**
+   * @brief Obtains speed linear y limit for current polygon.
+   * Applicable for LIMIT model.
+   * @return Speed limit y
+   */
+  double getLimitY() const;
+  /**
+   * @brief Obtains speed angular z limit for current polygon.
+   * Applicable for LIMIT model.
+   * @return Speed limit tw
+   */
+  double getLimitTW() const;
   /**
    * @brief Obtains required time before collision for current polygon.
    * Applicable for APPROACH model.
@@ -202,6 +220,12 @@ class Polygon
   int min_points_;
   /// @brief Robot slowdown (share of its actual speed)
   double slowdown_ratio_;
+  /// @brief Robot linear x limit
+  double limit_x_;
+  /// @brief Robot linear y limit
+  double limit_y_;
+  /// @brief Robot angular tw limit
+  double limit_tw_;
   /// @brief Time before collision in seconds
   double time_before_collision_;
   /// @brief Time step for robot movement simulation
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
index 585faeafcb..273a4868b3 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
@@ -67,7 +67,8 @@ enum ActionType
   DO_NOTHING = 0,  // No action
   STOP = 1,  // Stop the robot
   SLOWDOWN = 2,  // Slowdown in percentage from current operating speed
-  APPROACH = 3  // Keep constant time interval before collision
+  APPROACH = 3,  // Keep constant time interval before collision
+  LIMIT = 4  // Limit current operating speed
 };
 
 /// @brief Action for robot
diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
index 23a8b5548c..d90d2e239b 100644
--- a/nav2_collision_monitor/params/collision_monitor_params.yaml
+++ b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -9,7 +9,7 @@ collision_monitor:
     source_timeout: 5.0
     base_shift_correction: True
     stop_pub_timeout: 2.0
-    # Polygons represent zone around the robot for "stop" and "slowdown" action types,
+    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
     # and robot footprint for "approach" action type.
     # Footprint could be "polygon" type with dynamically set footprint from footprint_topic
     # or "circle" type with static footprint set by radius. "footprint_topic" parameter
@@ -30,6 +30,16 @@ collision_monitor:
       slowdown_ratio: 0.3
       visualize: True
       polygon_pub_topic: "polygon_slowdown"
+    PolygonLimit:
+      type: "polygon"
+      points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5]
+      action_type: "limit"
+      max_points: 3
+      limit_x: 0.3
+      limit_y: 0.0
+      limit_tw: 0.5
+      visualize: True
+      polygon_pub_topic: "polygon_limit"
     FootprintApproach:
       type: "polygon"
       action_type: "approach"
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index fb330ab43d..07975f5786 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -380,9 +380,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     }
 
     const ActionType at = polygon->getActionType();
-    if (at == STOP || at == SLOWDOWN) {
+    if (at == STOP || at == SLOWDOWN || at == LIMIT) {
       // Process STOP/SLOWDOWN for the selected polygon
-      if (processStopSlowdown(polygon, collision_points, cmd_vel_in, robot_action)) {
+      if (processStopSlowdownLimit(polygon, collision_points, cmd_vel_in, robot_action)) {
         action_polygon = polygon;
       }
     } else if (at == APPROACH) {
@@ -407,7 +407,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
   robot_action_prev_ = robot_action;
 }
 
-bool CollisionMonitor::processStopSlowdown(
+bool CollisionMonitor::processStopSlowdownLimit(
   const std::shared_ptr<Polygon> polygon,
   const std::vector<Point> & collision_points,
   const Velocity & velocity,
@@ -426,7 +426,7 @@ bool CollisionMonitor::processStopSlowdown(
       robot_action.req_vel.y = 0.0;
       robot_action.req_vel.tw = 0.0;
       return true;
-    } else {  // SLOWDOWN
+    } else if (polygon->getActionType() == SLOWDOWN) {  // SLOWDOWN
       const Velocity safe_vel = velocity * polygon->getSlowdownRatio();
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
@@ -436,6 +436,18 @@ bool CollisionMonitor::processStopSlowdown(
         robot_action.req_vel = safe_vel;
         return true;
       }
+    } else {  // Limit
+      Velocity safe_vel;
+      safe_vel.x = std::clamp(velocity.x, -polygon->getLimitX(), polygon->getLimitX());
+      safe_vel.y = std::clamp(velocity.x, -polygon->getLimitY(), polygon->getLimitY());
+      safe_vel.tw = std::clamp(velocity.x, -polygon->getLimitTW(), polygon->getLimitTW());
+      // Check that currently calculated velocity is safer than
+      // chosen for previous shapes one
+      if (safe_vel < robot_action.req_vel) {
+        robot_action.action_type = LIMIT;
+        robot_action.req_vel = safe_vel;
+        return true;
+      }
     }
   }
 
@@ -487,6 +499,11 @@ void CollisionMonitor::notifyActionState(
       "Robot to slowdown for %f percents due to %s polygon",
       action_polygon->getSlowdownRatio() * 100,
       action_polygon->getName().c_str());
+  } else if (robot_action.action_type == LIMIT) {
+    RCLCPP_INFO(
+      get_logger(),
+      "Robot to limit speed due to %s polygon",
+      action_polygon->getName().c_str());
   } else if (robot_action.action_type == APPROACH) {
     RCLCPP_INFO(
       get_logger(),
diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
index edde883c95..e447f14aa2 100644
--- a/nav2_collision_monitor/src/polygon.cpp
+++ b/nav2_collision_monitor/src/polygon.cpp
@@ -35,7 +35,8 @@ Polygon::Polygon(
   const std::string & base_frame_id,
   const tf2::Duration & transform_tolerance)
 : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING),
-  slowdown_ratio_(0.0), footprint_sub_(nullptr), tf_buffer_(tf_buffer),
+  slowdown_ratio_(0.0), limit_x_(0.0), limit_y_(0.0), limit_tw_(0.0),
+  footprint_sub_(nullptr), tf_buffer_(tf_buffer),
   base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance)
 {
   RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str());
@@ -138,6 +139,21 @@ double Polygon::getSlowdownRatio() const
   return slowdown_ratio_;
 }
 
+double Polygon::getLimitX() const
+{
+  return limit_x_;
+}
+
+double Polygon::getLimitY() const
+{
+  return limit_y_;
+}
+
+double Polygon::getLimitTW() const
+{
+  return limit_tw_;
+}
+
 double Polygon::getTimeBeforeCollision() const
 {
   return time_before_collision_;
@@ -256,6 +272,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
       action_type_ = STOP;
     } else if (at_str == "slowdown") {
       action_type_ = SLOWDOWN;
+    } else if (at_str == "limit") {
+      action_type_ = LIMIT;
     } else if (at_str == "approach") {
       action_type_ = APPROACH;
     } else {  // Error if something else
@@ -286,6 +304,18 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
       slowdown_ratio_ = node->get_parameter(polygon_name_ + ".slowdown_ratio").as_double();
     }
 
+    if (action_type_ == LIMIT) {
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".limit_x", rclcpp::ParameterValue(0.5));
+      limit_x_ = node->get_parameter(polygon_name_ + ".limit_x").as_double();
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".limit_y", rclcpp::ParameterValue(0.5));
+      limit_y_ = node->get_parameter(polygon_name_ + ".limit_y").as_double();
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".limit_tw", rclcpp::ParameterValue(0.5));
+      limit_tw_ = node->get_parameter(polygon_name_ + ".limit_tw").as_double();
+    }
+
     if (action_type_ == APPROACH) {
       nav2_util::declare_parameter_if_not_declared(
         node, polygon_name_ + ".time_before_collision", rclcpp::ParameterValue(2.0));
@@ -374,7 +404,7 @@ bool Polygon::getParameters(
         polygon_name_.c_str());
     }
 
-    if (action_type_ == STOP || action_type_ == SLOWDOWN) {
+    if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT) {
       // Dynamic polygon will be used
       nav2_util::declare_parameter_if_not_declared(
         node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING);
diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
index 5ba19d443c..44f0b64f79 100644
--- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -54,6 +54,9 @@ static const char POINTCLOUD_NAME[]{"PointCloud"};
 static const char RANGE_NAME[]{"Range"};
 static const int MIN_POINTS{2};
 static const double SLOWDOWN_RATIO{0.7};
+static const double LIMIT_X{0.4};
+static const double LIMIT_Y{0.15};
+static const double LIMIT_TW{0.09};
 static const double TIME_BEFORE_COLLISION{1.0};
 static const double SIMULATION_TIME_STEP{0.01};
 static const double TRANSFORM_TOLERANCE{0.5};
@@ -321,6 +324,21 @@ void Tester::addPolygon(
   cm_->set_parameter(
     rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO));
 
+  cm_->declare_parameter(
+    polygon_name + ".limit_x", rclcpp::ParameterValue(LIMIT_X));
+  cm_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".limit_x", LIMIT_X));
+
+  cm_->declare_parameter(
+    polygon_name + ".limit_y", rclcpp::ParameterValue(LIMIT_Y));
+  cm_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".limit_y", LIMIT_Y));
+
+  cm_->declare_parameter(
+    polygon_name + ".limit_tw", rclcpp::ParameterValue(LIMIT_TW));
+  cm_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".limit_tw", LIMIT_TW));
+
   cm_->declare_parameter(
     polygon_name + ".time_before_collision", rclcpp::ParameterValue(TIME_BEFORE_COLLISION));
   cm_->set_parameter(
@@ -590,17 +608,18 @@ void Tester::actionStateCallback(nav2_msgs::msg::CollisionMonitorState::SharedPt
   action_state_ = msg;
 }
 
-TEST_F(Tester, testProcessStopSlowdown)
+TEST_F(Tester, testProcessStopSlowdownLimit)
 {
   rclcpp::Time curr_time = cm_->now();
 
   // Set Collision Monitor parameters.
   // Making two polygons: outer polygon for slowdown and inner for robot stop.
   setCommonParameters();
+  addPolygon("Limit", POLYGON, 3.0, "limit");
   addPolygon("SlowDown", POLYGON, 2.0, "slowdown");
   addPolygon("Stop", POLYGON, 1.0, "stop");
   addSource(SCAN_NAME, SCAN);
-  setVectors({"SlowDown", "Stop"}, {SCAN_NAME});
+  setVectors({"Limit", "SlowDown", "Stop"}, {SCAN_NAME});
 
   // Start Collision Monitor node
   cm_->start();
@@ -609,15 +628,24 @@ TEST_F(Tester, testProcessStopSlowdown)
   sendTransforms(curr_time);
 
   // 1. Obstacle is far away from robot
-  publishScan(3.0, curr_time);
-  ASSERT_TRUE(waitData(3.0, 500ms, curr_time));
+  publishScan(3.5, curr_time);
+  ASSERT_TRUE(waitData(3.5, 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
   ASSERT_TRUE(waitCmdVel(500ms));
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON);
 
-  // 2. Obstacle is in slowdown robot zone
+  // 2. Obstacle is in limit robot zone
+  publishScan(2.5, curr_time);
+  ASSERT_TRUE(waitData(2.5, 500ms, curr_time));
+  publishCmdVel(0.5, 0.2, 0.1);
+  ASSERT_TRUE(waitCmdVel(500ms));
+  ASSERT_NEAR(cmd_vel_out_->linear.x, 0.4, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->linear.y, 0.15, EPSILON);
+  ASSERT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON);
+
+  // 3. Obstacle is in slowdown robot zone
   publishScan(1.5, curr_time);
   ASSERT_TRUE(waitData(1.5, 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
@@ -625,11 +653,8 @@ TEST_F(Tester, testProcessStopSlowdown)
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * SLOWDOWN_RATIO, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON);
-  ASSERT_TRUE(waitActionState(500ms));
-  ASSERT_EQ(action_state_->action_type, SLOWDOWN);
-  ASSERT_EQ(action_state_->polygon_name, "SlowDown");
 
-  // 3. Obstacle is inside stop zone
+  // 4. Obstacle is inside stop zone
   publishScan(0.5, curr_time);
   ASSERT_TRUE(waitData(0.5, 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
@@ -637,21 +662,15 @@ TEST_F(Tester, testProcessStopSlowdown)
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON);
-  ASSERT_TRUE(waitActionState(500ms));
-  ASSERT_EQ(action_state_->action_type, STOP);
-  ASSERT_EQ(action_state_->polygon_name, "Stop");
 
-  // 4. Restoring back normal operation
-  publishScan(3.0, curr_time);
-  ASSERT_TRUE(waitData(3.0, 500ms, curr_time));
+  // 5. Restoring back normal operation
+  publishScan(3.5, curr_time);
+  ASSERT_TRUE(waitData(3.5, 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
   ASSERT_TRUE(waitCmdVel(500ms));
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON);
-  ASSERT_TRUE(waitActionState(500ms));
-  ASSERT_EQ(action_state_->action_type, DO_NOTHING);
-  ASSERT_EQ(action_state_->polygon_name, "");
 
   // Stop Collision Monitor node
   cm_->stop();
diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp
index db3e80805f..84df2aed39 100644
--- a/nav2_collision_monitor/test/polygons_test.cpp
+++ b/nav2_collision_monitor/test/polygons_test.cpp
@@ -53,6 +53,9 @@ static const std::vector<double> ARBITRARY_POLYGON {
 static const double CIRCLE_RADIUS{0.5};
 static const int MIN_POINTS{2};
 static const double SLOWDOWN_RATIO{0.7};
+static const double LIMIT_X{0.4};
+static const double LIMIT_Y{0.2};
+static const double LIMIT_TW{0.09};
 static const double TIME_BEFORE_COLLISION{1.0};
 static const double SIMULATION_TIME_STEP{0.01};
 static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)};
@@ -276,6 +279,21 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st
   test_node_->set_parameter(
     rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO));
 
+  test_node_->declare_parameter(
+    polygon_name + ".limit_x", rclcpp::ParameterValue(LIMIT_X));
+  test_node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".limit_x", LIMIT_X));
+
+  test_node_->declare_parameter(
+    polygon_name + ".limit_y", rclcpp::ParameterValue(LIMIT_Y));
+  test_node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".limit_y", LIMIT_Y));
+
+  test_node_->declare_parameter(
+    polygon_name + ".limit_tw", rclcpp::ParameterValue(LIMIT_TW));
+  test_node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".limit_tw", LIMIT_TW));
+
   test_node_->declare_parameter(
     polygon_name + ".time_before_collision",
     rclcpp::ParameterValue(TIME_BEFORE_COLLISION));
@@ -462,6 +480,21 @@ TEST_F(Tester, testPolygonGetSlowdownParameters)
   EXPECT_NEAR(polygon_->getSlowdownRatio(), SLOWDOWN_RATIO, EPSILON);
 }
 
+TEST_F(Tester, testPolygonGetLimitParameters)
+{
+  createPolygon("limit", true);
+
+  // Check that common parameters set correctly
+  EXPECT_EQ(polygon_->getName(), POLYGON_NAME);
+  EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::LIMIT);
+  EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS);
+  EXPECT_EQ(polygon_->isVisualize(), true);
+  // Check that limit params are correct
+  EXPECT_NEAR(polygon_->getLimitX(), LIMIT_X, EPSILON);
+  EXPECT_NEAR(polygon_->getLimitY(), LIMIT_Y, EPSILON);
+  EXPECT_NEAR(polygon_->getLimitTW(), LIMIT_TW, EPSILON);
+}
+
 TEST_F(Tester, testPolygonGetApproachParameters)
 {
   createPolygon("approach", true);

From e47cad37008a1e0d988ed95168150d7f85e8642d Mon Sep 17 00:00:00 2001
From: Brice <brice.renaudeau@gmail.com>
Date: Thu, 30 Mar 2023 19:31:01 +0200
Subject: [PATCH 02/11] fix unit tests

---
 .../test/collision_monitor_node_test.cpp             | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
index 44f0b64f79..3086c6685c 100644
--- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -644,6 +644,9 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.4, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.15, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON);
+  ASSERT_TRUE(waitActionState(500ms));
+  ASSERT_EQ(action_state_->action_type, LIMIT);
+  ASSERT_EQ(action_state_->polygon_name, "Limit");
 
   // 3. Obstacle is in slowdown robot zone
   publishScan(1.5, curr_time);
@@ -653,6 +656,9 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2 * SLOWDOWN_RATIO, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON);
+  ASSERT_TRUE(waitActionState(500ms));
+  ASSERT_EQ(action_state_->action_type, SLOWDOWN);
+  ASSERT_EQ(action_state_->polygon_name, "SlowDown");
 
   // 4. Obstacle is inside stop zone
   publishScan(0.5, curr_time);
@@ -662,6 +668,9 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON);
+  ASSERT_TRUE(waitActionState(500ms));
+  ASSERT_EQ(action_state_->action_type, STOP);
+  ASSERT_EQ(action_state_->polygon_name, "Stop");
 
   // 5. Restoring back normal operation
   publishScan(3.5, curr_time);
@@ -671,6 +680,9 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON);
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON);
+  ASSERT_TRUE(waitActionState(500ms));
+  ASSERT_EQ(action_state_->action_type, DO_NOTHING);
+  ASSERT_EQ(action_state_->polygon_name, "");
 
   // Stop Collision Monitor node
   cm_->stop();

From 4821f070366401eac9e9c922252b3aa0fc86c069 Mon Sep 17 00:00:00 2001
From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Date: Fri, 31 Mar 2023 10:37:02 +0200
Subject: [PATCH 03/11] Fix MIN_POINT doesn't exist

---
 nav2_collision_monitor/test/polygons_test.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp
index 84df2aed39..cca2930169 100644
--- a/nav2_collision_monitor/test/polygons_test.cpp
+++ b/nav2_collision_monitor/test/polygons_test.cpp
@@ -487,7 +487,7 @@ TEST_F(Tester, testPolygonGetLimitParameters)
   // Check that common parameters set correctly
   EXPECT_EQ(polygon_->getName(), POLYGON_NAME);
   EXPECT_EQ(polygon_->getActionType(), nav2_collision_monitor::LIMIT);
-  EXPECT_EQ(polygon_->getMaxPoints(), MAX_POINTS);
+  EXPECT_EQ(polygon_->getMinPoints(), MIN_POINTS);
   EXPECT_EQ(polygon_->isVisualize(), true);
   // Check that limit params are correct
   EXPECT_NEAR(polygon_->getLimitX(), LIMIT_X, EPSILON);

From 1484b68d05ce157c1b4e228a36f1afc768ad875a Mon Sep 17 00:00:00 2001
From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Date: Fri, 31 Mar 2023 10:51:24 +0200
Subject: [PATCH 04/11] Fix Action type enum

---
 nav2_collision_monitor/test/collision_monitor_node_test.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
index 3086c6685c..3a02a1e5e4 100644
--- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -84,6 +84,7 @@ enum ActionType
   STOP = 1,
   SLOWDOWN = 2,
   APPROACH = 3,
+  LIMIT = 4,
 };
 
 class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor

From 5ae8201e78ae46196af5f044da950bf841554e0c Mon Sep 17 00:00:00 2001
From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Date: Fri, 31 Mar 2023 18:28:09 +0200
Subject: [PATCH 05/11] FIX velocity used

---
 nav2_collision_monitor/src/collision_monitor_node.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index 07975f5786..a093161563 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -439,8 +439,8 @@ bool CollisionMonitor::processStopSlowdownLimit(
     } else {  // Limit
       Velocity safe_vel;
       safe_vel.x = std::clamp(velocity.x, -polygon->getLimitX(), polygon->getLimitX());
-      safe_vel.y = std::clamp(velocity.x, -polygon->getLimitY(), polygon->getLimitY());
-      safe_vel.tw = std::clamp(velocity.x, -polygon->getLimitTW(), polygon->getLimitTW());
+      safe_vel.y = std::clamp(velocity.y, -polygon->getLimitY(), polygon->getLimitY());
+      safe_vel.tw = std::clamp(velocity.tw, -polygon->getLimitTW(), polygon->getLimitTW());
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
       if (safe_vel < robot_action.req_vel) {

From 279dd3c5cd4e03230bcc1630242db1bae1f27f9e Mon Sep 17 00:00:00 2001
From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Date: Mon, 3 Apr 2023 11:06:11 +0200
Subject: [PATCH 06/11] FIX unit test point distance

increase point distance to not be in limit field
---
 .../test/collision_monitor_node_test.cpp                  | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
index 3a02a1e5e4..7bf4dc2c18 100644
--- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -629,8 +629,8 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   sendTransforms(curr_time);
 
   // 1. Obstacle is far away from robot
-  publishScan(3.5, curr_time);
-  ASSERT_TRUE(waitData(3.5, 500ms, curr_time));
+  publishScan(4.5, curr_time);
+  ASSERT_TRUE(waitData(4.5, 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
   ASSERT_TRUE(waitCmdVel(500ms));
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON);
@@ -674,8 +674,8 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   ASSERT_EQ(action_state_->polygon_name, "Stop");
 
   // 5. Restoring back normal operation
-  publishScan(3.5, curr_time);
-  ASSERT_TRUE(waitData(3.5, 500ms, curr_time));
+  publishScan(4.5, curr_time);
+  ASSERT_TRUE(waitData(4.5, 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
   ASSERT_TRUE(waitCmdVel(500ms));
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON);

From 4f8b4100ba332a08eacd3fb23615bc48830899a0 Mon Sep 17 00:00:00 2001
From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Date: Mon, 3 Apr 2023 11:08:21 +0200
Subject: [PATCH 07/11] Update collision_monitor_node_test.cpp

---
 nav2_collision_monitor/test/collision_monitor_node_test.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
index 7bf4dc2c18..22ead7db89 100644
--- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -638,8 +638,8 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON);
 
   // 2. Obstacle is in limit robot zone
-  publishScan(2.5, curr_time);
-  ASSERT_TRUE(waitData(2.5, 500ms, curr_time));
+  publishScan(3.0, curr_time);
+  ASSERT_TRUE(waitData(3.0, 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
   ASSERT_TRUE(waitCmdVel(500ms));
   ASSERT_NEAR(cmd_vel_out_->linear.x, 0.4, EPSILON);

From 75ce3145b43daa3e1dbeec53a79334c74134240a Mon Sep 17 00:00:00 2001
From: Brice <brice.renaudeau@gmail.com>
Date: Mon, 3 Apr 2023 15:42:47 +0200
Subject: [PATCH 08/11] fix status name not updated

---
 nav2_collision_monitor/src/collision_monitor_node.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index a093161563..43a4aaebb8 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -444,6 +444,7 @@ bool CollisionMonitor::processStopSlowdownLimit(
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
       if (safe_vel < robot_action.req_vel) {
+        robot_action.polygon_name = polygon->getName();
         robot_action.action_type = LIMIT;
         robot_action.req_vel = safe_vel;
         return true;

From 3577999870dc39d3e11f3012cab89109d15e12f7 Mon Sep 17 00:00:00 2001
From: Brice <brice.renaudeau@gmail.com>
Date: Tue, 4 Apr 2023 15:52:55 +0200
Subject: [PATCH 09/11] [MOD] only single linear limit

---
 .../nav2_collision_monitor/polygon.hpp        | 26 +++++++------------
 .../params/collision_monitor_params.yaml      |  5 ++--
 .../src/collision_monitor_node.cpp            | 13 +++++++---
 nav2_collision_monitor/src/polygon.cpp        | 26 +++++++------------
 .../test/collision_monitor_node_test.cpp      | 24 +++++++----------
 nav2_collision_monitor/test/polygons_test.cpp | 23 ++++++----------
 6 files changed, 48 insertions(+), 69 deletions(-)

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
index ed0dbfc3fa..9ff6e09ee9 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -97,23 +97,17 @@ class Polygon
    */
   double getSlowdownRatio() const;
   /**
-   * @brief Obtains speed linear x limit for current polygon.
+   * @brief Obtains speed linear limit for current polygon.
    * Applicable for LIMIT model.
-   * @return Speed limit x
+   * @return Speed linear limit
    */
-  double getLimitX() const;
-  /**
-   * @brief Obtains speed linear y limit for current polygon.
-   * Applicable for LIMIT model.
-   * @return Speed limit y
-   */
-  double getLimitY() const;
+  double getLinearLimit() const;
   /**
    * @brief Obtains speed angular z limit for current polygon.
    * Applicable for LIMIT model.
-   * @return Speed limit tw
+   * @return Speed angular limit
    */
-  double getLimitTW() const;
+  double getAngularLimit() const;
   /**
    * @brief Obtains required time before collision for current polygon.
    * Applicable for APPROACH model.
@@ -220,12 +214,10 @@ class Polygon
   int min_points_;
   /// @brief Robot slowdown (share of its actual speed)
   double slowdown_ratio_;
-  /// @brief Robot linear x limit
-  double limit_x_;
-  /// @brief Robot linear y limit
-  double limit_y_;
-  /// @brief Robot angular tw limit
-  double limit_tw_;
+  /// @brief Robot linear limit
+  double linear_limit_;
+  /// @brief Robot angular limit
+  double angular_limit_;
   /// @brief Time before collision in seconds
   double time_before_collision_;
   /// @brief Time step for robot movement simulation
diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
index d90d2e239b..25bf9335f1 100644
--- a/nav2_collision_monitor/params/collision_monitor_params.yaml
+++ b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -35,9 +35,8 @@ collision_monitor:
       points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5]
       action_type: "limit"
       max_points: 3
-      limit_x: 0.3
-      limit_y: 0.0
-      limit_tw: 0.5
+      linear_limit: 0.4
+      angular_limit: 0.5
       visualize: True
       polygon_pub_topic: "polygon_limit"
     FootprintApproach:
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index 43a4aaebb8..67212e95bb 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -437,10 +437,17 @@ bool CollisionMonitor::processStopSlowdownLimit(
         return true;
       }
     } else {  // Limit
+      // Compute linear velocity
+      const double linear_vel = std::sqrt(velocity.x * velocity.x + velocity.y * velocity.y); // absolute
       Velocity safe_vel;
-      safe_vel.x = std::clamp(velocity.x, -polygon->getLimitX(), polygon->getLimitX());
-      safe_vel.y = std::clamp(velocity.y, -polygon->getLimitY(), polygon->getLimitY());
-      safe_vel.tw = std::clamp(velocity.tw, -polygon->getLimitTW(), polygon->getLimitTW());
+      double ratio = 1.0;
+      if (linear_vel != 0.0) {
+        ratio = std::clamp(polygon->getLinearLimit() / linear_vel, 0.0, 1.0);
+      }
+      safe_vel.x = velocity.x * ratio;
+      safe_vel.y = velocity.y * ratio;
+      safe_vel.tw = std::clamp(
+        velocity.tw, -polygon->getAngularLimit(), polygon->getAngularLimit());
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
       if (safe_vel < robot_action.req_vel) {
diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
index e447f14aa2..8af65d9159 100644
--- a/nav2_collision_monitor/src/polygon.cpp
+++ b/nav2_collision_monitor/src/polygon.cpp
@@ -35,7 +35,7 @@ Polygon::Polygon(
   const std::string & base_frame_id,
   const tf2::Duration & transform_tolerance)
 : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING),
-  slowdown_ratio_(0.0), limit_x_(0.0), limit_y_(0.0), limit_tw_(0.0),
+  slowdown_ratio_(0.0), linear_limit_(0.0), angular_limit_(0.0),
   footprint_sub_(nullptr), tf_buffer_(tf_buffer),
   base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance)
 {
@@ -139,19 +139,14 @@ double Polygon::getSlowdownRatio() const
   return slowdown_ratio_;
 }
 
-double Polygon::getLimitX() const
+double Polygon::getLinearLimit() const
 {
-  return limit_x_;
+  return linear_limit_;
 }
 
-double Polygon::getLimitY() const
+double Polygon::getAngularLimit() const
 {
-  return limit_y_;
-}
-
-double Polygon::getLimitTW() const
-{
-  return limit_tw_;
+  return angular_limit_;
 }
 
 double Polygon::getTimeBeforeCollision() const
@@ -306,14 +301,11 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
 
     if (action_type_ == LIMIT) {
       nav2_util::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".limit_x", rclcpp::ParameterValue(0.5));
-      limit_x_ = node->get_parameter(polygon_name_ + ".limit_x").as_double();
-      nav2_util::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".limit_y", rclcpp::ParameterValue(0.5));
-      limit_y_ = node->get_parameter(polygon_name_ + ".limit_y").as_double();
+        node, polygon_name_ + ".linear_limit", rclcpp::ParameterValue(0.5));
+      linear_limit_ = node->get_parameter(polygon_name_ + ".linear_limit").as_double();
       nav2_util::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".limit_tw", rclcpp::ParameterValue(0.5));
-      limit_tw_ = node->get_parameter(polygon_name_ + ".limit_tw").as_double();
+        node, polygon_name_ + ".angular_limit", rclcpp::ParameterValue(0.5));
+      angular_limit_ = node->get_parameter(polygon_name_ + ".angular_limit").as_double();
     }
 
     if (action_type_ == APPROACH) {
diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
index 22ead7db89..5491e948e3 100644
--- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp
+++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -54,9 +54,8 @@ static const char POINTCLOUD_NAME[]{"PointCloud"};
 static const char RANGE_NAME[]{"Range"};
 static const int MIN_POINTS{2};
 static const double SLOWDOWN_RATIO{0.7};
-static const double LIMIT_X{0.4};
-static const double LIMIT_Y{0.15};
-static const double LIMIT_TW{0.09};
+static const double LINEAR_LIMIT{0.4};
+static const double ANGULAR_LIMIT{0.09};
 static const double TIME_BEFORE_COLLISION{1.0};
 static const double SIMULATION_TIME_STEP{0.01};
 static const double TRANSFORM_TOLERANCE{0.5};
@@ -326,19 +325,14 @@ void Tester::addPolygon(
     rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO));
 
   cm_->declare_parameter(
-    polygon_name + ".limit_x", rclcpp::ParameterValue(LIMIT_X));
+    polygon_name + ".linear_limit", rclcpp::ParameterValue(LINEAR_LIMIT));
   cm_->set_parameter(
-    rclcpp::Parameter(polygon_name + ".limit_x", LIMIT_X));
+    rclcpp::Parameter(polygon_name + ".linear_limit", LINEAR_LIMIT));
 
   cm_->declare_parameter(
-    polygon_name + ".limit_y", rclcpp::ParameterValue(LIMIT_Y));
+    polygon_name + ".angular_limit", rclcpp::ParameterValue(ANGULAR_LIMIT));
   cm_->set_parameter(
-    rclcpp::Parameter(polygon_name + ".limit_y", LIMIT_Y));
-
-  cm_->declare_parameter(
-    polygon_name + ".limit_tw", rclcpp::ParameterValue(LIMIT_TW));
-  cm_->set_parameter(
-    rclcpp::Parameter(polygon_name + ".limit_tw", LIMIT_TW));
+    rclcpp::Parameter(polygon_name + ".angular_limit", ANGULAR_LIMIT));
 
   cm_->declare_parameter(
     polygon_name + ".time_before_collision", rclcpp::ParameterValue(TIME_BEFORE_COLLISION));
@@ -642,8 +636,10 @@ TEST_F(Tester, testProcessStopSlowdownLimit)
   ASSERT_TRUE(waitData(3.0, 500ms, curr_time));
   publishCmdVel(0.5, 0.2, 0.1);
   ASSERT_TRUE(waitCmdVel(500ms));
-  ASSERT_NEAR(cmd_vel_out_->linear.x, 0.4, EPSILON);
-  ASSERT_NEAR(cmd_vel_out_->linear.y, 0.15, EPSILON);
+  const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2);
+  const double ratio = LINEAR_LIMIT / speed;
+  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_TRUE(waitActionState(500ms));
   ASSERT_EQ(action_state_->action_type, LIMIT);
diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp
index cca2930169..4b4f42ec55 100644
--- a/nav2_collision_monitor/test/polygons_test.cpp
+++ b/nav2_collision_monitor/test/polygons_test.cpp
@@ -53,9 +53,8 @@ static const std::vector<double> ARBITRARY_POLYGON {
 static const double CIRCLE_RADIUS{0.5};
 static const int MIN_POINTS{2};
 static const double SLOWDOWN_RATIO{0.7};
-static const double LIMIT_X{0.4};
-static const double LIMIT_Y{0.2};
-static const double LIMIT_TW{0.09};
+static const double LINEAR_LIMIT{0.4};
+static const double ANGULAR_LIMIT{0.09};
 static const double TIME_BEFORE_COLLISION{1.0};
 static const double SIMULATION_TIME_STEP{0.01};
 static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)};
@@ -280,19 +279,14 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st
     rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO));
 
   test_node_->declare_parameter(
-    polygon_name + ".limit_x", rclcpp::ParameterValue(LIMIT_X));
+    polygon_name + ".linear_limit", rclcpp::ParameterValue(LINEAR_LIMIT));
   test_node_->set_parameter(
-    rclcpp::Parameter(polygon_name + ".limit_x", LIMIT_X));
+    rclcpp::Parameter(polygon_name + ".linear_limit", LINEAR_LIMIT));
 
   test_node_->declare_parameter(
-    polygon_name + ".limit_y", rclcpp::ParameterValue(LIMIT_Y));
+    polygon_name + ".angular_limit", rclcpp::ParameterValue(ANGULAR_LIMIT));
   test_node_->set_parameter(
-    rclcpp::Parameter(polygon_name + ".limit_y", LIMIT_Y));
-
-  test_node_->declare_parameter(
-    polygon_name + ".limit_tw", rclcpp::ParameterValue(LIMIT_TW));
-  test_node_->set_parameter(
-    rclcpp::Parameter(polygon_name + ".limit_tw", LIMIT_TW));
+    rclcpp::Parameter(polygon_name + ".angular_limit", ANGULAR_LIMIT));
 
   test_node_->declare_parameter(
     polygon_name + ".time_before_collision",
@@ -490,9 +484,8 @@ TEST_F(Tester, testPolygonGetLimitParameters)
   EXPECT_EQ(polygon_->getMinPoints(), MIN_POINTS);
   EXPECT_EQ(polygon_->isVisualize(), true);
   // Check that limit params are correct
-  EXPECT_NEAR(polygon_->getLimitX(), LIMIT_X, EPSILON);
-  EXPECT_NEAR(polygon_->getLimitY(), LIMIT_Y, EPSILON);
-  EXPECT_NEAR(polygon_->getLimitTW(), LIMIT_TW, EPSILON);
+  EXPECT_NEAR(polygon_->getLinearLimit(), LINEAR_LIMIT, EPSILON);
+  EXPECT_NEAR(polygon_->getAngularLimit(), ANGULAR_LIMIT, EPSILON);
 }
 
 TEST_F(Tester, testPolygonGetApproachParameters)

From 48b0272637e29d42236a295fc3f5b2046a829167 Mon Sep 17 00:00:00 2001
From: Brice <brice.renaudeau@gmail.com>
Date: Wed, 5 Apr 2023 08:58:31 +0200
Subject: [PATCH 10/11] Apply review comments

---
 nav2_collision_monitor/src/collision_monitor_node.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index 67212e95bb..290ffb3c12 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -426,7 +426,7 @@ bool CollisionMonitor::processStopSlowdownLimit(
       robot_action.req_vel.y = 0.0;
       robot_action.req_vel.tw = 0.0;
       return true;
-    } else if (polygon->getActionType() == SLOWDOWN) {  // SLOWDOWN
+    } else if (polygon->getActionType() == SLOWDOWN) {
       const Velocity safe_vel = velocity * polygon->getSlowdownRatio();
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
@@ -438,7 +438,7 @@ bool CollisionMonitor::processStopSlowdownLimit(
       }
     } else {  // Limit
       // Compute linear velocity
-      const double linear_vel = std::sqrt(velocity.x * velocity.x + velocity.y * velocity.y); // absolute
+      const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute
       Velocity safe_vel;
       double ratio = 1.0;
       if (linear_vel != 0.0) {

From 53ad02f4bb2e836ce93863f505bae5a41fad1814 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Mon, 22 May 2023 18:52:51 -0700
Subject: [PATCH 11/11] Update
 nav2_collision_monitor/include/nav2_collision_monitor/types.hpp

---
 nav2_collision_monitor/include/nav2_collision_monitor/types.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
index 273a4868b3..ac93b916be 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
@@ -68,7 +68,7 @@ enum ActionType
   STOP = 1,  // Stop the robot
   SLOWDOWN = 2,  // Slowdown in percentage from current operating speed
   APPROACH = 3,  // Keep constant time interval before collision
-  LIMIT = 4  // Limit current operating speed
+  LIMIT = 4  // Limit absolute velocity from current operating speed
 };
 
 /// @brief Action for robot