From 782f3492138df04d67b46554e7337f8497ea7436 Mon Sep 17 00:00:00 2001 From: Jaerak Son Date: Fri, 13 Feb 2026 08:57:12 +0900 Subject: [PATCH 1/2] Update velocity polygon linear_min/max Description Update collision monitor tutorial for holonomic robot Signed-off-by: Jaerak Son --- .../configuring-collision-monitor-node.rst | 14 +- tutorials/docs/using_collision_monitor.rst | 141 +++++++++++++++++- 2 files changed, 150 insertions(+), 5 deletions(-) diff --git a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst index 340f36678..52aa5786e 100644 --- a/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst +++ b/configuration/packages/collision_monitor/configuring-collision-monitor-node.rst @@ -457,7 +457,10 @@ All previous Polygon parameters apply, in addition to the following unique param ============== ============================= Description: - Minimum linear velocity for the sub polygon. In holonomic mode, this is the minimum resultant velocity. Causes an error, if not specified. + Minimum linear velocity for the sub-polygon. Causes an error, if not specified. + + * **Non-holonomic:** This is the minimum signed velocity along the x-axis (allows negative values for reverse motion). + * **Holonomic:** This is the minimum magnitude of the resultant velocity, which must be ``>= 0.0``. :``.``.linear_max: @@ -468,7 +471,10 @@ All previous Polygon parameters apply, in addition to the following unique param ============== ============================= Description: - Maximum linear velocity for the sub polygon. In holonomic mode, this is the maximum resultant velocity. Causes an error, if not specified. + Maximum linear velocity for the sub polygon. Causes an error, if not specified. + + * **Non-holonomic:** This is the maximum signed velocity along the x-axis. (allows negative values for reverse motion). + * **Holonomic:** This is the maximum magnitude of the resultant velocity, which must be ``>= 0.0``. :``.``.theta_min: @@ -501,7 +507,7 @@ All previous Polygon parameters apply, in addition to the following unique param ============== ============================= Description: - Start angle of the movement direction(for holomic robot only). Refer to the `Example`_ section for the common configurations. Applicable for `holonomic` mode only. + Start angle of the movement direction(for holonomic robot only). Refer to the `Example`_ section for the common configurations. Applicable for `holonomic` mode only. :``.``.direction_end_angle: @@ -512,7 +518,7 @@ All previous Polygon parameters apply, in addition to the following unique param ============== ============================= Description: - End angle of the movement direction(for holomic robot only). Refer to the `Example`_ section for the common configurations. Applicable for `holonomic` mode only. + End angle of the movement direction(for holonomic robot only). Refer to the `Example`_ section for the common configurations. Applicable for `holonomic` mode only. Observation sources parameters ============================== diff --git a/tutorials/docs/using_collision_monitor.rst b/tutorials/docs/using_collision_monitor.rst index ea57f69b7..6d8998859 100644 --- a/tutorials/docs/using_collision_monitor.rst +++ b/tutorials/docs/using_collision_monitor.rst @@ -169,6 +169,8 @@ Below is the example configuration using 4 sub-polygons to cover the full range theta_min: -1.0 theta_max: 1.0 + + .. note:: It is recommended to include a ``stopped`` sub polygon as the last entry in the ``velocity_polygons`` list to cover the entire range of the robot's velocity limits. In cases where the velocity is not within the scope of any sub polygons, the Collision Monitor will log a warning message and continue with the previously matched polygon. @@ -177,7 +179,7 @@ Below is the example configuration using 4 sub-polygons to cover the full range **For holomic robots:** -For holomic robots, the ``holonomic`` property should be set to ``true``. In this scenario, the ``linear_min`` and ``linear_max`` parameters should cover the robot's resultant velocity limits, while the ``theta_min`` and ``theta_max`` parameters should cover the robot's angular velocity limits. Additionally, there will be 2 more parameters, ``direction_start_angle`` and ``direction_end_angle``, to specify the resultant velocity direction. The covered direction will always span from ``direction_start_angle`` to ``direction_end_angle`` in the **counter-clockwise** direction. +For holomic robots, the ``holonomic`` property should be set to ``true``. In this scenario, the ``linear_min`` and ``linear_max`` parameters should cover the magnitude of the robot’s resultant velocity limits (using only non-negative values ``>=0.0``), while the ``theta_min`` and ``theta_max`` parameters should cover the robot's angular velocity limits. Additionally, there will be 2 more parameters, ``direction_start_angle`` and ``direction_end_angle``, to specify the resultant velocity direction. The covered direction will always span from ``direction_start_angle`` to ``direction_end_angle`` in the **counter-clockwise** direction. .. image:: images/Collision_Monitor/holonomic_direction.png :width: 365px @@ -187,6 +189,143 @@ Below shows some common configurations for holonomic robots that cover multiple .. image:: images/Collision_Monitor/holonomic_examples.png :height: 2880px +.. code-block:: yaml + + collision_monitor: + ros__parameters: + base_frame_id: "base_link" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "nav_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.3 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 20000.0 + holonomic: true + + polygons: ["VelocityPolygonSlow", "VelocityPolygonStop"] + + # ========================================== + # 1. Slowdown Polygon + # ========================================== + VelocityPolygonSlow: + type: "velocity_polygon" + action_type: "slowdown" + slowdown_ratio: 0.5 + holonomic: true + visualize: True + enabled: True + velocity_polygons: [ + "forward", "forward_left", "left", "backward_left", + "backward", "backward_right", "right", "forward_right", + "stopped" + ] + + forward: + points: "[[0.6, 0.4], [0.6, -0.4], [-0.3, -0.4], [-0.3, 0.4]]" + linear_min: 0.05 + linear_max: 1.0 + direction_start_angle: -0.39 + direction_end_angle: 0.39 + theta_min: -1.0 + theta_max: 1.0 + + forward_left: + points: "[[0.55, 0.55], [0.55, -0.3], [-0.3, -0.3], [-0.3, 0.55]]" + linear_min: 0.05 + linear_max: 1.0 + direction_start_angle: 0.39 + direction_end_angle: 1.18 + theta_min: -1.0 + theta_max: 1.0 + + left: + points: "[[0.4, 0.6], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.6]]" + linear_min: 0.05 + linear_max: 1.0 + direction_start_angle: 1.18 + direction_end_angle: 1.96 + theta_min: -1.0 + theta_max: 1.0 + + backward_left: + points: "[[0.3, 0.55], [0.3, -0.3], [-0.55, -0.3], [-0.55, 0.55]]" + linear_min: 0.05 + linear_max: 1.0 + direction_start_angle: 1.96 + direction_end_angle: 2.75 + theta_min: -1.0 + theta_max: 1.0 + + backward: + points: "[[0.3, 0.4], [0.3, -0.4], [-0.6, -0.4], [-0.6, 0.4]]" + linear_min: 0.05 + linear_max: 1.0 + direction_start_angle: 2.75 + direction_end_angle: -2.75 + theta_min: -1.0 + theta_max: 1.0 + + backward_right: + points: "[[0.3, 0.3], [0.3, -0.55], [-0.55, -0.55], [-0.55, 0.3]]" + linear_min: 0.05 + linear_max: 1.0 + direction_start_angle: -2.75 + direction_end_angle: -1.96 + theta_min: -1.0 + theta_max: 1.0 + + right: + points: "[[0.4, 0.4], [0.4, -0.6], [-0.4, -0.6], [-0.4, 0.4]]" + linear_min: 0.05 + linear_max: 1.0 + direction_start_angle: -1.96 + direction_end_angle: -1.18 + theta_min: -1.0 + theta_max: 1.0 + + forward_right: + points: "[[0.55, 0.3], [0.55, -0.55], [-0.3, -0.55], [-0.3, 0.3]]" + linear_min: 0.05 + linear_max: 1.0 + direction_start_angle: -1.18 + direction_end_angle: -0.39 + theta_min: -1.0 + theta_max: 1.0 + + stopped: + points: "[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]" + linear_min: 0.0 + linear_max: 0.05 + direction_start_angle: -3.1415 + direction_end_angle: 3.1415 + theta_min: -1.0 + theta_max: 1.0 + + # ========================================== + # 2. Stop Polygon + # ========================================== + VelocityPolygonStop: + type: "velocity_polygon" + action_type: "stop" + holonomic: true + visualize: True + enabled: True + velocity_polygons: ["moving_stop", "stopped"] + moving_stop: + points: "[[0.35, 0.35], [0.35, -0.35], [-0.35, -0.35], [-0.35, 0.35]]" + linear_min: 0.05 + linear_max: 1.0 + theta_min: -1.0 + theta_max: 1.0 + stopped: + points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]" + linear_min: 0.0 + linear_max: 0.05 + theta_min: -1.0 + theta_max: 1.0 + Preparing Nav2 stack ==================== From e74c75cbc1c19b158783d6c7db6d92773c2b95ec Mon Sep 17 00:00:00 2001 From: Jaerak Son Date: Tue, 17 Feb 2026 15:02:26 +0900 Subject: [PATCH 2/2] [collision_monitor] Update velocity polygon angles and refine documentation - Updated `direction_start_angle` and `direction_end_angle` parameters in YAML to match the reference diagram. - Refined `using_collision_monitor.rst` based on review feedback (removed redundant non-negative description). Signed-off-by: Jaerak Son --- tutorials/docs/using_collision_monitor.rst | 71 +++++++--------------- 1 file changed, 22 insertions(+), 49 deletions(-) diff --git a/tutorials/docs/using_collision_monitor.rst b/tutorials/docs/using_collision_monitor.rst index 6d8998859..40e15d30e 100644 --- a/tutorials/docs/using_collision_monitor.rst +++ b/tutorials/docs/using_collision_monitor.rst @@ -169,8 +169,6 @@ Below is the example configuration using 4 sub-polygons to cover the full range theta_min: -1.0 theta_max: 1.0 - - .. note:: It is recommended to include a ``stopped`` sub polygon as the last entry in the ``velocity_polygons`` list to cover the entire range of the robot's velocity limits. In cases where the velocity is not within the scope of any sub polygons, the Collision Monitor will log a warning message and continue with the previously matched polygon. @@ -179,7 +177,7 @@ Below is the example configuration using 4 sub-polygons to cover the full range **For holomic robots:** -For holomic robots, the ``holonomic`` property should be set to ``true``. In this scenario, the ``linear_min`` and ``linear_max`` parameters should cover the magnitude of the robot’s resultant velocity limits (using only non-negative values ``>=0.0``), while the ``theta_min`` and ``theta_max`` parameters should cover the robot's angular velocity limits. Additionally, there will be 2 more parameters, ``direction_start_angle`` and ``direction_end_angle``, to specify the resultant velocity direction. The covered direction will always span from ``direction_start_angle`` to ``direction_end_angle`` in the **counter-clockwise** direction. +For holomic robots, the ``holonomic`` property should be set to ``true``. In this scenario, the ``linear_min`` and ``linear_max`` parameters should cover the magnitude of the robot’s resultant velocity limits (using only non-negative values), while the ``theta_min`` and ``theta_max`` parameters should cover the robot's angular velocity limits. Additionally, there will be 2 more parameters, ``direction_start_angle`` and ``direction_end_angle``, to specify the resultant velocity direction. The covered direction will always span from ``direction_start_angle`` to ``direction_end_angle`` in the **counter-clockwise** direction. .. image:: images/Collision_Monitor/holonomic_direction.png :width: 365px @@ -193,7 +191,7 @@ Below shows some common configurations for holonomic robots that cover multiple collision_monitor: ros__parameters: - base_frame_id: "base_link" + base_frame_id: "base_footprint" odom_frame_id: "odom" cmd_vel_in_topic: "cmd_vel_smoothed" cmd_vel_out_topic: "nav_vel" @@ -201,14 +199,11 @@ Below shows some common configurations for holonomic robots that cover multiple transform_tolerance: 0.3 source_timeout: 1.0 base_shift_correction: True - stop_pub_timeout: 20000.0 - holonomic: true + stop_pub_timeout: 2.0 + holonomic: true # Set to true for holonomic robots - polygons: ["VelocityPolygonSlow", "VelocityPolygonStop"] + polygons: ["VelocityPolygonSlow"] - # ========================================== - # 1. Slowdown Polygon - # ========================================== VelocityPolygonSlow: type: "velocity_polygon" action_type: "slowdown" @@ -226,8 +221,8 @@ Below shows some common configurations for holonomic robots that cover multiple points: "[[0.6, 0.4], [0.6, -0.4], [-0.3, -0.4], [-0.3, 0.4]]" linear_min: 0.05 linear_max: 1.0 - direction_start_angle: -0.39 - direction_end_angle: 0.39 + direction_start_angle: -0.785 # -0.25pi + direction_end_angle: 0.785 # 0.25pi theta_min: -1.0 theta_max: 1.0 @@ -235,8 +230,8 @@ Below shows some common configurations for holonomic robots that cover multiple points: "[[0.55, 0.55], [0.55, -0.3], [-0.3, -0.3], [-0.3, 0.55]]" linear_min: 0.05 linear_max: 1.0 - direction_start_angle: 0.39 - direction_end_angle: 1.18 + direction_start_angle: 0.0 # 0.0pi + direction_end_angle: 1.571 # 0.5pi theta_min: -1.0 theta_max: 1.0 @@ -244,8 +239,8 @@ Below shows some common configurations for holonomic robots that cover multiple points: "[[0.4, 0.6], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.6]]" linear_min: 0.05 linear_max: 1.0 - direction_start_angle: 1.18 - direction_end_angle: 1.96 + direction_start_angle: 0.785 # 0.25pi + direction_end_angle: 2.356 # 0.75pi theta_min: -1.0 theta_max: 1.0 @@ -253,8 +248,8 @@ Below shows some common configurations for holonomic robots that cover multiple points: "[[0.3, 0.55], [0.3, -0.3], [-0.55, -0.3], [-0.55, 0.55]]" linear_min: 0.05 linear_max: 1.0 - direction_start_angle: 1.96 - direction_end_angle: 2.75 + direction_start_angle: -3.1415 # -pi + direction_end_angle: -1.571 # -0.5pi theta_min: -1.0 theta_max: 1.0 @@ -262,8 +257,8 @@ Below shows some common configurations for holonomic robots that cover multiple points: "[[0.3, 0.4], [0.3, -0.4], [-0.6, -0.4], [-0.6, 0.4]]" linear_min: 0.05 linear_max: 1.0 - direction_start_angle: 2.75 - direction_end_angle: -2.75 + direction_start_angle: 2.356 # 0.75pi + direction_end_angle: -2.356 # -0.75pi theta_min: -1.0 theta_max: 1.0 @@ -271,8 +266,8 @@ Below shows some common configurations for holonomic robots that cover multiple points: "[[0.3, 0.3], [0.3, -0.55], [-0.55, -0.55], [-0.55, 0.3]]" linear_min: 0.05 linear_max: 1.0 - direction_start_angle: -2.75 - direction_end_angle: -1.96 + direction_start_angle: 1.571 # 0.5pi + direction_end_angle: 3.1415 # pi theta_min: -1.0 theta_max: 1.0 @@ -280,8 +275,8 @@ Below shows some common configurations for holonomic robots that cover multiple points: "[[0.4, 0.4], [0.4, -0.6], [-0.4, -0.6], [-0.4, 0.4]]" linear_min: 0.05 linear_max: 1.0 - direction_start_angle: -1.96 - direction_end_angle: -1.18 + direction_start_angle: -2.356 # -0.75pi + direction_end_angle: -0.785 # -0.25pi theta_min: -1.0 theta_max: 1.0 @@ -289,11 +284,12 @@ Below shows some common configurations for holonomic robots that cover multiple points: "[[0.55, 0.3], [0.55, -0.55], [-0.3, -0.55], [-0.3, 0.3]]" linear_min: 0.05 linear_max: 1.0 - direction_start_angle: -1.18 - direction_end_angle: -0.39 + direction_start_angle: -1.571 # -0.5pi + direction_end_angle: 0.0 # 0.0pi theta_min: -1.0 theta_max: 1.0 + # Stopped stopped: points: "[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]" linear_min: 0.0 @@ -303,29 +299,6 @@ Below shows some common configurations for holonomic robots that cover multiple theta_min: -1.0 theta_max: 1.0 - # ========================================== - # 2. Stop Polygon - # ========================================== - VelocityPolygonStop: - type: "velocity_polygon" - action_type: "stop" - holonomic: true - visualize: True - enabled: True - velocity_polygons: ["moving_stop", "stopped"] - moving_stop: - points: "[[0.35, 0.35], [0.35, -0.35], [-0.35, -0.35], [-0.35, 0.35]]" - linear_min: 0.05 - linear_max: 1.0 - theta_min: -1.0 - theta_max: 1.0 - stopped: - points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]" - linear_min: 0.0 - linear_max: 0.05 - theta_min: -1.0 - theta_max: 1.0 - Preparing Nav2 stack ====================