Skip to content
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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``.

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

Expand All @@ -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``.

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

Expand Down Expand Up @@ -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.

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

Expand All @@ -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
==============================
Expand Down
141 changes: 140 additions & 1 deletion tutorials/docs/using_collision_monitor.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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



Comment thread
SteveMacenski marked this conversation as resolved.
Outdated
.. 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.

Expand All @@ -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 robots 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.
Comment thread
SteveMacenski marked this conversation as resolved.
Outdated

.. image:: images/Collision_Monitor/holonomic_direction.png
:width: 365px
Expand All @@ -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:

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where did this come from and does this match the image?

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I created this example following the pattern of the non-holonomic robot example above. regarding the parameters, the previous commit had some discrepancies with the image. I have now updated the angle ranges to match the image exactly.

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
====================

Expand Down
Loading