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

Signed-off-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
@@ -74,8 +76,8 @@ 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 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 +87,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_;
+  /// @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

@@ -165,14 +165,21 @@ 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.
    * @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);
 
   /**
    * @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 +191,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

@@ -80,12 +80,14 @@ bool Circle::getParameters(
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  if (!getCommonParameters(polygon_pub_topic)) {
+  // Clear the polygon subscription topic. It will be set later, if necessary.
+  polygon_sub_topic.clear();
+
+  if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
     return false;
   }
 
-  // There is no polygon or footprint subscription for the Circle. Thus, set strings as empty.
-  polygon_sub_topic.clear();
+  // There is no footprint subscription for the Circle. Thus, set string as empty.
   footprint_topic.clear();
 
   try {
@@ -94,15 +96,77 @@ bool Circle::getParameters(
       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) {
+
+    // Do not need to proceed further, if "radius" parameter is defined.
+    // Static polygon will be used.
+    polygon_sub_topic.clear();
+    return true;
+  } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
+    RCLCPP_INFO(
+      logger_,
+      "[%s]: Polygon circle radius is not defined. Using dynamic subscription instead.",
+      polygon_name_.c_str());
+  }
+
+  if (polygon_sub_topic.empty()) {
     RCLCPP_ERROR(
       logger_,
-      "[%s]: Error while getting circle parameters: %s",
-      polygon_name_.c_str(), ex.what());
+      "[%s]: Error while getting circle parameters: static radius and sub topic both not defined",
+      polygon_name_.c_str());
     return false;
   }
 
   return true;
 }
 
+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

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,10 @@ 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)
 {
   auto node = node_.lock();
   if (!node) {
@@ -372,6 +363,26 @@ 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();
     }
+
+    if (action_type_ != APPROACH) {
+      // Get polygon sub topic, in case dynamic polygon will be used
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + ".polygon_sub_topic", rclcpp::ParameterValue(""));
+      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();
+    }
+
+    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_,
@@ -393,63 +404,66 @@ 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();
 
-  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(
-        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)) {
+    return false;
+  }
 
-    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();
-    }
+  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_ + ".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) {
+      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.
+    polygon_sub_topic.clear();
+    footprint_topic.clear();
+    return 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 (polygon_sub_topic.empty() && footprint_topic.empty()) {
     RCLCPP_ERROR(
       logger_,
-      "[%s]: Error while getting polygon parameters: %s",
-      polygon_name_.c_str(), ex.what());
+      "[%s]: Error while getting polygon parameters: static points and sub topic both not defined",
+      polygon_name_.c_str());
     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();