Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
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
1 change: 1 addition & 0 deletions configuration/packages/configuring-costmaps.rst
Original file line number Diff line number Diff line change
Expand Up @@ -386,6 +386,7 @@ Plugin Parameters
costmap-plugins/static.rst
costmap-plugins/inflation.rst
costmap-plugins/inflation_legacy.rst
costmap-plugins/asymmetric_inflation.rst
costmap-plugins/obstacle.rst
costmap-plugins/voxel.rst
costmap-plugins/range.rst
Expand Down
126 changes: 126 additions & 0 deletions configuration/packages/costmap-plugins/asymmetric_inflation.rst
Comment thread
SteveMacenski marked this conversation as resolved.
Original file line number Diff line number Diff line change
@@ -0,0 +1,126 @@
.. asymmetric_inflation:

Asymmetric Inflation Layer Parameters
======================================

This layer implements an asymmetric variant of the inflation layer, by scaling inflation costs based on the planned path.

.. figure:: images/asymmetric_layer_active.png
:align: center
:alt: Costmap with asymmetric inflation layer

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


:``<asymmetric inflation layer>``.enabled:

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

Description
Whether it is enabled.

:``<asymmetric inflation layer>``.inflation_radius:

====== =======
Type Default
------ -------
double 2.0
====== =======

Description
Radius to inflate costmap around lethal obstacles.

:``<asymmetric inflation layer>``.cost_scaling_factor_left:

====== =======
Type Default
------ -------
double 4.0
====== =======

Description
Decay factor across inflation radius for obstacles on the left side of the path.

:``<asymmetric inflation layer>``.cost_scaling_factor_right:

====== =======
Type Default
------ -------
double 4.0
====== =======

Description
Decay factor across inflation radius for obstacles on the right side of the path.


:``<asymmetric inflation layer>``.inflate_around_unknown:

==== =======
Type Default
---- -------
bool False
==== =======

Description
Whether to treat unknown cells as lethal for inflation purposes.

:``<asymmetric inflation layer>``.plan_topic:

====== =======
Type Default
------ -------
string "plan"
====== =======

Description
Topic on which to receive the global path (``nav_msgs/msg/Path``).

:``<asymmetric inflation layer>``.goal_distance_threshold:

====== =======
Type Default
------ -------
double 1.5
====== =======

Description
Distance to the goal (m) below which asymmetric inflation is disabled.

This prevents oscillations when the robot is approaching the target pose.


Example
----------

.. code-block:: yaml

global_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 10.0
global_frame: map
robot_base_frame: base_link
footprint: "[ [0.525, 0.325], [0.525, -0.325], [-0.525, -0.325], [-0.525, 0.325] ]"
width: 10
height: 10
origin_x: -5.0
origin_y: -5.0
resolution: 0.05
track_unknown_space: false
plugins: ["static_layer", "asymmetric_inflation_layer"]
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
asymmetric_inflation_layer:
plugin: "nav2_costmap_2d::AsymmetricInflationLayer"
enabled: True
inflation_radius: 3.0
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.

Q: Should L/R have different radius parameter options as well?

cost_scaling_factor_left: 1.0
cost_scaling_factor_right: 4.0
inflate_around_unknown: False
plan_topic: "plan"
goal_distance_threshold: 1.5
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
11 changes: 11 additions & 0 deletions migration/Kilted.rst
Original file line number Diff line number Diff line change
Expand Up @@ -982,3 +982,14 @@ The goal is cosidered reached when one of the following conditions is met:
- The robot is within the coarse goal tolerance and it has passed the finish line (the line perpendicular to the first robot pose within the coarse tolerance and passing through the goal pose)

See :ref:`configuring_nav2_controller_adaptive_tolerance_goal_checker_plugin` for full details.

Asymmetric Inflation Field
--------------------------

`PR #6118 <https://github.com/ros-navigation/navigation2/pull/6118>`_ adds a new asymmetric inflation field.

The asymmetric inflation field allows the user to create an asymmetry that shifts the Voronoi border depending on the global path. This is useful for situations where the robot should prefer to imitate a keep-right or keep-left behavior, keeping enough space for another actor to pass by the robot without requiring an evasive maneuver.

.. figure:: images/asymmetric_layer_active.png
:align: center
:alt: Costmap with asymmetric inflation layer
Binary file added migration/images/asymmetric_layer_active.png
Comment thread
SteveMacenski marked this conversation as resolved.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
5 changes: 5 additions & 0 deletions plugins/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,10 @@ Costmap Layers
| `Legacy Inflation Layer`_ | Eitan Marder-Eppstein | Inflates lethal obstacles in |
| | | costmap with exponential decay |
+--------------------------------+------------------------+----------------------------------+
| `Asymmetric Inflation Layer`_ | Marc Blöchlinger | Uses the global plan to |
| | | asymmetrically inflate lethal |
| | | obstacles depending on path side |
+--------------------------------+------------------------+----------------------------------+
| `Obstacle Layer`_ | Eitan Marder-Eppstein | Maintains persistent 2D costmap |
| | | from 2D laser scans with |
| | | raycasting to clear free space |
Expand Down Expand Up @@ -87,6 +91,7 @@ Costmap Layers
.. _Range Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/range_sensor_layer.cpp
.. _Inflation Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/inflation_layer.cpp
.. _Legacy Inflation Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/legacy_inflation_layer.cpp
.. _Asymmetric Inflation Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/asymmetric_inflation_layer.cpp
.. _Obstacle Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/obstacle_layer.cpp
.. _Spatio-Temporal Voxel Layer: https://github.com/SteveMacenski/spatio_temporal_voxel_layer/
.. _Non-Persistent Voxel Layer: https://github.com/SteveMacenski/nonpersistent_voxel_layer
Expand Down
Loading