Skip to content

Commit 9693282

Browse files
authored
docs: Update collision monitor tutorial for holonomic robot (#871)
* Update velocity polygon linear_min/max Description Update collision monitor tutorial for holonomic robot Signed-off-by: Jaerak Son <sjr9017@naver.com> * [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 <sjr9017@naver.com> --------- Signed-off-by: Jaerak Son <sjr9017@naver.com>
1 parent dc6f24b commit 9693282

2 files changed

Lines changed: 123 additions & 5 deletions

File tree

configuration/packages/collision_monitor/configuring-collision-monitor-node.rst

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -457,7 +457,10 @@ All previous Polygon parameters apply, in addition to the following unique param
457457
============== =============================
458458

459459
Description:
460-
Minimum linear velocity for the sub polygon. In holonomic mode, this is the minimum resultant velocity. Causes an error, if not specified.
460+
Minimum linear velocity for the sub-polygon. Causes an error, if not specified.
461+
462+
* **Non-holonomic:** This is the minimum signed velocity along the x-axis (allows negative values for reverse motion).
463+
* **Holonomic:** This is the minimum magnitude of the resultant velocity, which must be ``>= 0.0``.
461464

462465
:``<vel_poly>.<subpoly>``.linear_max:
463466

@@ -468,7 +471,10 @@ All previous Polygon parameters apply, in addition to the following unique param
468471
============== =============================
469472

470473
Description:
471-
Maximum linear velocity for the sub polygon. In holonomic mode, this is the maximum resultant velocity. Causes an error, if not specified.
474+
Maximum linear velocity for the sub polygon. Causes an error, if not specified.
475+
476+
* **Non-holonomic:** This is the maximum signed velocity along the x-axis. (allows negative values for reverse motion).
477+
* **Holonomic:** This is the maximum magnitude of the resultant velocity, which must be ``>= 0.0``.
472478

473479
:``<vel_poly>.<subpoly>``.theta_min:
474480

@@ -501,7 +507,7 @@ All previous Polygon parameters apply, in addition to the following unique param
501507
============== =============================
502508

503509
Description:
504-
Start angle of the movement direction(for holomic robot only). Refer to the `Example`_ section for the common configurations. Applicable for `holonomic` mode only.
510+
Start angle of the movement direction(for holonomic robot only). Refer to the `Example`_ section for the common configurations. Applicable for `holonomic` mode only.
505511

506512
:``<vel_poly>.<subpoly>``.direction_end_angle:
507513

@@ -512,7 +518,7 @@ All previous Polygon parameters apply, in addition to the following unique param
512518
============== =============================
513519

514520
Description:
515-
End angle of the movement direction(for holomic robot only). Refer to the `Example`_ section for the common configurations. Applicable for `holonomic` mode only.
521+
End angle of the movement direction(for holonomic robot only). Refer to the `Example`_ section for the common configurations. Applicable for `holonomic` mode only.
516522

517523
Observation sources parameters
518524
==============================

tutorials/docs/using_collision_monitor.rst

Lines changed: 113 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -177,7 +177,7 @@ Below is the example configuration using 4 sub-polygons to cover the full range
177177

178178
**For holomic robots:**
179179

180-
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.
180+
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 robots 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.
181181

182182
.. image:: images/Collision_Monitor/holonomic_direction.png
183183
:width: 365px
@@ -187,6 +187,118 @@ Below shows some common configurations for holonomic robots that cover multiple
187187
.. image:: images/Collision_Monitor/holonomic_examples.png
188188
:height: 2880px
189189

190+
.. code-block:: yaml
191+
192+
collision_monitor:
193+
ros__parameters:
194+
base_frame_id: "base_footprint"
195+
odom_frame_id: "odom"
196+
cmd_vel_in_topic: "cmd_vel_smoothed"
197+
cmd_vel_out_topic: "nav_vel"
198+
state_topic: "collision_monitor_state"
199+
transform_tolerance: 0.3
200+
source_timeout: 1.0
201+
base_shift_correction: True
202+
stop_pub_timeout: 2.0
203+
holonomic: true # Set to true for holonomic robots
204+
205+
polygons: ["VelocityPolygonSlow"]
206+
207+
VelocityPolygonSlow:
208+
type: "velocity_polygon"
209+
action_type: "slowdown"
210+
slowdown_ratio: 0.5
211+
holonomic: true
212+
visualize: True
213+
enabled: True
214+
velocity_polygons: [
215+
"forward", "forward_left", "left", "backward_left",
216+
"backward", "backward_right", "right", "forward_right",
217+
"stopped"
218+
]
219+
220+
forward:
221+
points: "[[0.6, 0.4], [0.6, -0.4], [-0.3, -0.4], [-0.3, 0.4]]"
222+
linear_min: 0.05
223+
linear_max: 1.0
224+
direction_start_angle: -0.785 # -0.25pi
225+
direction_end_angle: 0.785 # 0.25pi
226+
theta_min: -1.0
227+
theta_max: 1.0
228+
229+
forward_left:
230+
points: "[[0.55, 0.55], [0.55, -0.3], [-0.3, -0.3], [-0.3, 0.55]]"
231+
linear_min: 0.05
232+
linear_max: 1.0
233+
direction_start_angle: 0.0 # 0.0pi
234+
direction_end_angle: 1.571 # 0.5pi
235+
theta_min: -1.0
236+
theta_max: 1.0
237+
238+
left:
239+
points: "[[0.4, 0.6], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.6]]"
240+
linear_min: 0.05
241+
linear_max: 1.0
242+
direction_start_angle: 0.785 # 0.25pi
243+
direction_end_angle: 2.356 # 0.75pi
244+
theta_min: -1.0
245+
theta_max: 1.0
246+
247+
backward_left:
248+
points: "[[0.3, 0.55], [0.3, -0.3], [-0.55, -0.3], [-0.55, 0.55]]"
249+
linear_min: 0.05
250+
linear_max: 1.0
251+
direction_start_angle: -3.1415 # -pi
252+
direction_end_angle: -1.571 # -0.5pi
253+
theta_min: -1.0
254+
theta_max: 1.0
255+
256+
backward:
257+
points: "[[0.3, 0.4], [0.3, -0.4], [-0.6, -0.4], [-0.6, 0.4]]"
258+
linear_min: 0.05
259+
linear_max: 1.0
260+
direction_start_angle: 2.356 # 0.75pi
261+
direction_end_angle: -2.356 # -0.75pi
262+
theta_min: -1.0
263+
theta_max: 1.0
264+
265+
backward_right:
266+
points: "[[0.3, 0.3], [0.3, -0.55], [-0.55, -0.55], [-0.55, 0.3]]"
267+
linear_min: 0.05
268+
linear_max: 1.0
269+
direction_start_angle: 1.571 # 0.5pi
270+
direction_end_angle: 3.1415 # pi
271+
theta_min: -1.0
272+
theta_max: 1.0
273+
274+
right:
275+
points: "[[0.4, 0.4], [0.4, -0.6], [-0.4, -0.6], [-0.4, 0.4]]"
276+
linear_min: 0.05
277+
linear_max: 1.0
278+
direction_start_angle: -2.356 # -0.75pi
279+
direction_end_angle: -0.785 # -0.25pi
280+
theta_min: -1.0
281+
theta_max: 1.0
282+
283+
forward_right:
284+
points: "[[0.55, 0.3], [0.55, -0.55], [-0.3, -0.55], [-0.3, 0.3]]"
285+
linear_min: 0.05
286+
linear_max: 1.0
287+
direction_start_angle: -1.571 # -0.5pi
288+
direction_end_angle: 0.0 # 0.0pi
289+
theta_min: -1.0
290+
theta_max: 1.0
291+
292+
# Stopped
293+
stopped:
294+
points: "[[0.4, 0.4], [0.4, -0.4], [-0.4, -0.4], [-0.4, 0.4]]"
295+
linear_min: 0.0
296+
linear_max: 0.05
297+
direction_start_angle: -3.1415
298+
direction_end_angle: 3.1415
299+
theta_min: -1.0
300+
theta_max: 1.0
301+
190302
Preparing Nav2 stack
191303
====================
192304

0 commit comments

Comments
 (0)