[collision monitor] Add temporal axis to polygon detection (ros-navig…

…ation#4364)

The polygon action triggers when `min_points` from the detection sources
are inside the polygon under test. In case of noisy measurement close to
the threshold, this could cause jerks on the robot velocity command.

* Add polygon parameters `min_consecutive_in` / `min_consecutive_out`
  that defines the consecutive number of times the controller loop must
  detect `min_points` inside / outside the given polygon to activate its
  action. Those parameters act as a Schmitt trigger low and high
  threshold.

Signed-off-by: Antoine Gennart <[email protected]>

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -145,6 +145,19 @@ class Polygon
    */
   virtual int getPointsInside(const std::vector<Point> & points) const;
 
+  /**
+   * @brief Add a new decision point to count the consecutive number of
+   * time enough point are in the polygon. Updates the current decision value
+   * based on previous measurements.
+   */
+  void newDecisionPoint(bool is_in);
+
+  /**
+   * @brief Get current polygon decision
+   * @return True if the polygon action must be applied, false otherwise
+   */
+  bool getDecision() const;
+
   /**
    * @brief Obtains estimated (simulated) time before a collision.
    * Applicable for APPROACH model.
@@ -226,6 +239,10 @@ class Polygon
   ActionType action_type_;
   /// @brief Minimum number of data readings within a zone to trigger the action
   int min_points_;
+  /// @brief Minimum number of consecutive data with minimum min_points to trigger the action
+  int min_consecutive_in_;
+  /// @brief Minimum number of consecutive data without minimum min_points to stop the action
+  int min_consecutive_out_;
   /// @brief Robot slowdown (share of its actual speed)
   double slowdown_ratio_;
   /// @brief Robot linear limit
@@ -252,6 +269,12 @@ class Polygon
   std::string base_frame_id_;
   /// @brief Transform tolerance
   tf2::Duration transform_tolerance_;
+  /// @brief Current decision for the polygon (apply or not apply the action)
+  bool current_decision_;
+  /// @brief Current number of consecutive data in to trigger the action
+  int current_consecutive_in_;
+  /// @brief Current number of consecutive data out to trigger the action
+  int current_consecutive_out_;
 
   // Visualization
   /// @brief Whether to publish the polygon

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -349,9 +349,11 @@ void CollisionDetector::process()
       continue;
     }
     state_msg->polygons.push_back(polygon->getName());
-    state_msg->detections.push_back(
-      polygon->getPointsInside(
-        collision_points) >= polygon->getMinPoints());
+    polygon->newDecisionPoint(
+      polygon->getPointsInside(collision_points) >=
+      polygon->getMinPoints());
+
+    state_msg->detections.push_back(polygon->getDecision());
   }
 
   state_pub_->publish(std::move(state_msg));

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -480,7 +480,9 @@ bool CollisionMonitor::processStopSlowdownLimit(
     return false;
   }
 
-  if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) {
+  polygon->newDecisionPoint(polygon->getPointsInside(collision_points) >= polygon->getMinPoints());
+
+  if (polygon->getDecision()) {
     if (polygon->getActionType() == STOP) {
       // Setting up zero velocity for STOP model
       robot_action.polygon_name = polygon->getName();
@@ -537,7 +539,8 @@ bool CollisionMonitor::processApproach(
 
   // Obtain time before a collision
   const double collision_time = polygon->getCollisionTime(collision_points, velocity);
-  if (collision_time >= 0.0) {
+  polygon->newDecisionPoint(collision_time >= 0.0);
+  if (polygon->getDecision() && collision_time >= 0.0) {
     // If collision will occurr, reduce robot speed
     const double change_ratio = collision_time / polygon->getTimeBeforeCollision();
     const Velocity safe_vel = velocity * change_ratio;

nav2_collision_monitor/src/polygon.cpp

@@ -37,7 +37,8 @@ Polygon::Polygon(
 : node_(node), polygon_name_(polygon_name), action_type_(DO_NOTHING),
   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)
+  base_frame_id_(base_frame_id), transform_tolerance_(transform_tolerance),
+  current_decision_(false), current_consecutive_in_(0), current_consecutive_out_(0)
 {
   RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str());
 }
@@ -239,6 +240,28 @@ int Polygon::getPointsInside(const std::vector<Point> & points) const
   return num;
 }
 
+void Polygon::newDecisionPoint(bool is_in)
+{
+  if (is_in && current_consecutive_in_ < min_consecutive_in_) {
+    current_consecutive_in_++;
+    current_consecutive_out_ = 0;
+    if (current_consecutive_in_ >= min_consecutive_in_) {
+      current_decision_ = true;
+    }
+  } else if (!is_in && current_consecutive_out_ < min_consecutive_out_) {
+    current_consecutive_in_ = 0;
+    current_consecutive_out_++;
+    if (current_consecutive_out_ >= min_consecutive_out_) {
+      current_decision_ = false;
+    }
+  }
+}
+
+bool Polygon::getDecision() const
+{
+  return current_decision_;
+}
+
 double Polygon::getCollisionTime(
   const std::vector<Point> & collision_points,
   const Velocity & velocity) const
@@ -323,6 +346,14 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
       node, polygon_name_ + ".min_points", rclcpp::ParameterValue(4));
     min_points_ = node->get_parameter(polygon_name_ + ".min_points").as_int();
 
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".min_consecutive_in", rclcpp::ParameterValue(1));
+    min_consecutive_in_ = node->get_parameter(polygon_name_ + ".min_consecutive_in").as_int();
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".min_consecutive_out", rclcpp::ParameterValue(1));
+    min_consecutive_out_ = node->get_parameter(polygon_name_ + ".min_consecutive_out").as_int();
+
     try {
       nav2_util::declare_parameter_if_not_declared(
         node, polygon_name_ + ".max_points", rclcpp::PARAMETER_INTEGER);

nav2_collision_monitor/test/polygons_test.cpp

@@ -52,6 +52,8 @@ static const std::vector<double> ARBITRARY_POLYGON {
   1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 2.0, -1.0, -1.0, -1.0, -1.0, 1.0};
 static const double CIRCLE_RADIUS{0.5};
 static const int MIN_POINTS{2};
+static const int MIN_CONSECUTIVE_IN{1};
+static const int MIN_CONSECUTIVE_OUT{1};
 static const double SLOWDOWN_RATIO{0.7};
 static const double LINEAR_LIMIT{0.4};
 static const double ANGULAR_LIMIT{0.09};
@@ -308,6 +310,17 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st
     polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(POLYGON_PUB_TOPIC));
   test_node_->set_parameter(
     rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC));
+
+  test_node_->declare_parameter(
+    polygon_name + ".min_consecutive_in", rclcpp::ParameterValue(MIN_CONSECUTIVE_IN));
+  test_node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".min_consecutive_in", MIN_CONSECUTIVE_IN));
+
+  test_node_->declare_parameter(
+    polygon_name + ".min_consecutive_out", rclcpp::ParameterValue(MIN_CONSECUTIVE_OUT));
+  test_node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".min_consecutive_out", MIN_CONSECUTIVE_OUT));
+
 }
 
 void Tester::setPolygonParameters(
@@ -900,6 +913,50 @@ TEST_F(Tester, testPolygonDefaultVisualize)
   ASSERT_EQ(test_node_->waitPolygonReceived(100ms), nullptr);
 }
 
+TEST_F(Tester, testPolygonMinConsecutiveParam)
+{
+  createPolygon("stop", true);
+
+  // Test polygon with default parameters
+  ASSERT_FALSE(polygon_->getDecision());
+  polygon_->newDecisionPoint(false);
+  ASSERT_FALSE(polygon_->getDecision());
+  polygon_->newDecisionPoint(true);
+  ASSERT_TRUE(polygon_->getDecision());
+  polygon_->newDecisionPoint(false);
+  ASSERT_FALSE(polygon_->getDecision());
+
+  // Create new polygon with non default parameter
+  test_node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + ".min_consecutive_in", 3));
+  test_node_->set_parameter(
+    rclcpp::Parameter(std::string(POLYGON_NAME) + ".min_consecutive_out", 2));
+
+  polygon_ = std::make_shared<PolygonWrapper>(
+    test_node_, POLYGON_NAME,
+    tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
+  ASSERT_TRUE(polygon_->configure());
+  polygon_->activate();
+
+  ASSERT_FALSE(polygon_->getDecision());
+  polygon_->newDecisionPoint(false);
+  ASSERT_FALSE(polygon_->getDecision());
+  polygon_->newDecisionPoint(true);
+  ASSERT_FALSE(polygon_->getDecision());
+  polygon_->newDecisionPoint(true);
+  ASSERT_FALSE(polygon_->getDecision());
+  polygon_->newDecisionPoint(true);
+  ASSERT_TRUE(polygon_->getDecision());
+  polygon_->newDecisionPoint(false);
+  ASSERT_TRUE(polygon_->getDecision());
+  polygon_->newDecisionPoint(true);
+  ASSERT_TRUE(polygon_->getDecision());
+  polygon_->newDecisionPoint(false);
+  ASSERT_TRUE(polygon_->getDecision());
+  polygon_->newDecisionPoint(false);
+  ASSERT_FALSE(polygon_->getDecision());
+}
+
 int main(int argc, char ** argv)
 {
   // Initialize the system