rewrite velocity polygon with polygon base class

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

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -33,6 +33,7 @@
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"
 #include "nav2_collision_monitor/circle.hpp"
+#include "nav2_collision_monitor/velocity_polygon.hpp"
 #include "nav2_collision_monitor/source.hpp"
 #include "nav2_collision_monitor/scan.hpp"
 #include "nav2_collision_monitor/pointcloud.hpp"

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -28,7 +28,6 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
 
-#include "nav2_collision_monitor/velocity_polygon.hpp"
 #include "nav2_collision_monitor/types.hpp"
 
 namespace nav2_collision_monitor
@@ -133,15 +132,10 @@ class Polygon
    */
   virtual bool isShapeSet();
 
-  /**
-   * @brief Returns true if using velocity based polygon
-   */
-  bool isUsingVelocityPolygonSelector();
-
   /**
    * @brief Updates polygon from footprint subscriber (if any)
    */
-  void updatePolygon(const Velocity & cmd_vel_in);
+  virtual void updatePolygon(const Velocity & /*cmd_vel_in*/);
 
   /**
    * @brief Gets number of points inside given polygon
@@ -168,14 +162,6 @@ class Polygon
    */
   void publish();
 
-  /**
-   * @brief Set the polygon shape based on the polygon points
-   * @param poly_points Polygon points in the format of x1, y1, x2, y2 ...
-   * @param poly Output polygon points (vertices)
-   * @return True if successfully set the polygon shape
-   */
-  static bool setPolygonShape(std::vector<double> & poly_points, std::vector<Point> & poly);
-
 protected:
   /**
    * @brief Supporting routine obtaining ROS-parameters common for all shapes
@@ -281,9 +267,6 @@ class Polygon
   /// @brief Polygon publisher for visualization purposes
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
 
-  /// @brief Velocity polygon (if any)
-  std::vector<std::shared_ptr<VelocityPolygon>> velocity_polygons_;
-
   /// @brief Polygon points (vertices) in a base_frame_id_
   std::vector<Point> poly_;
 };  // class Polygon

nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp

@@ -1,4 +1,4 @@
-// Copyright (c) 2022 Dexory
+// Copyright (c) 2023 Dexory
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -18,12 +18,12 @@
 #include <string>
 #include <vector>
 
-#include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
-#include "tf2_ros/buffer.h"
-
-#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_collision_monitor/polygon.hpp"
 #include "nav2_collision_monitor/types.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/buffer.h"
 
 namespace nav2_collision_monitor
 {
@@ -33,21 +33,17 @@ namespace nav2_collision_monitor
  * This class contains all the points of the polygon and
  * the expected condition of the velocity based polygon.
  */
-class VelocityPolygon
+class VelocityPolygon : public Polygon
 {
 public:
   /**
    * @brief VelocityPolygon constructor
    * @param node Collision Monitor node pointer
    * @param polygon_name Name of main polygon
-   * @param velocity_polygon_name Name of velocity polygon
    */
   VelocityPolygon(
-    const nav2_util::LifecycleNode::WeakPtr & node,
-    const std::string & polygon_name,
-    const std::string & velocity_polygon_name,
-    const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
-    const std::string & base_frame_id,
+    const nav2_util::LifecycleNode::WeakPtr & node, const std::string & polygon_name,
+    const std::shared_ptr<tf2_ros::Buffer> tf_buffer, const std::string & base_frame_id,
     const tf2::Duration & transform_tolerance);
   /**
    * @brief VelocityPolygon destructor
@@ -58,73 +54,39 @@ class VelocityPolygon
    * @brief Supporting routine obtaining velocity polygon specific ROS-parameters
    * @return True if all parameters were obtained or false in failure case
    */
-  bool getParameters();
-
-  /**
-   * @brief Check if the velocities and direction is in expected range.
-   * @param cmd_vel_in Robot twist command input
-   * @return True if speed and direction is within the condition
-   */
-  bool isInRange(const Velocity & cmd_vel_in);
-
-  /**
-   * @brief Check if the velocities and direction is in expected range.
-   * @param cmd_vel_in Robot twist command input
-   * @return True if speed and direction is within the condition
-   */
-  std::vector<Point> getPolygon();
+  bool getParameters(
+    std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic,
+    std::string & /*footprint_topic*/) override;
 
 protected:
-  // ----- Variables -----
+  // override the base class update polygon
+  void updatePolygon(const Velocity & cmd_vel_in) override;
 
-  /// @brief Collision Monitor node
-  nav2_util::LifecycleNode::WeakPtr node_;
-  /// @brief Collision monitor node logger stored for further usage
-  rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")};
+  bool holonomic_;
 
-  /**
-   * @brief Dynamic polygon callback
-   * @param msg Shared pointer to the polygon message
-   */
-  void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg);
+  // Define a structure to store the basic parameters
+  struct SubPolygonParameter
+  {
+    std::vector<Point> poly_;
+    std::string velocity_polygon_name_;
+    double linear_min_;
+    double linear_max_;
+    double theta_min_;
+    double theta_max_;
+    double direction_end_angle_;
+    double direction_start_angle_;
+  };
+
+  // Create a vector to store instances of BasicParameters
+  std::vector<SubPolygonParameter> sub_polygons_;
 
   /**
-   * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message
-   * @param msg Message to update polygon from
+   * @brief Check if the velocities and direction is in expected range.
+   * @param cmd_vel_in Robot twist command input
+   * @return True if speed and direction is within the condition
    */
