Delaunay Edge Selection

The Problem

The failures were real but slippery. The node is memoryless, so a bad frame is not sticky: failures came in bursts of roughly 0.4 to 0.8 seconds, four to eight frames, and then fixed themselves. That is long enough for the controller to act on a wrong path and short enough to be hard to catch.

The only diagnostic was a log line, throttled to once a second. At 10 Hz that is one report per ten frames, which is coarser than the failures themselves. I could watch the car misbehave in the simulator and have no idea which code path had produced the path.

Instrumentation Added

Before touching any logic, I made the node explain itself. The selection runs as a cascade of six attempts, each looser than the last, stopping at the first one that finds any gate at all:

I published which attempt won on every frame, and every candidate edge each attempt had considered, on its own topic. I also made the debug colours distinguish chain steps properly, since the bags had been recorded with more steps than there were colours, so step 3 was being drawn identically to step 0.

Alongside that I built a replay harness: feed a recorded bag back through a freshly launched node, record every output topic into a new file, and save the exact parameters and git commit next to it. Runs are timestamped and never overwrite each other, so any result can be re-examined later against the config that produced it.

Measured Behaviour

On 345 frames of recorded cone data, the strict attempt handled 78% of frames and the rest fell through to looser ones, including two frames that reached the last-resort attempt. The second attempt never fired once: by the time the angle limit fails, the width limit has already failed too, so a whole rung of the cascade was dead weight.

It also showed how much the old visualisation had been hiding: 84% of candidate edges were legal at more than one step of the chain but had only ever been drawn at the first one.

The Problem

With the instrumentation in place, the failures turned out to share a theme: the filters assume the track runs roughly straight past the car.

The angle filter punishes real corners. In a turn the true gate is diagonal, because cones on the inside of the corner are spaced more tightly than those on the outside. Measured on real frames, genuine gates in turns sit at around 38° against a 40° rejection threshold, while a wrong edge running along one boundary passes at 47°. Tightening the angle limit punishes the true gate and the false one about equally. Angle is not what separates them.

The straddle test used the wrong reference. The check for whether an edge crosses the track measured from the car’s own origin at every step of the chain, rather than from the previous gate’s midpoint. Several steps into a chain, the car’s origin is the wrong place to be asking the question from. On one burst of frames, a wrong edge qualified by four to nine centimetres.

The last-resort attempt gives up the one rule that matters. Attempt six drops the straddle requirement entirely, so it will happily join two cones on the same boundary and put the “midpoint” on the edge of the track rather than the middle. That is where the worst paths came from.

The Forward-Crossing Invariant

The fix that mattered most was a rule that is obvious once stated: the midline should never enter and leave a gate from the same side. If the chain approaches a gate from one side, the next midpoint has to be on the other side of it. Any candidate that would double the path back on itself is disqualified outright.

It is implemented as a sign comparison across the previous gate’s line, which makes it orientation-free: it does not care which way the gate’s normal happens to point, so it survives the case where the usual “forward” heuristic flips past 90° of turn.

I also promoted the straddle requirement from a hardcoded condition to a parameter so it could be measured rather than argued about.

Filtering the Triangulation Itself

The other half of the fix happens before selection even starts. Triangulating a set of cones produces triangles that have nothing to do with the track: long slivers between distant cones, and exterior triangles that close the hull along the outside of a boundary rather than spanning the corridor. Those triangles contribute edges that then have to be rejected one at a time by the selection filters, which is where the near-misses come from.

So I added a quality filter on the triangles: a minimum area, plus unconditional rejection of obtuse triangles. A sliver cannot be part of a sensible gate pair, and rejecting it early removes a whole class of candidates rather than arguing with them downstream.

Measured Effect of Each Fix

Measured over 345 frames of replay, counting frames where the planned path contains an implausible kink:

The triangulation filter then took the last one: it changes selection on only four frames out of 345, and takes kinked frames from five to one. Cheapest win of the whole project, and the one piece of this I wrote before any of the analysis.

Why Soft Penalties Cannot Substitute

The obvious alternative is to keep the bad edges legal and just make them expensive. I tried it and it cannot work in this architecture. Raising the same-side penalty by a factor of 200, from 0.1 to 20, moved the number of kinked frames from 330 to 331.

The reason is structural: the penalty is a constant added to every same-side candidate equally, so it shifts all of their scores by the same amount and never changes their ranking against each other. A soft rejection that does not vary with how wrong the candidate is cannot discriminate, no matter how you tune it.

The Problem

Gate width and angle are continuous, but the solver treated them as pass/fail against a threshold that changed in steps between attempts. A gate 1.01 m from the ideal width was rejected while one at 0.99 m sailed through, and the only way to express “slightly worse” was to drop to a whole looser attempt.

I did not want to throw the cascade away, since it is a reasonable safety ladder. I wanted the ranking within each attempt to reflect how good a candidate actually is.

What I Added

Continuous cost terms alongside the existing filters, each with a weight and an exponent so the shape can be tuned rather than just the magnitude:

