navstack
a 2D autonomy stack, built from the math up

What it does

Seven subsystems, one integration state machine, and a hard rule about ground truth.

The rule that shapes everything

Ground truth lives in the simulator and nowhere else.

Mapping, localization, planning and control may only ever consume what a real robot would have: a LidarScan and a drifting OdomReading. Ground-truth maps are produced only by explicitly-named constructors that stamp a known_map provenance flag, so a demo running on a handed-to-it map cannot report itself as having mapped anything. The true pose appears in the live view only beneath a line that says diagnostics only.

That constraint is what makes the rest of the project mean anything. It is also why the odometry has to genuinely drift — if dead reckoning were good enough, the particle filter would be decoration.

Localization

A particle filter, and the two ways it fails.

A Kalman filter represents belief as a single Gaussian, so it cannot express “I am in one of four identical corridors” — the mean of two hypotheses is a pose inside a wall. A particle filter can, and pays for it with sampling failure modes instead. Both are demonstrated here rather than described.

Planning

A* wins at 2D. The interesting question is why you would ever use RRT.

Twenty randomized start/goal pairs per world. Every pair is verified connected by a flood fill before it is used, so a reported failure is a real planner failure and not an impossible instance — reporting success rates against unsolvable problems is the most common way planner benchmarks lie. Every returned path is re-checked for collisions before it counts.

So at 2D, A* simply wins. The reason to have RRT at all is dimensional: A* costs O(cells) and cells grow exponentially with degrees of freedom, so the ranking in that table inverts long before you reach a 6-DOF arm, where a grid is not implementable at all.

Replanning

A failed plan is a result, not a retry condition.

A box appears in the corridor. The robot sees it on LiDAR, the map updates, the current path stops being clear, and the executive replans around it. If the corridor is genuinely sealed it reports UNREACHABLE and stops — because replanning every control step against a closed corridor burns CPU, keeps commanding motion into the obstacle, and never tells anyone. An early version did exactly that: 2721 replans and 2644 collisions in one run.

There is also a RECOVERING state, which exists because pure pursuit only drives forwards. An obstacle appearing close ahead can leave the robot pressed against it inside the planner's inflation margin, facing the wrong way — a pose from which every valid plan begins with a manoeuvre the tracker cannot execute.

Bugs worth reading about

Found by tests and benchmarks. Each one has a regression test.

1. The textbook exact-integration formula annihilates its own answer. R·[sin(θ+φ) − sin θ] with R = v/ω loses about three decimal digits by φ = 1e-7, and by φ = 1e-8 the cosine difference underflows to exactly zero — the lateral displacement is not approximated, it is destroyed. Rewriting product-to-sum removes the cancellation and needs no small-angle branch at all.
2. A vectorized DDA is not Bresenham. Rounding disagrees with the error accumulator on roughly a quarter of integer lines. The accumulator turns out to have a closed form, ⌊(2k·d + n − 1)/2n⌋, whose − 1 is the tie-break. Verified byte-identical on 5000 random lines.
3. Path smoothing vetoed the wrong thing. Checking that a relaxed waypoint sits in free space says nothing about the leg joining it to its neighbour, which can sweep straight through a wall corner.
4. Sampled collision checks disagree with themselves. Splitting a segment changes which points get sampled, so a verified path fails after being densified without moving. Fixed with exact grid traversal — whose boundary crossings must be recomputed rather than accumulated, since the accumulated form drifts within a hundred cells.
5. The planner and its own replan trigger disagreed. A* snapped a blocked start out of the inflation margin; the clearance check did not. Every freshly planned path was therefore declared blocked the instant it was returned.
6. The odometry motion model cannot represent reverse motion. A backward step gives rot1 ≈ π, so the model believes the robot spun round and drove off, while the noise terms — which scale with rot1² — explode. The executive reverses on purpose during recovery, so every recovery destroyed the filter: 0.09 m → 11.2 m. It was first misdiagnosed as a tuning problem, and the wrong conclusion reached the docs before the real cause was found.
7. Zeroing odometry on collision is wrong twice over. It discards rotation that physically happened, and it hides wheel slip — one of the largest real sources of odometry error. A real encoder counts turns of the wheel, not displacement of the robot.

Run it

Clone to live demo in under ten minutes.

# the whole stack: unknown world, mapped from LiDAR, localized with MCL,
# explored autonomously, then navigated to a goal — on estimated state only
python3.11 -m venv .venv && .venv/bin/pip install -e ".[dev]"
.venv/bin/python demos/demo.py --world maps/house6.json --goal storage

# a maze it has never seen, and an ablation with the filter switched off
.venv/bin/python demos/demo.py --world maps/unseen_maze.txt
.venv/bin/python demos/demo.py --world maps/house6.json --known-pose

# reproduce every number and figure on this page
.venv/bin/python -m pytest
.venv/bin/python tools/benchmark.py
.venv/bin/python tools/plots.py

Source is not published yet — the repository link goes here.