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 statically user-defined polygon relative to the robot base frame, or dynamically published in a topic.
 * 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

@@ -69,12 +69,16 @@ class Circle : public Polygon
 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"
@@ -138,7 +137,7 @@ class Polygon
   /**
    * @brief Publishes polygon message into a its own topic
    */
-  void publish() const;
+  void publish();
 
 protected:
   /**
@@ -150,11 +149,37 @@ class Polygon
 
   /**
    * @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. 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 Obtains a transform from source_frame_id -> to base_frame_id_
+   * @param source_frame_id Source frame ID to convert data from
+   * @param tf_transform Output source->base transform
+   * @return True if got correct transform, otherwise false
+   */
+  bool getTransform(
+    const std::string & source_frame_id,
+    tf2::Transform & tf2_transform) const;
+
+  /**
+   * @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 +208,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 +224,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/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/polygon.cpp

@@ -20,6 +20,9 @@
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/point32.hpp"
 
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
 #include "nav2_util/node_utils.hpp"
 
 #include "nav2_collision_monitor/kinematics.hpp"
@@ -43,6 +46,8 @@ Polygon::Polygon(
 Polygon::~Polygon()
 {
   RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str());
+  polygon_sub_.reset();
+  polygon_pub_.reset();
   poly_.clear();
 }
 
@@ -53,28 +58,44 @@ bool Polygon::configure()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  std::string polygon_pub_topic, footprint_topic;
+  std::string polygon_sub_topic, polygon_pub_topic, footprint_topic;
 
-  if (!getParameters(polygon_pub_topic, footprint_topic)) {
+  if (!getParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
     return false;
   }
 
+  if (!polygon_sub_topic.empty()) {
+    RCLCPP_INFO(
+      logger_,
+      "[%s]: Subscribing on %s topic for polygon",
+      polygon_name_.c_str(), polygon_sub_topic.c_str());
+    rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS();  // set to default
+    polygon_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
+      polygon_sub_topic, polygon_qos,
+      std::bind(&Polygon::polygonCallback, this, std::placeholders::_1));
+  }
+
   if (!footprint_topic.empty()) {
+    RCLCPP_INFO(
+      logger_,
+      "[%s]: Making footprint subscriber on %s topic",
+      polygon_name_.c_str(), footprint_topic.c_str());
     footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
       node, footprint_topic, *tf_buffer_,
       base_frame_id_, tf2::durationToSec(transform_tolerance_));
   }
 
   if (visualize_) {
-    // Fill polygon_ points for future usage
+    // Fill polygon_ for future usage
+    polygon_.header.frame_id = base_frame_id_;
     std::vector<Point> poly;
     getPolygon(poly);
     for (const Point & p : poly) {
       geometry_msgs::msg::Point32 p_s;
       p_s.x = p.x;
       p_s.y = p.y;
       // p_s.z will remain 0.0
-      polygon_.points.push_back(p_s);
+      polygon_.polygon.points.push_back(p_s);
     }
 
     rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS();  // set to default
@@ -139,14 +160,15 @@ void Polygon::updatePolygon()
 
     std::size_t new_size = footprint_vec.size();
     poly_.resize(new_size);
-    polygon_.points.resize(new_size);
+    polygon_.header.frame_id = base_frame_id_;
+    polygon_.polygon.points.resize(new_size);
 
     geometry_msgs::msg::Point32 p_s;
     for (std::size_t i = 0; i < new_size; i++) {
       poly_[i] = {footprint_vec[i].x, footprint_vec[i].y};
       p_s.x = footprint_vec[i].x;
       p_s.y = footprint_vec[i].y;
-      polygon_.points[i] = p_s;
+      polygon_.polygon.points[i] = p_s;
     }
   }
 }
@@ -192,7 +214,7 @@ double Polygon::getCollisionTime(
   return -1.0;
 }
 
-void Polygon::publish() const
+void Polygon::publish()
 {
   if (!visualize_) {
     return;
@@ -203,15 +225,9 @@ void Polygon::publish() const
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  // Fill PolygonStamped struct
-  std::unique_ptr<geometry_msgs::msg::PolygonStamped> poly_s =
-    std::make_unique<geometry_msgs::msg::PolygonStamped>();
-  poly_s->header.stamp = node->now();
-  poly_s->header.frame_id = base_frame_id_;
-  poly_s->polygon = polygon_;
-
-  // Publish polygon
-  polygon_pub_->publish(std::move(poly_s));
+  // Actualize the time to current and publish the polygon
+  polygon_.header.stamp = node->now();
+  polygon_pub_->publish(polygon_);
 }
 
 bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
@@ -280,7 +296,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
   return true;
 }
 
-bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
+bool Polygon::getParameters(
+  std::string & polygon_sub_topic,
+  std::string & polygon_pub_topic,
+  std::string & footprint_topic)
 {
   auto node = node_.lock();
   if (!node) {
@@ -300,14 +319,29 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp
       footprint_topic =
         node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
 
-      // This is robot footprint: do not need to get polygon points from ROS parameters.
+      // This is robot footprint: do not need to get polygon points from ROS parameters or topic.
       // It will be set dynamically later.
+      polygon_sub_topic.clear();
       return true;
     } else {
       // Make it empty otherwise
       footprint_topic.clear();
     }
 
+    try {
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING);
+      polygon_sub_topic =
+        node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string();
+      // Do not need to proceed further, if polygon_sub_topic parameter is defined.
+      // Dynamic polygon will be used.
+      return true;
+    } catch (const rclcpp::exceptions::ParameterUninitializedException & ex) {
+      // polygon_sub_topic parameter is not defined: initializing polygon with points.
+      // Static polygon will be used.
+      polygon_sub_topic.clear();
+    }
+
     // Leave it not initialized: the will cause an error if it will not set
     nav2_util::declare_parameter_if_not_declared(
       node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY);
@@ -345,6 +379,80 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp
   return true;
 }
 
+bool Polygon::getTransform(
+  const std::string & source_frame_id,
+  tf2::Transform & tf2_transform) const
+{
+  geometry_msgs::msg::TransformStamped transform;
+  tf2_transform.setIdentity();  // initialize by identical transform
+
+  if (source_frame_id == base_frame_id_) {
+    // We are already in required frame
+    return true;
+  }
+
+  try {
+    // Obtaining the transform to get data from source to base frame
+    transform = tf_buffer_->lookupTransform(
+      base_frame_id_, source_frame_id,
+      tf2::TimePointZero, transform_tolerance_);
+  } catch (tf2::TransformException & e) {
+    RCLCPP_ERROR(
+      logger_,
+      "[%s]: Failed to get \"%s\"->\"%s\" frame transform: %s",
+      polygon_name_.c_str(), source_frame_id.c_str(), base_frame_id_.c_str(), e.what());
+    return false;
+  }
+
+  // Convert TransformStamped to TF2 transform
+  tf2::fromMsg(transform.transform, tf2_transform);
+  return true;
+}
+
+void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
+{
+  std::size_t new_size = msg->polygon.points.size();
+
+  if (new_size < 3) {
+    RCLCPP_ERROR(
+      logger_,
+      "[%s]: Polygon should have at least 3 points",
+      polygon_name_.c_str());
+    return;
+  }
+
+  // Get the transform from PolygonStamped frame to base_frame_id_
+  tf2::Transform tf_transform;
+  if (!getTransform(msg->header.frame_id, tf_transform)) {
+    return;
+  }
+
+  // Set main polygon 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(msg->polygon.points[i].x, msg->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()};
+  }
+
+  if (visualize_) {
+    // Store polygon_ for visualization
+    polygon_ = *msg;
+  }
+}
+
+void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
+{
+  RCLCPP_INFO(
+    logger_,
+    "[%s]: Polygon shape update has been arrived",
+    polygon_name_.c_str());
+  updatePolygon(msg);
+}
+
 inline bool Polygon::isPointInside(const Point & point) const
 {
   // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."