diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 40415868f54..a044f14f76f 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,10 +1508,11 @@ TEST_F(Tester, testCollisionPointsMarkers) // Share TF sendTransforms(curr_time); + // If no source published, no marker publishCmdVel(0.5, 0.2, 0.1); - ASSERT_TRUE(waitCollisionPointsMarker(500ms)); - ASSERT_EQ(collision_points_marker_msg_->markers[0].points.size(), 0u); + ASSERT_FALSE(waitCollisionPointsMarker(500ms)); + // Publish source and check markers are published publishScan(0.5, curr_time); ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); @@ -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..82edcc3a41c 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,9 @@ 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 & sources_names = std::vector()); void setPolygonParameters( const char * points, const bool is_static); @@ -289,7 +292,9 @@ 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 & sources_names) { test_node_->declare_parameter( polygon_name + ".action_type", rclcpp::ParameterValue(action_type)); @@ -336,6 +341,21 @@ 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)); + + if (!test_node_->has_parameter("observation_sources")) { + std::vector default_observation_sources = {OBSERVATION_SOURCE_NAME}; + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(default_observation_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", default_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); } @@ -978,6 +998,57 @@ TEST_F(Tester, testPolygonInvalidPointsString) ASSERT_FALSE(polygon_->configure()); } +TEST_F(Tester, testPolygonSourceDefaultAssociation) +{ + std::vector all_sources = {"source_1", "source_2", "source_3"}; + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(all_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", all_sources)); + + setCommonParameters(POLYGON_NAME, "stop"); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + polygon_->configure(); + ASSERT_EQ(polygon_->getSourcesNames(), all_sources); +} + +TEST_F(Tester, testPolygonSourceInvalidAssociation) +{ + std::vector all_sources = {"source_1", "source_2", "source_3"}; + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(all_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", all_sources)); + + setCommonParameters(POLYGON_NAME, "stop", std::vector({"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) +{ + std::vector all_sources = {"source_1", "source_2", "source_3"}; + std::vector partial_sources = {"source_1", "source_3"}; + test_node_->declare_parameter( + "observation_sources", rclcpp::ParameterValue(all_sources)); + test_node_->set_parameter( + rclcpp::Parameter("observation_sources", all_sources)); + + setCommonParameters(POLYGON_NAME, "stop", partial_sources); + setPolygonParameters(SQUARE_POLYGON_STR, true); + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, + tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); + polygon_->configure(); + ASSERT_EQ(polygon_->getSourcesNames(), partial_sources); +} + int main(int argc, char ** argv) { // Initialize the system