Upgrade collision monitor

nav2_collision_monitor/CMakeLists.txt

@@ -8,13 +8,15 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_util REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 ### Header ###
 
@@ -31,12 +33,14 @@ set(dependencies
   rclcpp_components
   sensor_msgs
   geometry_msgs
+  std_msgs
   tf2
   tf2_ros
   tf2_geometry_msgs
   nav2_util
   nav2_costmap_2d
   nav2_msgs
+  visualization_msgs
 )
 
 set(monitor_executable_name collision_monitor)
@@ -47,20 +51,24 @@ 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
   src/pointcloud.cpp
+  src/polygon_source.cpp
   src/range.cpp
   src/kinematics.cpp
 )
 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
   src/pointcloud.cpp
+  src/polygon_source.cpp
   src/range.cpp
   src/kinematics.cpp
 )
@@ -127,6 +135,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
 endif()
 
 ### Ament stuff ###

nav2_collision_monitor/README.md

@@ -1,5 +1,7 @@
 # Nav2 Collision Monitor
 
+## Collision Monitor
+
 The Collision Monitor is a node providing an additional level of robot safety.
 It performs several collision avoidance related tasks using incoming data from the sensors, bypassing the costmap and trajectory planners, to monitor for and prevent potential collisions at the emergency-stop level.
 
@@ -12,7 +14,11 @@ The costmaps / trajectory planners will handle most situations, but this is to h
 
 ![polygons.png](doc/polygons.png)
 
-## Features
+Demonstration of Collision Monitor abilities presented at 6th ROS Developers Day 2023, could be found below:
+
+[![cm-ros-devday.png](doc/cm_ros_devday.png)](https://www.youtube.com/watch?v=bWliK0PC5Ms)
+
+### Features
 
 The Collision Monitor uses polygons relative the robot's base frame origin to define "zones".
 Data that fall into these zones trigger an operation depending on the model being used.
@@ -27,36 +33,42 @@ The following models of safety behaviors are employed by Collision Monitor:
 
 The zones around the robot can take the following shapes:
 
-* Arbitrary user-defined polygon relative to the robot base frame.
-* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape.
-* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time.
+* 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:
 
 * Laser scanners (`sensor_msgs::msg::LaserScan` messages)
 * PointClouds (`sensor_msgs::msg::PointCloud2` messages)
 * IR/Sonars (`sensor_msgs::msg::Range` messages)
 
-## Design
+### Design
 
 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.
-![HDL.png](doc/HLD.png)
+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)
 
