Dynamically changing polygons support (ros-navigation#3245)

* Add Collision Monitor polygon topics subscription

* Add the support of polygons published in different frame

* Internal review

* Fix working with polygons visualization

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <[email protected]>

* Move getTransform to nav2_util

* Fix misprint

* Meet remaining review items:
* Update polygon params handling logic
* Warn if polygon shape was not set
* Publish with ownership movement

* Correct polygons_test.cpp parameters handling logic

* Adjust README for dynamic polygons logic update

Co-authored-by: Steve Macenski <[email protected]>

nav2_collision_monitor/README.md

@@ -27,9 +27,9 @@ The following models of safety behaviors are employed by Collision Monitor:
 
 The zones around the robot can take the following shapes:
 
-* Arbitrary user-defined polygon relative to the robot base frame.
-* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape.
-* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time.
+* Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface.
+* Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time.
+* Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape.
 
 The data may be obtained from different data sources:
 
@@ -43,7 +43,7 @@ The Collision Monitor is designed to operate below Nav2 as an independent safety
 This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate.
 
 The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.
-![HDL.png](doc/HLD.png)
+![HLD.png](doc/HLD.png)
 
 ## Configuration
 

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp

@@ -66,15 +66,24 @@ class Circle : public Polygon
    */
   int getPointsInside(const std::vector<Point> & points) const override;
 
+  /**
+   * @brief Specifies that the shape is always set for a circle object
+   */
+  bool isShapeSet() override {return true;}
+
 protected:
   /**
    * @brief Supporting routine obtaining polygon-specific ROS-parameters
+   * @brief polygon_sub_topic Input name of polygon subscription topic
    * @param polygon_pub_topic Output name of polygon publishing topic
    * @param footprint_topic Output name of footprint topic. For Circle returns empty string,
    * there is no footprint subscription in this class.
    * @return True if all parameters were obtained or false in failure case
    */
-  bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) override;
+  bool getParameters(
+    std::string & polygon_sub_topic,
+    std::string & polygon_pub_topic,
+    std::string & footprint_topic) override;
 
   // ----- Variables -----
 

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -21,7 +21,6 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
-#include "geometry_msgs/msg/polygon.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
@@ -110,6 +109,12 @@ class Polygon
    */
   virtual void getPolygon(std::vector<Point> & poly) const;
 
+  /**
+   * @brief Returns true if polygon points were set.
+   * Othewise, prints a warning and returns false.
+   */
+  virtual bool isShapeSet();
+
   /**
    * @brief Updates polygon from footprint subscriber (if any)
    */
@@ -138,7 +143,7 @@ class Polygon
   /**
    * @brief Publishes polygon message into a its own topic
    */
-  void publish() const;
+  void publish();
 
 protected:
   /**
@@ -150,11 +155,29 @@ class Polygon
 
   /**
    * @brief Supporting routine obtaining polygon-specific ROS-parameters
+   * @brief polygon_sub_topic Output name of polygon subscription topic.
+   * Empty, if no polygon subscription.
    * @param polygon_pub_topic Output name of polygon publishing topic
-   * @param footprint_topic Output name of footprint topic. Empty, if no footprint subscription
+   * @param footprint_topic Output name of footprint topic.
+   * Empty, if no footprint subscription.
    * @return True if all parameters were obtained or false in failure case
    */
-  virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic);
+  virtual bool getParameters(
+    std::string & polygon_sub_topic,
+    std::string & polygon_pub_topic,
+    std::string & footprint_topic);
+
+  /**
+   * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message
+   * @param msg Message to update polygon from
+   */
+  void updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg);
+
+  /**
+   * @brief Dynamic polygon callback
+   * @param msg Shared pointer to the polygon message
+   */
+  void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg);
 
   /**
    * @brief Checks if point is inside polygon
@@ -183,6 +206,8 @@ class Polygon
   double time_before_collision_;
   /// @brief Time step for robot movement simulation
   double simulation_time_step_;
+  /// @brief Polygon subscription
+  rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
   /// @brief Footprint subscriber
   std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
 
@@ -197,8 +222,8 @@ class Polygon
   // Visualization
   /// @brief Whether to publish the polygon
   bool visualize_;
-  /// @brief Polygon points stored for later publishing
-  geometry_msgs::msg::Polygon polygon_;
+  /// @brief Polygon stored for later publishing
+  geometry_msgs::msg::PolygonStamped polygon_;
   /// @brief Polygon publisher for visualization purposes
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
 

nav2_collision_monitor/include/nav2_collision_monitor/source.hpp

@@ -88,21 +88,6 @@ class Source
     const rclcpp::Time & source_time,
     const rclcpp::Time & curr_time) const;
 
-  /**
-   * @brief Obtains a transform from source_frame_id at source_time ->
-   * to base_frame_id_ at curr_time time
-   * @param source_frame_id Source frame ID to convert data from
-   * @param source_time Source timestamp to convert data from
-   * @param curr_time Current node time to interpolate data to
-   * @param tf_transform Output source->base transform
-   * @return True if got correct transform, otherwise false
-   */
-  bool getTransform(
-    const std::string & source_frame_id,
-    const rclcpp::Time & source_time,
-    const rclcpp::Time & curr_time,
-    tf2::Transform & tf_transform) const;
-
   // ----- Variables -----
 
   /// @brief Collision Monitor node

nav2_collision_monitor/src/circle.cpp

@@ -70,7 +70,10 @@ int Circle::getPointsInside(const std::vector<Point> & points) const
   return num;
 }
 
-bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
+bool Circle::getParameters(
+  std::string & polygon_sub_topic,
+  std::string & polygon_pub_topic,
+  std::string & footprint_topic)
 {
   auto node = node_.lock();
   if (!node) {
@@ -81,7 +84,8 @@ bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footpr
     return false;
   }
 
-  // There is no footprint subscription for the Circle. Thus, set string as empty.
+  // There is no polygon or footprint subscription for the Circle. Thus, set strings as empty.
+  polygon_sub_topic.clear();
   footprint_topic.clear();
 
   try {

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -389,6 +389,10 @@ bool CollisionMonitor::processStopSlowdown(
   const Velocity & velocity,
   Action & robot_action) const
 {
+  if (!polygon->isShapeSet()) {
+    return false;
+  }
+
   if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) {
     if (polygon->getActionType() == STOP) {
       // Setting up zero velocity for STOP model
@@ -420,6 +424,10 @@ bool CollisionMonitor::processApproach(
 {
   polygon->updatePolygon();
 
+  if (!polygon->isShapeSet()) {
+    return false;
+  }
+
   // Obtain time before a collision
   const double collision_time = polygon->getCollisionTime(collision_points, velocity);
   if (collision_time >= 0.0) {

nav2_collision_monitor/src/pointcloud.cpp

@@ -19,6 +19,7 @@
 #include "sensor_msgs/point_cloud2_iterator.hpp"
 
 #include "nav2_util/node_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_collision_monitor
 {
@@ -78,7 +79,12 @@ void PointCloud::getData(
   // Obtaining the transform to get data from source frame and time where it was received
   // to the base frame and current time
   tf2::Transform tf_transform;
-  if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) {
+  if (
+    !nav2_util::getTransform(
+      data_->header.frame_id, data_->header.stamp,
+      base_frame_id_, curr_time, global_frame_id_,
+      transform_tolerance_, tf_buffer_, tf_transform))
+  {
     return;
   }