Collision monitor (#2982)

* Add Collision Monitor node

* Meet review items

* Fix next review items

* Code cleanup

* Support dynamic footprint. More optimizations.

* Switch to multiple footprints. Move variables.
Remove odom subscriber. Add 0-velocity optimization

* Update nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/params/collision_monitor_params.yaml

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/params/collision_monitor_params.yaml

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/params/collision_monitor_params.yaml

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Meet smaller review items

* Add fixes found during unit test development

* Fix uncrustify issues

* Add unit tests

* Fix number of polygons points

* Move tests

* Add kinematics unit test

* Minor tests fixes

* Remove commented line

* Add edge case checking testcase and references

* Update comment

* Add README.md

* Fixed table

* Minor changes in README.md

* Fix README.md for documentation pages

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_collision_monitor/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Meet review items

* Meet review items (part 2)

* Update polygons picture for README

* Change simulation_time_step to 0.1

* Fix bounding boxes to fit the demo from README.md

* Terminology fixes

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

nav2_collision_monitor/CMakeLists.txt

@@ -0,0 +1,109 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_collision_monitor)
+
+### Dependencies ###
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_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)
+
+### Header ###
+
+nav2_package()
+
+### Libraries and executables ###
+
+include_directories(
+  include
+)
+
+set(dependencies
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  nav2_util
+  nav2_costmap_2d
+)
+
+set(executable_name collision_monitor)
+set(library_name ${executable_name}_core)
+
+add_library(${library_name} SHARED
+  src/collision_monitor_node.cpp
+  src/polygon.cpp
+  src/circle.cpp
+  src/source.cpp
+  src/scan.cpp
+  src/pointcloud.cpp
+  src/kinematics.cpp
+)
+
+add_executable(${executable_name}
+  src/main.cpp
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+target_link_libraries(${executable_name}
+  ${library_name}
+)
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor")
+
+### Install ###
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
+
+### Testing ###
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
+endif()
+
+### Ament stuff ###
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+
+ament_package()

nav2_collision_monitor/README.md

@@ -0,0 +1,67 @@
+# Nav2 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.
+
+This is analogous to safety sensor and hardware features; take in laser scans from a real-time certified safety scanner, detect if there is to be an imminent collision in a configurable bounding box, and either emergency-stop the certified robot controller or slow the robot to avoid such collision.
+However, this node is done at the CPU level with any form of sensor.
+As such, this does not provide hard real-time safety certifications, but uses the same types of techniques with the same types of data for users that do not have safety-rated laser sensors, safety-rated controllers, or wish to use any type of data input (e.g. pointclouds from depth or stereo or range sensors).
+
+This is a useful and integral part of large heavy industrial robots, or robots moving with high velocities, around people or other dynamic agents (e.g. other robots) as a safety mechanism for high-response emergency stopping.
+The costmaps / trajectory planners will handle most situations, but this is to handle obstacles that virtually appear out of no where (from the robot's perspective) or approach the robot at such high speed it needs to immediately stop to prevent collision.
+
+![polygons.png](doc/polygons.png)
+
+## 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.
+A given instance of the Collision Monitor can have many zones with different models at the same time.
+When multiple zones trigger at once, the most aggressive one is used (e.g. stop > slow 50% > slow 10%).
+
+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.
+* **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:
+
+* 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.
+
+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, not implemented yet)
+
+## 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
+
+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
+
+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 |
+|-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------|
+| 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 |
+
+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.

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp

@@ -0,0 +1,85 @@
+// Copyright (c) 2022 Samsung Research 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__CIRCLE_HPP_
+#define NAV2_COLLISION_MONITOR__CIRCLE_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
+ * while for APPROACH model it represents robot footprint.
+ */
+class Circle : public Polygon
+{
+public:
+  /**
+   * @brief Circle class constructor
+   * @param node Collision Monitor node pointer
+   * @param polygon_name Name of circle
+   * @param tf_buffer Shared pointer to a TF buffer
+   * @param base_frame_id Robot base frame ID
+   * @param transform_tolerance Transform tolerance
+   */
+  Circle(
+    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 Circle class destructor
+   */
+  ~Circle();
+
+  /**
+   * @brief Gets polygon points, approximated to the circle.
+   * To be used in visualization purposes.
+   * @param poly Output polygon points (vertices)
+   */
+  void getPolygon(std::vector<Point> & poly) const override;
+
+  /**
+   * @brief Gets number of points inside circle
+   * @param points Input array of points to be checked
+   * @return Number of points inside circle. If there are no points,
+   * returns zero value.
+   */
+  int getPointsInside(const std::vector<Point> & points) const override;
+
+protected:
+  /**
+   * @brief Supporting routine obtaining polygon-specific ROS-parameters
+   * @param polygon_pub_topic Output name of polygon 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;
+
+  // ----- Variables -----
+
+  /// @brief Radius of the circle
+  double radius_;
+  /// @brief (radius * radius) value. Stored for optimization.
+  double radius_squared_;
+};  // class Circle
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__CIRCLE_HPP_