From bc24238274632195278ebc56a2be40128cf3959b Mon Sep 17 00:00:00 2001 From: asarazin Date: Thu, 28 Mar 2024 17:43:07 +0100 Subject: [PATCH 1/3] Collision monitor: select specific observation sources for polygon Signed-off-by: asarazin --- .../nav2_collision_monitor/polygon.hpp | 8 ++++ .../src/collision_detector_node.cpp | 8 ++-- .../src/collision_monitor_node.cpp | 39 ++++++++++++++++--- nav2_collision_monitor/src/polygon.cpp | 26 +++++++++++++ 4 files changed, 72 insertions(+), 9 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index fdc9e07b7a3..fe132fcd798 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -126,6 +126,12 @@ class Polygon */ virtual void getPolygon(std::vector & poly) const; + /** + * @brief Obtains the name of the observation sources for current polygon. + * @return Names of the observation sources + */ + std::vector getSourcesNames() const; + /** * @brief Returns true if polygon points were set. * Otherwise, prints a warning and returns false. @@ -269,6 +275,8 @@ class Polygon rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; + /// @brief Name of the observation sources to check for polygon + std::vector sources_names_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 4d8da1e241d..d8e03e19ed4 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -168,10 +168,6 @@ bool CollisionDetector::getParameters() const bool base_shift_correction = get_parameter("base_shift_correction").as_bool(); - if (!configurePolygons(base_frame_id, transform_tolerance)) { - return false; - } - if (!configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -179,6 +175,10 @@ bool CollisionDetector::getParameters() return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 766ed713cd8..20ff3924c0e 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -270,10 +270,6 @@ bool CollisionMonitor::getParameters( stop_pub_timeout_ = rclcpp::Duration::from_seconds(get_parameter("stop_pub_timeout").as_double()); - if (!configurePolygons(base_frame_id, transform_tolerance)) { - return false; - } - if ( !configureSources( base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) @@ -281,6 +277,10 @@ bool CollisionMonitor::getParameters( return false; } + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + return true; } @@ -412,7 +412,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: } // Points array collected from different data sources in a robot base frame - std::vector collision_points; + std::unordered_map> sources_collision_points_map; + + // Fill collision_points array from different data sources + for (std::shared_ptr source : sources_) { + std::vector collision_points; + source->getData(curr_time, collision_points); + sources_collision_points_map.insert({source->getSourceName(), collision_points}); + } // By default - there is no action Action robot_action{DO_NOTHING, cmd_vel_in, ""}; @@ -476,6 +483,28 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: // Update polygon coordinates polygon->updatePolygon(cmd_vel_in); + // Get collision points for sources associated to polygon + std::vector sources_names = polygon->getSourcesNames(); + std::vector collision_points; + for (auto source_name : sources_names) { + try { + // Get vector for source + auto source_collision_points = sources_collision_points_map.at(source_name); + // Concatenate vectors + collision_points.insert( + collision_points.end(), + source_collision_points.begin(), + source_collision_points.end()); + } catch (std::exception & e) { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), + *get_clock(), + 1000, + "Observation source [" << source_name << + "] configured for polygon [" << polygon->getName() << "] is not defined!"); + } + } + const ActionType at = polygon->getActionType(); if (at == STOP || at == SLOWDOWN || at == LIMIT) { // Process STOP/SLOWDOWN for the selected polygon diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 523b446b25a..ddef394305e 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -157,6 +157,11 @@ double Polygon::getTimeBeforeCollision() const return time_before_collision_; } +std::vector Polygon::getSourcesNames() const +{ + return sources_names_; +} + void Polygon::getPolygon(std::vector & poly) const { poly = poly_; @@ -392,6 +397,27 @@ bool Polygon::getCommonParameters( node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); } } + + // By default, use all observation sources for polygon + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + std::vector source_names = + node->get_parameter("observation_sources").as_string_array(); + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(source_names)); + sources_names_ = node->get_parameter(polygon_name_ + ".sources_names").as_string_array(); + + // Check the observation sources configured for polygon are defined + for (auto source_name : sources_names_) { + if (std::find(source_names.begin(), source_names.end(), source_name) == source_names.end()) { + RCLCPP_ERROR_STREAM( + logger_, + "Observation source [" << source_name << + "] configured for polygon [" << getName() << + "] is not defined!"); + return false; + } + } } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, From 2d55ca78ec0e68b015f81e10f6c0295c2cb69936 Mon Sep 17 00:00:00 2001 From: asarazin Date: Fri, 7 Jun 2024 16:48:06 +0200 Subject: [PATCH 2/3] optimization Signed-off-by: asarazin --- .../collision_monitor_node.hpp | 11 +- .../nav2_collision_monitor/polygon.hpp | 17 ++- .../src/collision_monitor_node.cpp | 101 +++++++----------- nav2_collision_monitor/src/polygon.cpp | 43 ++++++-- nav2_collision_monitor/test/polygons_test.cpp | 35 +++--- 5 files changed, 117 insertions(+), 90 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index ce4d96504bb..c6caeeb5b2d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -159,28 +160,30 @@ class CollisionMonitor : public nav2_util::LifecycleNode /** * @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 sources_collision_points_map Map containing source name as key and + * array of source's 2D obstacle points as value * @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 processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const; /** * @brief Processes APPROACH action type * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key and + * array of source's 2D obstacle points as value * @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 processApproach( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index fe132fcd798..6bb3b5dd011 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" @@ -151,16 +152,28 @@ class Polygon */ virtual int getPointsInside(const std::vector & points) const; + /** + * @brief Gets number of points inside given polygon + * @param sources_collision_points_map Map containing source name as key, + * and input array of source's points to be checked as value + * @return Number of points inside polygon, + * for sources in map that are associated with current polygon. + * If there are no points, returns zero value. + */ + virtual int getPointsInside( + const std::unordered_map> & sources_collision_points_map) const; + /** * @brief Obtains estimated (simulated) time before a collision. * Applicable for APPROACH model. - * @param collision_points Array of 2D obstacle points + * @param sources_collision_points_map Map containing source name as key, + * and input array of source's 2D obstacle points as value * @param velocity Simulated robot velocity * @return Estimated time before a collision. If there is no collision, * return value will be negative. */ double getCollisionTime( - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const; /** diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 20ff3924c0e..c0bd15cb63a 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -414,22 +414,19 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: // Points array collected from different data sources in a robot base frame std::unordered_map> sources_collision_points_map; - // Fill collision_points array from different data sources - for (std::shared_ptr source : sources_) { - std::vector collision_points; - source->getData(curr_time, collision_points); - sources_collision_points_map.insert({source->getSourceName(), collision_points}); - } - // By default - there is no action Action robot_action{DO_NOTHING, cmd_vel_in, ""}; // Polygon causing robot action (if any) std::shared_ptr action_polygon; - // Fill collision_points array from different data sources + // Fill collision points array from different data sources + auto marker_array = std::make_unique(); for (std::shared_ptr source : sources_) { + auto iter = sources_collision_points_map.insert( + {source->getSourceName(), std::vector()}); + if (source->getEnabled()) { - if (!source->getData(curr_time, collision_points) && + if (!source->getData(curr_time, iter.first->second) && source->getSourceTimeout().seconds() != 0.0) { action_polygon = nullptr; @@ -441,33 +438,35 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: break; } } + + if (collision_points_marker_pub_->get_subscription_count() > 0) { + // visualize collision points with markers + visualization_msgs::msg::Marker marker; + marker.header.frame_id = get_parameter("base_frame_id").as_string(); + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = "collision_points_" + source->getSourceName(); + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.color.r = 1.0; + marker.color.a = 1.0; + marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = true; + + for (const auto & point : iter.first->second) { + geometry_msgs::msg::Point p; + p.x = point.x; + p.y = point.y; + p.z = 0.0; + marker.points.push_back(p); + } + marker_array->markers.push_back(marker); + } } if (collision_points_marker_pub_->get_subscription_count() > 0) { - // visualize collision points with markers - auto marker_array = std::make_unique(); - visualization_msgs::msg::Marker marker; - marker.header.frame_id = get_parameter("base_frame_id").as_string(); - marker.header.stamp = rclcpp::Time(0, 0); - marker.ns = "collision_points"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::POINTS; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale.x = 0.02; - marker.scale.y = 0.02; - marker.color.r = 1.0; - marker.color.a = 1.0; - marker.lifetime = rclcpp::Duration(0, 0); - marker.frame_locked = true; - - for (const auto & point : collision_points) { - geometry_msgs::msg::Point p; - p.x = point.x; - p.y = point.y; - p.z = 0.0; - marker.points.push_back(p); - } - marker_array->markers.push_back(marker); collision_points_marker_pub_->publish(std::move(marker_array)); } @@ -483,37 +482,17 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: // Update polygon coordinates polygon->updatePolygon(cmd_vel_in); - // Get collision points for sources associated to polygon - std::vector sources_names = polygon->getSourcesNames(); - std::vector collision_points; - for (auto source_name : sources_names) { - try { - // Get vector for source - auto source_collision_points = sources_collision_points_map.at(source_name); - // Concatenate vectors - collision_points.insert( - collision_points.end(), - source_collision_points.begin(), - source_collision_points.end()); - } catch (std::exception & e) { - RCLCPP_ERROR_STREAM_THROTTLE( - get_logger(), - *get_clock(), - 1000, - "Observation source [" << source_name << - "] configured for polygon [" << polygon->getName() << "] is not defined!"); - } - } - const ActionType at = polygon->getActionType(); if (at == STOP || at == SLOWDOWN || at == LIMIT) { // Process STOP/SLOWDOWN for the selected polygon - if (processStopSlowdownLimit(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processStopSlowdownLimit( + polygon, sources_collision_points_map, cmd_vel_in, robot_action)) + { action_polygon = polygon; } } else if (at == APPROACH) { // Process APPROACH for the selected polygon - if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { + if (processApproach(polygon, sources_collision_points_map, cmd_vel_in, robot_action)) { action_polygon = polygon; } } @@ -535,7 +514,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: bool CollisionMonitor::processStopSlowdownLimit( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { @@ -543,7 +522,7 @@ bool CollisionMonitor::processStopSlowdownLimit( return false; } - if (polygon->getPointsInside(collision_points) >= polygon->getMinPoints()) { + if (polygon->getPointsInside(sources_collision_points_map) >= polygon->getMinPoints()) { if (polygon->getActionType() == STOP) { // Setting up zero velocity for STOP model robot_action.polygon_name = polygon->getName(); @@ -590,7 +569,7 @@ bool CollisionMonitor::processStopSlowdownLimit( bool CollisionMonitor::processApproach( const std::shared_ptr polygon, - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity, Action & robot_action) const { @@ -599,7 +578,7 @@ bool CollisionMonitor::processApproach( } // Obtain time before a collision - const double collision_time = polygon->getCollisionTime(collision_points, velocity); + const double collision_time = polygon->getCollisionTime(sources_collision_points_map, velocity); if (collision_time >= 0.0) { // If collision will occurr, reduce robot speed const double change_ratio = collision_time / polygon->getTimeBeforeCollision(); diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index ddef394305e..19ac5053de8 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -234,19 +234,48 @@ int Polygon::getPointsInside(const std::vector & points) const return num; } +int Polygon::getPointsInside( + const std::unordered_map> & sources_collision_points_map) const +{ + int num = 0; + std::vector polygon_sources_names = getSourcesNames(); + + // Sum the number of points from all sources associated with current polygon + for (const auto & source_name : polygon_sources_names) { + const auto & iter = sources_collision_points_map.find(source_name); + if (iter != sources_collision_points_map.end()) { + num += getPointsInside(iter->second); + } + } + + return num; +} + double Polygon::getCollisionTime( - const std::vector & collision_points, + const std::unordered_map> & sources_collision_points_map, const Velocity & velocity) const { // Initial robot pose is {0,0} in base_footprint coordinates Pose pose = {0.0, 0.0, 0.0}; Velocity vel = velocity; + std::vector polygon_sources_names = getSourcesNames(); + std::vector collision_points; + + // Save all points coming from the sources associated with current polygon + for (const auto & source_name : polygon_sources_names) { + const auto & iter = sources_collision_points_map.find(source_name); + if (iter != sources_collision_points_map.end()) { + collision_points.insert(collision_points.end(), iter->second.begin(), iter->second.end()); + } + } + // Array of points transformed to the frame concerned with pose on each simulation step std::vector points_transformed = collision_points; // Check static polygon - if (getPointsInside(points_transformed) >= min_points_) { + if (getPointsInside(collision_points) >= min_points_) { return 0.0; } @@ -401,20 +430,22 @@ bool Polygon::getCommonParameters( // By default, use all observation sources for polygon nav2_util::declare_parameter_if_not_declared( node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); - std::vector source_names = + const std::vector observation_sources = node->get_parameter("observation_sources").as_string_array(); nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(source_names)); + node, polygon_name_ + ".sources_names", rclcpp::ParameterValue(observation_sources)); sources_names_ = node->get_parameter(polygon_name_ + ".sources_names").as_string_array(); // Check the observation sources configured for polygon are defined for (auto source_name : sources_names_) { - if (std::find(source_names.begin(), source_names.end(), source_name) == source_names.end()) { + if (std::find(observation_sources.begin(), observation_sources.end(), source_name) == + observation_sources.end()) + { RCLCPP_ERROR_STREAM( logger_, "Observation source [" << source_name << "] configured for polygon [" << getName() << - "] is not defined!"); + "] is not defined as one of the node's observation_source!"); return false; } } diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index c703e12557b..add67a64d4d 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -862,25 +862,26 @@ TEST_F(Tester, testPolygonGetCollisionTime) // Forward movement check nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.2 m ahead the footprint (0.5 m) - std::vector points{{0.7, -0.01}, {0.7, 0.01}}; + std::unordered_map> points_map; + points_map.insert({"source", {{0.7, -0.01}, {0.7, 0.01}}}); // Collision is expected to be ~= 0.2 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); // Backward movement check vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement // Two points 0.2 m behind the footprint (0.5 m) - points.clear(); - points = {{-0.7, -0.01}, {-0.7, 0.01}}; + points_map.clear(); + points_map.insert({"source", {{-0.7, -0.01}, {-0.7, 0.01}}}); // Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.4, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); // Sideway movement check vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement // Two points 0.1 m ahead the footprint (0.5 m) - points.clear(); - points = {{-0.01, 0.6}, {0.01, 0.6}}; + points_map.clear(); + points_map.insert({"source", {{-0.01, 0.6}, {0.01, 0.6}}}); // Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.2, SIMULATION_TIME_STEP); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.2, SIMULATION_TIME_STEP); // Rotation check vel = {0.0, 0.0, 1.0}; // 1.0 rad/s rotation @@ -894,27 +895,27 @@ TEST_F(Tester, testPolygonGetCollisionTime) // | ' | // ----------- // ' - points.clear(); - points = {{0.49, -0.01}, {0.49, 0.01}}; + points_map.clear(); + points_map.insert({"source", {{0.49, -0.01}, {0.49, 0.01}}}); // Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds double exp_res = 45 / 180 * M_PI; - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), exp_res, EPSILON); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), exp_res, EPSILON); // Two points are already inside footprint vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points inside - points.clear(); - points = {{0.1, -0.01}, {0.1, 0.01}}; + points_map.clear(); + points_map.insert({"source", {{0.1, -0.01}, {0.1, 0.01}}}); // Collision already appeared: collision time should be 0 - EXPECT_NEAR(polygon_->getCollisionTime(points, vel), 0.0, EPSILON); + EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.0, EPSILON); // All points are out of simulation prediction vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.6 m ahead the footprint (0.5 m) - points.clear(); - points = {{1.1, -0.01}, {1.1, 0.01}}; + points_map.clear(); + points_map.insert({"source", {{1.1, -0.01}, {1.1, 0.01}}}); // There is no collision: return value should be negative - EXPECT_LT(polygon_->getCollisionTime(points, vel), 0.0); + EXPECT_LT(polygon_->getCollisionTime(points_map, vel), 0.0); } TEST_F(Tester, testPolygonPublish) From b4c2925624e6d7d5bf3a91772e669be21bd20dc9 Mon Sep 17 00:00:00 2001 From: asarazin Date: Fri, 23 Aug 2024 15:42:51 +0000 Subject: [PATCH 3/3] add tests Signed-off-by: asarazin --- .../test/collision_monitor_node_test.cpp | 58 +++++++++++++- nav2_collision_monitor/test/polygons_test.cpp | 77 +++++++++++++++++-- .../test/velocity_polygons_test.cpp | 6 ++ 3 files changed, 130 insertions(+), 11 deletions(-) diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 40415868f54..f297ca363a2 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -143,7 +143,8 @@ class Tester : public ::testing::Test void setCommonParameters(); void addPolygon( const std::string & polygon_name, const PolygonType type, - const double size, const std::string & at); + const double size, const std::string & at, + const std::vector & sources_names = std::vector()); void addPolygonVelocitySubPolygon( const std::string & polygon_name, const std::string & sub_polygon_name, const double linear_min, const double linear_max, @@ -309,7 +310,8 @@ void Tester::setCommonParameters() void Tester::addPolygon( const std::string & polygon_name, const PolygonType type, - const double size, const std::string & at) + const double size, const std::string & at, + const std::vector & sources_names) { if (type == POLYGON) { cm_->declare_parameter( @@ -408,6 +410,13 @@ void Tester::addPolygon( polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name)); cm_->set_parameter( rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name)); + + if (!sources_names.empty()) { + cm_->declare_parameter( + polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names)); + cm_->set_parameter( + rclcpp::Parameter(polygon_name + ".sources_names", sources_names)); + } } void Tester::addPolygonVelocitySubPolygon( @@ -1499,9 +1508,10 @@ TEST_F(Tester, testCollisionPointsMarkers) // Share TF sendTransforms(curr_time); + // No source published, empty marker array published publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCollisionPointsMarker(500ms)); - ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + ASSERT_EQ(collision_points_marker_msg_->markers.size(), 0u); publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); @@ -1580,6 +1590,48 @@ TEST_F(Tester, testVelocityPolygonStop) cm_->stop(); } +TEST_F(Tester, testSourceAssociatedToPolygon) +{ + // Set Collision Monitor parameters: + // - 2 sources (scan and range) + // - 1 stop polygon associated to range source + // - 1 slowdown polygon (associated with all sources by default) + setCommonParameters(); + addSource(SCAN_NAME, SCAN); + addSource(RANGE_NAME, RANGE); + std::vector range_only_sources_names = {RANGE_NAME}; + std::vector all_sources_names = {SCAN_NAME, RANGE_NAME}; + addPolygon("StopOnRangeSource", POLYGON, 1.0, "stop", range_only_sources_names); + addPolygon("SlowdownOnAllSources", POLYGON, 1.0, "slowdown"); + setVectors({"StopOnRangeSource", "SlowdownOnAllSources"}, {SCAN_NAME, RANGE_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + rclcpp::Time curr_time = cm_->now(); + sendTransforms(curr_time); + + // Publish sources so that : + // - scan obstacle is in polygons + // - range obstacle is far away from polygons + publishScan(0.5, curr_time); + publishRange(4.5, curr_time); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); + + // Publish cmd vel + publishCmdVel(0.5, 0.0, 0.0); + ASSERT_TRUE(waitCmdVel(500ms)); + + // Since the stop polygon is only checking range source, slowdown action should be applied + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, SLOWDOWN); + ASSERT_EQ(action_state_->polygon_name, "SlowdownOnAllSources"); + + // Stop Collision Monitor node + cm_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index add67a64d4d..a0dbf9fcc96 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -46,6 +46,7 @@ static const char POLYGON_SUB_TOPIC[]{"polygon_sub"}; static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; static const char POLYGON_NAME[]{"TestPolygon"}; static const char CIRCLE_NAME[]{"TestCircle"}; +static const char OBSERVATION_SOURCE_NAME[]{"source"}; static const std::vector SQUARE_POLYGON { 0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5}; static const std::vector ARBITRARY_POLYGON { @@ -235,7 +236,11 @@ class Tester : public ::testing::Test protected: // Working with parameters - void setCommonParameters(const std::string & polygon_name, const std::string & action_type); + void setCommonParameters( + const std::string & polygon_name, const std::string & action_type, + const std::vector & observation_sources = + std::vector({OBSERVATION_SOURCE_NAME}), + const std::vector & sources_names = std::vector()); void setPolygonParameters( const char * points, const bool is_static); @@ -289,7 +294,10 @@ Tester::~Tester() tf_buffer_.reset(); } -void Tester::setCommonParameters(const std::string & polygon_name, const std::string & action_type) +void Tester::setCommonParameters( + const std::string & polygon_name, const std::string & action_type, + const std::vector & observation_sources, + const std::vector & sources_names) { test_node_->declare_parameter( polygon_name + ".action_type", rclcpp::ParameterValue(action_type)); @@ -336,6 +344,18 @@ 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( + "observation_sources", rclcpp::ParameterValue(observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", observation_sources)); + + if (!sources_names.empty()) { + test_node_->declare_parameter( + polygon_name + ".sources_names", rclcpp::ParameterValue(sources_names)); + test_node_->set_parameter( + rclcpp::Parameter(polygon_name + ".sources_names", sources_names)); + } } void Tester::setPolygonParameters( @@ -863,7 +883,7 @@ TEST_F(Tester, testPolygonGetCollisionTime) nav2_collision_monitor::Velocity vel{0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.2 m ahead the footprint (0.5 m) std::unordered_map> points_map; - points_map.insert({"source", {{0.7, -0.01}, {0.7, 0.01}}}); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.7, -0.01}, {0.7, 0.01}}}); // Collision is expected to be ~= 0.2 m / 0.5 m/s seconds EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); @@ -871,7 +891,7 @@ TEST_F(Tester, testPolygonGetCollisionTime) vel = {-0.5, 0.0, 0.0}; // 0.5 m/s backward movement // Two points 0.2 m behind the footprint (0.5 m) points_map.clear(); - points_map.insert({"source", {{-0.7, -0.01}, {-0.7, 0.01}}}); + points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.7, -0.01}, {-0.7, 0.01}}}); // Collision is expected to be in ~= 0.2 m / 0.5 m/s seconds EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.4, SIMULATION_TIME_STEP); @@ -879,7 +899,7 @@ TEST_F(Tester, testPolygonGetCollisionTime) vel = {0.0, 0.5, 0.0}; // 0.5 m/s sideway movement // Two points 0.1 m ahead the footprint (0.5 m) points_map.clear(); - points_map.insert({"source", {{-0.01, 0.6}, {0.01, 0.6}}}); + points_map.insert({OBSERVATION_SOURCE_NAME, {{-0.01, 0.6}, {0.01, 0.6}}}); // Collision is expected to be in ~= 0.1 m / 0.5 m/s seconds EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.2, SIMULATION_TIME_STEP); @@ -896,7 +916,7 @@ TEST_F(Tester, testPolygonGetCollisionTime) // ----------- // ' points_map.clear(); - points_map.insert({"source", {{0.49, -0.01}, {0.49, 0.01}}}); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.49, -0.01}, {0.49, 0.01}}}); // Collision is expected to be in ~= 45 degrees * M_PI / (180 degrees * 1.0 rad/s) seconds double exp_res = 45 / 180 * M_PI; EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), exp_res, EPSILON); @@ -905,7 +925,7 @@ TEST_F(Tester, testPolygonGetCollisionTime) vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points inside points_map.clear(); - points_map.insert({"source", {{0.1, -0.01}, {0.1, 0.01}}}); + points_map.insert({OBSERVATION_SOURCE_NAME, {{0.1, -0.01}, {0.1, 0.01}}}); // Collision already appeared: collision time should be 0 EXPECT_NEAR(polygon_->getCollisionTime(points_map, vel), 0.0, EPSILON); @@ -913,7 +933,7 @@ TEST_F(Tester, testPolygonGetCollisionTime) vel = {0.5, 0.0, 0.0}; // 0.5 m/s forward movement // Two points 0.6 m ahead the footprint (0.5 m) points_map.clear(); - points_map.insert({"source", {{1.1, -0.01}, {1.1, 0.01}}}); + points_map.insert({OBSERVATION_SOURCE_NAME, {{1.1, -0.01}, {1.1, 0.01}}}); // There is no collision: return value should be negative EXPECT_LT(polygon_->getCollisionTime(points_map, vel), 0.0); } @@ -946,6 +966,9 @@ TEST_F(Tester, testPolygonDefaultVisualize) std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); + std::vector observation_sources = {OBSERVATION_SOURCE_NAME}; + test_node_->declare_parameter("observation_sources", rclcpp::ParameterValue(observation_sources)); + test_node_->set_parameter(rclcpp::Parameter("observation_sources", observation_sources)); setPolygonParameters(SQUARE_POLYGON_STR, true); // Create new polygon @@ -978,6 +1001,44 @@ TEST_F(Tester, testPolygonInvalidPointsString) ASSERT_FALSE(polygon_->configure()); } +TEST_F(Tester, testPolygonSourceDefaultAssociation) +{ + // By default, a polygon uses all observation sources + std::vector all_sources = {"source_1", "source_2", "source_3"}; + setCommonParameters(POLYGON_NAME, "stop", all_sources); // no polygon sources names specified + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + ASSERT_EQ(polygon_->getSourcesNames(), all_sources); +} + +TEST_F(Tester, testPolygonSourceInvalidAssociation) +{ + // If a source is not defined as observation source, polygon cannot use it: config should fail + setCommonParameters( + POLYGON_NAME, "stop", {"source_1", "source_2", "source_3"}, {"source_1", "source_4"}); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_FALSE(polygon_->configure()); +} + +TEST_F(Tester, testPolygonSourceAssociation) +{ + // Checks that, if declared, only the specific sources associated to polygon are saved + std::vector poly_sources = {"source_1", "source_3"}; + setCommonParameters(POLYGON_NAME, "stop", {"source_1", "source_2", "source_3"}, poly_sources); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + ASSERT_TRUE(polygon_->configure()); + ASSERT_EQ(polygon_->getSourcesNames(), poly_sources); +} + int main(int argc, char ** argv) { // Initialize the system diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 216a85b5145..8b9ecec6d75 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -208,6 +208,12 @@ 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)); + + std::vector default_observation_sources = {"source"}; + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(default_observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", default_observation_sources)); } void Tester::setVelocityPolygonParameters(const bool is_holonomic)