add tests

Signed-off-by: asarazin <[email protected]>

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<std::string> & sources_names = std::vector<std::string>());
   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<std::string> & 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<std::string> range_only_sources_names = {RANGE_NAME};
+  std::vector<std::string> 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

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<double> SQUARE_POLYGON {
   0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5};
 static const std::vector<double> 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<std::string> & observation_sources =
+    std::vector<std::string>({OBSERVATION_SOURCE_NAME}),
+    const std::vector<std::string> & sources_names = std::vector<std::string>());
   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<std::string> & observation_sources,
+  const std::vector<std::string> & 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,23 +883,23 @@ 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<std::string, std::vector<nav2_collision_monitor::Point>> 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);
 
   // 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_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);
 
   // 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_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,15 +925,15 @@ 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);
 
   // 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_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<std::string> 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<std::string> 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<PolygonWrapper>(
+    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<PolygonWrapper>(
+    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<std::string> 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<PolygonWrapper>(
+    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

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<std::string> 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)