Add velocity based polygon

nav2_collision_monitor/CMakeLists.txt

@@ -45,6 +45,7 @@ set(library_name ${executable_name}_core)
 add_library(${library_name} SHARED
   src/collision_monitor_node.cpp
   src/polygon.cpp
+  src/polygon_velocity.cpp
   src/circle.cpp
   src/source.cpp
   src/scan.cpp

nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

@@ -28,6 +28,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
 
+#include "nav2_collision_monitor/polygon_velocity.hpp"
 #include "nav2_collision_monitor/types.hpp"
 
 namespace nav2_collision_monitor
@@ -127,6 +128,16 @@ class Polygon
    */
   virtual bool isShapeSet();
 
+  /**
+   * @brief Returns true if using velocity based polygon
+   */
+  bool isUsingPolygonVelocitySelector();
+
+  /**
+   * @brief Updates polygon by velocity (if any)
+   */
+  void updatePolygonByVelocity(const Velocity & cmd_vel_in);
+
   /**
    * @brief Updates polygon from footprint subscriber (if any)
    */
@@ -243,6 +254,9 @@ class Polygon
   /// @brief Polygon publisher for visualization purposes
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr polygon_pub_;
 
+  /// @brief Polygon velocity (if any)
+  std::vector<std::shared_ptr<PolygonVelocity>> polygon_velocity_;
+
   /// @brief Polygon points (vertices) in a base_frame_id_
   std::vector<Point> poly_;
 };  // class Polygon

nav2_collision_monitor/include/nav2_collision_monitor/polygon_velocity.hpp

