Update documentation with SpeedFilter

configuration/packages/configuring-controller-server.rst

@@ -134,6 +134,17 @@ Parameters
     The controller server filters the velocity portion of the odometry messages received before sending them to the controller plugin.
     Odometry values below this threshold (in rad/s) will be set to 0.0.
 
+:speed_limit_topic:
+
+  ============== =============================
+  Type           Default
+  -------------- -----------------------------
+  string         "speed_limit"
+  ============== =============================
+
+  Description
+    Speed limiting topic name to subscribe. This could be published by Speed Filter (please refer to :ref:`speed_filter` configuration page). You can also use this without the Speed Filter as well if you provide an external server to publish `these messages <https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/SpeedLimit.msg>`_.
+
 Provided Plugins
 ****************
  The plugins listed below are inside the ``nav2_controller`` namespace.

configuration/packages/configuring-costmaps.rst

@@ -329,6 +329,7 @@ Costmap Filters Parameters
   :maxdepth: 1
 
   costmap-plugins/keepout_filter.rst
+  costmap-plugins/speed_filter.rst
 
 Example
 *******

configuration/packages/configuring-map-server.rst

@@ -97,15 +97,15 @@ Costmap Filter Info Server Parameters
     Type of costmap filter used. This is an enum for the type of filter this should be interpreted as. We provide the following pre-defined types:
 
     - 0: keepout zones / preferred lanes filter
-    - 1: speed filter, speed limit is specified in m/s
-    - 2: speed filter, speed limit is specified in % of maximum speed
+    - 1: speed filter, speed limit is specified in % of maximum speed
+    - 2: speed filter, speed limit is specified in absolute value (not implemented yet)
 
 :filter_info_topic:
 
   ============== =============================
   Type           Default
   -------------- -----------------------------
-  string         costmap_filter_info
+  string         "costmap_filter_info"
   ============== =============================
 
   Description
@@ -116,7 +116,7 @@ Costmap Filter Info Server Parameters
   ============== =============================
   Type           Default
   -------------- -----------------------------
-  string         filter_mask
+  string         "filter_mask"
   ============== =============================
 
   Description

configuration/packages/costmap-plugins/keepout_filter.rst

@@ -3,6 +3,8 @@
 Keepout Filter Parameters
 =========================
 
+Keepout Filter - is a Costmap Filter that enforces robot to avoid keepout areas or stay on preferred lanes, by updating corresponding costmap layer using filter mask information.
+
 `<filter name>`: is the corresponding plugin name selected for this type.
 
 :``<filter name>``.enabled:
@@ -25,15 +27,43 @@ Keepout Filter Parameters
   ====== =======
 
   Description
-    Name of the CostmapFilterInfo topic having filter-related information.
+    Name of the incoming `CostmapFilterInfo <https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/CostmapFilterInfo.msg>`_ topic having filter-related information. Published by Costmap Filter Info Server along with filter mask topic. For more details about Map and Costmap Filter Info servers configuration please refer to the :ref:`configuring_map_server` configuration page.
 
 :``<filter name>``.transform_tolerance:
 
   ====== =======
   Type   Default
   ------ -------
-  double 0.0
+  double 0.1
   ====== =======
 
   Description
     Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. Used when filter mask and current costmap layer are in different frames.
+
+Example
+*******
+.. code-block:: yaml
+
+    global_costmap:
+      global_costmap:
+        ros__parameters:
+          ...
+          plugins: ["static_layer", "obstacle_layer", "inflation_layer", "keepout_filter"]
+          ...
+          keepout_filter:
+            plugin: "nav2_costmap_2d::KeepoutFilter"
+            enabled: True
+            filter_info_topic: "/costmap_filter_info"
+            transform_tolerance: 0.1
+    ...
+    local_costmap:
+      local_costmap:
+        ros__parameters:
+          ...
+          plugins: ["voxel_layer", "inflation_layer", "keepout_filter"]
+          ...
+          keepout_filter:
+            plugin: "nav2_costmap_2d::KeepoutFilter"
+            enabled: True
+            filter_info_topic: "/costmap_filter_info"
+            transform_tolerance: 0.1

configuration/packages/costmap-plugins/speed_filter.rst

