[Collision monitor] Dynamic radius for circle polygon (#4226)

* Collision monitor: add a radius topic sub for dynamic circle polygon

Signed-off-by: asarazin <[email protected]>

* add test on circle radius update

Signed-off-by: asarazin <[email protected]>

---------

Signed-off-by: asarazin <[email protected]>
Co-authored-by: asarazin <[email protected]>

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

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

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

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>

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