Repository: ros-navigation/navigation2 (C++)

Issue:
[collision monitor] Dynamic radius for circle polygon
## Feature request

#### Feature description
The `collision_monitor` polygons are dynamic, by subscribing to a topic on which the list of polygon's points are published.
The circle polygon could be dynamic as well. 

#### Implementation considerations
In order to keep a design similar to the other polygons, the circle polygon could be updated by subscribing to a topic on which the radius would be published. 
The topic can be defined by using the same parameter than for the other polygons: `<polygon_name>.polygon_sub_topic` (the subscription type would be defined by the polygon type).
 In order to not break the current usage, if no topic is defined as a parameter, the parameter `<polygon_name>.radius` will be used.
 I can open a PR with an implementation of this solution.



Discussion: > The circle polygon could be dynamic as well.

All I need to hear, please open a PR! 





Fix (patch):
diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
index 8cef0f609fc..d812df7d654 100644
--- a/nav2_collision_monitor/CMakeLists.txt
+++ b/nav2_collision_monitor/CMakeLists.txt
@@ -8,6 +8,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
@@ -32,6 +33,7 @@ set(dependencies
   rclcpp_components
   sensor_msgs
   geometry_msgs
+  std_msgs
   tf2
   tf2_ros
   tf2_geometry_msgs
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
index c4400327013..a468be7c880 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
@@ -19,6 +19,8 @@
 #include <vector>
 #include <string>
 
+#include "std_msgs/msg/float32.hpp"
+
 #include "nav2_collision_monitor/polygon.hpp"
 
 namespace nav2_collision_monitor
@@ -67,15 +69,16 @@ 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
+   * @brief Returns true if circle radius is set.
+   * Otherwise, prints a warning and returns false.
    */
-  bool isShapeSet() override {return true;}
+  bool isShapeSet() override;
 
 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 polygon_sub_topic Input name of polygon subscription topic
+   * @param polygon_pub_topic Output name of polygon or radius 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
@@ -85,12 +88,34 @@ class Circle : public Polygon
     std::string & polygon_pub_topic,
     std::string & footprint_topic) override;
 
+  /**
+   * @brief Creates polygon or radius topic subscription
+   * @param polygon_sub_topic Output name of polygon or radius subscription topic.
+   * Empty, if no polygon subscription.
+   */
+  void createSubscription(std::string & polygon_sub_topic) override;
+
+  /**
+   * @brief Updates polygon from radius value
+   * @param radius New circle radius to update polygon
+   */
+  void updatePolygon(double radius);
+
+  /**
+   * @brief Dynamic circle radius callback
+   * @param msg Shared pointer to the radius value message
+   */
+  void radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg);
+
+
   // ----- Variables -----
 
   /// @brief Radius of the circle
   double radius_;
   /// @brief (radius * radius) value. Stored for optimization.
-  double radius_squared_;
+  double radius_squared_ = -1.0;
+  /// @brief Radius subscription
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr radius_sub_;
 };  // class Circle
 
 }  // namespace nav2_collision_monitor
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
index 1e6a72d1590..fdc9e07b7a3 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -128,7 +128,7 @@ class Polygon
 
   /**
    * @brief Returns true if polygon points were set.
-   * Othewise, prints a warning and returns false.
+   * Otherwise, prints a warning and returns false.
    */
   virtual bool isShapeSet();
 
@@ -165,14 +165,24 @@ class Polygon
 protected:
   /**
    * @brief Supporting routine obtaining ROS-parameters common for all shapes
-   * @param polygon_pub_topic Output name of polygon publishing topic
+   * @param polygon_pub_topic Output name of polygon or radius subscription topic.
+   * Empty, if no polygon subscription.
+   * @param polygon_sub_topic Output name of polygon publishing topic
+   * @param footprint_topic Output name of footprint topic.
+   * Empty, if no footprint subscription.
+   * @param use_dynamic_sub If false, the parameter polygon_sub_topic or footprint_topic
+   * will not be declared
    * @return True if all parameters were obtained or false in failure case
    */
