← All projects Munawar Kazmi
ROS2 · C++20 · Nav2 · CI-verified

Planning paths that survive a changing world

Two global planner plugins for ROS2 Nav2, written in modern C++20: classic A* and incremental D* Lite, benchmarked head-to-head on the event that matters - an obstacle lands on the route and the planner must recover.

View on GitHub Read the plain-language guide (PDF) Jump to results
4.3x
faster replanning with D* Lite (mean; 11x median), seed 42, 200 trials
0
path-cost mismatches across all 1,716 measured events - both planners provably optimal
185,237
fuzzed incremental replans validated against Dijkstra with exact equality

The problem

Warehouses, hospitals, and homes never hold still. A robot plans a route, someone steps into it, and the plan is suddenly wrong. Classic A* handles this by throwing everything away and searching again from scratch, which costs processing spikes exactly when smoothness matters most. D* Lite takes the other road: it repairs the existing plan incrementally, reusing everything the previous search already learned.

This project turns that textbook comparison into running code: a ROS-free C++20 planning core with Nav2 plugin adapters, and a benchmark that settles the question with numbers a stranger can reproduce - the per-event data, the seed, and the code that produced them are committed together, and CI re-runs the tests and a benchmark smoke pass on every push.

Getting D* Lite actually correct was the hard part: its priority keys routinely tie between vertices, and floating-point rounding of mathematically equal keys can bury a vertex the algorithm must expand, freezing stale state into the path. The fix is an exact integer cost metric - every g, rhs, and key an integer, every comparison exact - validated by 185,237 fuzzed incremental replans that must match a reference Dijkstra to the unit.

What the planner sees

A* global plan across the office map with its 125,760 expanded nodes shown as a blue cloud
A real A* run on the repository's 39.2 m × 58.4 m office map: 125,760 expanded nodes (blue cloud) on the way to a 919-cell path. Rendered from actual algorithm output by a committed script.
D* Lite repaired route bending around an obstacle that appeared on the original path
Mid-journey, an obstacle appears directly on the route. D* Lite repairs the same journey by expanding just 256 vertices - reusing everything from its previous search that is still valid. This event is what the benchmark measures.

The results

Histogram and bar chart of replan times: D* Lite 2.21 ms mean versus A* 9.42 ms mean over 1,493 replan events
1,493 replan events across 200 seeded trials, identical maps and obstacles for both planners, replanning work timed in isolation. D* Lite: 2.21 ms mean / 0.245 ms median. A*: 9.42 ms mean / 2.72 ms median. Every event asserts both planners returned equal-cost paths.
Log-log scatter of every replan event: D* Lite's cost stays in a narrow band while A*'s spans four orders of magnitude
Every event as a point. D* Lite's repair cost stays in a narrow band no matter how expensive the from-scratch search is; the events above the diagonal are the honest flip side - trivial replans where its bookkeeping loses.

What makes the comparison fair: both planners face byte-identical scenarios, obstacles are injected on the current path so the change always matters, measurement order alternates to cancel cache effects, and both planners optimize the same exact cost metric - so the zero cost-mismatch count means the race is between two provably optimal planners. The honest trade-off is stated with the win: D* Lite pays a ~2.7x costlier initial search and loses trivial replans; it dominates exactly where CPU spikes hurt a real robot.

Under the hood