Skip to content

Commit

Permalink
Collision Monitor max_points to min_points rename (ros-navigation#408)
Browse files Browse the repository at this point in the history
* Rename max_points to min_points CM parameter

* Refer to max_points parameter in Humble

* Fix misprint

---------

Co-authored-by: Steve Macenski <[email protected]>
  • Loading branch information
AlexeyMerzlyakov and SteveMacenski authored Mar 29, 2023
1 parent 9bea16f commit 4700799
Show file tree
Hide file tree
Showing 3 changed files with 16 additions and 12 deletions.
16 changes: 8 additions & 8 deletions configuration/packages/configuring-collision-monitor.rst
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,8 @@ When multiple zones trigger at once, the most aggressive one is used (e.g. stop

The following models of safety behaviors are employed by Collision Monitor:

- **Stop model**: Define a zone and a point threshold. If more that ``max_points`` 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 more than ``max_points`` points will appear inside the area.
- **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.
- **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:
Expand Down Expand Up @@ -240,16 +240,16 @@ Polygons parameters
Description:
Zone behavior model. Available values are ``stop``, ``slowdown``, ``approach``. Causes an error, if not specialized.

:``<polygon_name>``.max_points:
:``<polygon_name>``.min_points:

============== =============================
Type Default
-------------- -----------------------------
int 3
int 4
============== =============================

Description:
Maximum number of data readings within a zone to not trigger the action.
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.

:``<polygon_name>``.slowdown_ratio:

Expand Down Expand Up @@ -393,14 +393,14 @@ For more information how to bring-up your own Collision Monitor node, please ref
type: "circle"
radius: 0.3
action_type: "stop"
max_points: 3
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"
max_points: 3
min_points: 4 # max_points: 3 for Humble
slowdown_ratio: 0.3
visualize: True
polygon_pub_topic: "polygon_slowdown"
Expand All @@ -410,7 +410,7 @@ For more information how to bring-up your own Collision Monitor node, please ref
footprint_topic: "/local_costmap/published_footprint"
time_before_collision: 2.0
simulation_time_step: 0.02
max_points: 5
min_points: 6 # max_points: 5 for Humble
visualize: False
observation_sources: ["scan", "pointcloud"]
scan:
Expand Down
4 changes: 4 additions & 0 deletions migration/Humble.rst
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,10 @@ Publish Collision Monitor State

`PR #3504 <https://github.com/ros-planning/navigation2/pull/3504>`_ adds a new ``state_topic`` parameter to the CollisionMonitor. If specified, this optional parameter enables the state topic publisher. The topic reports the currently activated polygon action type and name.

Renamed ROS-parameter in Collision Monitor
******************************************

`PR #3513 <https://github.com/ros-planning/navigation2/pull/3513>`_ renames ``max_points`` parameter to ``min_points`` and changes its meaning. Formerly ``max_points`` meant the maximum number of points inside the area still not triggering the action, while ``min_points`` - is a minimal number of points starting from the action to be initiated. In other words ``min_points`` now should be adjusted as ``max_points + 1``.

Velocity smoother applies deceleration when timeout
***************************************************
Expand Down
8 changes: 4 additions & 4 deletions tutorials/docs/using_collision_monitor.rst
Original file line number Diff line number Diff line change
Expand Up @@ -43,14 +43,14 @@ For this setup, the following lines should be added into ``collision_monitor_par
type: "polygon"
points: [0.4, 0.3, 0.4, -0.3, 0.0, -0.3, 0.0, 0.3]
action_type: "stop"
max_points: 3
min_points: 4 # max_points: 3 for Humble
visualize: True
polygon_pub_topic: "polygon_stop"
PolygonSlow:
type: "polygon"
points: [0.6, 0.4, 0.6, -0.4, 0.0, -0.4, 0.0, 0.4]
action_type: "slowdown"
max_points: 3
min_points: 4 # max_points: 3 for Humble
slowdown_ratio: 0.3
visualize: True
polygon_pub_topic: "polygon_slowdown"
Expand Down Expand Up @@ -91,14 +91,14 @@ The whole ``nav2_collision_monitor/params/collision_monitor_params.yaml`` file i
type: "polygon"
points: [0.4, 0.3, 0.4, -0.3, 0.0, -0.3, 0.0, 0.3]
action_type: "stop"
max_points: 3
min_points: 4 # max_points: 3 for Humble
visualize: True
polygon_pub_topic: "polygon_stop"
PolygonSlow:
type: "polygon"
points: [0.6, 0.4, 0.6, -0.4, 0.0, -0.4, 0.0, 0.4]
action_type: "slowdown"
max_points: 3
min_points: 4 # max_points: 3 for Humble
slowdown_ratio: 0.3
visualize: True
polygon_pub_topic: "polygon_slowdown"
Expand Down

0 comments on commit 4700799

Please sign in to comment.