@@ -0,0 +1,90 @@
+.. _speed_filter:
+
+Speed Filter Parameters
+=======================
+
+Speed Filter - is a Costmap Filter that restricting maximum velocity of robot. The areas where robot should slow down and values of maximum allowed velocities are encoded at filter mask. Filter mask published by Map Server, goes in a pair with filter info topic published by Costmap Filter Info Server. Speed Filter itself publishes a speed restricting messages which are targeted for a Controller in order to make the robot to not exceed the required velocity.
+
+.. raw:: html
+
+    <h1 align="center">
+      <div style="position: relative; padding-bottom: 0%; overflow: hidden; max-width: 100%; height: auto;">
+        <iframe width="800" height="450" src="https://www.youtube.com/embed/pq0r0lqi0Sc?autoplay=1" frameborder="1" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
+      </div>
+    </h1>
+
+`<filter name>`: is the corresponding plugin name selected for this type.
+
+:``<filter name>``.enabled:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  bool   True
+  ====== =======
+
+  Description
+    Whether it is enabled.
+
+:``<filter name>``.filter_info_topic:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  string N/A
+  ====== =======
+
+  Description
+    Name of the incoming `CostmapFilterInfo <https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/CostmapFilterInfo.msg>`_ topic having filter-related information. Published by Costmap Filter Info Server along with filter mask topic. For more details about Map and Costmap Filter Info servers configuration please refer to the :ref:`configuring_map_server` configuration page.
+
+:``<filter name>``.speed_limit_topic:
+
+  ====== =============
+  Type   Default
+  ------ -------------
+  string "speed_limit"
+  ====== =============
+
+  Description
+    Topic to publish speed limit to. The `messages <https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/SpeedLimit.msg>`_ have the following fields' meaning:
+
+    - ``percentage``: speed limit is expressed in percentage if ``true`` or in absolute values in ``false`` case. This parameter is set depending on ``type`` field of ``CostmapFilterInfo`` message.
+
+      Note
+        Speed limit in absolute values is not supported yet.
+
+    - ``speed_limit``: non-zero values show maximum allowed speed expressed in a percent of maximum robot speed or in absolute value depending on ``percentage`` value. Zero value means no speed restriction (independently on ``percentage``). ``speed_limit`` is being linearly converted from ``OccupancyGrid`` filter mask value as: ``speed_limit = base + multiplier * mask_value``, where ``base`` and ``multiplier`` coefficients are taken from ``CostmapFilterInfo`` message.
+
+      Note
+        ``speed_limit`` expressed in a percent should belong to ``(0.0 .. 100.0]`` range.
+
+    This topic will be used by a Controller Server. Please refer to :ref:`configuring_controller_server` configuration page to set it appropriately.
+
+
+:``<filter name>``.transform_tolerance:
+
+  ====== =======
+  Type   Default
+  ------ -------
+  double 0.1
+  ====== =======
+
+  Description
+    Time with which to post-date the transform that is published, to indicate that this transform is valid into the future. Used when filter mask and current costmap layer are in different frames.
+
+Example
+*******
+.. code-block:: yaml
+
+    global_costmap:
+      global_costmap:
+        ros__parameters:
+        ...
+        plugins: ["static_layer", "obstacle_layer", "inflation_layer", "speed_filter"]
+        ...
+        speed_filter:
+          plugin: "nav2_costmap_2d::SpeedFilter"
+          enabled: True
+          filter_info_topic: "/costmap_filter_info"
+          speed_limit_topic: "/speed_limit"
+          transform_tolerance: 0.1

migration/Foxy.rst

@@ -78,6 +78,7 @@ Architecturally, costmap filters consists from ``CostmapFilter`` class which is
 - Preferred lanes in industries. This plugin is covered by ``KeepoutFilter`` (see discussion in `corresponding PR <https://github.com/ros-planning/navigation2/issues/1522>`_ for more details).
 
 Each costmap filter subscribes to filter info topic (publishing by `Costmap Filter Info Publisher Server <https://github.com/ros-planning/navigation2/tree/main/nav2_map_server/src/costmap_filter_info>`_) having all necessary information for loaded costmap filter and filter mask topic.
+``SpeedFilter`` additionally publishes maximum speed restricting `messages <https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/SpeedLimit.msg>`_ targeted for a Controller to enforce robot won't exceed given limit.
 
 High-level design of this concept could be found `here <https://github.com/ros-planning/navigation2/tree/main/doc/design/CostmapFilters_design.pdf>`_. The functionality of costmap filters is being disscussed in `the ticket #1263 <https://github.com/ros-planning/navigation2/issues/1263>`_ and carried out by `PR #1882 <https://github.com/ros-planning/navigation2/pull/1882>`_. The following tutorial: :ref:`navigation2_with_keepout_filter` will help to easily get involved with ``KeepoutFilter`` functionality.
 
@@ -107,4 +108,4 @@ To follow the SI units outlined in REP-103 to the "T" nodes below were modified
 - smac planner
  - ``max_planning_time_ms`` became ``max_planning_time`` in seconds
 - map saver
- - ``save_map_timeout`` in seconds
+ - ``save_map_timeout`` in seconds

plugins/index.rst

@@ -63,8 +63,12 @@ Costmap Filters
 | `Keepout Filter`_  | Alexey Merzlyakov  | Maintains keep-out/safety zones   |
 |                    |                    | and preferred lanes for moving    |
 +--------------------+--------------------+-----------------------------------+
+| `Speed Filter`_    | Alexey Merzlyakov  | Limits maximum velocity of robot  |
+|                    |                    | in speed restriction areas        |
++--------------------+--------------------+-----------------------------------+
 
 .. _Keepout Filter: https://github.com/ros-planning/navigation2/tree/main/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
+.. _Speed Filter: https://github.com/ros-planning/navigation2/tree/main/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp
 
 Controllers
 ===========