Merge pull request #71 from logivations/backport_velocity_polygon

Backport velocity polygons

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

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,12 @@ 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.
+
+
 ### 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,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/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

@@ -142,7 +142,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
@@ -217,6 +217,15 @@ class Polygon
    */
   bool isPointInside(const Point & point) const;
 
+  /**
+   * @brief Extracts Polygon points from a string with of the form [[x1,y1],[x2,y2],[x3,y3]...]
+   * @param poly_string Input String containing the verteceis of the polygon
+   * @param polygon Output Point vector with all the vertecies of the polygon
+   * @return True if all parameters were obtained or false in failure case
+   */
+  bool getPolygonFromString(std::string & poly_string, std::vector<Point> & polygon);
+
+
   // ----- Variables -----
 
   /// @brief Collision Monitor node

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_

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"

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(),

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(),
@@ -458,7 +462,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) {

nav2_collision_monitor/src/polygon.cpp

@@ -23,6 +23,7 @@
 
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_util/array_parser.hpp"
 
 #include "nav2_collision_monitor/kinematics.hpp"
 using rcl_interfaces::msg::ParameterType;
@@ -191,7 +192,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
@@ -580,4 +581,45 @@ inline bool Polygon::isPointInside(const Point & point) const
   return res;
 }
 
+bool Polygon::getPolygonFromString(
+  std::string & poly_string,
+  std::vector<Point> & polygon)
+{
+  std::string error;
+  std::vector<std::vector<float>> vvf = nav2_util::parseVVF(poly_string, error);
+
+  if (error != "") {
+    RCLCPP_ERROR(
+      logger_, "Error parsing polygon parameter %s: '%s'",
+      poly_string.c_str(), error.c_str());
+    return false;
+  }
+
+  // Check for minimum 4 points
+  if (vvf.size() <= 3) {
+    RCLCPP_ERROR(
+      logger_,
+      "Polygon must have at least three points.");
+    return false;
+  }
+  for (unsigned int i = 0; i < vvf.size(); i++) {
+    if (vvf[i].size() == 2) {
+      Point point;
+      point.x = vvf[i][0];
+      point.y = vvf[i][1];
+      polygon.push_back(point);
+    } else {
+      RCLCPP_ERROR(
+        logger_,
+        "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;
+    }
+  }
+
+  return true;
+}
+
 }  // namespace nav2_collision_monitor