Collision detector adaptation

nav2_collision_monitor/CMakeLists.txt

@@ -37,10 +37,12 @@ set(dependencies
   nav2_costmap_2d
 )
 
-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
@@ -50,34 +52,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}
 )
 
@@ -104,7 +131,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.
@@ -37,20 +39,20 @@ 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.
 ![HDL.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).
@@ -65,3 +67,9 @@ 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
+
+Another node exists in the nav2_collision_monitor package called the Collision Detector. This node works similarly to the collision monitor
+except that it 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. 

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

@@ -46,12 +46,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();
 
@@ -105,14 +105,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @brief Supporting routine obtaining all ROS-parameters
    * @param cmd_vel_in_topic Output name of cmd_vel_in topic
    * @param cmd_vel_out_topic Output name of cmd_vel_out topic
-   * @param frequency Frequency of the loop running process_without_vel
    * is required.
    * @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,
-    double & frequency);
+    std::string & cmd_vel_out_topic);
   /**
    * @brief Supporting routine creating and configuring all polygons
    * @param base_frame_id Robot base frame ID
@@ -126,7 +124,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,
@@ -146,11 +144,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    */
   void process(const Velocity & cmd_vel_in);
 
-  /**
-   * @brief Timer callback for actions not requiring vel
-   */
-  void process_without_vel();
-
   /**
    * @brief Processes the polygon of STOP and SLOWDOWN action type
    * @param polygon Polygon to process
@@ -179,18 +172,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode
     const Velocity & velocity,
     Action & robot_action) const;
 
-  /**
-   * @brief Processes Publish action type
-   * @param polygon Polygon to process
-   * @param collision_points Array of 2D obstacle points
-   * @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 processPublish(
-    const std::shared_ptr<Polygon> polygon,
-    const std::vector<Point> & collision_points) const;
-
   /**
    * @brief Prints robot action and polygon caused it (if it was)
    * @param robot_action Robot action to print
@@ -222,8 +203,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_in_sub_;
   /// @brief Output cmd_vel publisher
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
-  rclcpp::TimerBase::SharedPtr timer_;
-
 
   /// @brief Whether main routine is active
   bool process_active_;