Add VelocityPolygon docs

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

@@ -47,8 +47,9 @@ 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.
+- VelocityPolygon: allow switching of polygons based on the command velocity. This is useful for robots to set different safety zones based on their velocity (e.g. a robot that has a larger safety zone when moving at 1.0 m/s than when moving at 0.5 m/s). 
 
-All shapes (``Polygon`` and ``Circle``) are derived from base ``Polygon`` class, so without loss of generality they would be called as "polygons".
+All shapes (``Polygon``, ``Circle`` and ``VelocityPolygon``) 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:
@@ -364,6 +365,110 @@ Polygons parameters
   Description:
     Whether to use this polygon for collision monitoring. (Can be dynamically set)
 
+VelocityPolygon parameters
+==========================
+
+All previous Polygon parameters apply, in addition to the following unique parameters for VelocityPolygon.
+
+:``<vel_poly>``.holonomic:
+
+  ============== =============================
+  Type           Default
+  -------------- -----------------------------
+  bool           False
+  ============== =============================
+
+  Description:
+    Whether to use holonomic or non-holonomic robot model for collision prediction. For holonomic robot model, the resultant velocity will be used to compare the linear velocity range. Additionally, there will be 2 more parameters, ``direction_start_angle`` and ``direction_end_angle``, to specify the resultant velocity direction.
+
+:``<vel_poly>``.velocity_polygons:
+
+  ============== =============================
+  Type           Default
+  -------------- -----------------------------
+  vector<string> N/A
+  ============== =============================
+
+  Description:
+    List of sub polygons for switching based on the robot's current velocity. When velocity is covered by multiple sub polygons, the first sub polygon in the list will be used. Causes an error, if not specified.
+
+:``<vel_poly>.<subpoly>``.points:
+
+  ============== =============================
+  Type           Default
+  -------------- -----------------------------
+  vector<string> 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. Causes an error, if not specified.
+
+:``<vel_poly>.<subpoly>``.linear_min:
+
+    ============== =============================
+    Type           Default
+    -------------- -----------------------------
+    double         N/A
+    ============== =============================
+
+    Description:
+      Minimum linear velocity for the sub polygon. In holonomic mode, this is the minimum resultant velocity. Causes an error, if not specified.
+
+:``<vel_poly>.<subpoly>``.linear_max:
+
+    ============== =============================
+    Type           Default
+    -------------- -----------------------------
+    double         N/A
+    ============== =============================
+
+    Description:
+      Maximum linear velocity for the sub polygon. In holonomic mode, this is the maximum resultant velocity. Causes an error, if not specified.
+
+:``<vel_poly>.<subpoly>``.theta_min:
+
+    ============== =============================
+    Type           Default
+    -------------- -----------------------------
+    double         N/A
+    ============== =============================
+
+    Description:
+      Minimum angular velocity for the sub polygon. Causes an error, if not specified.
+
+:``<vel_poly>.<subpoly>``.theta_max:
+
+    ============== =============================
+    Type           Default
+    -------------- -----------------------------
+    double         N/A
+    ============== =============================
+
+    Description:
+      Maximum angular velocity for the sub polygon. Causes an error, if not specified.
+
+:``<vel_poly>.<subpoly>``.direction_start_angle:
+
+    ============== =============================
+    Type           Default
+    -------------- -----------------------------
+    double         -PI
+    ============== =============================
+
+    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.
+
+:``<vel_poly>.<subpoly>``.direction_end_angle:
+
+    ============== =============================
+    Type           Default
+    -------------- -----------------------------
+    double         PI
+    ============== =============================
+
+    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.
+
 Observation sources parameters
 ==============================
 
@@ -450,6 +555,11 @@ Observation sources parameters
 Example
 *******
 
+Here is an example illustrating the common configurations for holonomic robots that cover multiple directions of the resultant velocity:
+
+.. image:: ../images/holonomic_examples.png
+  :height: 2880px
+
 Here is an example of configuration YAML for the Collision Monitor.
 
 .. code-block:: yaml
@@ -503,6 +613,42 @@ Here is an example of configuration YAML for the Collision Monitor.
           min_points: 6  # max_points: 5 for Humble
           visualize: False
           enabled: True
+        VelocityPolygonStop:
+          type: "velocity_polygon"
+          action_type: "stop"
+          min_points: 6
+          visualize: True
+          enabled: True
+          polygon_pub_topic: "velocity_polygon_stop"
+          velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
+          holonomic: false
+          rotation:
+            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
+          translation_forward:
+            points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]"
+            linear_min: 0.0
+            linear_max: 1.0
+            theta_min: -1.0
+            theta_max: 1.0
+          translation_backward:
+            points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]"
+            linear_min: -1.0
+            linear_max: 0.0
+            theta_min: -1.0
+            theta_max: 1.0
+          # This is the last polygon to be checked, it should cover the entire range of robot's velocities
+          # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity
+          # is not covered by any of the other sub-polygons 
+          stopped:
+            points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
+            linear_min: -1.0
+            linear_max: 1.0
+            theta_min: -1.0
+            theta_max: 1.0
         observation_sources: ["scan", "pointcloud"]
         scan:
           source_timeout: 0.2

migration/Iron.rst

@@ -5,6 +5,12 @@ Iron to Jazzy
 
 Moving from ROS 2 Iron to Jazzy, a number of stability improvements were added that we will not specifically address here.
 
