From 6d3507fabd5fc13a8e0a41915e96664cb7009b0f Mon Sep 17 00:00:00 2001 From: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Date: Thu, 29 Aug 2024 15:21:04 +0200 Subject: [PATCH] [collision monitor] Select the observation sources used with each polygon (#4227) * Collision monitor: select specific observation sources for polygon Signed-off-by: asarazin * optimization Signed-off-by: asarazin * add tests Signed-off-by: asarazin --------- Signed-off-by: asarazin Co-authored-by: asarazin Signed-off-by: Joseph Duchesne --- .../collision_monitor_node.hpp | 11 +- .../nav2_collision_monitor/polygon.hpp | 25 ++++- .../src/collision_detector_node.cpp | 8 +- .../src/collision_monitor_node.cpp | 82 +++++++------- nav2_collision_monitor/src/polygon.cpp | 61 ++++++++++- .../test/collision_monitor_node_test.cpp | 58 +++++++++- nav2_collision_monitor/test/polygons_test.cpp | 100 ++++++++++++++---- .../test/velocity_polygons_test.cpp | 6 ++ 8 files changed, 280 insertions(+), 71 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 fdc9e07b7a3..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" @@ -126,6 +127,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. @@ -145,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; /** @@ -269,6 +288,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..c0bd15cb63a 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,17 +412,21 @@ 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; // 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; @@ -434,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)); } @@ -479,12 +485,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: 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; } } @@ -506,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 { @@ -514,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(); @@ -561,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 { @@ -570,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 523b446b25a..19ac5053de8 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_; @@ -229,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; } @@ -392,6 +426,29 @@ 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); + 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(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(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 as one of the node's observation_source!"); + return false; + } + } } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, 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 c703e12557b..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( @@ -862,25 +882,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({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, 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({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, 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({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, 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 +915,27 @@ TEST_F(Tester, testPolygonGetCollisionTime) // | ' | // ----------- // ' - points.clear(); - points = {{0.49, -0.01}, {0.49, 0.01}}; + points_map.clear(); + 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, 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({OBSERVATION_SOURCE_NAME, {{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({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, vel), 0.0); + EXPECT_LT(polygon_->getCollisionTime(points_map, vel), 0.0); } TEST_F(Tester, testPolygonPublish) @@ -945,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 @@ -977,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)