Skip to content

Commit

Permalink
Collision monitor (ros-navigation#2982)
Browse files Browse the repository at this point in the history
* 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>
  • Loading branch information
2 people authored and Joshua Wallace committed Dec 14, 2022
1 parent b15367f commit 35df8e6
Show file tree
Hide file tree
Showing 28 changed files with 4,748 additions and 0 deletions.
109 changes: 109 additions & 0 deletions nav2_collision_monitor/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
67 changes: 67 additions & 0 deletions nav2_collision_monitor/README.md
Original file line number Diff line number Diff line change
@@ -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.
Binary file added nav2_collision_monitor/doc/HLD.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added nav2_collision_monitor/doc/polygons.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
85 changes: 85 additions & 0 deletions nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp
Original file line number Diff line number Diff line change
@@ -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_
Loading

0 comments on commit 35df8e6

Please sign in to comment.