[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 <gennart.antoine@gmail.com>

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_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