(collision monitor) add limit polygon type (ros-navigation#3519)

* add LIMIT polygon type

* fix unit tests

* Fix MIN_POINT doesn't exist

* Fix Action type enum

* FIX velocity used

* FIX unit test point distance

increase point distance to not be in limit field

* Update collision_monitor_node_test.cpp

* fix status name not updated

* [MOD] only single linear limit

* Apply review comments

* Update nav2_collision_monitor/include/nav2_collision_monitor/types.hpp

---------

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: enricosutera <enricosutera@outlook.com>

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 |

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

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,

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,18 @@ class Polygon
    * @return Speed slowdown ratio
    */
   double getSlowdownRatio() const;
+  /**
+   * @brief Obtains speed linear limit for current polygon.
+   * Applicable for LIMIT model.
+   * @return Speed linear limit
+   */
+  double getLinearLimit() const;
+  /**
+   * @brief Obtains speed angular z limit for current polygon.
+   * Applicable for LIMIT model.
+   * @return Speed angular limit
+   */
+  double getAngularLimit() const;
   /**
    * @brief Obtains required time before collision for current polygon.
    * Applicable for APPROACH model.
@@ -202,6 +214,10 @@ class Polygon
   int min_points_;
   /// @brief Robot slowdown (share of its actual speed)
   double slowdown_ratio_;
+  /// @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

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 absolute velocity from current operating speed
 };
 
 /// @brief Action for robot

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,15 @@ 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
+      linear_limit: 0.4
+      angular_limit: 0.5
+      visualize: True
+      polygon_pub_topic: "polygon_limit"
     FootprintApproach:
       type: "polygon"
       action_type: "approach"

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) {
       const Velocity safe_vel = velocity * polygon->getSlowdownRatio();
       // Check that currently calculated velocity is safer than
       // chosen for previous shapes one
@@ -436,6 +436,26 @@ bool CollisionMonitor::processStopSlowdown(
         robot_action.req_vel = safe_vel;
         return true;
       }
+    } else {  // Limit
+      // Compute linear velocity
+      const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute
+      Velocity safe_vel;
+      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) {
+        robot_action.polygon_name = polygon->getName();
+        robot_action.action_type = LIMIT;
+        robot_action.req_vel = safe_vel;
+        return true;
+      }
     }
   }
 
@@ -487,6 +507,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(),

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), 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)
 {
   RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str());
@@ -138,6 +139,16 @@ double Polygon::getSlowdownRatio() const
   return slowdown_ratio_;
 }
 
+double Polygon::getLinearLimit() const
+{
+  return linear_limit_;
+}
+
+double Polygon::getAngularLimit() const
+{
+  return angular_limit_;
+}
+
 double Polygon::getTimeBeforeCollision() const
 {
   return time_before_collision_;
@@ -256,6 +267,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 +299,15 @@ 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_ + ".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_ + ".angular_limit", rclcpp::ParameterValue(0.5));
+      angular_limit_ = node->get_parameter(polygon_name_ + ".angular_limit").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 +396,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);

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -54,6 +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 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};
@@ -81,6 +83,7 @@ enum ActionType
   STOP = 1,
   SLOWDOWN = 2,
   APPROACH = 3,
+  LIMIT = 4,
 };
 
 class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor
@@ -321,6 +324,16 @@ void Tester::addPolygon(
   cm_->set_parameter(
     rclcpp::Parameter(polygon_name + ".slowdown_ratio", SLOWDOWN_RATIO));
 
+  cm_->declare_parameter(
+    polygon_name + ".linear_limit", rclcpp::ParameterValue(LINEAR_LIMIT));
+  cm_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".linear_limit", LINEAR_LIMIT));
+
+  cm_->declare_parameter(
+    polygon_name + ".angular_limit", rclcpp::ParameterValue(ANGULAR_LIMIT));
+  cm_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".angular_limit", ANGULAR_LIMIT));
+
   cm_->declare_parameter(
     polygon_name + ".time_before_collision", rclcpp::ParameterValue(TIME_BEFORE_COLLISION));
   cm_->set_parameter(
@@ -590,17 +603,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 +623,29 @@ 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(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);
   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(3.0, curr_time);
+  ASSERT_TRUE(waitData(3.0, 500ms, curr_time));
+  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;
+  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);
+  ASSERT_EQ(action_state_->polygon_name, "Limit");
+
+  // 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);
@@ -629,7 +657,7 @@ TEST_F(Tester, testProcessStopSlowdown)
   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);
@@ -641,9 +669,9 @@ TEST_F(Tester, testProcessStopSlowdown)
   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(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);