+Add VelocityPolygon in Collision Monitor
+****************************************
+
+`PR #3708 <https://github.com/ros-planning/navigation2/pull/3708>`_ adds ``VelocityPolgon`` type in Collision Monitor. This allows the user to setup multiple polygons to cover the range of the robot's velocity limits. For example, the user can configure different polygons for rotation, moving forward, or moving backward. The Collision Monitor will check the robot's velocity against each sub polygon to determine the approriate polygon to be used for collision checking. The tutorial is available in the :ref:`Configuring Collision Monitor with VelocityPolygon <collision_monitor_tutorial>` section.
+
+
 Change polygon points parameter format in Collision Monitor
 ***********************************************************
 
@@ -38,7 +44,7 @@ The Collision Monitor and Velocity Smoothers also had ``use_realtime_priority``
 ``opennav_coverage`` Project
 ****************************
 
-A new metapackage exists in: https://github.com/open-navigation/opennav_coverage which contains complete coverage navigator plugins, BT nodes, behavior tree demos, and coverage planning server based on ``Fields2Cover``. See that project for more information. It is on long-term trajectory for inclusion into ``Nav2``, but there are still yet a few missing features from Fields2Cover before we can integrate that into the main project to be up to snuff in terms of all the major features and capabilities users would expect from a coverage planning system.
+A new metapackage exists in: https://github.com/open-navigation/opennav_coverage which contains complete coverage navigator plugins, BT nodes, behavior tree demos, and coverage planning server based on ``Fields2Cover``. See that project for more information. It is on long-term trajectory for inclusion into ``Nav2``, but there are still yet a few missing features from Fields2Cover before we can integrate that into the main project to be up to snuff in terms of all the major features and capabilities users would expect from a coverage planning system. 
 
 If you'd like to see coverage planning in Nav2 directly, please consider contributing `to the as-of-yet needed features described here <https://github.com/Fields2Cover/Fields2Cover/issues/73>`_.
 

tutorials/docs/using_collision_monitor.rst

@@ -7,6 +7,7 @@ Using Collision Monitor
 - `Requirements`_
 - `Preparing Nav2 stack`_
 - `Configuring Collision Monitor`_
+- `Configuring Collision Monitor with VelocityPolygon`_
 - `Demo Execution`_
 
 .. image:: images/Collision_Monitor/collision_monitor.gif
@@ -107,6 +108,84 @@ The whole ``nav2_collision_monitor/params/collision_monitor_params.yaml`` file i
           type: "scan"
           topic: "scan"
 
+Configuring Collision Monitor with VelocityPolygon
+==================================================
+
+.. image:: images/Collision_Monitor/dexory_velocity_polygon.gif
+  :width: 800px
+
+For this part of tutorial, we will set up the Collision Monitor with ``VelocityPolygon`` type for a ``stop`` action. ``VelocityPolygon`` allows the user to setup multiple polygons to cover the range of the robot's velocity limits. For example, the user can configure different polygons for rotation, moving forward, or moving backward. The Collision Monitor will check the robot's velocity against each sub polygon to determine the approriate polygon to be used for collision checking.
+
+In general, here are the steps to configure the Collision Monitor with ``VelocityPolygon`` type:
+
+#. Add a ``VelocityPolygon`` to the ``polygons`` param list
+#. Configure the ``VelocityPolygon``
+#. Specify the ``holonomic`` property of the polygon (default is ``false``)
+#. Start by adding a ``stopped`` sub polygon to cover the full range of the robot's velocity limits in ``velocity_polygons`` list
+#. Add additional sub polygons to the front of the ``velocity_polygons`` list to cover the range of the robot's velocity limits for each type of motion (e.g. rotation, moving forward, moving backward)
+
+In this example, we will consider a **non-holonomic** robot with linear velocity limits of ``-1.0`` to ``1.0`` m/s and angular velocity limits of ``-1.0`` to ``1.0`` rad/s. The ``linear_min`` and ``linear_max`` parameters of the sub polygons should be set to the robot's linear velocity limits, while the ``theta_min`` and ``theta_max`` parameters should be set to the robot's angular velocity limits.
+
+Below is the example configuration using 4 sub-polygons to cover the full range of the robot's velocity limits:
+
+.. code-block:: yaml
+
+    polygons: ["VelocityPolygonStop"]
+    VelocityPolygonStop:
+      type: "velocity_polygon"
+      action_type: "stop"
+      min_points: 6
+      visualize: True
+      enabled: True
+      polygon_pub_topic: "velocity_polygon_stop"
+      velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
+      holonomic: false
+      rotation:
+        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
+      translation_forward:
+        points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]"
+        linear_min: 0.0
+        linear_max: 1.0
+        theta_min: -1.0
+        theta_max: 1.0
+      translation_backward:
+        points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]"
+        linear_min: -1.0
+        linear_max: 0.0
+        theta_min: -1.0
+        theta_max: 1.0
+      # This is the last polygon to be checked, it should cover the entire range of robot's velocities
+      # It is used as the stopped polygon when the robot is not moving and as a fallback if the velocity
+      # is not covered by any of the other sub-polygons 
+      stopped:
+        points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
+        linear_min: -1.0
+        linear_max: 1.0
+        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.
+
+.. note::
+  When velocity is covered by multiple sub polygons, the first sub polygon in the list will be used.
+
+**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. 
+
+.. image:: images/Collision_Monitor/holonomic_direction.png
+  :width: 365px
+
+Below shows some common configurations for holonomic robots that cover multiple directions of the resultant velocity:
+
+.. image:: images/Collision_Monitor/holonomic_examples.png
+  :height: 2880px
+
 Preparing Nav2 stack
 ====================