Add velocity based polygon

nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp

@@ -15,6 +15,7 @@
 #ifndef NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
 #define NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -99,6 +100,9 @@ class VelocityPolygon : public Polygon
    */
   bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param);
 
+  // Clock
+  rclcpp::Clock::SharedPtr clock_;
+
   // Variables
   /// @brief Flag to indicate if the robot is holonomic
   bool holonomic_;

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -11,10 +11,10 @@ collision_monitor:
     stop_pub_timeout: 2.0
     # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
     # and robot footprint for "approach" action type.
-    # Footprint could be "polygon" type with dynamically set footprint from footprint_topic
-    # or "circle" type with static footprint set by radius. "footprint_topic" parameter
+    # (1) Footprint could be "polygon" type with dynamically set footprint from footprint_topic
+    # (2) "circle" type with static footprint set by radius. "footprint_topic" parameter
     # to be ignored in circular case.
-    # or "velocity_polygon" type with dynamically set polygon from velocity_polygons
+    # (3) "velocity_polygon" type with dynamically set polygon from velocity_polygons
     polygons: ["PolygonStop"]
     PolygonStop:
       type: "polygon"
@@ -52,16 +52,16 @@ collision_monitor:
       min_points: 6
       visualize: False
       enabled: True
-    VelocityPolygonApproach:
+    VelocityPolygonStop:
       type: "velocity_polygon"
-      action_type: "approach"
+      action_type: "stop"
       time_before_collision: 2.0
       simulation_time_step: 0.1
       min_points: 6
       visualize: True
       enabled: True
       polygon_pub_topic: "polygon_approach"
-      velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stop"]
+      velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
       holonomic: false
       rotation:
         points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]"
@@ -81,7 +81,7 @@ collision_monitor:
         linear_max: 0.0
         theta_min: -1.0
         theta_max: 1.0
-      stop:
+      stopped:
         points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
         linear_min: -1.0
         linear_max: 1.0

nav2_collision_monitor/src/velocity_polygon.cpp

@@ -41,6 +41,7 @@ bool VelocityPolygon::getParameters(
   if (!node) {
     throw std::runtime_error{"Failed to lock node"};
   }
+  clock_ = node->get_clock();
 
   if (!getCommonParameters(polygon_pub_topic)) {
     return false;
@@ -74,31 +75,31 @@ bool VelocityPolygon::getParameters(
       double linear_min;
       nav2_util::declare_parameter_if_not_declared(
         node, polygon_name_ + "." + velocity_polygon_name + ".linear_min",
-        rclcpp::ParameterValue(0.0));
+        rclcpp::PARAMETER_DOUBLE);
       linear_min = node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".linear_min")
         .as_double();
 
       // linear_max param
       double linear_max;
       nav2_util::declare_parameter_if_not_declared(
         node, polygon_name_ + "." + velocity_polygon_name + ".linear_max",
-        rclcpp::ParameterValue(0.0));
+        rclcpp::PARAMETER_DOUBLE);
       linear_max = node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".linear_max")
         .as_double();
 
       // theta_min param
       double theta_min;
       nav2_util::declare_parameter_if_not_declared(
         node, polygon_name_ + "." + velocity_polygon_name + ".theta_min",
-        rclcpp::ParameterValue(0.0));
+        rclcpp::PARAMETER_DOUBLE);
       theta_min =
         node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".theta_min").as_double();
 
       // theta_max param
       double theta_max;
       nav2_util::declare_parameter_if_not_declared(
         node, polygon_name_ + "." + velocity_polygon_name + ".theta_max",
-        rclcpp::ParameterValue(0.0));
+        rclcpp::PARAMETER_DOUBLE);
       theta_max =
         node->get_parameter(polygon_name_ + "." + velocity_polygon_name + ".theta_max").as_double();
 
@@ -122,10 +123,9 @@ bool VelocityPolygon::getParameters(
           .as_double();
       }
 
-      SubPolygonParameter sub_polygon = {
-        poly, velocity_polygon_name, linear_min, linear_max, theta_min,
-        theta_max, direction_end_angle, direction_start_angle};
-      sub_polygons_.emplace_back(sub_polygon);
+      sub_polygons_.emplace_back(
+        SubPolygonParameter{poly, velocity_polygon_name, linear_min, linear_max, theta_min,
+          theta_max, direction_end_angle, direction_start_angle});
     }
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(
@@ -157,12 +157,8 @@ void VelocityPolygon::updatePolygon(const Velocity & cmd_vel_in)
   }
 
   // Log for uncovered velocity
-  auto node = node_.lock();
-  if (!node) {
-    throw std::runtime_error{"Failed to lock node"};
-  }
   RCLCPP_WARN_THROTTLE(
-    logger_, *node->get_clock(), 2.0,
+    logger_, *(clock_), 2.0,
     "Velocity is not covered by any of the velocity polygons. x: %.3f y: %.3f tw: %.3f ",
     cmd_vel_in.x, cmd_vel_in.y, cmd_vel_in.tw);
   return;