diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index fdc9e07b7a..2ff51c090f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -145,6 +145,22 @@ class Polygon */ virtual int getPointsInside(const std::vector & 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 & 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 diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 4d8da1e241..b35db472e7 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/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)); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 766ed713cd..487c47b8e2 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/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(); diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 523b446b25..60b441e4f2 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/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 & points) const return num; } +bool Polygon::shouldApplyAction() const +{ + return should_apply_action_; +} + +void Polygon::newCollisionPoints( + const std::vector & 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 & 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); diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index c703e12557..ff99e940b9 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/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(