(wip) fix tests

ros-navigation · Aug 22, 2024 · 3186c2b · 3186c2b
1 parent 7501c94
commit 3186c2b
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 10 deletions.
diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp
@@ -168,17 +168,17 @@ 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))
   {
     return false;
   }
 
+  if (!configurePolygons(base_frame_id, transform_tolerance)) {
+    return false;
+  }
+
   return true;
 }
 

diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
@@ -440,17 +440,19 @@ bool Polygon::getCommonParameters(
     }
 
     // By default, use all observation sources for polygon
+    const std::vector<std::string> default_observation_sources;
     nav2_util::declare_parameter_if_not_declared(
-      node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY);
-    std::vector<std::string> source_names =
+      node, "observation_sources", rclcpp::ParameterValue(default_observation_sources));
+    const std::vector<std::string> 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 <<

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp
@@ -1499,10 +1499,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);

diff --git 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 std::vector<std::string> OBSERVATION_SOURCES {"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 {
@@ -336,6 +337,11 @@ 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));
 }
 
 void Tester::setPolygonParameters(