(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);