-## Configuration
+`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)
 
-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.
 
+### Configuration
 
-## Metrics
+Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://docs.nav2.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://docs.nav2.org/tutorials/docs/using_collision_monitor.html) pages.
+
+
+### Metrics
 
 Designed to be used in wide variety of robots (incl. moving fast) and have a high level of reliability, Collision Monitor node should operate at fast rates.
 Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points).
 The table below represents the details of operating times for different behavior models and shapes:
 
-| | Stop/Slowdown model, Polygon area | Stop/Slowdown model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint |
+| | Stop/Slowdown/Limit model, Polygon area | Stop/Slowdown/Limit model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint |
 |-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------|
 | LaserScan (360 points) processing time, ms  | 4.45 | 4.45 | 4.93  | 4.86  |
 | PointCloud (24K points) processing time, ms | 4.94 | 4.06 | 20.67 | 10.87 |
@@ -65,3 +77,21 @@ The following notes could be made:
 
  * Due to sheer speed, circle shapes are preferred for the approach behavior models if you can approximately model your robot as circular.
  * More points mean lower performance. Pointclouds could be culled or filtered before the Collision Monitor to improve performance.
+
+
+## Collision Detector
+
+In some cases, the user may want to be informed about the detected obstacles without affecting the robot's velocity and instead take a different action within an external node. For example, the user may want to blink LEDs or sound an alarm when the robot is close to an obstacle. Another use case could be to detect data points in particular regions (e.g extremely close to the sensor) and warn of malfunctioning sensors. For this purpose, the Collision Detector node was introduced.
+
+It works similarly to the Collision Monitor, but does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `collision_detector_state` topic.
+
+### Features
+
+Similarly to the Collision Monitor, the Collision Detector uses polygons relative the robot's base frame origin to define "zones".
+However, unlike the Collision Monitor that uses different behavior models, the Collision Detector does not use any of them and therefore the `action_type` should always be set to `none`. If set to anything else, it will implicitly be set to `none` and yield a warning.
+
+The zones around the robot and the data sources are the same as for the Collision Monitor, with the exception of the footprint polygon, which is not supported by the Collision Detector.
+
+### Configuration
+
+Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html).

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp

@@ -19,14 +19,16 @@
 #include <vector>
 #include <string>
 
+#include "std_msgs/msg/float32.hpp"
+
 #include "nav2_collision_monitor/polygon.hpp"
 
 namespace nav2_collision_monitor
 {
 
 /**
  * @brief Circle shape implementaiton.
- * For STOP/SLOWDOWN model it represents zone around the robot
+ * For STOP/SLOWDOWN/LIMIT model it represents zone around the robot
  * while for APPROACH model it represents robot footprint.
  */
 class Circle : public Polygon
@@ -66,22 +68,54 @@ class Circle : public Polygon
    */
   int getPointsInside(const std::vector<Point> & points) const override;
 
+  /**
+   * @brief Returns true if circle radius is set.
+   * Otherwise, prints a warning and returns false.
+   */
+  bool isShapeSet() override;
+
 protected:
   /**
    * @brief Supporting routine obtaining polygon-specific ROS-parameters
-   * @param polygon_pub_topic Output name of polygon publishing topic
+   * @param polygon_sub_topic Input name of polygon subscription topic
+   * @param polygon_pub_topic Output name of polygon or radius publishing topic
    * @param footprint_topic Output name of footprint topic. For Circle returns empty string,
    * there is no footprint subscription in this class.
    * @return True if all parameters were obtained or false in failure case
    */
-  bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) override;
+  bool getParameters(
+    std::string & polygon_sub_topic,
+    std::string & polygon_pub_topic,
+    std::string & footprint_topic) override;
+
+  /**
+   * @brief Creates polygon or radius topic subscription
+   * @param polygon_sub_topic Output name of polygon or radius subscription topic.
+   * Empty, if no polygon subscription.
+   */
+  void createSubscription(std::string & polygon_sub_topic) override;
+
+  /**
+   * @brief Updates polygon from radius value
+   * @param radius New circle radius to update polygon
+   */
+  void updatePolygon(double radius);
+
+  /**
+   * @brief Dynamic circle radius callback
+   * @param msg Shared pointer to the radius value message
+   */
+  void radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg);
+
 
   // ----- Variables -----
 
   /// @brief Radius of the circle
   double radius_;
   /// @brief (radius * radius) value. Stored for optimization.
-  double radius_squared_;
+  double radius_squared_ = -1.0;
+  /// @brief Radius subscription
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr radius_sub_;
 };  // class Circle
 
 }  // namespace nav2_collision_monitor

nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp

@@ -28,14 +28,17 @@
 
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_msgs/msg/collision_detector_state.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #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"
 #include "nav2_collision_monitor/range.hpp"
+#include "nav2_collision_monitor/polygon_source.hpp"
 
 namespace nav2_collision_monitor
 {
@@ -147,6 +150,9 @@ class CollisionDetector : public nav2_util::LifecycleNode
   /// @brief collision monitor state publisher
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionDetectorState>::SharedPtr
     state_pub_;
+  /// @brief Collision points marker publisher
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_pub_;
   /// @brief timer that runs actions
   rclcpp::TimerBase::SharedPtr timer_;
 

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -18,24 +18,28 @@
 #include <string>
 #include <vector>
 #include <memory>
+#include <unordered_map>
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
 #include "nav2_util/lifecycle_node.hpp"
-#include "nav2_util/robot_utils.hpp"
+#include "nav2_msgs/msg/collision_monitor_state.hpp"
 
 #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"
 #include "nav2_collision_monitor/range.hpp"
+#include "nav2_collision_monitor/polygon_source.hpp"
 
 namespace nav2_collision_monitor
 {
@@ -107,11 +111,13 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @param cmd_vel_in_topic Output name of cmd_vel_in topic
    * @param cmd_vel_out_topic Output name of cmd_vel_out topic
    * is required.
+   * @param state_topic topic name for publishing collision monitor state
    * @return True if all parameters were obtained or false in failure case
    */
   bool getParameters(
     std::string & cmd_vel_in_topic,
-    std::string & cmd_vel_out_topic);
+    std::string & cmd_vel_out_topic,
+    std::string & state_topic);
   /**
    * @brief Supporting routine creating and configuring all polygons
    * @param base_frame_id Robot base frame ID
@@ -146,39 +152,41 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   void process(const Velocity & cmd_vel_in);
 
   /**
-   * @brief Processes the polygon of STOP and SLOWDOWN action type
+   * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type
    * @param polygon Polygon to process
-   * @param collision_points Array of 2D obstacle points
+   * @param sources_collision_points_map Map containing source name as key and
+   * array of source's 2D obstacle points as value
    * @param velocity Desired robot velocity
    * @param robot_action Output processed robot action
    * @return True if returned action is caused by current polygon, otherwise false
    */
-  bool processStopSlowdown(
+  bool processStopSlowdownLimit(
     const std::shared_ptr<Polygon> polygon,
-    const std::vector<Point> & collision_points,
+    const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
     const Velocity & velocity,
     Action & robot_action) const;
 
   /**
    * @brief Processes APPROACH action type
    * @param polygon Polygon to process
-   * @param collision_points Array of 2D obstacle points
+   * @param sources_collision_points_map Map containing source name as key and
+   * array of source's 2D obstacle points as value
    * @param velocity Desired robot velocity
    * @param robot_action Output processed robot action
    * @return True if returned action is caused by current polygon, otherwise false
    */
   bool processApproach(
     const std::shared_ptr<Polygon> polygon,
-    const std::vector<Point> & collision_points,
+    const std::unordered_map<std::string, std::vector<Point>> & sources_collision_points_map,
     const Velocity & velocity,
     Action & robot_action) const;
 
   /**
-   * @brief Prints robot action and polygon caused it (if it was)
-   * @param robot_action Robot action to print
+   * @brief Log and publish current robot action and polygon
+   * @param robot_action Robot action to notify
    * @param action_polygon Pointer to a polygon causing a selected action
    */
-  void printAction(
+  void notifyActionState(
     const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const;
 
   /**
@@ -205,6 +213,14 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   /// @brief Output cmd_vel publisher
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
 
+  /// @brief CollisionMonitor state publisher
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionMonitorState>::SharedPtr
+    state_pub_;
+
+  /// @brief Collision points marker publisher
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr
+    collision_points_marker_pub_;
+
   /// @brief Whether main routine is active
   bool process_active_;
 

nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp

@@ -20,7 +20,6 @@
 #include <string>
 
 #include "sensor_msgs/msg/point_cloud2.hpp"
-#include "nav2_util/robot_utils.hpp"
 
 #include "nav2_collision_monitor/source.hpp"
 
@@ -70,10 +69,11 @@ class PointCloud : public Source
    * @param curr_time Current node time for data interpolation
    * @param data Array where the data from source to be added.
    * Added data is transformed to base_frame_id_ coordinate system at curr_time.
+   * @return false if an invalid source should block the robot
    */
-  void getData(
+  bool getData(
     const rclcpp::Time & curr_time,
-    std::vector<Point> & data) const;
+    std::vector<Point> & data);
 
 protected:
   /**