Add velocity based polygon (#3708)

* add velocity based polygon * fix header, copyright and variable name change * optimise polygon update * optimise duplicated code with setPolygonShape * add warning log for uncovered speed * update feedback * rename polygon velocity to velocity polygon * cleanup * fix typo * add dynamic support for velocity polygon * wrap try catch for getting parameters * update naming and linting * use switch case * Revert "use switch case" This reverts commit 1230ede. * fix proper return for invalid parameters * remove topic parameter for velocity polygon * fix formatting manually * continue if points are not defined * rewrite velocity polygon with polygon base class Signed-off-by: nelson <kaichie.lee@gmail.com> * update review comments and description Signed-off-by: nelson <kaichie.lee@gmail.com> * add VelocityPolygon to detector node Signed-off-by: nelson <kaichie.lee@gmail.com> * review update Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> * add velocity polygon tests Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * add in-line comment Signed-off-by: nelson <kaichie.lee@gmail.com> * fix push back Signed-off-by: nelson <kaichie.lee@gmail.com> * minor change and update README Signed-off-by: nelson <kaichie.lee@gmail.com> * update README Signed-off-by: nelson <kaichie.lee@gmail.com> --------- Signed-off-by: nelson <kaichie.lee@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
ros-navigation · Apr 4, 2024 · a3ed745 · a3ed745
1 parent 4c3174e
commit a3ed745
Show file tree

Hide file tree

Showing 16 changed files with 1,092 additions and 10 deletions.
diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt
@@ -49,6 +49,7 @@ set(detector_library_name ${detector_executable_name}_core)
 add_library(${monitor_library_name} SHARED
   src/collision_monitor_node.cpp
   src/polygon.cpp
+  src/velocity_polygon.cpp
   src/circle.cpp
   src/source.cpp
   src/scan.cpp
@@ -59,6 +60,7 @@ add_library(${monitor_library_name} SHARED
 add_library(${detector_library_name} SHARED
   src/collision_detector_node.cpp
   src/polygon.cpp
+  src/velocity_polygon.cpp
   src/circle.cpp
   src/source.cpp
   src/scan.cpp

diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md
@@ -36,6 +36,8 @@ The zones around the robot can take the following shapes:
 * Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface.
 * Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time.
 * Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape.
+* VelocityPolygon: allow switching of polygons based on the command velocity. When the velocity is covered by multiple sub polygons, the first sub polygon in the `velocity_polygons` list will be used. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s). 
+
 
 The data may be obtained from different data sources:
 
@@ -48,8 +50,13 @@ The data may be obtained from different data sources:
 The Collision Monitor is designed to operate below Nav2 as an independent safety node.
 This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate.
 
-The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.
+The following diagram is showing the high-level design of Collision Monitor module. All shapes (`Polygon`, `Circle` and `VelocityPolygon`) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model.
 ![HLD.png](doc/HLD.png)
+
+`VelocityPolygon` can be configured with multiple sub polygons and can switch between them based on the velocity.
+![dexory_velocity_polygon.gif](doc/dexory_velocity_polygon.gif)
+
+
 ### Configuration
 
 Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages.

diff --git a/nav2_collision_monitor/doc/dexory_velocity_polygon.gif b/nav2_collision_monitor/doc/dexory_velocity_polygon.gif
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_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"

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/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"

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
@@ -135,7 +135,7 @@ class Polygon
   /**
    * @brief Updates polygon from footprint subscriber (if any)
    */
-  void updatePolygon();
+  virtual void updatePolygon(const Velocity & /*cmd_vel_in*/);
 
   /**
    * @brief Gets number of points inside given polygon

diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/velocity_polygon.hpp
@@ -0,0 +1,115 @@
+// 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
+#define NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/polygon_stamped.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
+{
+
+/**
+ * @brief Velocity polygon class.
+ * This class contains all the points of the polygon and
+ * the expected condition of the velocity based polygon.
+ */
+class VelocityPolygon : public Polygon
+{
+public:
+  /**
+   * @brief VelocityPolygon constructor
+   * @param node Collision Monitor node pointer
+   * @param polygon_name Name of main polygon
+   */
+  VelocityPolygon(
+    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
+   */
+  virtual ~VelocityPolygon();
+
+  /**
+   * @brief Overriden getParameters function for VelocityPolygon parameters
+   * @param polygon_sub_topic Not used in VelocityPolygon
+   * @param polygon_pub_topic Output name of polygon publishing topic
+   * @param footprint_topic Not used in VelocityPolygon
+   * @return True if all parameters were obtained or false in failure case
+   */
+  bool getParameters(
+    std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic,
+    std::string & /*footprint_topic*/) override;
+
+  /**
+   * @brief Overriden updatePolygon function for VelocityPolygon
+   * @param cmd_vel_in Robot twist command input
+   */
+  void updatePolygon(const Velocity & cmd_vel_in) override;
+
+protected:
+  /**
+    * @brief Custom struc to store the parameters of the sub-polygon
+    * @param poly_ The points of the sub-polygon
+    * @param velocity_polygon_name_ The name of the sub-polygon
+    * @param linear_min_ The minimum linear velocity
+    * @param linear_max_ The maximum linear velocity
+    * @param theta_min_ The minimum angular velocity
+    * @param theta_max_ The maximum angular velocity
+    * @param direction_end_angle_ The end angle of the direction(For holonomic robot only)
+    * @param direction_start_angle_ The start angle of the direction(For holonomic robot only)
+    */
+  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_;
+  };
+
+  /**
+   * @brief Check if the velocities and direction is in expected range.
+   * @param cmd_vel_in Robot twist command input
+   * @param sub_polygon_param Sub polygon parameters
+   * @return True if speed and direction is within the condition
+   */
+  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_;
+  /// @brief Vector to store the parameters of the sub-polygon
+  std::vector<SubPolygonParameter> sub_polygons_;
+};  // class VelocityPolygon
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__VELOCITY_POLYGON_HPP_
diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -11,9 +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.
+    # (3) "velocity_polygon" type with dynamically set polygon from velocity_polygons
     polygons: ["PolygonStop"]
     PolygonStop:
       type: "polygon"
@@ -51,6 +52,42 @@ collision_monitor:
       min_points: 6
       visualize: False
       enabled: True
+    VelocityPolygonStop:
+      type: "velocity_polygon"
+      action_type: "stop"
+      min_points: 6
+      visualize: True
+      enabled: True
+      polygon_pub_topic: "velocity_polygon_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]]"
+        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]]"
+        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]]"
+        linear_min: -1.0
+        linear_max: 0.0
+        theta_min: -1.0
+        theta_max: 1.0
+      # This is the last polygon to be checked, it should cover the entire range of robot's velocities
+      # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity
+      # is not covered by any of the other sub-polygons 
+      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
+        theta_min: -1.0
+        theta_max: 1.0
     observation_sources: ["scan"]
     scan:
       type: "scan"

diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp
@@ -207,6 +207,10 @@ bool CollisionDetector::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(),

diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -297,6 +297,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(),
@@ -440,7 +444,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
     }
 
     // Update polygon coordinates
-    polygon->updatePolygon();
+    polygon->updatePolygon(cmd_vel_in);
 
     const ActionType at = polygon->getActionType();
     if (at == STOP || at == SLOWDOWN || at == LIMIT) {

diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp
@@ -181,7 +181,7 @@ bool Polygon::isShapeSet()
   return true;
 }
 
-void Polygon::updatePolygon()
+void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/)
 {
   if (footprint_sub_ != nullptr) {
     // Get latest robot footprint from footprint subscriber