The third one was my idea and it is worth dwelling on, because it half failed. The thought was that a gate which bends the midline sharply is suspicious: some turning is expected, since that is what tracks do, but a ridiculous dogleg usually means a false cone or a skipped gate. So I scored the angle between the path segment and the two gates it connects.

It is sign-blind by construction. A path that reverses back down the corridor crosses both gates perfectly square and therefore scores the best possible value. Sweeping its weight upward produced paths with 177° kinks, which is to say paths that double back. Two attempted rescues, a flat penalty on backward motion and a tangent-continuity term, both measured as useless, and I deleted them rather than defend them.

The term earns its place only in combination with the forward-crossing rule, which supplies the sign information it lacks. On its own it is a liability.

A Cost Ceiling on Selection

The cascade’s success test was “at least one gate,” which means a terrible gate from a strict attempt beats three good ones from a looser attempt. So I added a cost ceiling: if the best candidate scores worse than the ceiling, move on to the next attempt, and if the last attempt still cannot beat it, publish nothing at all.

The thresholded quantity is deliberately not the ranking score. Distance-based terms are subtracted out first, so a gate is not punished for being far away, only for being a bad gate.

The Problem

Every fix so far improves one decision at a time. The solver still picked the best gate at step zero, then the best gate after that, and so on, with no ability to notice that a locally attractive first gate leads nowhere good.

In the simulator that is exactly the failure I kept seeing: coming out of a turn, the first gate the car picks is sometimes a pair of same-coloured cones on the outside of the corner, running parallel to the track instead of across it. It corrects a frame or two later, but by then the controller has already acted on it.

Chain Generation and Objective

So I made it generate several complete chains from different first gates and compare them as whole paths. The scoring problem is that a longer chain always costs more in total, and averaging is worse, since one cheap gate gives a great average and no reason to continue. What I wanted was to reward reaching further with gates that are individually good:

The credit is literally “the worst a gate may be and still earn its place”: a gate is worth adding if and only if its cost is below the credit. The horizon term pays for lookahead the chain failed to deliver.

Diversity is deliberately limited to the first gate. The failure mode is a bad opening move, and alternatives that differ only deep in the chain cost compute without changing much. First-gate variants are allowed to share cones, because two first gates that differ by one cone can diverge a long way downstream.

The defaults are set so that one root candidate reproduces the old greedy behaviour exactly. I verified that: with the search enabled but set to a single root, the output is byte-identical to the previous code across all 345 frames. Nobody has to trust the new path to run the node.

Adaptive Search Budget

Two chains are almost always enough, but some frames need five. A fixed budget is wasteful or inadequate. So the search stops generating alternatives once a chain is good enough, measured by gate count, while always rolling out at least two, and crucially it still compares every chain it has already generated. Early exit can never discard a winner.

That cut mean roots per frame from 4.99 to 2.75, a 45% reduction in search, and chains came out longer on average, not shorter.

Latency Analysis

I wanted to know whether any of this fits the budget, and the first budget figure I was given did not have a source, so I went and found one: the controller runs at 10 Hz, so 100 ms is the number that binds, with the cone stream arriving at about 9.3 Hz and the system watchdog at 2.5 s as the hard fault.

Against that, the search is not the problem. With debug publishing off, five roots cost 0.30 ms at the median and 1.08 ms at p95 for the solver, well under 1% of the budget. The surprise was that debug publishing costs more than the search does, because recording every attempt forces all six attempts to run even after one has won.

A useful way to make latency legible: at 10 m/s, 10 ms of planning delay is 10 cm of travel. Two milliseconds is two centimetres.

Rationale

Cones are blue on one side of the track and yellow on the other, so a gate joining two cones of the same colour is definitionally wrong. We do not yet get reliable colours from perception, so the node runs colourless today. But when colours arrive, the same algorithm gets strictly better, and in the meantime we will want to run both ways for testing.

So I built the node to accept either input: an explicit mode selects between the colourless and coloured message types, with a shared code path underneath. The alternative on the table was a small bridge node to convert between message types, which I rejected as the kind of thing that stays in the system forever.

Implementation

I kept the colour rule a hard filter rather than another weighted cost: reject an edge only when both endpoints have the same known colour. Blue with unknown, orange with anything, and unknown with unknown all survive. Applied once, immediately after triangulation, since colour does not depend on chain state.

It also closes the worst failure mode: with colours on, even the last-resort attempt can no longer join two cones on the same boundary.

One scoping decision I am glad I made: this node identifies no colours of its own and I left it that way. It does edge selection. Having it also guess at cone colours would have blurred what it is responsible for.

The interesting part is which half breaks. The planner holds up at both speeds: it keeps finding the corridor and the car gets around the track either way. What changes is how well the car holds the line it was given, and at 10 m/s it does not, so it clips cones through the corners it took cleanly at 6.

That is a controls problem sitting downstream of a path planning problem, and seeing the two fail separately is most of why I want to work on both. Giving the controller a corridor it can trust is only useful up to the point where the controller can follow it, and there is clearly work left on each side of that handoff.