-  void updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg);
-
-  // Basic parameters
-  /// @brief Points of the polygon
-  std::vector<Point> poly_;
-  /// @brief Name of polygon
-  std::string polygon_name_;
-  /// @brief Polygon subscription
-  rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_sub_;
-  /// @brief velocity_polygon_name Name of velocity polygon
-  std::string velocity_polygon_name_;
-  /// @brief Holonomic flag (true for holonomic, false for non-holonomic)
-  bool holonomic_;
-  /// @brief Maximum twist linear velocity
-  double linear_max_;
-  /// @brief Minimum twist linear velocity
-  double linear_min_;
-  /// @brief End angle of velocity direction for holonomic model
-  double direction_end_angle_;
-  /// @brief Start angle of velocity direction for holonomic model
-  double direction_start_angle_;
-  /// @brief Maximum twist rotational speed
-  double theta_max_;
-  /// @brief Minimum twist rotational speed
-  double theta_min_;
+  bool isInRange(const Velocity & cmd_vel_in, const SubPolygonParameter & sub_polygon_param);
 
-  // Global variables
-  /// @brief TF buffer
-  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  /// @brief Base frame ID
-  std::string base_frame_id_;
-  /// @brief Transform tolerance
-  tf2::Duration transform_tolerance_;
 };  // class VelocityPolygon
 
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -14,7 +14,8 @@ collision_monitor:
     # Footprint could be "polygon" type with dynamically set footprint from footprint_topic
     # or "circle" type with static footprint set by radius. "footprint_topic" parameter
     # to be ignored in circular case.
-    polygons: ["PolygonStop" , "FootprintApproach"]
+    # or "velocity_polygon" type with dynamically set polygon from velocity_polygons
+    polygons: ["PolygonStop"]
     PolygonStop:
       type: "polygon"
       points: "[[0.3, 0.3], [0.3, -0.3], [0.0, -0.3], [0.0, 0.3]]"
@@ -45,38 +46,43 @@ collision_monitor:
     FootprintApproach:
       type: "polygon"
       action_type: "approach"
-      # footprint_topic: "/local_costmap/published_footprint"
+      footprint_topic: "/local_costmap/published_footprint"
       time_before_collision: 2.0
       simulation_time_step: 0.1
       min_points: 6
       visualize: False
       enabled: True
+    VelocityPolygonApproach:
+      type: "velocity_polygon"
+      action_type: "approach"
+      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"] # third priority
+      velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stop"]
+      holonomic: false
       rotation:
-        points: [0.3, 0.3, 0.3, -0.3, -0.3, -0.3, -0.3, 0.3]
-        holonomic: false
+        points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]"
         linear_min: 0.0
         linear_max: 0.05
         theta_min: -1.0
         theta_max: 1.0
       translation_forward:
-        points: [0.35, 0.3, 0.35, -0.3, -0.2, -0.3, -0.2, 0.3]
-        holonomic: false
+        points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]"
         linear_min: 0.0
         linear_max: 1.0
         theta_min: -1.0
         theta_max: 1.0
       translation_backward:
-        points: [0.2, 0.3, 0.2, -0.3, -0.35, -0.3, -0.35, 0.3]
-        holonomic: false
+        points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]"
         linear_min: -1.0
         linear_max: 0.0
         theta_min: -1.0
         theta_max: 1.0
       stop:
-        points: [0.25, 0.25, 0.25, -0.25, -0.25, -0.25, -0.25, 0.25]
-        holonomic: false
+        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
         theta_min: -1.0

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -298,6 +298,10 @@ bool CollisionMonitor::configurePolygons(
         polygons_.push_back(
           std::make_shared<Circle>(
             node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
+      } else if (polygon_type == "velocity_polygon") {
+        polygons_.push_back(
+          std::make_shared<VelocityPolygon>(
+            node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance));
       } else {  // Error if something else
         RCLCPP_ERROR(
           get_logger(),