Feature/smac incremental obstacle heuristic

[Functionhx]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	( #5770)
Primary OS tested on	(Ubuntu24.04 Rolling)
Robotic platform tested on	(Gazebo + TB3)
Does this PR contain AI generated software?	(Yes and it is marked inline in the code)
Was this PR description generated by AI software?	NO

---

Description of contribution in a few bullet points

• Adds an opt-in parameter incremental_obstacle_heuristic (default: false) for SmacPlannerHybrid and SmacPlannerLattice
• When enabled, the goal-rooted obstacle heuristic field is kept between replannings of the same goal and repaired incrementally (LPA*) for only the costmap cells that changed, instead of being wiped and recomputed from scratch
• Produces the same heuristic as a full recompute (bit-identical), but is much cheaper to update when the costmap changes little between plans (dynamic replanning)
• Off by default; the existing lazy obstacle-heuristic path is unchanged. SmacPlanner2D is not affected (it does not use the obstacle heuristic)
• When enabled it forces downsample_obstacle_heuristic off, since the incremental field is kept at full resolution

Description of documentation updates required from your changes

• New parameter incremental_obstacle_heuristic needs to be added to the docs.nav2.org configuration and tuning pages
• The nav2_smac_planner README is already updated in this PR

Description of how this change was tested

• Added a unit test comparing the incremental field cell-by-cell to an independent Dijkstra reference: identical result (0 mismatch), and about 13x faster to update on a small change
• Full colcon test for nav2_smac_planner passes: 359 tests, 0 failures (no regression)
• Linting passes (cpplint / uncrustify)
• Verified in a full headless Gazebo (gz) + TurtleBot3 stack (ROS 2 Rolling): SmacPlannerHybrid with incremental_obstacle_heuristic enabled planned and the robot navigated to the goal end-to-end (result: SUCCEEDED) with the MPPI controller

---

Future work that may be required in bullet points

• Support the downsampled heuristic (this version runs at full resolution only)

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

[mergify]

This pull request is in conflict. Could you fix it @Functionhx?

[codecov]

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
..._planner/include/nav2_smac_planner/a_star_impl.hpp	95.32% <100.00%> (+0.20%)	⬆️
...r/include/nav2_smac_planner/obstacle_heuristic.hpp	100.00% <100.00%> (ø)
...ude/nav2_smac_planner/smac_planner_hybrid_impl.hpp	90.02% <100.00%> (+0.27%)	⬆️
...de/nav2_smac_planner/smac_planner_lattice_impl.hpp	90.35% <100.00%> (+0.30%)	⬆️
...2_smac_planner/include/nav2_smac_planner/types.hpp	100.00% <ø> (+5.26%)	⬆️
nav2_smac_planner/src/obstacle_heuristic.cpp	98.86% <100.00%> (+1.11%)	⬆️

... and 38 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

I think a little more description and design intent is good here. This is a big change without much architecture or intent discussion (and looks rather AI generated with markers for it in all key algorithmic code). I don't actually see any changes using LPA* for the planning in the path planner for incremental path planning; only adjustments to the obstacle heuristic. I'm also not seeing any performance metrics about the improvements and otherwise the planners continued correctness.

How does this perform to keep the initial plan in the same area and incrementally replan the parts required only? The obstacle heuristic is perhaps an important component of this, but that is not itself the planner, that is in the a_star file. Speeding up the heuristic computation is of course a good direction to go as well and has really nice performance improvements as long as it is still verifiably correct. If that is the case, then I'm not sure why we would ever not do the incremental heuristic field calculation, assuming if the costmap changes greatly it still does the proper job.

A quick review on with Claude (which I don't fall back on with much certainty) also points to some technical issues as well with the implementation, but I'm more concerned to start off with architectural and higher level alignment before getting into the details.

I suppose a good closing question: how much design effort or algorithmic analysis did you put into this - or did you point an AI at this and largely just go with it?

[Functionhx]

Thanks for your review,I maybe mixed two different ideas together in my earlier comment on #6110.Precisely, this PR not incremental re-planning but incremental heuristic.

The design path was roughly this: I first explored incremental replanning as a standalone D Lite planner in #6110,I found it would be better integrate it in Smac planner,When I try to do it,I found some hard issues:

• 1.D* Lite needs each node's predecessors. Hybrid-A* stores a continuous pose inside each bin, so predecessors aren't well-defined. A lattice planner would be a cleaner fit.
• 2.Hybrid-A* jumps to the goal with an analytic Reeds-Shepp/Dubins shortcut, which doesn't fit a tree that grows backward from the goal
• 3.Each motion sweeps many cells, so one costmap change can invalidate many edges at once. If too many change, incremental repair isn't cheaper than a full replan.

Because of that, I narrowed this PR to the obstacle heuristic only: a smaller, self-contained optimization that can be verified independently.

[Functionhx]

For this PR, the evidence I have so far is at the heuristic level:

• the incremental field is bit-identical to a full recompute in the test case;
• it also matches an independent plain Dijkstra result cell-by-cell;
• the heuristic update is about ~13x faster in the unit-test case;
• the existing test suite still passes with no regressions.
  But I agree that this is not enough evidence for the planner-level behavior.I still need to add end-to-end Smac planning-time benchmarks.
  I will add those before asking for deeper review.

[Functionhx]

About opt-in:
The current obstacle heuristic is lazy / on-demand, so it only computes what the current search needs.The incremental version has to maintain a full field for a goal, so for a one-shot plan or a goal change it can be more expensive.It should mainly help when repeatedly replanning to the same goal with small costmap changes.For big change, it can approach the cost of a full recompute.
And,downsampling is currently disabled in incremental mode,is to keep the v1 change-detection logic simple with a 1:1 costmap-cell mapping; I think it can be re-enabled with more work, but I did not want to mix that into v1.

So the intent is an opt-in optimization for repeated replanning to the same goal with relatively small costmap changes.

[Functionhx]

About AI：
Yes，I used AI(DeepSeek V4 Flash & Opus) heavily, both to write the code and to work through the analysis.I marked that inline.But the direction and the design decisions are mine.

[SteveMacenski]

1.D* Lite needs each node's predecessors. Hybrid-A* stores a continuous pose inside each bin, so predecessors aren't well-defined. A lattice planner would be a cleaner fit.

Each bin is mapped to a pose, but you have all the predecessor bins and poses available, no? State Lattice does the same.

2.Hybrid-A* jumps to the goal with an analytic Reeds-Shepp/Dubins shortcut, which doesn't fit a tree that grows backward from the goal

This process however connects those bins to fill in the path, so from the outside perspective, this looks the same as if it had searched to find these

3.Each motion sweeps many cells, so one costmap change can invalidate many edges at once. If too many change, incremental repair isn't cheaper than a full replan.

That's only true of State Lattice. Hybrid-A* nominally does ~1-2 cells but may be larger depending on some esoteric settings. But the 'too many changing' is definitely something and I would think the incremental planning methods would handle that or we should detect that to replan more fully in which case. I'm sure issues like that aren't unique to us and there's advice/methods regarding it.

Less that replanning, then an incremental heuristic generation would just be a run-time performance improvement. That's certainly not nothing though! We'd need some more evidence though that this actually works beyond a couple of unit test cases & understand if/when this breaks down if the costmap changes "too much". If it degrades into a full update in a heavy change situation, that's basically perfect so that we don't have any edge cases we need to calculate to know if we can incrementally update or fully recompute. Otherwise, we'd need to know what case we should fall back to the full update case.

The current obstacle heuristic is lazy / on-demand, so it only computes what the current search needs.The incremental version has to maintain a full field for a goal, so for a one-shot plan or a goal change it can be more expensive.It should mainly help when repeatedly replanning to the same goal with small costmap changes.For big change, it can approach the cost of a full recompute.

This is a good point for us to document and be aware of. Benchmarking this would be good to understand the performance changes. If the first plan takes ~2 seconds for a 1,000,000 sqft warehouse (rather than ~800ms) due to this but every other plan to that goal is guaranteed to be incrementally computed and makes the planning ~10x faster, I think that's a trade off many (though not all) would make. Definitely a useful feature to have around!

[Functionhx]

Thanks, that makes sense.

Your points about Hybrid-A* predecessors, analytic expansion, and swept cells make sense. Those are things I had not considered carefully enough before.

I’ll focus this PR on proving whether the incremental obstacle heuristic is useful as a runtime optimization.

For True incremental replanning, I’d like to explore it separately later