[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_time_in_sec` / `min_time_out_sec`
  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,22 @@ class Polygon
    */
   virtual int getPointsInside(const std::vector<Point> & points) const;
 
+
+  /**
+   * @brief Get current action status 
+   * @return: True if the action must be applied, false otherwise
+   */
+  bool shouldApplyAction() const;
+
+  /**
+   * @brief Add collision points to be taken into account 
+   * @param Input array of points to be checked
+   * @param Current time
+   */
+  void newCollisionPoints(
+    const std::vector<Point> & collision_points,
+    const rclcpp::Time & current_time);
+
   /**
    * @brief Obtains estimated (simulated) time before a collision.
    * Applicable for APPROACH model.
@@ -251,6 +267,10 @@ class Polygon
   ActionType action_type_;
   /// @brief Minimum number of data readings within a zone to trigger the action
   int min_points_;
+  /// @brief Minimum amout of time with only measurements inside the polygon to trigger the action
+  int min_time_in_sec_;
+  /// @brief Minimum amount of time with only measurements outside the polygon to stop applying the action
+  int min_time_out_sec_;
   /// @brief Robot slowdown (share of its actual speed)
   double slowdown_ratio_;
   /// @brief Robot linear limit
@@ -277,6 +297,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 should_apply_action_;
+  /// @brief Current number of consecutive data in to trigger the action
+  rclcpp::Time last_time_data_in_;
+  /// @brief Current number of consecutive data out to trigger the action
+  rclcpp::Time last_time_data_out_;
 
   // Visualization
   /// @brief Whether to publish the polygon

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -370,9 +370,9 @@ void CollisionDetector::process()
       continue;
     }
     state_msg->polygons.push_back(polygon->getName());
-    state_msg->detections.push_back(
-      polygon->getPointsInside(
-        collision_points) >= polygon->getMinPoints());
+    polygon->newCollisionPoints(collision_points, curr_time);
+
+    state_msg->detections.push_back(polygon->shouldApplyAction());
   }
 
   state_pub_->publish(std::move(state_msg));

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -513,8 +513,8 @@ bool CollisionMonitor::processStopSlowdownLimit(
   if (!polygon->isShapeSet()) {
     return false;
   }
-
-  if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) {
+  polygon->newCollisionPoints(collision_points, get_clock()->now());
+  if (polygon->shouldApplyAction()) {
     if (polygon->getActionType() == STOP) {
       // Setting up zero velocity for STOP model
       robot_action.polygon_name = polygon->getName();

nav2_collision_monitor/src/polygon.cpp

@@ -1,5 +1,4 @@
-// Copyright (c) 2022 Samsung R&D Institute Russia
-//
+// Copyright (c) 2022 Samsung R&D Institute Russia //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
 // You may obtain a copy of the License at
@@ -39,7 +38,9 @@ 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),
+  should_apply_action_(false), last_time_data_in_(rclcpp::Time(0,0)), 
+  last_time_data_out_(rclcpp::Time(0,0))
 {
   RCLCPP_INFO(logger_, "[%s]: Creating Polygon", polygon_name_.c_str());
 }
@@ -229,6 +230,34 @@ int Polygon::getPointsInside(const std::vector<Point> & points) const
   return num;
 }
 
+bool Polygon::shouldApplyAction() const
+{
+    return should_apply_action_;
+}
+
+void Polygon::newCollisionPoints(
+  const std::vector<Point> & collision_points,
+  const rclcpp::Time & current_time) 
+{
+  bool is_in = getPointsInside(collision_points) >= getMinPoints();
+  // Record time of last data in the same direction as the current decision
+  if (should_apply_action_ && is_in) {
+    last_time_data_in_ = current_time;
+  } else if (!should_apply_action_ && !is_in){
+    last_time_data_out_ = current_time;
+  }
+
+  // Update the decision when required
+  if (should_apply_action_ 
+    && (last_time_data_out_ - current_time).seconds() >= min_time_out_sec_ ) {
+      should_apply_action_ = false;
+  }
+  else if (!should_apply_action_ 
+    && (last_time_data_in_ - current_time).seconds() >= min_time_out_sec_) {
+      should_apply_action_ = true;
+  }
+}
+
 double Polygon::getCollisionTime(
   const std::vector<Point> & collision_points,
   const Velocity & velocity) const
@@ -322,6 +351,14 @@ bool Polygon::getCommonParameters(
       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_time_in_sec", rclcpp::ParameterValue(1));
+    min_time_in_sec_ = node->get_parameter(polygon_name_ + ".min_time_in_sec").as_double();
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".min_time_out_sec", rclcpp::ParameterValue(1));
+    min_time_out_sec_ = node->get_parameter(polygon_name_ + ".min_time_out_sec").as_double();
+
     try {
       nav2_util::declare_parameter_if_not_declared(
         node, polygon_name_ + ".max_points", rclcpp::PARAMETER_INTEGER);

nav2_collision_monitor/test/polygons_test.cpp

@@ -65,6 +65,8 @@ static const char INVALID_POINTS_STR[]{
 };
 static const double CIRCLE_RADIUS{0.5};
 static const int MIN_POINTS{2};
+static const double MIN_TIME_IN_SEC{1.0};
+static const double MIN_TIME_OUT_SEC{1.0};
 static const double SLOWDOWN_RATIO{0.7};
 static const double LINEAR_LIMIT{0.4};
 static const double ANGULAR_LIMIT{0.09};
@@ -336,6 +338,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_time_in_sec", rclcpp::ParameterValue(MIN_TIME_IN_SEC));
+  test_node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".min_time_in_sec",MIN_TIME_IN_SEC));
+
+  test_node_->declare_parameter(
+    polygon_name + ".min_time_out_sec", rclcpp::ParameterValue(MIN_TIME_OUT_SEC));
+  test_node_->set_parameter(
+    rclcpp::Parameter(polygon_name + ".min_time_out_sec", MIN_TIME_OUT_SEC));
+
 }
 
 void Tester::setPolygonParameters(