-  bool getCommonParameters(std::string & polygon_pub_topic);
+  bool getCommonParameters(
+    std::string & polygon_sub_topic,
+    std::string & polygon_pub_topic,
+    std::string & footprint_topic,
+    bool use_dynamic_sub = false);
 
   /**
    * @brief Supporting routine obtaining polygon-specific ROS-parameters
-   * @brief polygon_sub_topic Output name of polygon subscription topic.
+   * @param polygon_sub_topic Output name of polygon or radius 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.
@@ -184,6 +194,13 @@ class Polygon
     std::string & polygon_pub_topic,
     std::string & footprint_topic);
 
+  /**
+   * @brief Creates polygon or radius topic subscription
+   * @param polygon_sub_topic Output name of polygon or radius subscription topic.
+   * Empty, if no polygon subscription.
+   */
+  virtual void createSubscription(std::string & polygon_sub_topic);
+
   /**
    * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message
    * @param msg Message to update polygon from
diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
index 4889ea77f16..fd663526e86 100644
--- a/nav2_collision_monitor/package.xml
+++ b/nav2_collision_monitor/package.xml
@@ -17,6 +17,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
   <depend>nav2_common</depend>
   <depend>nav2_util</depend>
   <depend>nav2_costmap_2d</depend>
diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp
index fa0fc33a4f8..353db72dc07 100644
--- a/nav2_collision_monitor/src/circle.cpp
+++ b/nav2_collision_monitor/src/circle.cpp
@@ -70,6 +70,15 @@ int Circle::getPointsInside(const std::vector<Point> & points) const
   return num;
 }
 
+bool Circle::isShapeSet()
+{
+  if (radius_squared_ == -1.0) {
+    RCLCPP_WARN(logger_, "[%s]: Circle radius is not set yet", polygon_name_.c_str());
+    return false;
+  }
+  return true;
+}
+
 bool Circle::getParameters(
   std::string & polygon_sub_topic,
   std::string & polygon_pub_topic,
@@ -80,29 +89,91 @@ bool Circle::getParameters(
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  if (!getCommonParameters(polygon_pub_topic)) {
-    return false;
-  }
-
-  // There is no polygon or footprint subscription for the Circle. Thus, set strings as empty.
+  // Clear the polygon subscription topic. It will be set later, if necessary.
   polygon_sub_topic.clear();
-  footprint_topic.clear();
 
+  bool use_dynamic_sub = true;  // if getting parameter radius fails, use dynamic subscription
   try {
     // Leave it not initialized: the will cause an error if it will not set
     nav2_util::declare_parameter_if_not_declared(
       node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE);
     radius_ = node->get_parameter(polygon_name_ + ".radius").as_double();
     radius_squared_ = radius_ * radius_;
-  } catch (const std::exception & ex) {
-    RCLCPP_ERROR(
+    use_dynamic_sub = false;
+  } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
+    RCLCPP_INFO(
       logger_,
-      "[%s]: Error while getting circle parameters: %s",
-      polygon_name_.c_str(), ex.what());
-    return false;
+      "[%s]: Polygon circle radius is not defined. Using dynamic subscription instead.",
+      polygon_name_.c_str());
   }
 
-  return true;
+  bool ret = true;
+  if (!getCommonParameters(
+      polygon_sub_topic, polygon_pub_topic, footprint_topic, use_dynamic_sub))
+  {
+    if (use_dynamic_sub && polygon_sub_topic.empty()) {
+      RCLCPP_ERROR(
+        logger_,
+        "[%s]: Error while getting circle parameters: static radius and sub topic both not defined",
+        polygon_name_.c_str());
+    }
+    ret = false;
+  }
+
+  // There is no footprint subscription for the Circle. Thus, set string as empty.
+  footprint_topic.clear();
+
+  return ret;
+}
+
+void Circle::createSubscription(std::string & polygon_sub_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  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
+    if (polygon_subscribe_transient_local_) {
+      polygon_qos.transient_local();
+    }
+    radius_sub_ = node->create_subscription<std_msgs::msg::Float32>(
+      polygon_sub_topic, polygon_qos,
+      std::bind(&Circle::radiusCallback, this, std::placeholders::_1));
+  }
+}
+
+void Circle::updatePolygon(double radius)
+{
+  // Update circle radius
+  radius_ = radius;
+  radius_squared_ = radius_ * radius_;
+
+  // Create a polygon from radius and store it
+  std::vector<Point> poly;
+  getPolygon(poly);
+  polygon_.polygon.points.clear();  // clear polygon points
+  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_.polygon.points.push_back(p_s);  // add new points
+  }
+}
+
+void Circle::radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg)
+{
+  RCLCPP_INFO(
+    logger_,
+    "[%s]: Polygon circle radius update has been arrived",
+    polygon_name_.c_str());
+  updatePolygon(msg->data);
 }
 
 }  // namespace nav2_collision_monitor
diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
index 6a0801d1d26..7e0a69b84f4 100644
--- a/nav2_collision_monitor/src/polygon.cpp
+++ b/nav2_collision_monitor/src/polygon.cpp
@@ -65,19 +65,7 @@ bool Polygon::configure()
     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
-    if (polygon_subscribe_transient_local_) {
-      polygon_qos.transient_local();
-    }
-    polygon_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
-      polygon_sub_topic, polygon_qos,
-      std::bind(&Polygon::polygonCallback, this, std::placeholders::_1));
-  }
+  createSubscription(polygon_sub_topic);
 
   if (!footprint_topic.empty()) {
     RCLCPP_INFO(
@@ -287,7 +275,11 @@ void Polygon::publish()
   polygon_pub_->publish(std::move(msg));
 }
 
-bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
+bool Polygon::getCommonParameters(
+  std::string & polygon_sub_topic,
+  std::string & polygon_pub_topic,
+  std::string & footprint_topic,
+  bool use_dynamic_sub_topic)
 {
   auto node = node_.lock();
   if (!node) {
@@ -372,6 +364,28 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)
         node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_));
       polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string();
     }
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false));
+    polygon_subscribe_transient_local_ =
+      node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool();
+
+    if (use_dynamic_sub_topic) {
+      if (action_type_ != APPROACH) {
+        // Get polygon sub topic
+        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();
+      } else {
+        // Obtain the footprint topic to make a footprint subscription for approach polygon
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + ".footprint_topic",
+          rclcpp::ParameterValue("local_costmap/published_footprint"));
+        footprint_topic =
+          node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
+      }
+    }
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(
       logger_,
@@ -393,63 +407,64 @@ bool Polygon::getParameters(
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  if (!getCommonParameters(polygon_pub_topic)) {
-    return false;
-  }
-
   // Clear the subscription topics. They will be set later, if necessary.
   polygon_sub_topic.clear();
   footprint_topic.clear();
 
+  bool use_dynamic_sub = true;  // if getting parameter points fails, use dynamic subscription
   try {
-    try {
-      // Leave it uninitialized: it will throw an inner exception if the parameter is not set
-      nav2_util::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING);
-      std::string poly_string =
-        node->get_parameter(polygon_name_ + ".points").as_string();
-
-      // Do not need to proceed further, if "points" parameter is defined.
-      // Static polygon will be used.
-      return getPolygonFromString(poly_string, poly_);
-    } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
-      RCLCPP_INFO(
+    // Leave it uninitialized: it will throw an inner exception if the parameter is not set
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING);
+    std::string poly_string =
+      node->get_parameter(polygon_name_ + ".points").as_string();
+
+    use_dynamic_sub = !getPolygonFromString(poly_string, poly_);
+  } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
+    RCLCPP_INFO(
+      logger_,
+      "[%s]: Polygon points are not defined. Using dynamic subscription instead.",
+      polygon_name_.c_str());
+  }
+
+  if (!getCommonParameters(
+      polygon_sub_topic, polygon_pub_topic, footprint_topic, use_dynamic_sub))
+  {
+    if (use_dynamic_sub && polygon_sub_topic.empty() && footprint_topic.empty()) {
+      RCLCPP_ERROR(
         logger_,
-        "[%s]: Polygon points are not defined. Using dynamic subscription instead.",
+        "[%s]: Error while getting polygon parameters:"
+        " static points and sub topic both not defined",
         polygon_name_.c_str());
     }
-
-    if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT ||
-      action_type_ == DO_NOTHING)
-    {
-      // Dynamic polygon will be used
-      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();
-    } else if (action_type_ == APPROACH) {
-      // Obtain the footprint topic to make a footprint subscription for approach polygon
-      nav2_util::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".footprint_topic",
-        rclcpp::ParameterValue("local_costmap/published_footprint"));
-      footprint_topic =
-        node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
-    }
-    nav2_util::declare_parameter_if_not_declared(
-      node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false));
-    polygon_subscribe_transient_local_ =
-      node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool();
-  } catch (const std::exception & ex) {
-    RCLCPP_ERROR(
-      logger_,
-      "[%s]: Error while getting polygon parameters: %s",
-      polygon_name_.c_str(), ex.what());
     return false;
   }
 
   return true;
 }
 
+void Polygon::createSubscription(std::string & polygon_sub_topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  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
+    if (polygon_subscribe_transient_local_) {
+      polygon_qos.transient_local();
+    }
+    polygon_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
+      polygon_sub_topic, polygon_qos,
+      std::bind(&Polygon::polygonCallback, this, std::placeholders::_1));
+  }
+}
+
 void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg)
 {
   std::size_t new_size = msg->polygon.points.size();
@@ -582,6 +597,7 @@ bool Polygon::getPolygonFromString(
         "Points in the polygon specification must be pairs of numbers"
         "Found a point with %d numbers.",
         static_cast<int>(vvf[i].size()));
+      polygon.clear();
       return false;
     }
   }
diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp
index 84779a1a2d4..602a018c2bb 100644
--- a/nav2_collision_monitor/src/velocity_polygon.cpp
+++ b/nav2_collision_monitor/src/velocity_polygon.cpp
@@ -34,8 +34,9 @@ VelocityPolygon::~VelocityPolygon()
 }
 
 bool VelocityPolygon::getParameters(
-  std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic,
-  std::string & /*footprint_topic*/)
+  std::string & polygon_sub_topic,
+  std::string & polygon_pub_topic,
+  std::string & footprint_topic)
 {
   auto node = node_.lock();
   if (!node) {
@@ -43,7 +44,7 @@ bool VelocityPolygon::getParameters(
   }
   clock_ = node->get_clock();
 
-  if (!getCommonParameters(polygon_pub_topic)) {
+  if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic, false)) {
     return false;
   }