← All work 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 one event: 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 return optimal paths
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 whose 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 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 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 other side of the result: 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 optimal planners. The trade-off: D* Lite pays a ~2.7x costlier initial search and loses trivial replans; it wins where the from-scratch search is expensive, which is where a CPU spike costs a robot most.

Under the hood