Add AsymmetricInflationLayer costmap plugin for path-aware lane-keeping

[mbloechli]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#6109
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation (for the PR in Rolling) & Duatic Alpha (when I was developing in Jazzy)
Does this PR contain AI generated software?	Yes, it was proofread and corrected by hand
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

This contribution adds an Asymmetric Inflation Layer, that encourages lane-keeping behaviour without requiring routing. This works by subscribing to the plan topic -> classifying each lethal obstacle as left or right of the global path, then using asymmetric effective distances to bias the BFS priority queue.

Description of documentation updates required from your changes

The layer will need to be documented along the other layers on the nav2 wiki.

Description of how this change was tested

• I wrote integration and unit test to see if the layer behaves as expected
• I wrote a performance test to see how its runtime scales with map size
• Performed linting validation using pre-commit run --all and colcon test

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[SteveMacenski]

Quick initial architectural review:

Out of curiosity, does this share many of the same methods as the legacy layer? If so, can this derive from that layer to make use of the same implementations of those methods?

Does this do incremental updates like the inflation layer? The needs_reinflation_ implies so but I thought you mentioned it does it clean each iteration instead.

[mbloechli]

Out of curiosity, does this share many of the same methods as the legacy layer? If so, can this derive from that layer to make use of the same implementations of those methods?

Yes, this does indeed share some of the same methods with the legacy layer:

• distanceLookup() - same
• cellDistance() - same
• onFootprintChanged() - nearly the same, a small change in the debug message for both would make them the same
• updateBounds()- nearly the same, moving the robot_pose update to updateCosts() would make them the same

I'm not sure though if the additional coupling outweighs the inherited methods.

Does this do incremental updates like the inflation layer? The needs_reinflation_ implies so but I thought you mentioned it does it clean each iteration instead.

When I opened the feature request it didn't do incremental updates yet, because when I tried to implement it a month ago, I was having issues with artifacts in the costmap. Yesterday when I was making performance improvements to the updateCosts() and computeObstacleSide() functions, I reimplemented the incremental updates as well and saw now artifacts this time. Maybe I did an error when I tried to implement the iterative updates a month ago.

[mbloechli]

I've created a PR for docs.nav2.org, adding a basic description of the layer and it's parameters.

[SteveMacenski]

Rebase/pull in main to get CI to turn over

[SteveMacenski]

@tonynajjar please review since this relates to some of your recent work on this style of new inflation layer

[SteveMacenski]

Default 1 for non-multi-threaded by default

[mbloechli]

The inflation_layer uses -1 by default. Should I still change it to 1?

[SteveMacenski]

Yeah I think so

[tonynajjar]

@tonynajjar please review since this relates to some of your recent work on this style of new inflation layer

I'm a bit overwhelmed with other tasks currently to thoroughly review such an algorithm-heavy PR that we are not of immediate need for :/

[SteveMacenski]

Please pull in main / rebase to have CI turn over. I need to see the coverage metrics for the unit tests

[Copilot]

Pull request overview

Adds a new AsymmetricInflationLayer costmap plugin that biases inflation relative to the global path to encourage lane-keeping behavior, along with unit, integration, and benchmark coverage.

Changes:

• Adds the asymmetric inflation plugin implementation, header, build registration, and pluginlib metadata.
• Adds unit and integration tests for LUT generation, side classification, and basic layer behavior.
• Adds benchmark coverage comparing asymmetric inflation against existing inflation implementations.

Reviewed changes

Copilot reviewed 10 out of 10 changed files in this pull request and generated 5 comments.

Show a summary per file

File	Description
nav2_costmap_2d/CMakeLists.txt	Builds the new asymmetric inflation plugin source into the layers library.
nav2_costmap_2d/costmap_plugins.xml	Registers AsymmetricInflationLayer as a costmap plugin.
nav2_costmap_2d/include/nav2_costmap_2d/asymmetric_inflation_layer.hpp	Declares the new path-aware asymmetric inflation layer API and state.
nav2_costmap_2d/plugins/asymmetric_inflation_layer.cpp	Implements path subscription, side classification, asymmetric distance transform, and dynamic parameters.
nav2_costmap_2d/test/unit/CMakeLists.txt	Adds the asymmetric inflation unit test target.
nav2_costmap_2d/test/unit/asymmetric_inflation_layer_test.cpp	Tests disfavored LUT construction and obstacle side classification.
nav2_costmap_2d/test/integration/CMakeLists.txt	Adds the asymmetric inflation integration test target and launch registration.
nav2_costmap_2d/test/integration/asymmetric_inflation_tests.cpp	Tests standalone layer behavior with and without paths and interface conformance.
nav2_costmap_2d/test/benchmark/CMakeLists.txt	Adds the asymmetric inflation benchmark target.
nav2_costmap_2d/test/benchmark/asymmetric_layer_updatecosts_benchmark.cpp	Adds full-map and incremental benchmarks for asymmetric, current, and legacy inflation layers.

