diff --git a/configuration/packages/configuring-costmaps.rst b/configuration/packages/configuring-costmaps.rst index f5d9da4910..e5e68196ea 100644 --- a/configuration/packages/configuring-costmaps.rst +++ b/configuration/packages/configuring-costmaps.rst @@ -378,6 +378,7 @@ Plugin Parameters costmap-plugins/static.rst costmap-plugins/inflation.rst + costmap-plugins/inflation_legacy.rst costmap-plugins/obstacle.rst costmap-plugins/voxel.rst costmap-plugins/range.rst diff --git a/configuration/packages/costmap-plugins/inflation.rst b/configuration/packages/costmap-plugins/inflation.rst index 0ad9918cbb..51ee25edbe 100644 --- a/configuration/packages/costmap-plugins/inflation.rst +++ b/configuration/packages/costmap-plugins/inflation.rst @@ -64,3 +64,45 @@ This layer places an exponential decay functions around obstacles to increase co Description Whether to inflate unknown cells. + + +:````.num_threads: + + ==== ======= + Type Default + ---- ------- + int -1 + ==== ======= + + Description + Number of threads to use for inflation computation when OpenMP is enabled. + Set to ``-1`` for auto-detection (uses half of available CPU cores), or specify a positive integer for explicit thread count. + Ignored if OpenMP support is not available. + +Building with OpenMP +-------------------- + +OpenMP parallelization is disabled by default. To enable it, pass the ``ENABLE_OPENMP`` CMake option when building: + +.. code-block:: bash + + # Build with OpenMP enabled + colcon build --packages-select nav2_costmap_2d --cmake-args -DENABLE_OPENMP=ON + + # Build with OpenMP disabled (default) + colcon build --packages-select nav2_costmap_2d --cmake-args -DENABLE_OPENMP=OFF + +Performance Benchmarks +---------------------- + +**Test Configuration:** 2000×2000 Grid (4M cells, 50% occupancy, 2m inflation radius) + +**Robot Hardware:** 16 cores × 5000 MHz + ++---------------------+----------+------------------+---------------------------+ +| Configuration | Time | Throughput | vs Legacy Inflation Layer | ++=====================+==========+==================+===========================+ +| OpenMP disabled | 48.9 ms | 81.8 M cells/s | 2.1× faster | ++---------------------+----------+------------------+---------------------------+ +| OpenMP enabled | 9.11 ms | 468.9 M cells/s | 11.5× faster | ++---------------------+----------+------------------+---------------------------+ diff --git a/configuration/packages/costmap-plugins/inflation_legacy.rst b/configuration/packages/costmap-plugins/inflation_legacy.rst new file mode 100644 index 0000000000..5b5c7a1516 --- /dev/null +++ b/configuration/packages/costmap-plugins/inflation_legacy.rst @@ -0,0 +1,70 @@ +.. inflation: + +Legacy Inflation Layer Parameters +================================= + +.. warning:: + + This layer has been deprecated in favor of the refactored :doc:`Inflation Layer `, which offers significantly improved performance (up to 11.5× faster with OpenMP enabled). Consider migrating to the new implementation. + +This layer places an exponential decay functions around obstacles to increase cost to traverse near collision. It also places a lethal cost around obstacles within the robot's fully inscribed radius - even if a robot is non-circular for optimized first-order collision checking. + +```` is the corresponding plugin name selected for this type. + + +:````.enabled: + + ==== ======= + Type Default + ---- ------- + bool True + ==== ======= + + Description + Whether it is enabled. + +:````.inflation_radius: + + ====== ======= + Type Default + ------ ------- + double 0.55 + ====== ======= + + Description + Radius to inflate costmap around lethal obstacles. + +:````.cost_scaling_factor: + + ====== ======= + Type Default + ------ ------- + double 10.0 + ====== ======= + + Description + Exponential decay factor across inflation radius. + + +:````.inflate_unknown: + + ==== ======= + Type Default + ---- ------- + bool False + ==== ======= + + Description + Whether to inflate unknown cells as if lethal. + + +:````.inflate_around_unknown: + + ==== ======= + Type Default + ---- ------- + bool False + ==== ======= + + Description + Whether to inflate unknown cells. diff --git a/migration/Kilted.rst b/migration/Kilted.rst index 8dffb57e93..2bab238704 100644 --- a/migration/Kilted.rst +++ b/migration/Kilted.rst @@ -825,3 +825,11 @@ Additionally, warnings have been added to the parameter handler to alert users o - When using a constant ``lookahead_dist``, a warning is emitted if ``min_distance_to_obstacle`` exceeds ``lookahead_dist``. A new parameter ``allow_obstacle_checking_beyond_goal`` (default: false) has also been added. By default, obstacle checking along the projected trajectory stops at the goal position (end of the path). When enabled, collision checking continues past the goal up to ``min_distance_to_obstacle``, regardless of the remaining path length. This parameter requires ``use_velocity_scaled_lookahead_dist`` to be enabled and ``min_distance_to_obstacle`` > 0.0. + +Refactored Inflation layer powered by OpenMP +-------------------------------------------- + +`PR #5933 `_ refactors the Inflation layer to leverage OpenMP for parallel processing, significantly improving performance in large maps. + +The new implementation replaces the previous queue-based cell iteration with a Felzenszwalb-Huttenlocher distance transform algorithm. +When OpenMP is not available at compile time, the layer falls back to single-threaded operation. diff --git a/plugins/index.rst b/plugins/index.rst index 78f7cffa83..d8fa1e1480 100644 --- a/plugins/index.rst +++ b/plugins/index.rst @@ -51,7 +51,12 @@ Costmap Layers | | | occupancy information into | | | | costmap | +--------------------------------+------------------------+----------------------------------+ -| `Inflation Layer`_ | Eitan Marder-Eppstein | Inflates lethal obstacles in | +| `Inflation Layer`_ | Tony Najjar | Inflates lethal obstacles in | +| | | costmap with exponential decay | +| | | (with the option to use OpenMP | +| | | for parallelization) | ++--------------------------------+------------------------+----------------------------------+ +| `Legacy Inflation Layer`_ | Eitan Marder-Eppstein | Inflates lethal obstacles in | | | | costmap with exponential decay | +--------------------------------+------------------------+----------------------------------+ | `Obstacle Layer`_ | Eitan Marder-Eppstein | Maintains persistent 2D costmap | @@ -81,6 +86,7 @@ Costmap Layers .. _Static Layer: https://github.com/ros-navigation/navigation2/tree/main/nav2_costmap_2d/plugins/static_layer.cpp .. _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 .. _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