From 1654d5b852ca905e3202f0beb34bd2e1fe995d7a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 12:06:44 +0200 Subject: [PATCH 01/11] Add collision detector docu --- .../configuring-collision-detector.rst | 312 ++++++++++++++++++ 1 file changed, 312 insertions(+) create mode 100644 configuration/packages/configuring-collision-detector.rst diff --git a/configuration/packages/configuring-collision-detector.rst b/configuration/packages/configuring-collision-detector.rst new file mode 100644 index 000000000..3ed7d9930 --- /dev/null +++ b/configuration/packages/configuring-collision-detector.rst @@ -0,0 +1,312 @@ +.. _configuring_collision_monitor: + +Collision Detector +################# + +The Collision Detector is a node similar to the Collision Monitor, so it is recommended to read the :ref:`collision_monitor_tutorial` tutorial first. + +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 within the robot's footprint or 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. + +See the package's ``README`` for more information. + +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. + +Parameters +********** + +:base_frame_id: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "base_footprint" + ============== ============================= + + Description: + Robot base frame. + +:odom_frame_id: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "odom" + ============== ============================= + + Description: + Which frame to use for odometry. + +:transform_tolerance: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. + +:source_timeout: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 2.0 + ============== ============================= + + Description: + Maximum time interval in which source data is considered as valid. + +:base_shift_correction: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool True + ============== ============================= + + Description: + Whether to correct source data towards to base frame movement, considering the difference between current time and latest source time. If enabled, produces more accurate sources positioning in the robot base frame, at the cost of slower performance. This will cause average delays for ``~1/(2*odom_rate)`` per each ``cmd_vel`` calculation cycle. However, disabling this option for better performance is not recommended for the fast moving robots, where during the typical rate of data sources, robot could move unacceptably far. Thus reasonable odometry rates are recommended (~100 hz). + +:polygons: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector N/A + ============== ============================= + + Description: + List of zones (stop/slowdown/limit bounding boxes, footprint, approach circle, etc...). Causes an error, if not specialized. + + +:observation_sources: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector N/A + ============== ============================= + + Description: + List of data sources (laser scanners, pointclouds, etc...). Causes an error, if not specialized. + +Polygons parameters +=================== + +```` is the corresponding polygon name ID selected for this type. + +:````.type: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description: + Type of polygon shape. Available values are ``polygon``, ``circle``. Causes an error, if not specialized. + +:````.points: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector N/A + ============== ============================= + + Description: + Polygon vertexes, listed in ``{p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, ...}`` format (e.g. ``{0.5, 0.25, 0.5, -0.25, 0.0, -0.25, 0.0, 0.25}`` for the square in the front). Used for ``polygon`` type. Minimum 3 points for a triangle polygon. If not specified, the collision monitor will use dynamic polygon subscription to ``polygon_sub_topic`` for points in the ``stop``/``slowdown``/``limit`` action types, or footprint subscriber to ``footprint_topic`` for ``approach`` action type. + +:````.polygon_sub_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description: + Topic to listen the polygon points from. Applicable only for ``polygon`` type and ``stop``/``slowdown``/``limit`` action types. Causes an error, if not specified **and** points are also not specified. If both ``points`` and ``polygon_sub_topic`` are specified, the static ``points`` takes priority. + +:````.radius: + + ============== ============================= + Type Default + -------------- ----------------------------- + double N/A + ============== ============================= + + Description: + Circle radius. Used for ``circle`` type. Causes an error, if not specialized. + +:````.action_type: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description: + Only ``none`` action type is supported (more options available for collision monitor) + +:````.min_points: + + ============== ============================= + Type Default + -------------- ----------------------------- + int 4 + ============== ============================= + + Description: + Minimum number of data readings within a zone to trigger the action. Former ``max_points`` parameter for Humble, that meant the maximum number of data readings within a zone to not trigger the action). ``min_points`` is equal to ``max_points + 1`` value. + +:````.time_before_collision: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 2.0 + ============== ============================= + + Description: + Time before collision in seconds. Maximum simulation time used in collision prediction. Higher values mean lower performance. Applicable for ``approach`` action type. + +:````.simulation_time_step: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description: + Time iteration step for robot movement simulation during collision prediction. Higher values mean lower prediction accuracy but better performance. Applicable for ``approach`` action type. + +:````.visualize: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool False + ============== ============================= + + Description: + Whether to publish the polygon in a separate topic. + +:````.polygon_pub_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string + ============== ============================= + + Description: + Topic name to publish a polygon to. Used only if ``visualize`` is true. + + + +Observation sources parameters +============================== + +```` is the corresponding data source name ID selected for this type. + +:````.type: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "scan" + ============== ============================= + + Description: + Type of polygon shape. Could be ``scan``, ``pointcloud`` or ``range``. + +:````.topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "scan" + ============== ============================= + + Description: + Topic to listen the source data from. + +:````.min_height: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.05 + ============== ============================= + + Description: + Minimum height the PointCloud projection to 2D space started from. Applicable for ``pointcloud`` type. + +:````.max_height: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.5 + ============== ============================= + + Description: + Maximum height the PointCloud projection to 2D space ended with. Applicable for ``pointcloud`` type. + +:````.obstacles_angle: + + ============== ============================= + Type Default + -------------- ----------------------------- + double PI / 180 (1 degree) + ============== ============================= + + Description: + Angle increment (in radians) between nearby obstacle points at the range arc. Two outermost points from the field of view are not taken into account (they will always exist regardless of this value). Applicable for ``range`` type. + + +Example +******* + +Here is an example of configuration YAML for the Collision Detector. + +.. code-block:: yaml + + collision_detector: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + transform_tolerance: 0.5 + source_timeout: 5.0 + base_shift_correction: True + polygons: ["PolygonFront"] + PolygonFront: + type: "polygon" + points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + action_type: "none" + min_points: 4 + visualize: True + polygon_pub_topic: "polygon_front" + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + pointcloud: + type: "pointcloud" + topic: "/intel_realsense_r200_depth/points" + min_height: 0.1 + max_height: 0.5 + From ce634f84baebff044c420a6a5023ae1c9bdfa4c2 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 12:42:39 +0200 Subject: [PATCH 02/11] fixes --- .../configuring-collision-detector.rst | 43 +++++++------------ 1 file changed, 16 insertions(+), 27 deletions(-) diff --git a/configuration/packages/configuring-collision-detector.rst b/configuration/packages/configuring-collision-detector.rst index 3ed7d9930..3544656ce 100644 --- a/configuration/packages/configuring-collision-detector.rst +++ b/configuration/packages/configuring-collision-detector.rst @@ -1,4 +1,4 @@ -.. _configuring_collision_monitor: +.. _configuring_collision_detector: Collision Detector ################# @@ -6,7 +6,7 @@ Collision Detector The Collision Detector is a node similar to the Collision Monitor, so it is recommended to read the :ref:`collision_monitor_tutorial` tutorial first. 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 within the robot's footprint or extremely close to the sensor) and warn of malfunctioning sensors. For this purpose, the Collision Detector node was introduced. +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. See the package's ``README`` for more information. @@ -22,6 +22,17 @@ The zones around the robot and the data sources are the same as for the Collisio Parameters ********** +:frequency: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 10.0 + ============== ============================= + + Description: + Frequency of the main loop that checks for detections. + :base_frame_id: ============== ============================= @@ -86,7 +97,7 @@ Parameters ============== ============================= Description: - List of zones (stop/slowdown/limit bounding boxes, footprint, approach circle, etc...). Causes an error, if not specialized. + List of zones to check for data points. Causes an error, if not specialized. :observation_sources: @@ -125,7 +136,7 @@ Polygons parameters ============== ============================= Description: - Polygon vertexes, listed in ``{p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, ...}`` format (e.g. ``{0.5, 0.25, 0.5, -0.25, 0.0, -0.25, 0.0, 0.25}`` for the square in the front). Used for ``polygon`` type. Minimum 3 points for a triangle polygon. If not specified, the collision monitor will use dynamic polygon subscription to ``polygon_sub_topic`` for points in the ``stop``/``slowdown``/``limit`` action types, or footprint subscriber to ``footprint_topic`` for ``approach`` action type. + Polygon vertexes, listed in ``{p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, ...}`` format (e.g. ``{0.5, 0.25, 0.5, -0.25, 0.0, -0.25, 0.0, 0.25}`` for the square in the front). Used for ``polygon`` type. Minimum 3 points for a triangle polygon. If not specified, the collision detector will use dynamic polygon subscription to ``polygon_sub_topic`` :````.polygon_sub_topic: @@ -136,7 +147,7 @@ Polygons parameters ============== ============================= Description: - Topic to listen the polygon points from. Applicable only for ``polygon`` type and ``stop``/``slowdown``/``limit`` action types. Causes an error, if not specified **and** points are also not specified. If both ``points`` and ``polygon_sub_topic`` are specified, the static ``points`` takes priority. + Topic to listen the polygon points from. Causes an error, if not specified **and** points are also not specified. If both ``points`` and ``polygon_sub_topic`` are specified, the static ``points`` takes priority. :````.radius: @@ -171,28 +182,6 @@ Polygons parameters Description: Minimum number of data readings within a zone to trigger the action. Former ``max_points`` parameter for Humble, that meant the maximum number of data readings within a zone to not trigger the action). ``min_points`` is equal to ``max_points + 1`` value. -:````.time_before_collision: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 2.0 - ============== ============================= - - Description: - Time before collision in seconds. Maximum simulation time used in collision prediction. Higher values mean lower performance. Applicable for ``approach`` action type. - -:````.simulation_time_step: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.1 - ============== ============================= - - Description: - Time iteration step for robot movement simulation during collision prediction. Higher values mean lower prediction accuracy but better performance. Applicable for ``approach`` action type. - :````.visualize: ============== ============================= From 40f9ed3b8ec8adfe89a2ca27a7187911a52c8542 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 12:50:51 +0200 Subject: [PATCH 03/11] fix --- configuration/index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/configuration/index.rst b/configuration/index.rst index 325970ace..2924b4603 100644 --- a/configuration/index.rst +++ b/configuration/index.rst @@ -31,4 +31,5 @@ the best navigation performance. packages/configuring-constrained-smoother.rst packages/configuring-velocity-smoother.rst packages/configuring-collision-monitor.rst + packages/configuring-collision-detector.rst packages/configuring-waypoint-follower.rst From 24c7f578be6cab0fcdc52ccaae699c634d9887df Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 12:56:02 +0200 Subject: [PATCH 04/11] fix --- configuration/packages/configuring-collision-detector.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configuration/packages/configuring-collision-detector.rst b/configuration/packages/configuring-collision-detector.rst index 3544656ce..e6b9ed82e 100644 --- a/configuration/packages/configuring-collision-detector.rst +++ b/configuration/packages/configuring-collision-detector.rst @@ -1,7 +1,7 @@ .. _configuring_collision_detector: Collision Detector -################# +################## The Collision Detector is a node similar to the Collision Monitor, so it is recommended to read the :ref:`collision_monitor_tutorial` tutorial first. From c73e6ef49f97cc8f725a11d451babb9dd8433ce6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 18:14:09 +0200 Subject: [PATCH 05/11] fixes --- .../configuring-collision-detector-node.rst} | 2 +- .../configuring-collision-monitor-node.rst | 455 +++++++++++++++++ .../configuring-collision-monitor.rst | 458 +----------------- 3 files changed, 468 insertions(+), 447 deletions(-) rename configuration/packages/{configuring-collision-detector.rst => collision_monitor/configuring-collision-detector-node.rst} (99%) create mode 100644 configuration/packages/collision_monitor/configuring-collision-monitor-node.rst diff --git a/configuration/packages/configuring-collision-detector.rst b/configuration/packages/collision_monitor/configuring-collision-detector-node.rst similarity index 99% rename from configuration/packages/configuring-collision-detector.rst rename to configuration/packages/collision_monitor/configuring-collision-detector-node.rst index e6b9ed82e..d017115c6 100644 --- a/configuration/packages/configuring-collision-detector.rst +++ b/configuration/packages/collision_monitor/configuring-collision-detector-node.rst @@ -5,7 +5,7 @@ Collision Detector The Collision Detector is a node similar to the Collision Monitor, so it is recommended to read the :ref:`collision_monitor_tutorial` tutorial first. -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.\ +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. diff --git a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst new file mode 100644 index 000000000..4a8f66d9f --- /dev/null +++ b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst @@ -0,0 +1,455 @@ +.. _configuring_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. + +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. + +See the package's ``README`` for more complete information. + +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 ``min_points`` or more 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 ``slowdown_ratio``, if ``min_points`` or more points will appear inside the area. +- **Limit model**: Define a zone around the robot and restricts the maximum linear and angular velocities to ``linear_limit`` and ``angular_limit`` values accordingly, if ``min_points`` or more 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 ``time_before_collision`` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least ``time_before_collision`` seconds to collision. The effect here would be to keep the robot always ``time_before_collision`` seconds from any collision. + +The zones around the robot can take the following shapes: + +- 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. + +All shapes (``Polygon`` and ``Circle``) are derived from base ``Polygon`` class, so without loss of generality they would be called 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. + +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) + +Parameters +********** + +:base_frame_id: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "base_footprint" + ============== ============================= + + Description: + Robot base frame. + +:odom_frame_id: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "odom" + ============== ============================= + + Description: + Which frame to use for odometry. + +:cmd_vel_in_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "cmd_vel_raw" + ============== ============================= + + Description: + Input ``cmd_vel`` topic with desired robot velocity. + +:cmd_vel_out_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "cmd_vel" + ============== ============================= + + Description: + Output ``cmd_vel`` topic with output produced by Collision Monitor velocities. + +:state_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "" + ============== ============================= + + Description: + Output the currently activated polygon action type and name. Optional parameter. No publisher will be created if it is unspecified. + +:transform_tolerance: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description + Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. + +:source_timeout: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 2.0 + ============== ============================= + + Description: + Maximum time interval in which source data is considered as valid. + +:base_shift_correction: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool True + ============== ============================= + + Description: + Whether to correct source data towards to base frame movement, considering the difference between current time and latest source time. If enabled, produces more accurate sources positioning in the robot base frame, at the cost of slower performance. This will cause average delays for ``~1/(2*odom_rate)`` per each ``cmd_vel`` calculation cycle. However, disabling this option for better performance is not recommended for the fast moving robots, where during the typical rate of data sources, robot could move unacceptably far. Thus reasonable odometry rates are recommended (~100 hz). + +:stop_pub_timeout: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 1.0 + ============== ============================= + + Description: + Timeout, after which zero-velocity ceases to be published. It could be used for other overrode systems outside Nav2 are trying to bring the robot out of a state close to a collision, or to allow a standing robot to go into sleep mode. + +:polygons: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector N/A + ============== ============================= + + Description: + List of zones (stop/slowdown/limit bounding boxes, footprint, approach circle, etc...). Causes an error, if not specialized. + + +:observation_sources: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector N/A + ============== ============================= + + Description: + List of data sources (laser scanners, pointclouds, etc...). Causes an error, if not specialized. + +Polygons parameters +=================== + +```` is the corresponding polygon name ID selected for this type. + +:````.type: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description: + Type of polygon shape. Available values are ``polygon``, ``circle``. Causes an error, if not specialized. + +:````.points: + + ============== ============================= + Type Default + -------------- ----------------------------- + vector N/A + ============== ============================= + + Description: + Polygon vertexes, listed in ``{p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, ...}`` format (e.g. ``{0.5, 0.25, 0.5, -0.25, 0.0, -0.25, 0.0, 0.25}`` for the square in the front). Used for ``polygon`` type. Minimum 3 points for a triangle polygon. If not specified, the collision monitor will use dynamic polygon subscription to ``polygon_sub_topic`` for points in the ``stop``/``slowdown``/``limit`` action types, or footprint subscriber to ``footprint_topic`` for ``approach`` action type. + +:````.polygon_sub_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description: + Topic to listen the polygon points from. Applicable only for ``polygon`` type and ``stop``/``slowdown``/``limit`` action types. Causes an error, if not specified **and** points are also not specified. If both ``points`` and ``polygon_sub_topic`` are specified, the static ``points`` takes priority. + +:````.footprint_topic: + + ============== =================================== + Type Default + -------------- ----------------------------------- + string "local_costmap/published_footprint" + ============== =================================== + + Description: + Topic to listen the robot footprint from. Applicable only for ``polygon`` type and ``approach`` action type. If both ``points`` and ``footprint_topic`` are specified, the static ``points`` takes priority. + +:````.radius: + + ============== ============================= + Type Default + -------------- ----------------------------- + double N/A + ============== ============================= + + Description: + Circle radius. Used for ``circle`` type. Causes an error, if not specialized. + +:````.action_type: + + ============== ============================= + Type Default + -------------- ----------------------------- + string N/A + ============== ============================= + + Description: + Zone behavior model. Available values are ``stop``, ``slowdown``, ``limit``, ``approach``. Causes an error, if not specialized. + +:````.min_points: + + ============== ============================= + Type Default + -------------- ----------------------------- + int 4 + ============== ============================= + + Description: + Minimum number of data readings within a zone to trigger the action. Former ``max_points`` parameter for Humble, that meant the maximum number of data readings within a zone to not trigger the action). ``min_points`` is equal to ``max_points + 1`` value. + +:````.slowdown_ratio: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.5 + ============== ============================= + + Description: + Robot slowdown (share of its actual speed). Applicable for ``slowdown`` action type. + +:````.linear_limit: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.5 + ============== ============================= + + Description: + Robot linear speed limit. Applicable for ``limit`` action type. + +:````.angular_limit: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.5 + ============== ============================= + + Description: + Robot angular speed limit. Applicable for ``limit`` action type. + +:````.time_before_collision: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 2.0 + ============== ============================= + + Description: + Time before collision in seconds. Maximum simulation time used in collision prediction. Higher values mean lower performance. Applicable for ``approach`` action type. + +:````.simulation_time_step: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.1 + ============== ============================= + + Description: + Time iteration step for robot movement simulation during collision prediction. Higher values mean lower prediction accuracy but better performance. Applicable for ``approach`` action type. + +:````.visualize: + + ============== ============================= + Type Default + -------------- ----------------------------- + bool False + ============== ============================= + + Description: + Whether to publish the polygon in a separate topic. + +:````.polygon_pub_topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string + ============== ============================= + + Description: + Topic name to publish a polygon to. Used only if ``visualize`` is true. + + + +Observation sources parameters +============================== + +```` is the corresponding data source name ID selected for this type. + +:````.type: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "scan" + ============== ============================= + + Description: + Type of polygon shape. Could be ``scan``, ``pointcloud`` or ``range``. + +:````.topic: + + ============== ============================= + Type Default + -------------- ----------------------------- + string "scan" + ============== ============================= + + Description: + Topic to listen the source data from. + +:````.min_height: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.05 + ============== ============================= + + Description: + Minimum height the PointCloud projection to 2D space started from. Applicable for ``pointcloud`` type. + +:````.max_height: + + ============== ============================= + Type Default + -------------- ----------------------------- + double 0.5 + ============== ============================= + + Description: + Maximum height the PointCloud projection to 2D space ended with. Applicable for ``pointcloud`` type. + +:````.obstacles_angle: + + ============== ============================= + Type Default + -------------- ----------------------------- + double PI / 180 (1 degree) + ============== ============================= + + Description: + Angle increment (in radians) between nearby obstacle points at the range arc. Two outermost points from the field of view are not taken into account (they will always exist regardless of this value). Applicable for ``range`` type. + + +Example +******* + +Here is an example of configuration YAML for the Collision Monitor. +For more information how to bring-up your own Collision Monitor node, please refer to the :ref:`collision_monitor_tutorial` tutorial. + +.. code-block:: yaml + + collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_raw" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.5 + source_timeout: 5.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"] + PolygonStop: + type: "circle" + radius: 0.3 + action_type: "stop" + min_points: 4 # max_points: 3 for Humble + visualize: True + polygon_pub_topic: "polygon_stop" + PolygonSlow: + type: "polygon" + points: [1.0, 1.0, 1.0, -1.0, -0.5, -1.0, -0.5, 1.0] + action_type: "slowdown" + min_points: 4 # max_points: 3 for Humble + slowdown_ratio: 0.3 + visualize: True + polygon_pub_topic: "polygon_slowdown" + PolygonLimit: + type: "polygon" + points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] + action_type: "limit" + min_points: 4 # max_points: 3 for Humble + linear_limit: 0.4 + angular_limit: 0.5 + visualize: True + polygon_pub_topic: "polygon_limit" + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.02 + min_points: 6 # max_points: 5 for Humble + visualize: False + observation_sources: ["scan", "pointcloud"] + scan: + type: "scan" + topic: "/scan" + pointcloud: + type: "pointcloud" + topic: "/intel_realsense_r200_depth/points" + min_height: 0.1 + max_height: 0.5 diff --git a/configuration/packages/configuring-collision-monitor.rst b/configuration/packages/configuring-collision-monitor.rst index 4a8f66d9f..b46d7f52c 100644 --- a/configuration/packages/configuring-collision-monitor.rst +++ b/configuration/packages/configuring-collision-monitor.rst @@ -1,455 +1,21 @@ .. _configuring_collision_monitor: -Collision Monitor -################# +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. +Source code and ``README`` with design, explanations, and metrics can be found on Github_. -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). +.. _Github: https://github.com/ros-planning/navigation2/tree/main/nav2_collision_monitor -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. +The ``nav2_collision_monitor`` package contains nodes providing an additional level of robot safety. See the documentation for the individual nodes for more information. -See the package's ``README`` for more complete information. +Provided Nodes +**************** + The nodes listed below are inside the ``nav2_collision_monitor`` package. See the pages for individual configuration information. -Features -******** +.. toctree:: + :maxdepth: 1 -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%). + collision_monitor/configuring-collision-monitor-node.rst + collision_monitor/configuring-collision-detector-node.rst -The following models of safety behaviors are employed by Collision Monitor: - -- **Stop model**: Define a zone and a point threshold. If ``min_points`` or more 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 ``slowdown_ratio``, if ``min_points`` or more points will appear inside the area. -- **Limit model**: Define a zone around the robot and restricts the maximum linear and angular velocities to ``linear_limit`` and ``angular_limit`` values accordingly, if ``min_points`` or more 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 ``time_before_collision`` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least ``time_before_collision`` seconds to collision. The effect here would be to keep the robot always ``time_before_collision`` seconds from any collision. - -The zones around the robot can take the following shapes: - -- 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. - -All shapes (``Polygon`` and ``Circle``) are derived from base ``Polygon`` class, so without loss of generality they would be called 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. - -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) - -Parameters -********** - -:base_frame_id: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "base_footprint" - ============== ============================= - - Description: - Robot base frame. - -:odom_frame_id: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "odom" - ============== ============================= - - Description: - Which frame to use for odometry. - -:cmd_vel_in_topic: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "cmd_vel_raw" - ============== ============================= - - Description: - Input ``cmd_vel`` topic with desired robot velocity. - -:cmd_vel_out_topic: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "cmd_vel" - ============== ============================= - - Description: - Output ``cmd_vel`` topic with output produced by Collision Monitor velocities. - -:state_topic: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "" - ============== ============================= - - Description: - Output the currently activated polygon action type and name. Optional parameter. No publisher will be created if it is unspecified. - -:transform_tolerance: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.1 - ============== ============================= - - Description - Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. - -:source_timeout: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 2.0 - ============== ============================= - - Description: - Maximum time interval in which source data is considered as valid. - -:base_shift_correction: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool True - ============== ============================= - - Description: - Whether to correct source data towards to base frame movement, considering the difference between current time and latest source time. If enabled, produces more accurate sources positioning in the robot base frame, at the cost of slower performance. This will cause average delays for ``~1/(2*odom_rate)`` per each ``cmd_vel`` calculation cycle. However, disabling this option for better performance is not recommended for the fast moving robots, where during the typical rate of data sources, robot could move unacceptably far. Thus reasonable odometry rates are recommended (~100 hz). - -:stop_pub_timeout: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 1.0 - ============== ============================= - - Description: - Timeout, after which zero-velocity ceases to be published. It could be used for other overrode systems outside Nav2 are trying to bring the robot out of a state close to a collision, or to allow a standing robot to go into sleep mode. - -:polygons: - - ============== ============================= - Type Default - -------------- ----------------------------- - vector N/A - ============== ============================= - - Description: - List of zones (stop/slowdown/limit bounding boxes, footprint, approach circle, etc...). Causes an error, if not specialized. - - -:observation_sources: - - ============== ============================= - Type Default - -------------- ----------------------------- - vector N/A - ============== ============================= - - Description: - List of data sources (laser scanners, pointclouds, etc...). Causes an error, if not specialized. - -Polygons parameters -=================== - -```` is the corresponding polygon name ID selected for this type. - -:````.type: - - ============== ============================= - Type Default - -------------- ----------------------------- - string N/A - ============== ============================= - - Description: - Type of polygon shape. Available values are ``polygon``, ``circle``. Causes an error, if not specialized. - -:````.points: - - ============== ============================= - Type Default - -------------- ----------------------------- - vector N/A - ============== ============================= - - Description: - Polygon vertexes, listed in ``{p1.x, p1.y, p2.x, p2.y, p3.x, p3.y, ...}`` format (e.g. ``{0.5, 0.25, 0.5, -0.25, 0.0, -0.25, 0.0, 0.25}`` for the square in the front). Used for ``polygon`` type. Minimum 3 points for a triangle polygon. If not specified, the collision monitor will use dynamic polygon subscription to ``polygon_sub_topic`` for points in the ``stop``/``slowdown``/``limit`` action types, or footprint subscriber to ``footprint_topic`` for ``approach`` action type. - -:````.polygon_sub_topic: - - ============== ============================= - Type Default - -------------- ----------------------------- - string N/A - ============== ============================= - - Description: - Topic to listen the polygon points from. Applicable only for ``polygon`` type and ``stop``/``slowdown``/``limit`` action types. Causes an error, if not specified **and** points are also not specified. If both ``points`` and ``polygon_sub_topic`` are specified, the static ``points`` takes priority. - -:````.footprint_topic: - - ============== =================================== - Type Default - -------------- ----------------------------------- - string "local_costmap/published_footprint" - ============== =================================== - - Description: - Topic to listen the robot footprint from. Applicable only for ``polygon`` type and ``approach`` action type. If both ``points`` and ``footprint_topic`` are specified, the static ``points`` takes priority. - -:````.radius: - - ============== ============================= - Type Default - -------------- ----------------------------- - double N/A - ============== ============================= - - Description: - Circle radius. Used for ``circle`` type. Causes an error, if not specialized. - -:````.action_type: - - ============== ============================= - Type Default - -------------- ----------------------------- - string N/A - ============== ============================= - - Description: - Zone behavior model. Available values are ``stop``, ``slowdown``, ``limit``, ``approach``. Causes an error, if not specialized. - -:````.min_points: - - ============== ============================= - Type Default - -------------- ----------------------------- - int 4 - ============== ============================= - - Description: - Minimum number of data readings within a zone to trigger the action. Former ``max_points`` parameter for Humble, that meant the maximum number of data readings within a zone to not trigger the action). ``min_points`` is equal to ``max_points + 1`` value. - -:````.slowdown_ratio: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.5 - ============== ============================= - - Description: - Robot slowdown (share of its actual speed). Applicable for ``slowdown`` action type. - -:````.linear_limit: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.5 - ============== ============================= - - Description: - Robot linear speed limit. Applicable for ``limit`` action type. - -:````.angular_limit: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.5 - ============== ============================= - - Description: - Robot angular speed limit. Applicable for ``limit`` action type. - -:````.time_before_collision: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 2.0 - ============== ============================= - - Description: - Time before collision in seconds. Maximum simulation time used in collision prediction. Higher values mean lower performance. Applicable for ``approach`` action type. - -:````.simulation_time_step: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.1 - ============== ============================= - - Description: - Time iteration step for robot movement simulation during collision prediction. Higher values mean lower prediction accuracy but better performance. Applicable for ``approach`` action type. - -:````.visualize: - - ============== ============================= - Type Default - -------------- ----------------------------- - bool False - ============== ============================= - - Description: - Whether to publish the polygon in a separate topic. - -:````.polygon_pub_topic: - - ============== ============================= - Type Default - -------------- ----------------------------- - string - ============== ============================= - - Description: - Topic name to publish a polygon to. Used only if ``visualize`` is true. - - - -Observation sources parameters -============================== - -```` is the corresponding data source name ID selected for this type. - -:````.type: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "scan" - ============== ============================= - - Description: - Type of polygon shape. Could be ``scan``, ``pointcloud`` or ``range``. - -:````.topic: - - ============== ============================= - Type Default - -------------- ----------------------------- - string "scan" - ============== ============================= - - Description: - Topic to listen the source data from. - -:````.min_height: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.05 - ============== ============================= - - Description: - Minimum height the PointCloud projection to 2D space started from. Applicable for ``pointcloud`` type. - -:````.max_height: - - ============== ============================= - Type Default - -------------- ----------------------------- - double 0.5 - ============== ============================= - - Description: - Maximum height the PointCloud projection to 2D space ended with. Applicable for ``pointcloud`` type. - -:````.obstacles_angle: - - ============== ============================= - Type Default - -------------- ----------------------------- - double PI / 180 (1 degree) - ============== ============================= - - Description: - Angle increment (in radians) between nearby obstacle points at the range arc. Two outermost points from the field of view are not taken into account (they will always exist regardless of this value). Applicable for ``range`` type. - - -Example -******* - -Here is an example of configuration YAML for the Collision Monitor. -For more information how to bring-up your own Collision Monitor node, please refer to the :ref:`collision_monitor_tutorial` tutorial. - -.. code-block:: yaml - - collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_raw" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.5 - source_timeout: 5.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"] - PolygonStop: - type: "circle" - radius: 0.3 - action_type: "stop" - min_points: 4 # max_points: 3 for Humble - visualize: True - polygon_pub_topic: "polygon_stop" - PolygonSlow: - type: "polygon" - points: [1.0, 1.0, 1.0, -1.0, -0.5, -1.0, -0.5, 1.0] - action_type: "slowdown" - min_points: 4 # max_points: 3 for Humble - slowdown_ratio: 0.3 - visualize: True - polygon_pub_topic: "polygon_slowdown" - PolygonLimit: - type: "polygon" - points: [0.5, 0.5, 0.5, -0.5, -0.5, -0.5, -0.5, 0.5] - action_type: "limit" - min_points: 4 # max_points: 3 for Humble - linear_limit: 0.4 - angular_limit: 0.5 - visualize: True - polygon_pub_topic: "polygon_limit" - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.02 - min_points: 6 # max_points: 5 for Humble - visualize: False - observation_sources: ["scan", "pointcloud"] - scan: - type: "scan" - topic: "/scan" - pointcloud: - type: "pointcloud" - topic: "/intel_realsense_r200_depth/points" - min_height: 0.1 - max_height: 0.5 From 8110ecbfd2e68cb2e18cab4b0e9e6b493e11fa78 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 19:40:59 +0200 Subject: [PATCH 06/11] fix references --- .../collision_monitor/configuring-collision-detector-node.rst | 2 +- .../collision_monitor/configuring-collision-monitor-node.rst | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/configuration/packages/collision_monitor/configuring-collision-detector-node.rst b/configuration/packages/collision_monitor/configuring-collision-detector-node.rst index d017115c6..067776f43 100644 --- a/configuration/packages/collision_monitor/configuring-collision-detector-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-detector-node.rst @@ -1,4 +1,4 @@ -.. _configuring_collision_detector: +.. _configuring_collision_detector_node: Collision Detector ################## diff --git a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst index 4a8f66d9f..cba2cf79f 100644 --- a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst @@ -1,4 +1,4 @@ -.. _configuring_collision_monitor: +.. _configuring_collision_monitor_node: Collision Monitor ################# From bd34e9d16b22eab8366e70410410abd4763935c3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 10 Aug 2023 15:43:31 +0200 Subject: [PATCH 07/11] fix --- .../configuring-collision-monitor-node.rst | 3 +-- configuration/packages/configuring-collision-monitor.rst | 6 ++++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst index cba2cf79f..434b3e4be 100644 --- a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst @@ -13,7 +13,7 @@ As such, this does not provide hard real-time safety certifications, but uses th 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. -See the package's ``README`` for more complete information. +See the package's ``README`` for more complete information. For more information how to bring-up your own Collision Monitor node, please refer to the :ref:`collision_monitor_tutorial` tutorial. Features ******** @@ -396,7 +396,6 @@ Example ******* Here is an example of configuration YAML for the Collision Monitor. -For more information how to bring-up your own Collision Monitor node, please refer to the :ref:`collision_monitor_tutorial` tutorial. .. code-block:: yaml diff --git a/configuration/packages/configuring-collision-monitor.rst b/configuration/packages/configuring-collision-monitor.rst index b46d7f52c..5ff8d9b13 100644 --- a/configuration/packages/configuring-collision-monitor.rst +++ b/configuration/packages/configuring-collision-monitor.rst @@ -7,11 +7,13 @@ Source code and ``README`` with design, explanations, and metrics can be found o .. _Github: https://github.com/ros-planning/navigation2/tree/main/nav2_collision_monitor -The ``nav2_collision_monitor`` package contains nodes providing an additional level of robot safety. See the documentation for the individual nodes for more information. +The ``nav2_collision_monitor`` package contains nodes providing an additional level of robot safety, namely the Collision Detector and the Collision Monitor. +The Collision Detector is responsible for detecting obstacles in a certain static region or in the robot's path and reporting them/ +The Collision Monitor also detects obstacles and additionally limits the robot's velocity to avoid collisions with these obstacles. Provided Nodes **************** - The nodes listed below are inside the ``nav2_collision_monitor`` package. See the pages for individual configuration information. +The nodes listed below are inside the ``nav2_collision_monitor`` package. See the pages for individual configuration information. .. toctree:: :maxdepth: 1 From bcd0d2a28912e56cd77c935f80fd9eb40a5db9b9 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 11 Aug 2023 11:22:41 +0200 Subject: [PATCH 08/11] fixes --- configuration/packages/configuring-collision-monitor.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/configuration/packages/configuring-collision-monitor.rst b/configuration/packages/configuring-collision-monitor.rst index 5ff8d9b13..80973fe0e 100644 --- a/configuration/packages/configuring-collision-monitor.rst +++ b/configuration/packages/configuring-collision-monitor.rst @@ -1,6 +1,6 @@ .. _configuring_collision_monitor: -Nav2 collision monitor +Collision monitor ###################### Source code and ``README`` with design, explanations, and metrics can be found on Github_. @@ -8,7 +8,7 @@ Source code and ``README`` with design, explanations, and metrics can be found o .. _Github: https://github.com/ros-planning/navigation2/tree/main/nav2_collision_monitor The ``nav2_collision_monitor`` package contains nodes providing an additional level of robot safety, namely the Collision Detector and the Collision Monitor. -The Collision Detector is responsible for detecting obstacles in a certain static region or in the robot's path and reporting them/ +The Collision Detector is responsible for detecting obstacles in a certain static region or in the robot's path and reporting them. The Collision Monitor also detects obstacles and additionally limits the robot's velocity to avoid collisions with these obstacles. Provided Nodes From 0aff0d10a109cbf206f588c672092f1d2aaff770 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 16 Aug 2023 10:29:05 +0200 Subject: [PATCH 09/11] fixes --- .../configuring-collision-detector-node.rst | 8 ++++---- .../configuring-collision-monitor-node.rst | 4 ++-- .../packages/configuring-collision-monitor.rst | 10 +++++----- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/configuration/packages/collision_monitor/configuring-collision-detector-node.rst b/configuration/packages/collision_monitor/configuring-collision-detector-node.rst index 067776f43..40aa3372f 100644 --- a/configuration/packages/collision_monitor/configuring-collision-detector-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-detector-node.rst @@ -1,7 +1,7 @@ .. _configuring_collision_detector_node: -Collision Detector -################## +Collision Detector Node +####################### The Collision Detector is a node similar to the Collision Monitor, so it is recommended to read the :ref:`collision_monitor_tutorial` tutorial first. @@ -14,8 +14,8 @@ See the package's ``README`` for more information. 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. +Similarly to the Collision Monitor, the Collision Detector uses robot's relative polygons 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 throw an error 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. diff --git a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst index 434b3e4be..a9b325a18 100644 --- a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst @@ -1,7 +1,7 @@ .. _configuring_collision_monitor_node: -Collision Monitor -################# +Collision Monitor Node +###################### 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. diff --git a/configuration/packages/configuring-collision-monitor.rst b/configuration/packages/configuring-collision-monitor.rst index 80973fe0e..3caed6033 100644 --- a/configuration/packages/configuring-collision-monitor.rst +++ b/configuration/packages/configuring-collision-monitor.rst @@ -1,15 +1,15 @@ .. _configuring_collision_monitor: -Collision monitor -###################### +Collision Monitor +################# Source code and ``README`` with design, explanations, and metrics can be found on Github_. .. _Github: https://github.com/ros-planning/navigation2/tree/main/nav2_collision_monitor -The ``nav2_collision_monitor`` package contains nodes providing an additional level of robot safety, namely the Collision Detector and the Collision Monitor. -The Collision Detector is responsible for detecting obstacles in a certain static region or in the robot's path and reporting them. -The Collision Monitor also detects obstacles and additionally limits the robot's velocity to avoid collisions with these obstacles. +The ``nav2_collision_monitor`` package contains nodes providing an additional level of robot safety, namely the Collision Monitor and the Collision Detector. +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. +The Collision Detector 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 a message to a topic. Provided Nodes **************** From 6eeca7224096c20c5e15c6fad5cf4f2bffc1cdbb Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 17 Aug 2023 13:41:51 +0200 Subject: [PATCH 10/11] fix --- migration/Humble.rst | 6 ++++++ migration/Iron.rst | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/migration/Humble.rst b/migration/Humble.rst index 9a52ea252..608d54128 100644 --- a/migration/Humble.rst +++ b/migration/Humble.rst @@ -225,3 +225,9 @@ More information about ``Denoise Layer`` plugin and how it works could be found SmacPlannerHybrid viz_expansions parameter ****************************************** `PR #3577 `_ adds a new paremeter for visualising SmacPlannerHybrid expansions for debug purpose. + +New node in nav2_collision_monitor: Collision Detector +****************************************************** + +In this `PR #3693 `_ A new node was introduced in the nav2_collision_monitor: Collision Detector. +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. \ No newline at end of file diff --git a/migration/Iron.rst b/migration/Iron.rst index 752b9a4ef..e5054db89 100644 --- a/migration/Iron.rst +++ b/migration/Iron.rst @@ -59,3 +59,9 @@ Smac Planner Debug Param Name Change ************************************ ``debug_visualizations`` replaces ``viz_expansions`` parameter in Hybrid-A* to reflect the new inclusion of footprint debug information being published as well. + +New node in nav2_collision_monitor: Collision Detector +****************************************************** + +In this `PR #3693 `_ A new node was introduced in the nav2_collision_monitor: Collision Detector. +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. \ No newline at end of file From 6a23c6a0f92b6af0123fb8833022168c7faf201f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 22 Aug 2023 10:05:05 +0200 Subject: [PATCH 11/11] fixes --- .../configuring-collision-detector-node.rst | 2 +- migration/Humble.rst | 6 ------ migration/Iron.rst | 2 +- 3 files changed, 2 insertions(+), 8 deletions(-) diff --git a/configuration/packages/collision_monitor/configuring-collision-detector-node.rst b/configuration/packages/collision_monitor/configuring-collision-detector-node.rst index 40aa3372f..d3cae7633 100644 --- a/configuration/packages/collision_monitor/configuring-collision-detector-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-detector-node.rst @@ -7,7 +7,7 @@ The Collision Detector is a node similar to the Collision Monitor, so it is reco 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. +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. See the package's ``README`` for more information. diff --git a/migration/Humble.rst b/migration/Humble.rst index 608d54128..9a52ea252 100644 --- a/migration/Humble.rst +++ b/migration/Humble.rst @@ -225,9 +225,3 @@ More information about ``Denoise Layer`` plugin and how it works could be found SmacPlannerHybrid viz_expansions parameter ****************************************** `PR #3577 `_ adds a new paremeter for visualising SmacPlannerHybrid expansions for debug purpose. - -New node in nav2_collision_monitor: Collision Detector -****************************************************** - -In this `PR #3693 `_ A new node was introduced in the nav2_collision_monitor: Collision Detector. -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. \ No newline at end of file diff --git a/migration/Iron.rst b/migration/Iron.rst index e5054db89..298fae4e6 100644 --- a/migration/Iron.rst +++ b/migration/Iron.rst @@ -64,4 +64,4 @@ New node in nav2_collision_monitor: Collision Detector ****************************************************** In this `PR #3693 `_ A new node was introduced in the nav2_collision_monitor: Collision Detector. -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. \ No newline at end of file +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. \ No newline at end of file