Revert "Add velocity based polygon (#3708)"

This reverts commit a3ed745.

nav2_collision_monitor/CMakeLists.txt

@@ -49,7 +49,6 @@ 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
@@ -60,7 +59,6 @@ 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

nav2_collision_monitor/README.md

@@ -36,8 +36,6 @@ 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:
 
@@ -50,13 +48,8 @@ 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 (`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.
+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.
 ![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.

nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp

@@ -33,7 +33,6 @@
 #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/collision_monitor_node.hpp

@@ -33,7 +33,6 @@
 #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

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

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -11,10 +11,9 @@ 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.
-    # (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
+    # 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.
-    # (3) "velocity_polygon" type with dynamically set polygon from velocity_polygons
     polygons: ["PolygonStop"]
     PolygonStop:
       type: "polygon"
@@ -52,42 +51,6 @@ 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"

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -207,10 +207,6 @@ 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(),

nav2_collision_monitor/src/collision_monitor_node.cpp

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

nav2_collision_monitor/src/polygon.cpp

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