Comments suppressed due to low confidence (3)

nav2_costmap_2d/test/benchmark/asymmetric_layer_updatecosts_benchmark.cpp:864

• These registered benchmark cases allocate extremely large costmaps (up to 25,600 × 25,600 cells) and the update path also creates float distance maps, which can require multiple gigabytes of RAM per case and make the benchmark OOM on typical developer or CI machines. Please gate the extreme cases behind an opt-in flag or reduce the default registered sizes so the benchmark target remains runnable by default.

->Args({25600, 25600, 100, 0})  // 1280×1280 m map, ~5 m patch, full diagonal path
->Args({25600, 25600, 200, 0})  // 1280×1280 m map, ~10 m patch, full diagonal path
->Args({800, 800, 100, 20})  // 40×40 m map, ~5 m patch, fixed 20 m path
->Args({1600, 1600, 100, 20})  // 80×80 m map, ~5 m patch, fixed 20 m path
->Args({3200, 3200, 100, 20})  // 160×160 m map, ~5 m patch, fixed 20 m path
->Args({6400, 6400, 100, 20})  // 320×320 m map, ~5 m patch, fixed 20 m path
->Args({12800, 12800, 100, 20})  // 640×640 m map, ~5 m patch, fixed 20 m path
->Args({25600, 25600, 100, 20})  // 1280×1280 m map, ~5 m patch, fixed 20 m path
->Iterations(50)

nav2_costmap_2d/test/benchmark/asymmetric_layer_updatecosts_benchmark.cpp:480

• The asymmetric full-map benchmark registers the same 12,800 × 12,800 default case, but this fixture runs both the inherited inflation pass and the asymmetric overlay, multiplying the memory pressure from full-size distance maps. Please keep such extreme sizes behind an explicit opt-in instead of registering them by default.

->Args({3200, 3200})  // 160x160 m
->Args({6400, 6400})  // 320x320 m
->Args({12800, 12800})  // 640x640 m
->Iterations(50)

nav2_costmap_2d/test/benchmark/asymmetric_layer_updatecosts_benchmark.cpp:586

• The incremental baseline registrations include 25,600 × 25,600 maps by default. Even with a small update window, fixture setup allocates and pre-warms the full costmap, which can consume gigabytes and make the benchmark unusable without a very large machine; please make these stress cases opt-in.

->Args({12800, 12800, 100})  // 640×640 m map, ~5 m patch
->Args({12800, 12800, 200})  // 640×640 m map, ~10 m patch
->Args({25600, 25600, 100})  // 1280×1280 m map, ~5 m patch
->Args({25600, 25600, 200})  // 1280×1280 m map, ~10 m patch
->Iterations(50)

[SteveMacenski]

Use nav2 qos settings https://github.com/ros-navigation/navigation2/blob/main/nav2_ros_common/include/nav2_ros_common/qos_profiles.hpp#L30

[SteveMacenski]

Can this be set before L215, so that in the if (need_transform) bit the doTransform can be done as well so its in 1 block?

[SteveMacenski]

Can you use the time of the path instead so its synchronized?

[SteveMacenski]

Is p+1 always valid for every candidate? Do we know these are never the last in local_path_pts?

[SteveMacenski]

Suggested change

	if (!((local_path_pts.size() >= 2) &&
	(cost_scaling_factor_left_ != cost_scaling_factor_right_)))
	if (local_path_pts.size() < 2 ||
	cost_scaling_factor_left_ == cost_scaling_factor_right_)

Less convoluted for the same thing, no?

[SteveMacenski]

Something to double check: we do things like this for optimized collision checking: https://github.com/ros-navigation/navigation2/blob/855ba78e14c69839cb7a798ce2634293d5150dc0/nav2_mppi_controller/src/critics/cost_critic.cpp#L114C31-L114C42 does this return the most conservative cost at all times for the L/R scale factor parameters?