nav2_collision_monitor: collision detector (#3500) (Iron backport) (#3758)

* nav2_collision_monitor: collision detector (#3500)

* fix

Author: Tony Najjar

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v14\
+              - "<< parameters.key >>-v16\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v14\
+              - "<< parameters.key >>-v16\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v14\
+            key: "<< parameters.key >>-v16\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_collision_monitor/CMakeLists.txt

@@ -39,10 +39,12 @@ set(dependencies
   nav2_msgs
 )
 
-set(executable_name collision_monitor)
-set(library_name ${executable_name}_core)
+set(monitor_executable_name collision_monitor)
+set(detector_executable_name collision_detector)
+set(monitor_library_name ${monitor_executable_name}_core)
+set(detector_library_name ${detector_executable_name}_core)
 
-add_library(${library_name} SHARED
+add_library(${monitor_library_name} SHARED
   src/collision_monitor_node.cpp
   src/polygon.cpp
   src/circle.cpp
@@ -52,34 +54,59 @@ add_library(${library_name} SHARED
   src/range.cpp
   src/kinematics.cpp
 )
+add_library(${detector_library_name} SHARED
+  src/collision_detector_node.cpp
+  src/polygon.cpp
+  src/circle.cpp
+  src/source.cpp
+  src/scan.cpp
+  src/pointcloud.cpp
+  src/range.cpp
+  src/kinematics.cpp
+)
 
-add_executable(${executable_name}
-  src/main.cpp
+add_executable(${monitor_executable_name}
+  src/collision_monitor_main.cpp
+)
+add_executable(${detector_executable_name}
+  src/collision_detector_main.cpp
 )
 
-ament_target_dependencies(${library_name}
+ament_target_dependencies(${monitor_library_name}
+  ${dependencies}
+)
+ament_target_dependencies(${detector_library_name}
   ${dependencies}
 )
 
-target_link_libraries(${executable_name}
-  ${library_name}
+target_link_libraries(${monitor_executable_name}
+  ${monitor_library_name}
+)
+target_link_libraries(${detector_executable_name}
+  ${detector_library_name}
+)
+
+ament_target_dependencies(${monitor_executable_name}
+  ${dependencies}
 )
 
-ament_target_dependencies(${executable_name}
+ament_target_dependencies(${detector_executable_name}
   ${dependencies}
 )
 
-rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor")
+rclcpp_components_register_nodes(${monitor_library_name} "nav2_collision_monitor::CollisionMonitor")
+
+rclcpp_components_register_nodes(${detector_library_name} "nav2_collision_monitor::CollisionDetector")
 
 ### Install ###
 
-install(TARGETS ${library_name}
+install(TARGETS ${monitor_library_name} ${detector_library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
-install(TARGETS ${executable_name}
+install(TARGETS ${monitor_executable_name} ${detector_executable_name}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -106,7 +133,7 @@ endif()
 ### Ament stuff ###
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name})
+ament_export_libraries(${monitor_library_name} ${detector_library_name})
 ament_export_dependencies(${dependencies})
 
 ament_package()

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,7 @@ The costmaps / trajectory planners will handle most situations, but this is to h
 
 ![polygons.png](doc/polygons.png)
 
-## Features
+### 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.
@@ -23,7 +25,6 @@ The following models of safety behaviors are employed by Collision Monitor:
 
 * **Stop model**: Define a zone and a point threshold. If more that `N` obstacle points appear inside this area, stop the robot until the obstacles will disappear.
 * **Slowdown model**: Define a zone around the robot and slow the maximum speed for a `%S` percent, if more than `N` points will appear inside the area.
-* **Limit model**: Define a zone around the robot and clamp the maximum speed below a fixed value, if more than `N` points will appear inside the area.
 * **Approach model**: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than `M` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least `M` seconds to collision. The effect here would be to keep the robot always `M` seconds from any collision.
 
 The zones around the robot can take the following shapes:
@@ -38,20 +39,19 @@ The data may be obtained from different data sources:
 * 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.
 ![HLD.png](doc/HLD.png)
-
-## Configuration
+### 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.
 
 
-## Metrics
+### 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).
@@ -66,3 +66,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/configuring-collision-detector.html).

nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp

@@ -0,0 +1,159 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+// Copyright (c) 2023 Pixel Robotics GmbH
+//
+// 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__COLLISION_DETECTOR_NODE_HPP_
+#define NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "tf2/time.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_msgs/msg/collision_detector_state.hpp"
+
+#include "nav2_collision_monitor/types.hpp"
+#include "nav2_collision_monitor/polygon.hpp"
+#include "nav2_collision_monitor/circle.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"
+
+namespace nav2_collision_monitor
+{
+
+/**
+ * @brief Collision Monitor ROS2 node
+ */
+class CollisionDetector : public nav2_util::LifecycleNode
+{
+public:
+  /**
+   * @brief Constructor for the nav2_collision_monitor::CollisionDetector
+   * @param options Additional options to control creation of the node.
+   */
+  explicit CollisionDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /**
+   * @brief Destructor for the nav2_collision_monitor::CollisionDetector
+   */
+  ~CollisionDetector();
+
+protected:
+  /**
+   * @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers,
+   * creates polygons and data sources objects
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief: Resets all subscribers/publishers, polygons/data sources arrays
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /**
+   * @brief Called in shutdown state
+   * @param state Lifecycle Node's state
+   * @return Success or Failure
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+protected:
+  /**
+   * @brief Supporting routine obtaining all ROS-parameters
+   * @return True if all parameters were obtained or false in failure case
+   */
+  bool getParameters();
+  /**
+   * @brief Supporting routine creating and configuring all polygons
+   * @param base_frame_id Robot base frame ID
+   * @param transform_tolerance Transform tolerance
+   * @return True if all polygons were configured successfully or false in failure case
+   */
+  bool configurePolygons(
+    const std::string & base_frame_id,
+    const tf2::Duration & transform_tolerance);
+  /**
+   * @brief Supporting routine creating and configuring all data sources
+   * @param base_frame_id Robot base frame ID
+   * @param odom_frame_id Odometry frame ID. Used as global frame to get
+   * source->base time interpolated transform.
+   * @param transform_tolerance Transform tolerance
+   * @param source_timeout Maximum time interval in which data is considered valid
+   * @param base_shift_correction Whether to correct source data towards to base frame movement,
+   * considering the difference between current time and latest source time
+   * @return True if all sources were configured successfully or false in failure case
+   */
+  bool configureSources(
+    const std::string & base_frame_id,
+    const std::string & odom_frame_id,
+    const tf2::Duration & transform_tolerance,
+    const rclcpp::Duration & source_timeout,
+    const bool base_shift_correction);
+
+  /**
+   * @brief Main processing routine
+   */
+  void process();
+
+  /**
+   * @brief Polygons publishing routine. Made for visualization.
+   */
+  void publishPolygons() const;
+
+  // ----- Variables -----
+
+  /// @brief TF buffer
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  /// @brief TF listener
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+  /// @brief Polygons array
+  std::vector<std::shared_ptr<Polygon>> polygons_;
+  /// @brief Data sources array
+  std::vector<std::shared_ptr<Source>> sources_;
+
+  /// @brief collision monitor state publisher
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionDetectorState>::SharedPtr
+    state_pub_;
+  /// @brief timer that runs actions
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  /// @brief main loop frequency
+  double frequency_;
+};  // class CollisionDetector
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -47,12 +47,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode
 {
 public:
   /**
-   * @brief Constructor for the nav2_collision_safery::CollisionMonitor
+   * @brief Constructor for the nav2_collision_monitor::CollisionMonitor
    * @param options Additional options to control creation of the node.
    */
   explicit CollisionMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   /**
-   * @brief Destructor for the nav2_collision_safery::CollisionMonitor
+   * @brief Destructor for the nav2_collision_monitor::CollisionMonitor
    */
   ~CollisionMonitor();
 
@@ -126,7 +126,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @brief Supporting routine creating and configuring all data sources
    * @param base_frame_id Robot base frame ID
    * @param odom_frame_id Odometry frame ID. Used as global frame to get
-   * source->base time inerpolated transform.
+   * source->base time interpolated transform.
    * @param transform_tolerance Transform tolerance
    * @param source_timeout Maximum time interval in which data is considered valid
    * @param base_shift_correction Whether to correct source data towards to base frame movement,