Dynamically changing polygons support

nav2_collision_monitor/README.md

@@ -27,7 +27,7 @@ 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.
+* 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.
 * 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.
 
@@ -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;
   }