Fix dynamic polygons vertices not updated for different frame

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -238,12 +238,12 @@ class Polygon
   // Visualization
   /// @brief Whether to publish the polygon
   bool visualize_;
-  /// @brief Polygon stored for later publishing
+  /// @brief Polygon, used for: 1. visualization; 2. storing latest dynamic polygon message
   geometry_msgs::msg::PolygonStamped polygon_;
   /// @brief Polygon publisher for visualization purposes
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
 
-  /// @brief Polygon points (vertices)
+  /// @brief Polygon points (vertices) in a base_frame_id_
   std::vector<Point> poly_;
 };  // class Polygon
 

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -379,6 +379,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
       break;
     }
 
+    // Update polygon coordinates
+    polygon->updatePolygon();
+
     const ActionType at = polygon->getActionType();
     if (at == STOP || at == SLOWDOWN || at == LIMIT) {
       // Process STOP/SLOWDOWN for the selected polygon
@@ -468,8 +471,6 @@ bool CollisionMonitor::processApproach(
   const Velocity & velocity,
   Action & robot_action) const
 {
-  polygon->updatePolygon();
-
   if (!polygon->isShapeSet()) {
     return false;
   }

nav2_collision_monitor/src/polygon.cpp

@@ -188,6 +188,30 @@ void Polygon::updatePolygon()
       p_s.y = footprint_vec[i].y;
       polygon_.polygon.points[i] = p_s;
     }
+  } else if (!polygon_.header.frame_id.empty() && polygon_.header.frame_id != base_frame_id_) {
+    // Polygon is published in another frame: correct poly_ vertices to the latest frame state
+    std::size_t new_size = polygon_.polygon.points.size();
+
+    // Get the transform from PolygonStamped frame to base_frame_id_
+    tf2::Transform tf_transform;
+    if (
+      !nav2_util::getTransform(
+        polygon_.header.frame_id, base_frame_id_,
+        transform_tolerance_, tf_buffer_, tf_transform))
+    {
+      return;
+    }
+
+    // Correct main poly_ vertices
+    poly_.resize(new_size);
+    for (std::size_t i = 0; i < new_size; i++) {
+      // Transform point coordinates from PolygonStamped frame -> to base frame
+      tf2::Vector3 p_v3_s(polygon_.polygon.points[i].x, polygon_.polygon.points[i].y, 0.0);
+      tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
+
+      // Fill poly_ array
+      poly_[i] = {p_v3_b.x(), p_v3_b.y()};
+    }
   }
 }
 
@@ -443,7 +467,7 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m
     return;
   }
 
-  // Set main polygon vertices
+  // Set main poly_ vertices first time
   poly_.resize(new_size);
   for (std::size_t i = 0; i < new_size; i++) {
     // Transform point coordinates from PolygonStamped frame -> to base frame
@@ -454,10 +478,9 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m
     poly_[i] = {p_v3_b.x(), p_v3_b.y()};
   }
 
-  if (visualize_) {
-    // Store polygon_ for visualization
-    polygon_ = *msg;
-  }
+  // Store incoming polygon for further (possible) poly_ vertices corrections
+  // from PolygonStamped frame -> to base frame
+  polygon_ = *msg;
 }
 
 void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)

nav2_collision_monitor/test/polygons_test.cpp

@@ -220,7 +220,7 @@ class Tester : public ::testing::Test
   void createPolygon(const std::string & action_type, const bool is_static);
   void createCircle(const std::string & action_type);
 
-  void sendTransforms();
+  void sendTransforms(double shift);
 
   // Wait until polygon will be received
   bool waitPolygon(
@@ -381,7 +381,7 @@ void Tester::createCircle(const std::string & action_type)
   circle_->activate();
 }
 
-void Tester::sendTransforms()
+void Tester::sendTransforms(double shift)
 {
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster =
     std::make_shared<tf2_ros::TransformBroadcaster>(test_node_);
@@ -393,8 +393,8 @@ void Tester::sendTransforms()
   transform.child_frame_id = BASE2_FRAME_ID;
 
   transform.header.stamp = test_node_->now();
-  transform.transform.translation.x = 0.1;
-  transform.transform.translation.y = 0.1;
+  transform.transform.translation.x = shift;
+  transform.transform.translation.y = shift;
   transform.transform.translation.z = 0.0;
   transform.transform.rotation.x = 0.0;
   transform.transform.rotation.y = 0.0;
@@ -652,7 +652,7 @@ TEST_F(Tester, testPolygonTopicUpdate)
 TEST_F(Tester, testPolygonTopicUpdateDifferentFrame)
 {
   createPolygon("stop", false);
-  sendTransforms();
+  sendTransforms(0.1);
 
   std::vector<nav2_collision_monitor::Point> poly;
   polygon_->getPolygon(poly);
@@ -670,12 +670,28 @@ TEST_F(Tester, testPolygonTopicUpdateDifferentFrame)
   EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5] + 0.1, EPSILON);
   EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6] + 0.1, EPSILON);
   EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7] + 0.1, EPSILON);
+
+  // Move BASE2_FRAME_ID to 0.2 m away from BASE_FRAME_ID
+  sendTransforms(0.2);
+  // updatePolygon() should update poly coordinates to correct ones in BASE_FRAME_ID
+  polygon_->updatePolygon();
+  // Check that polygon coordinates were updated correctly
+  ASSERT_TRUE(waitPolygon(500ms, poly));
+  ASSERT_EQ(poly.size(), 4u);
+  EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0] + 0.2, EPSILON);
+  EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1] + 0.2, EPSILON);
+  EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2] + 0.2, EPSILON);
+  EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3] + 0.2, EPSILON);
+  EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4] + 0.2, EPSILON);
+  EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5] + 0.2, EPSILON);
+  EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6] + 0.2, EPSILON);
+  EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7] + 0.2, EPSILON);
 }
 
 TEST_F(Tester, testPolygonTopicUpdateIncorrectFrame)
 {
   createPolygon("stop", false);
-  sendTransforms();
+  sendTransforms(0.1);
 
   std::vector<nav2_collision_monitor::Point> poly;
   polygon_->getPolygon(poly);