add test on circle radius update

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

nav2_collision_monitor/test/polygons_test.cpp

@@ -117,6 +117,17 @@ class TestNode : public nav2_util::LifecycleNode
     polygon_pub_->publish(std::move(msg));
   }
 
+  void publishRadius()
+  {
+    polygon_pub_ = this->create_publisher<std_msgs::msg::Float32>(
+      POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS());
+
+    std::unique_ptr<std_msgs::msg::Float32> msg = std::make_unique<std_msgs::msg::Float32>();
+    msg->data = CIRCLE_RADIUS;
+
+    polygon_pub_->publish(std::move(msg));
+  }
+
   void publishFootprint()
   {
     footprint_pub_ = this->create_publisher<geometry_msgs::msg::PolygonStamped>(
@@ -159,6 +170,7 @@ class TestNode : public nav2_util::LifecycleNode
 
 private:
   rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr radius_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
   rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
 
@@ -227,11 +239,11 @@ class Tester : public ::testing::Test
   void setPolygonParameters(
     const char * points,
     const bool is_static);
-  void setCircleParameters(const double radius);
+  void setCircleParameters(const double radius, const bool is_static);
   bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param);
   // Creating routines
   void createPolygon(const std::string & action_type, const bool is_static);
-  void createCircle(const std::string & action_type);
+  void createCircle(const std::string & action_type, const bool is_static);
 
   void sendTransforms(double shift);
 
@@ -344,12 +356,19 @@ void Tester::setPolygonParameters(
   }
 }
 
-void Tester::setCircleParameters(const double radius)
+void Tester::setCircleParameters(const double radius, const bool is_static)
 {
-  test_node_->declare_parameter(
-    std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius));
-  test_node_->set_parameter(
-    rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius));
+  if (is_static) {
+    test_node_->declare_parameter(
+      std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius));
+    test_node_->set_parameter(
+      rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius));
+  } else {
+    test_node_->declare_parameter(
+      std::string(CIRCLE_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC));
+    test_node_->set_parameter(
+      rclcpp::Parameter(std::string(CIRCLE_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC));
+  }
 }
 
 bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const std::string & param)
@@ -382,10 +401,10 @@ void Tester::createPolygon(const std::string & action_type, const bool is_static
   polygon_->activate();
 }
 
-void Tester::createCircle(const std::string & action_type)
+void Tester::createCircle(const std::string & action_type, const bool is_static)
 {
   setCommonParameters(CIRCLE_NAME, action_type);
-  setCircleParameters(CIRCLE_RADIUS);
+  setCircleParameters(CIRCLE_RADIUS, is_static);
 
   circle_ = std::make_shared<CircleWrapper>(
     test_node_, CIRCLE_NAME,
@@ -433,6 +452,21 @@ bool Tester::waitPolygon(
   return false;
 }
 
+bool Tester::waitRadius(
+  const std::chrono::nanoseconds & timeout,
+  std::vector<nav2_collision_monitor::Point> & poly)
+{
+  rclcpp::Time start_time = test_node_->now();
+  while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) {
+    if (circle_->getRadius() != 0.0) {
+      return true;
+    }
+    rclcpp::spin_some(test_node_->get_node_base_interface());
+    std::this_thread::sleep_for(10ms);
+  }
+  return false;
+}
+
 bool Tester::waitFootprint(
   const std::chrono::nanoseconds & timeout,
   std::vector<nav2_collision_monitor::Point> & footprint)
@@ -518,7 +552,7 @@ TEST_F(Tester, testPolygonGetApproachParameters)
 
 TEST_F(Tester, testCircleGetParameters)
 {
-  createCircle("approach");
+  createCircle("approach", true);
 
   // Check that common parameters set correctly
   EXPECT_EQ(circle_->getName(), CIRCLE_NAME);
@@ -628,8 +662,9 @@ TEST_F(Tester, testCircleUndeclaredRadius)
     tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE);
   ASSERT_FALSE(circle_->configure());
 
-  // Check that "radius" parameter is not set after configuring
+  // Check that "radius" and "polygon_sub_topic" parameters are not set after configuring
   ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius"));
+  ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "polygon_sub_topic"));
 }
 
 TEST_F(Tester, testPolygonTopicUpdate)
@@ -646,7 +681,7 @@ TEST_F(Tester, testPolygonTopicUpdate)
   ASSERT_FALSE(waitPolygon(100ms, poly));
   ASSERT_FALSE(polygon_->isShapeSet());
 
-  // Publush correct polygon and make shure that it was set correctly
+  // Publish correct polygon and make sure that it was set correctly
   test_node_->publishPolygon(BASE_FRAME_ID, true);
   ASSERT_TRUE(waitPolygon(500ms, poly));
   ASSERT_TRUE(polygon_->isShapeSet());
@@ -661,6 +696,19 @@ TEST_F(Tester, testPolygonTopicUpdate)
   EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON);
 }
 
+TEST_F(Tester, testCircleTopicUpdate)
+{
+  createCircle("stop", false);
+  ASSERT_EQ(circle_->getRadius(), 0);
+
+
+  // Publish radius and make sure that it was set correctly
+  test_node_->publishRadius(BASE_FRAME_ID, true);
+  ASSERT_TRUE(waitRadius(500ms));
+  EXPECT_NEAR(circle_->getRadius(), CIRCLE_RADIUS, EPSILON);
+  EXPECT_NEAR(circle_->getRadiusSquared(), CIRCLE_RADIUS * CIRCLE_RADIUS, EPSILON);
+}
+
 TEST_F(Tester, testPolygonTopicUpdateDifferentFrame)
 {
   createPolygon("stop", false);
@@ -789,7 +837,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge)
 
 TEST_F(Tester, testCircleGetPointsInside)
 {
-  createCircle("stop");
+  createCircle("stop", true);
 
   std::vector<nav2_collision_monitor::Point> points;
   // Point out of radius