Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plugin container layer docs #615

Merged
1 change: 1 addition & 0 deletions configuration/packages/configuring-costmaps.rst
Original file line number Diff line number Diff line change
Expand Up @@ -349,6 +349,7 @@ Plugin Parameters
costmap-plugins/voxel.rst
costmap-plugins/range.rst
costmap-plugins/denoise.rst
costmap-plugins/plugin_container.rst

Costmap Filters Parameters
**************************
Expand Down
95 changes: 95 additions & 0 deletions configuration/packages/costmap-plugins/plugin_container.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
.. plugin_container:

Plugin Container Layer Parameters
=================================

This implements a costmap layer which combines costmap layers within a submap, which can then be integrated with other submaps in the same parent costmap. An example would be the use of different inflation layers for different sensors, objects, or static layers

``<plugin container layer>`` is the corresponding plugin name selected for this type.

:``<plugin container layer>``.enabled:

==== =======
Type Default
---- -------
bool True
==== =======

Description
Whether it is enabled.

:``<plugin container layer>``.plugins:

============== =======
Type Default
-------------- -------
vector<string> {}
============== =======

Description
List of mapped costmap layer names for parameter namespaces and names.

Note
Costmap filters are presently unsupported

alexanderjyuen marked this conversation as resolved.
Show resolved Hide resolved
Example
*******
.. code-block:: yaml

global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
robot_radius: 0.22
resolution: 0.05
track_unknown_space: true

plugins: ["plugin_container_static_layer", "plugin_container_obstacle_layer"]

plugin_container_static_layer:
plugin: "nav2_costmap_2d::PluginContainerLayer"
enabled: True
combination_method: 1
plugins: ["static_layer", "inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
Copy link
Contributor Author

Choose a reason for hiding this comment

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

@SteveMacenski if the plugin type is explicitly defined in each constituent layer, do I still need an parameter for plugin_types

Copy link
Member

@SteveMacenski SteveMacenski Dec 11, 2024

Choose a reason for hiding this comment

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

Its not defined in the layer as I mentioned. Your util that loads the plugin (https://github.com/ros-navigation/navigation2/blob/684a4d037241674550b756c5014711fa6c13e35e/nav2_util/include/nav2_util/node_utils.hpp#L136) itself defines that .plugin parameter so it can read the string of the plugin class in order to load it. That is a parameter that you have actually added by using get_plugin_type_param. Yes, this is important to add as a parameter in the parameter guide and in this example YAML

map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 2.0
inflation_radius: 0.5

plugin_container_obstacle_layer:
plugin: "nav2_costmap_2d::PluginContainerLayer"
enabled: True
combination_method: 1
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 1.0

The above example settings, when applied to the Nav2 turtlebot simulation would give the following costmap on startup

.. figure:: plugin_container_layer.png
:align: center
:alt: Costmap generated by plugin container layer

Costmap generated by turtlebot using example plugin container layer settings

Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
7 changes: 6 additions & 1 deletion migration/Jazzy.rst
Original file line number Diff line number Diff line change
Expand Up @@ -198,4 +198,9 @@ This can be useful, for example, in cases where you want to move the robot even

Default value:

- false
- false

New Plugin Container Layer
**************************

In `PR #4781 <https://github.com/ros-navigation/navigation2/pull/4781>`_ a costmap layer plugin type was added to support the grouping of different costmap layers under a single costmap layer. This would allow for different isolated combinations of costmap layers to be combined under one parent costmap instead of the current implementation which would indiscriminately combine all costmap layers together.
7 changes: 7 additions & 0 deletions plugins/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,12 @@ Costmap Layers
| | | standalone obstacles or small |
| | | obstacles groups |
+--------------------------------+------------------------+----------------------------------+
| `Plugin Container Layer`_ | Alexander Yuen | Combines the different costmap |
| | | layers specified under this |
| | | layer in order populate the same |
| | | costmap with different isolated |
| | | combinations of costmap layers |
+--------------------------------+------------------------+----------------------------------+

.. _Voxel Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/voxel_layer.cpp
.. _Static Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/static_layer.cpp
Expand All @@ -79,6 +85,7 @@ Costmap Layers
.. _Spatio-Temporal Voxel Layer: https://github.com/SteveMacenski/spatio_temporal_voxel_layer/
.. _Non-Persistent Voxel Layer: https://github.com/SteveMacenski/nonpersistent_voxel_layer
.. _Denoise Layer: https://github.com/ryzhikovas/navigation2/tree/feature-costmap2d-denoise/nav2_costmap_2d/plugins/denoise_layer.cpp
.. _Plugin Container Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/plugin_container_layer.cpp

Costmap Filters
===============
Expand Down
1 change: 1 addition & 0 deletions tuning/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,7 @@ Costmap2D has a number of plugins that you can use (including the availability f
- ``SpatioTemporalVoxelLayer``: 3D costmap layer for 3D lidars, non-planar 2D lidars, or depth camera processing based on temporal decay. Useful for robots with high sensor coverage like 3D lidars or many depth cameras at a reduced computational overhead due to lack of raycasting.
- ``RangeLayer``: Models sonars, IR sensors, or other range sensors for costmap inclusion
- ``DenoiseLayer``: Removes salt and pepper noise from final costmap in order to remove unfiltered noise. Also has the option to remove clusters of configurable size to remove effects of dynamic obstacles without temporal decay.
- ``PluginContainerLayer``: Combines the costmap layers specified within this plugin, resulting in an internal costmap that is a product of the costmap layers specified under this layer. This would allow different isolated combinations of costmap layers within the same parent costmap, such as applying a different inflation layers to static layers and obstacle layers

In addition, costmap filters:
- ``KeepoutFilter``: Marks keepout, higher weighted, or lower weighted zones in the costmap
Expand Down
Loading