@@ -0,0 +1,103 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// 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__POLYGON_VELOCITY_HPP_
+#define NAV2_COLLISION_MONITOR__POLYGON_VELOCITY_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "nav2_collision_monitor/types.hpp"
+
+namespace nav2_collision_monitor
+{
+
+/**
+ * @brief Polygon velocity class.
+ * This class contains all the points of the polygon and
+ * the expected condition of the velocity based polygon.
+ */
+class PolygonVelocity
+{
+public:
+  /**
+   * @brief PolygonVelocity constructor
+   * @param poly Points of the polygon
+   * @param polygon_name Name of polygon
+   * @param linear_max Maximum twist linear velocity
+   * @param linear_min Minimum twist linear velocity
+   * @param direction_max Maximum twist direction angle
+   * @param direction_min Minimum twist direction angle
+   * @param theta_max Maximum twist rotational speed
+   * @param theta_min Minimum twist rotational speed
+   */
+  PolygonVelocity(
+    const std::vector<Point> & poly,
+    const std::string & polygon_name,
+    const double & linear_max,
+    const double & linear_min,
+    const double & direction_max,
+    const double & direction_min,
+    const double & theta_max,
+    const double & theta_min);
+  /**
+   * @brief PolygonVelocity destructor
+   */
+  virtual ~PolygonVelocity();
+
+  /**
+   * @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();
+
+protected:
+  // ----- Variables -----
+
+  /// @brief Collision monitor node logger stored for further usage
+  rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")};
+
+  // Basic parameters
+  /// @brief Points of the polygon
+  std::vector<Point> poly_;
+  /// @brief Name of polygon
+  std::string polygon_name_;
+  /// @brief Maximum twist linear velocity
+  double linear_max_;
+  /// @brief Minimum twist linear velocity
+  double linear_min_;
+  /// @brief Maximum twist direction angle
+  double direction_max_;
+  /// @brief Minimum twist direction angle
+  double direction_min_;
+  /// @brief Maximum twist rotational speed
+  double theta_max_;
+  /// @brief Minimum twist rotational speed
+  double theta_min_;
+};  // class PolygonVelocity
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__POLYGON_VELOCITY_HPP_

nav2_collision_monitor/params/collision_monitor_params.yaml

@@ -42,11 +42,45 @@ 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
+      visualize: True
+      polygon_pub_topic: "polygon_approach"
+      polygon_velocity: ["stop", "rotation", "translation_forward", "translation_backward"] # third priority
+      stop:
+        points: [0.25, 0.25, 0.25, -0.25, -0.25, -0.25, -0.25, 0.25]
+        linear_min: 0.0
+        linear_max: 0.01
+        direction_min: -3.14159
+        direction_max: 3.14159
+        theta_min: 0.0
+        theta_max: 0.01
+      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
+        direction_min: -3.14159
+        direction_max: 3.14159
+        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
+        direction_min: -1.5708
+        direction_max: 1.5708
+        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: 0.0
+        linear_max: 1.0
+        direction_min: 1.5708
+        direction_max: -1.5708
+        theta_min: -1.0
+        theta_max: 1.0
     observation_sources: ["scan"]
     scan:
       type: "scan"

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -423,6 +423,10 @@ bool CollisionMonitor::processStopSlowdownLimit(
   const Velocity & velocity,
   Action & robot_action) const
 {
+  if (polygon->isUsingPolygonVelocitySelector()) {
+    polygon->updatePolygonByVelocity(velocity);
+  }
+
   if (!polygon->isShapeSet()) {
     return false;
   }
@@ -478,6 +482,10 @@ bool CollisionMonitor::processApproach(
   const Velocity & velocity,
   Action & robot_action) const
 {
+  if (polygon->isUsingPolygonVelocitySelector()) {
+    polygon->updatePolygonByVelocity(velocity);
+  }
+
   if (!polygon->isShapeSet()) {
     return false;
   }

nav2_collision_monitor/src/polygon.cpp

@@ -168,6 +168,34 @@ bool Polygon::isShapeSet()
   return true;
 }
 
+bool Polygon::isUsingPolygonVelocitySelector()
+{
+  return !polygon_velocity_.empty();
+}
+
+void Polygon::updatePolygonByVelocity(const Velocity & cmd_vel_in)
+{
+  for (auto & polygon : polygon_velocity_) {
+
+    if (polygon->isInRange(cmd_vel_in)) {
+      // Set the polygon that is within the speed range
+      poly_ = polygon->getPolygon();
+
+      // Update visualization polygon
+      polygon_.polygon.points.clear();
+      for (const Point & p : poly_) {
+        geometry_msgs::msg::Point32 p_s;
+        p_s.x = p.x;
+        p_s.y = p.y;
+        // p_s.z will remain 0.0
+        polygon_.polygon.points.push_back(p_s);
+      }
+
+      return;
+    }
+  }
+}
+
 void Polygon::updatePolygon()
 {
   if (footprint_sub_ != nullptr) {
@@ -420,19 +448,121 @@ bool Polygon::getParameters(
         polygon_name_.c_str());
     }
 
-    if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT) {
-      // Dynamic polygon will be used
+    try {
+      if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT) {
+        // Dynamic polygon will be used
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING);
+        polygon_sub_topic =
+          node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string();
+        return true;
+      } else if (action_type_ == APPROACH) {
+        // Obtain the footprint topic to make a footprint subscription for approach polygon
+        nav2_util::declare_parameter_if_not_declared(
+          node, polygon_name_ + ".footprint_topic", rclcpp::PARAMETER_STRING);
+        footprint_topic =
+          node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
+        return true;
+      }
+    } catch (const rclcpp::exceptions::ParameterUninitializedException &) {
+      RCLCPP_INFO(
+        logger_,
+        "[%s]: Polygon topic are not defined. Using polygon velocity instead.",
+        polygon_name_.c_str());
+    }
+
+    // Polygon velocity will be used
+    nav2_util::declare_parameter_if_not_declared(
+      node, polygon_name_ + ".polygon_velocity", rclcpp::PARAMETER_STRING_ARRAY);
+    std::vector<std::string> polygon_velocity_array = node->get_parameter(
+      polygon_name_ + ".polygon_velocity").as_string_array();
+
+    for (std::string polygon_velocity_name : polygon_velocity_array) {
+
+      std::string polygon_name = polygon_velocity_name;
+
       nav2_util::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING);
-      polygon_sub_topic =
-        node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string();
-    } else if (action_type_ == APPROACH) {
-      // Obtain the footprint topic to make a footprint subscription for approach polygon
+        node, polygon_name_ + "." + polygon_velocity_name + ".points",
+        rclcpp::PARAMETER_DOUBLE_ARRAY);
+      std::vector<double> polygon_points = node->get_parameter(
+        polygon_name_ + "." + polygon_velocity_name + ".points").as_double_array();
+
+      // Check for points format correctness
+      if (polygon_points.size() <= 6 || polygon_points.size() % 2 != 0) {
+        RCLCPP_ERROR(
+          logger_,
+          "[%s]: Polygon has incorrect points description", polygon_name_.c_str());
+        return false;
+      }
+
+      // Obtain polygon vertices
+      Point point;
+      std::vector<nav2_collision_monitor::Point> poly;
+      bool first = true;
+      for (double val : polygon_points) {
+        if (first) {
+          point.x = val;
+        } else {
+          point.y = val;
+          poly.push_back(point);
+        }
+        first = !first;
+      }
+
+      // linear_max param
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + "." + polygon_velocity_name + ".linear_max",
+        rclcpp::ParameterValue(0.0));
+      double linear_max =
+        node->get_parameter(
+        polygon_name_ + "." + polygon_velocity_name +
+        ".linear_max").as_double();
+
+      // linear_min param
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + "." + polygon_velocity_name + ".linear_min",
+        rclcpp::ParameterValue(0.0));
+      double linear_min =
+        node->get_parameter(
+        polygon_name_ + "." + polygon_velocity_name +
+        ".linear_min").as_double();
+
+      // direction_max param
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + "." + polygon_velocity_name + ".direction_max",
+        rclcpp::ParameterValue(0.0));
+      double direction_max =
+        node->get_parameter(
+        polygon_name_ + "." + polygon_velocity_name +
+        ".direction_max").as_double();
+
+      // direction_min param
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + "." + polygon_velocity_name + ".direction_min",
+        rclcpp::ParameterValue(0.0));
+      double direction_min =
+        node->get_parameter(
+        polygon_name_ + "." + polygon_velocity_name +
+        ".direction_min").as_double();
+
+      // theta_max param
+      nav2_util::declare_parameter_if_not_declared(
+        node, polygon_name_ + "." + polygon_velocity_name + ".theta_max", rclcpp::ParameterValue(
+          0.0));
+      double theta_max =
+        node->get_parameter(polygon_name_ + "." + polygon_velocity_name + ".theta_max").as_double();
+
+      // theta_min param
       nav2_util::declare_parameter_if_not_declared(
-        node, polygon_name_ + ".footprint_topic",
-        rclcpp::ParameterValue("local_costmap/published_footprint"));
-      footprint_topic =
-        node->get_parameter(polygon_name_ + ".footprint_topic").as_string();
+        node, polygon_name_ + "." + polygon_velocity_name + ".theta_min", rclcpp::ParameterValue(
+          0.0));
+      double theta_min =
+        node->get_parameter(polygon_name_ + "." + polygon_velocity_name + ".theta_min").as_double();
+
+      polygon_velocity_.push_back(
+        std::make_shared<PolygonVelocity>(
+          poly, polygon_name, linear_max, linear_min, direction_max,
+          direction_min, theta_max, theta_min));
     }
   } catch (const std::exception & ex) {
     RCLCPP_ERROR(