add test on circle radius update

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

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp

@@ -69,9 +69,10 @@ 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:
   /**
@@ -112,7 +113,7 @@ class Circle : public Polygon
   /// @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

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();
 
@@ -170,12 +170,15 @@ class Polygon
    * @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_sub_topic,
     std::string & polygon_pub_topic,
-    std::string & footprint_topic);
+    std::string & footprint_topic,
+    bool use_dynamic_sub = false);
 
   /**
    * @brief Supporting routine obtaining polygon-specific ROS-parameters

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,
@@ -83,40 +92,38 @@ bool Circle::getParameters(
   // 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 footprint subscription for the Circle. Thus, set string as empty.
-  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_;
-
-    // Do not need to proceed further, if "radius" parameter is defined.
-    // Static polygon will be used.
-    polygon_sub_topic.clear();
-    return true;
+    use_dynamic_sub = false;
   } 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: static radius and sub topic both not defined",
-      polygon_name_.c_str());
-    return false;
+  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;
   }
 
-  return true;
+  // 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)

nav2_collision_monitor/src/polygon.cpp

@@ -278,7 +278,8 @@ void Polygon::publish()
 bool Polygon::getCommonParameters(
   std::string & polygon_sub_topic,
   std::string & polygon_pub_topic,
-  std::string & footprint_topic)
+  std::string & footprint_topic,
+  bool use_dynamic_sub_topic)
 {
   auto node = node_.lock();
   if (!node) {
@@ -364,25 +365,27 @@ bool Polygon::getCommonParameters(
       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();
+
+    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_,
@@ -408,34 +411,32 @@ bool Polygon::getParameters(
   polygon_sub_topic.clear();
   footprint_topic.clear();
 
-  if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
-    return false;
-  }
-
+  bool use_dynamic_sub = true;  // if getting parameter points fails, use dynamic subscription
   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.
-    polygon_sub_topic.clear();
-    footprint_topic.clear();
-    return getPolygonFromString(poly_string, poly_);
+    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 (polygon_sub_topic.empty() && footprint_topic.empty()) {
-    RCLCPP_ERROR(
-      logger_,
-      "[%s]: Error while getting polygon parameters: static points and sub topic both not defined",
-      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]: Error while getting polygon parameters:"
+        " static points and sub topic both not defined",
+        polygon_name_.c_str());
+    }
     return false;
   }
 
@@ -596,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;
     }
   }

nav2_collision_monitor/src/velocity_polygon.cpp

@@ -44,7 +44,7 @@ bool VelocityPolygon::getParameters(
   }
   clock_ = node->get_clock();
 
-  if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) {
+  if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic, false)) {
     return false;
   }