RRT* Path Planner

One planner, any vehicle

RRTStar only ever calls abstract interfaces: a state space that samples, measures distance and steers between states, a collision checker, a cost function and a goal. A planning problem is those four plus a start state. Swapping the R² space for the Dubins space turns the point-robot planner into a car planner without touching the planner.

Adding a vehicle model means implementing sampling, distance, exact steering between two states, and a Euclidean embedding whose distances never exceed the true ones. Distance is allowed to be asymmetric, which a car needs: getting from A to B is not the same manoeuvre as getting from B to A. The planner handles that by drawing parent candidates from nodes that can reach the new state and rewiring candidates from nodes the new state can reach.

Exact neighbours under a strange metric

A KD-tree only understands Euclidean distance, and Dubins distance is not Euclidean. The embedding is what makes it work: since a Dubins path is never shorter than the straight line between its ends, the tree can search (x, y) with a conservative radius and then filter the candidates by true Dubins distance without missing any.

Nearest-neighbour queries now walk outward in increasing embedded distance and stop as soon as that distance passes the best exact distance found so far. The planner also gets one superset of candidates per iteration and filters it with the paths it builds anyway, so it no longer solves each Dubins connection twice. Together with a tighter Dubins solver, that took the cluttered benchmark from 2,500 to 7,000 iterations a second.

The phantom loop

The steering tests check that the distance to a point partway along an optimal path is just that part of the path. In 56% of random cases it came out 2πρ too long, as if the car drove an extra full circle. Three numerical edge cases at degenerate geometry caused it: a zero turn coming out as −1e-16 and wrapping to 2π, the direction of a zero-length straight being atan2(0, 0), and a tangency rounding to a negative square that ruled out the correct path.

RRT* connects to truncated steers constantly, so the bug was quietly distorting rewiring across the whole tree. After the fix the ρ = 0.5 car solved every benchmark scenario, up from 80–100%, and found its first solution 3–5× sooner.

Construction-site terrain

The terrain model is a grid of the surfaces a wheeled site vehicle actually meets, each with a cost per metre relative to asphalt: 1.2 for a compacted haul road, 3.0 for sand, 5.0 for mud, 6.0 for standing water. Trenches, stockpiles, fresh pours and structures are impassable. A seeded generator lays out road works, building, demolition, quarry and pipeline sites, so the planner can be tested on terrain that looks like the real thing, and the tests check it finds cheaper ground than the shortest path on every site type.

Benchmarks and the performance gate

The benchmark runs seeded narrow-passage, cluttered and maze scenarios, each confirmed solvable by a flood fill, and records every run’s full convergence history from inside the planner. In two seconds the point robot solves everything within 0.1% of the best known path; the ρ = 0.5 car solves everything within about 6% of it, which is roughly what the turning radius costs.

CI builds the pull request and its base on the same runner and alternates them case by case, five rounds each, pinned to one core; running the two suites separately showed nearly 2× differences for identical code. It fails on time, on deterministic work counts like steering solves per iteration, on final cost and on success rate. Injected regressions confirmed it: extra arithmetic in the collision check failed on time, and reverting the neighbour search failed on both work and time.

Python

pybind11 bindings expose the whole library and return paths and convergence data as numpy arrays. solve() releases the GIL, so the benchmark runs planners in parallel threads. Every interface can also be subclassed in Python and handed to the C++ planner, which is slower but makes it quick to prototype a new space, checker or cost.