Add AsymmetricInflationLayer costmap plugin for path-aware lane-keeping by mbloechli · Pull Request #6118 · ros-navigation/navigation2

mbloechli · 2026-04-29T19:55:22Z

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-*.

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

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.

SteveMacenski · 2026-04-29T22:02:20Z

@@ -0,0 +1,807 @@
+// Copyright (c) 2026


You should add yourself if all novel, else the original inflation layer + you

Done. I've additionally now changed the license of the asymmetric_inflation_layer from apache 2.0 to BSD, because the legacy_inflation_layer was licensed with BSD

SteveMacenski · 2026-04-29T22:04:44Z

+    cost_scaling_factor_ = node->declare_or_get_parameter(
+      name_ + "." + "cost_scaling_factor", 10.0);
+    asymmetry_factor_ = node->declare_or_get_parameter(
+      name_ + "." + "asymmetry_factor", 0.75);


Please explain the asymmetry_factor and neutral_threshold parameters

asymmetry_factor

It's a value between 0 and 1, that influences how quickly the wave in BFS travels. When expanding the BFS the lowest distance cells get expanded first. When calculating distance of a cell to it's nearest lethal_cell the distance is scaled by std::max(0.0, 1.0 - asymmetry_factor_ * path_side), where path side is either 1 (left), -1 (right), 0 (neutral)

In short: with the asymmetry_factor the user can change on which side of the path the robot should be incentivized to drive through:

asymmetry_factor < 0: drive on the left side of the road

asymmetry_factor = 0: drive in the middle of the road

asymmetry_factor > 0: drive on the right side of the road

neutral_threshold

Any lethal_cell further away from the path than the neutal_threshold will not be inflated.

SteveMacenski · 2026-04-29T22:10:45Z

+  // Sanity check: bins must be empty at the start of each cycle.
+  // On failure, log a single summary (avoid per-cell log flood).
+  size_t leftover = 0;
+  for (const auto & bin : inflation_cells_) {
+    leftover += bin.size();
+  }
+  RCLCPP_FATAL_EXPRESSION(
+    logger_, leftover != 0,
+    "AsymmetricInflationLayer: BFS bins not empty at start of updateCosts() "
+    "(%zu leftover cells)", leftover);


When could this happen and could we add protections then instead? I'm not sure off hand the size of inflation_cells_ but this "it shouldn't happen" log seems like it may take some computation time. Could we make sure things are correct instead?

This is a log I copied over from legacy_inflation_layer. This log helped me find bugs while I was still developing the asymmetric layer. I didn't delete the logger after finishing the implementation of it, because the log still exists in the legacy_inflation_layer. Do you know, what this log still exists in the legacy_inflation_layer?

SteveMacenski · 2026-04-29T22:11:12Z

+  if (seen_.size() != size_x * size_y) {
+    RCLCPP_WARN(
+      logger_, "AsymmetricInflationLayer::updateCosts(): seen_ vector size is wrong");
+    seen_ = std::vector<bool>(size_x * size_y, false);
+    obstacle_side_grid_ = std::vector<int8_t>(size_x * size_y, 0);
+  }


Shouldn't match size handle this? This seems like an odd place to handle resizing (or that its a WARNING). Costmaps do change size sometimes and that's expected (matchSize)

Yes, I kept this here, because it also exists in legacy_inflation_layer

SteveMacenski · 2026-04-29T22:14:14Z

+  double bucket_size = std::max(neutral_threshold_, 1.0);
+  std::unordered_map<uint64_t, std::vector<size_t>> spatial_hash;
+
+  if (use_asymmetry) {


I think here and below is all only if use_asymmetry is true. You could check if its false when its computed and just setCurrent(true); return; at that point to unnest this.

SteveMacenski · 2026-04-29T22:16:29Z

+
+  // Spatial Hash Grid Construction
+  double bucket_size = std::max(neutral_threshold_, 1.0);
+  std::unordered_map<uint64_t, std::vector<size_t>> spatial_hash;


This first for loop could probably use a comment to explain what its doing and the hash map. I'm also not 100% sure if this is really the most efficient way to do this, but I'm also not 100% understanding what this is doing from an initial quick glance (i.e. neutral cost, hash table window size of min/maxs)

SteveMacenski · 2026-04-29T22:21:20Z

+    }
+  }
+
+  // The standard Nav2 inflation_layer has already populated master_array


I'm stopping here because I think this method already above is a bit convoluted and I'm not following it already without more comments / explanations. I'd rather read in detail once I understand what the intent is.

Above I'm seeing a for loop for each path point in some window iteratively, then a full grid window iteration, and finally the actual while loop (which can't be avoided). Populating a window-sized data structure and iterating over it again is not very efficient, I wouldn't think.

mbloechli · 2026-04-30T16:12:03Z

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.

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

…egacy_inflation_layer, of which I copied a lot of Signed-off-by: mbloechli <mbloechlinger@duatic.com>

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

mbloechli added 6 commits April 29, 2026 21:58

ported over to rolling

9b68ffc

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

improved efficiency of getObstacleSide

f9f8db3

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

update bounds iteratively

d75c323

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

renamed test file, added correct license headers

a563203

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

fix linting errors

fcbb4b3

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

please pre-commit

6265c9c

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

mbloechli force-pushed the feat/asymmetric_inflation_layer branch from 669704c to 6265c9c Compare April 29, 2026 19:58

revert unintended changes to nav2_bringup

81bbdc4

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

SteveMacenski reviewed Apr 29, 2026

View reviewed changes

mbloechli added 3 commits April 30, 2026 20:39

added benchmarking for incremental updates

b186319

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

updated license headers with my full name and authors who wrote the l…

0c67430

…egacy_inflation_layer, of which I copied a lot of Signed-off-by: mbloechli <mbloechlinger@duatic.com>

unnest if(use_asymmetry) statement

f296fa8

Signed-off-by: mbloechli <mbloechlinger@duatic.com>

Conversation

mbloechli commented Apr 29, 2026

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

mbloechli Apr 30, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

mbloechli Apr 30, 2026

Choose a reason for hiding this comment

asymmetry_factor

neutral_threshold

Uh oh!

SteveMacenski Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

mbloechli Apr 30, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Apr 29, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mbloechli Apr 30, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Apr 29, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mbloechli Apr 30, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Apr 29, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mbloechli commented Apr 30, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

mbloechli Apr 30, 2026 •

edited

Loading

mbloechli Apr 30, 2026 •

edited

Loading

SteveMacenski Apr 29, 2026 •

edited

Loading

SteveMacenski Apr 29, 2026 •

edited

Loading

SteveMacenski Apr 29, 2026 •

edited

Loading