# Navigation

*[Docs index](index.md) · [Control](control.md) · [Mission planning](mission-planning.md)*

How an Ackermann orchard robot lays out a field and plans its way round it: what
it assumes, what it computes, what it refuses to do, and what broke on the way.
Following the resulting path is [control](control.md); deciding which row is next
is [mission planning](mission-planning.md).

Everything below is implemented in `ros2_ws/src/kraken_nav`. Nothing here is
aspirational — where a number appears it was either measured off the level, read
off a run, or asserted in `test/test_headland.cpp`.

---

## 1. The job

An orchard is not a warehouse. The free space is a comb: long parallel aisles
with almost no lateral room, joined at each end by a strip of open ground called
the **headland**. The work happens *in* the aisles; the headland exists only so
the machine can get from one aisle to the next.

That geometry breaks the usual assumption of a navigation stack. Sending a goal
pose at the far end of a row and letting a search planner work it out is fine
for one row. It is not fine for a field, because:

- **The order matters.** The machine cannot turn into the neighbouring aisle, so
  "do them left to right" is not drivable.
- **The turn matters more than the row.** Driving 44 m down a corridor is easy.
  Reversing direction in 10 m of headland with a 2.6 m turning circle and a
  3.1 m body is the entire problem.
- **Stopping is a cost.** A machine that halts at the end of every row to think
  about the next one loses a large fraction of the day to standing still.

So the mission is a first-class part of the navigation stack rather than a
script sitting above it — see [mission planning](mission-planning.md).

---

## 2. Conventions

### Frames

| Frame | Meaning |
| --- | --- |
| `map` | The world. In this simulator the map frame and the world frame coincide, so prefab coordinates are map coordinates. |
| `<ns>/odom` | Smooth, drifting, continuous. The local costmap lives here. |
| `<ns>/base_link` | The **rear axle**, not the centre of the machine. |

Frames are namespaced per robot; `map` is shared. The launch file rewrites frame
names *by value* rather than by key, because `global_frame` means the odom frame
in the local costmap and the map frame in the global one.

### The footprint

Measured off the prefab's colliders:

```
footprint: [ [2.5, 0.45], [2.5, -0.45], [-0.6, -0.45], [-0.6, 0.45] ]
```

The box is **not symmetric about the origin**. `base_link` is the rear axle, so
the body runs 0.6 m behind it and 2.5 m ahead. This is the single most
load-bearing convention in the document: every clearance figure is a statement
about where the *front bumper* ends up, and the front bumper is 2.5 m from the
point the kinematics are written about.

- inscribed radius 0.45 m
- circumscribed radius 2.55 m
- overall 3.1 m long, 0.9 m wide

### The row frame

The mission does not think in map coordinates. It anchors a frame at wherever
the machine is standing when the goal arrives, oriented along the rows, and
describes the whole field in it. For a heading $\theta$ with
$c = \cos\theta$, $s = \sin\theta$, a map displacement $(dx, dy)$ resolves as

$$
\text{along} \;=\; c\,dx + s\,dy,
\qquad
\text{across} \;=\; -s\,dx + c\,dy .
$$

and the inverse, used to turn a row-frame target back into a goal pose:

$$
x \;=\; x_0 + c\,\text{along} - s\,\text{across},
\qquad
y \;=\; y_0 + s\,\text{along} + c\,\text{across}.
$$

**Sign convention:** positive `across` is to the machine's **left**. Positive
`along` is **forward**, i.e. into the row. A turn onto a row at
$\text{across} = +7$ is a left turn.

This is why the action takes `row_near_x`, `row_far_x` and `row_heading_deg`
rather than a list of poses: the field is described the way an orchard is laid
out — a count of aisles and their spacing — and pinned to the machine's actual
position at the moment of the goal.

> **Set the band from the trees, not from the block.** The surveyed row runs
> y 42 (mouth) to −1 (end), but the trees do not fill it. Measured with the
> trunk detector while driving line1 — dense detections every ~3 m from **y +35
> down to y +1** — there are 7 m of clear ground inside the mouth and 2 m inside
> the far end. A band of `row_near_x: 3.0, row_far_x: 43.0` therefore stops 4 m
> short of the first tree at one end and 2 m past the last tree at the other,
> which is the asymmetry seen on a real run. `row_near_x: 5.0` makes both ends
> 2 m clear of the outermost trees.

### The field, as surveyed

Read directly from `Project/Levels/Main/Main.prefab`: eighteen `RowSpawnPoint`
entities, all at world $y = 42$, at

$$
x_k = -63.25 + 3.5\,k, \qquad k = 0 \ldots 17
$$

Rows run along world $-y$; the machine spawns facing $-90°$. The drivable band
inside a row is 3.0 m to 43.0 m from the mouth. The headland at the mouth
measures 9–12 m.

---

## 3. The machine

Nothing here reaches the wheels directly. The controller and behaviour servers
publish to `cmd_vel_raw`, and `safety_monitor` is what turns that into
`cmd_vel`, so a lost filter can stop the machine and a recovery behaviour cannot
step around the stop. See
[localisation](localisation.md#7-stopping-when-the-pose-is-not-trustworthy).

A bicycle model is enough. With wheelbase $L$ and steering angle $\delta$, the
path curvature is

$$
\kappa \;=\; \frac{\tan\delta}{L},
\qquad
R \;=\; \frac{L}{\tan\delta}.
$$

At full lock $\delta_{\max} = 0.7\ \text{rad}$ and $L = 2.2\ \text{m}$:

$$
R_{\min} \;=\; \frac{2.2}{\tan 0.7} \;=\; 2.611932\ \text{m}.
$$

The controller emits `Twist` and clamps its curvature to $1/R_{\min}$; see
[control §5](control.md#5-from-curvature-to-steering) for what converts that
into steering on each simulator.

**The number that shapes everything:** the tightest circle the machine can
drive has diameter $2R_{\min} = 5.22\ \text{m}$, and the aisles are 3.5 m
apart. It physically cannot turn into the next aisle.

---

## 4. Laying out the field

### The skip rule

To reverse direction and land a row $d$ metres across, the machine needs
$d \ge 2R_{\min}$ for a plain U-turn. So it does not do adjacent aisles in
sequence; it skips

$$
\text{skip} \;=\; \left\lceil \frac{2R_{\min}}{p} \right\rceil
\;=\; \left\lceil \frac{5.223864}{3.5} \right\rceil
\;=\; \left\lceil 1.4925 \right\rceil
\;=\; 2 .
$$

and comes back for the ones it missed.

### The order

With a skip of $s$ over $n$ aisles, the order is built as one pass per
remainder class modulo $s$, with **odd passes reversed**:

```
for start in 0 .. s-1:
    pass = [start, start+s, start+2s, ...]
    if start is odd: reverse(pass)
    order += pass
```

For $n = 18$, $s = 2$:

```
0 2 4 6 8 10 12 14 16 | 17 15 13 11 9 7 5 3 1
```

Every consecutive pair is $2p = 7.0$ m apart **except one** — the join between
passes, aisles 16 and 17, which are neighbours at 3.5 m. Reversing the odd
passes is what buys that: without it, the join would be a full-field traverse
from aisle 16 back to aisle 1.

So the field needs exactly one turn tighter than a U-turn, no matter how many
aisles there are. That is a useful property, because that one turn is the
expensive case.

![Coverage order over 18 aisles](figures/coverage_order.svg)

---

## 5. Turning at the headland

### What a turn has to satisfy

A candidate manoeuvre is described as a list of constant-curvature segments,

$$
\text{Segment} = (\kappa,\; \ell,\; \text{reverse}),
$$

integrated with a midpoint rule at step $h$ (0.05 m):

$$
\theta_{\text{mid}} = \theta_n + \tfrac{1}{2}\,\Delta s\,\kappa,
\qquad
\begin{aligned}
x_{n+1} &= x_n + \Delta s\cos\theta_{\text{mid}} \\
y_{n+1} &= y_n + \Delta s\sin\theta_{\text{mid}} \\
\theta_{n+1} &= \theta_n + \Delta s\,\kappa
\end{aligned}
$$

with $\Delta s < 0$ on reverse segments. Two quantities are then computed.

**Reach** — how far up the headland the *bodywork* gets, not the axle. Over
every traced pose and every footprint corner $(f_x, f_y)$:

$$
\text{reach} \;=\; \max_{n}\ \max_{(f_x,f_y)}
\Big( x_n + f_x\cos\theta_n - f_y\sin\theta_n \Big).
$$

Because the front bumper is 2.5 m ahead of the axle, reach typically exceeds
the axle's own excursion by around 2.5 m. Planning on the axle would put the
front of the machine into the trees.

**Room** — how much headland there actually is:

$$
\text{room} \;=\; \text{depth} - \text{entry} - \text{clearance},
$$

with `entry` = 0.8 m (driven straight out of the row before any steering, so
the rear of the machine is clear of the trunks before it starts to swing) and
`clearance` = 1.0 m of margin. `depth` is either configured or measured — see
§6.

A candidate is acceptable when

$$
\boxed{\ \text{reach} \;\le\; \text{room}\ }
$$

**and** its swept footprint is collision-free (§6).

### The ladder

Three manoeuvres are tried in order of how pleasant they are to drive. Within
each, radii run from roomiest to tightest and the first acceptable one wins.

Let $o$ be the lateral offset, $R$ the radius, $\sigma = \operatorname{sign}(o)$.

**U-turn** — two quarter circles joined by a straight. Requires
$|o| \ge 2R$.

$$
\left(\tfrac{\sigma}{R},\ \tfrac{\pi R}{2}\right),\quad
\left(0,\ |o| - 2R\right),\quad
\left(\tfrac{\sigma}{R},\ \tfrac{\pi R}{2}\right)
$$

**Bulb turn** — for $|o| < 2R$, where a U-turn cannot reach. The machine first
steers *away* from the target row to buy lateral room, then loops back through
more than a half circle. With

$$
\gamma = \arccos\!\left(\frac{|o|}{2R}\right),
$$

the segments are

$$
\left(-\tfrac{\sigma}{R},\ R\gamma\right),\qquad
\left(\tfrac{\sigma}{R},\ R(\pi + \gamma)\right).
$$

Net heading change $-\gamma + (\pi + \gamma) = \pi$, as required. This costs
depth — the bulb reaches much further up the headland than a U-turn — but it is
the only forward-only way to change lanes by less than a turning diameter.

**Three-point (fishtail)** — when even the bulb will not fit. Three arcs, all
turning the machine the same way, the middle one **in reverse**:

$$
\left(\tfrac{\sigma}{R},\ R\alpha\right),\quad
\left(-\tfrac{\sigma}{R},\ R\beta,\ \text{reverse}\right),\quad
\left(\tfrac{\sigma}{R},\ R\gamma'\right),
\qquad \gamma' = \pi - \alpha - \beta .
$$

The steering is set the *other* way for the reverse arc precisely so that all
three contribute the same sign of heading change: on a reverse segment
$\Delta s < 0$, so $\Delta\theta = \kappa\,\Delta s = \sigma\beta$ is positive
for a negative curvature. Total heading change is
$\sigma(\alpha + \beta + \gamma') = \sigma\pi$.

$\alpha$ and $\beta$ trade against each other, so the pair is searched: for each
$\alpha$ on a 0.1 rad grid, the $\beta$ that lands the target row is found by
bracketing and 24 rounds of bisection. Of the solutions that fit,
**the one with the longest first arc wins** — not the shallowest. The shallowest
is always the degenerate manoeuvre that barely turns before backing up, and it
lands the row a metre out if the machine did not start exactly where it thought.
Spending spare headland on a balanced manoeuvre buys that back.

### Why not the tightest radius?

Each manoeuvre is tried from a **roomy** radius downwards:

$$
R_{\text{roomy}} = 1.35\,R_{\min} = 3.526\ \text{m}.
$$

A turn driven at full lock has nothing left to correct with. Every trim the
controller can apply only *widens* the arc, so the first centimetre of drift is
unrecoverable and the manoeuvre becomes a one-way bet on the arithmetic.
Backing off by a third leaves about a fifth of the steering in hand, which is
exactly what the tracker trims with.

This is not a refinement. Driven at full lock, the bulb turn ran 0.6 m wide and
had to be abandoned mid-manoeuvre.

![The three turns](figures/turns.svg)

*Grey outlines are the footprint swept along the manoeuvre; green is the rear
axle. The dotted line is reach, the dashed red line is the headland boundary.
Note how much further the bodywork gets than the axle, and how much more depth
the bulb needs than the U-turn for a smaller lateral move.*

### Reference values

Asserted in `test/test_headland.cpp` and re-asserted by the figure generator, at
$R_{\min} = 2.611932031$:

| offset (m) | room (m) | choice | R (m) | reach (m) | length (m) |
| ---: | ---: | :--- | ---: | ---: | ---: |
| 7.0 | 6.4 | u-turn | 3.500000 | 4.674579 | 10.995574 |
| 7.0 | 17.2 | u-turn | 3.500000 | 4.674579 | 10.995574 |
| 7.0 | 4.2 | u-turn | 2.900000 | 4.179916 | 10.310619 |
| 7.0 | 3.2 | *none* | | | |
| 3.5 | 17.4 | bulb | 3.526108 | 10.819108 | 18.492770 |
| 3.5 | 8.2 | bulb | 2.676108 | 8.052069 | 12.999565 |
| 3.5 | 4.2 | 3-point | 3.526108 | 4.100218 | 11.077596 |
| −7.0 | 6.4 | u-turn | 3.500000 | 4.674579 | 10.995574 |

Two things to read out of that table. A deeper headland changes nothing for a
U-turn — the ladder does not spend room it does not need. And the bulb at 3.5 m
across needs **more than twice** the depth of a U-turn at 7.0 m across; moving a
short distance sideways is the expensive case, not the long one.

---

## 6. Refusing a turn the machine cannot sweep

This is the part that a simulator taught, and it is the most important paragraph
in the document.

The planner measures available depth by ray-marching the costmap along the
machine's current heading, sampling three lateral offsets
$\{-w/2,\ 0,\ +w/2\}$, and stopping at the first cell that is at or above
`INSCRIBED_INFLATED_OBSTACLE`, or at the edge of the grid. **Unknown counts as
blocked**, which matters: an earlier version treated unknown as free, called the
probe from 12 m back where the headland was entirely unobserved, got an
optimistic answer and drove into the trees.

But fixing that was not enough, because:

> **A depth probe taken along the heading cannot certify a manoeuvre that leaves
> that heading.**

The probe answers "how far can I go *straight*". The manoeuvre immediately stops
going straight. Seven consecutive turns got away with the discrepancy. The
eighth did not.

So every candidate the ladder proposes is now swept before it is accepted. The
planner passes an acceptance predicate down into the geometry:

```cpp
using Accept = std::function<bool (const std::vector<Segment> &)>;
```

and the predicate traces the whole manoeuvre — entry straight included — at
0.05 m, transforms each pose into the map, and evaluates the **full padded
footprint** at that pose:

$$
\begin{pmatrix} p_x \\ p_y \end{pmatrix}
=
\begin{pmatrix} s_x \\ s_y \end{pmatrix}
+
\begin{pmatrix} c & -s \\ s & c \end{pmatrix}
\begin{pmatrix} x_n \\ y_n \end{pmatrix},
\qquad
\text{cost} = \text{footprintCostAtPose}(p_x, p_y, \theta + \theta_n).
$$

Any pose costing `LETHAL_OBSTACLE` (254) rejects the candidate and the ladder
moves on to the next radius, then the next manoeuvre. The costmap mutex is a
`std::recursive_mutex` and is held once across the whole search rather than
retaken per query.

If the ladder exhausts itself, the planner throws
`nav2_core::NoValidPathCouldBeFound` with the measured depth, the offset, how
many candidates were rejected, how far the best one got, the offending map
coordinate and cost, **and the cost of the machine's own footprint where it is
standing** — because a lethal reading on open headland means the costmap is
lying, and that is a different bug from a geometry that does not fit.

### What it caught

The eighth leg of an eighteen-aisle run, turning from aisle 14 to aisle 16:

```
no turn onto a row 6.999997 m across fits in it, not even a three-point one.
37 candidates hit something, the one that got furthest costs 254 at
(-8.689694, 43.530440), 9.500000 m into the manoeuvre;
the machine's own footprint here costs 0
```

Read that carefully, because every clause is doing work.

- **The straight-line probe said 25 m.** Directly ahead of the machine the
  headland is completely open — the ray ran to its ceiling without touching
  anything.
- **The obstruction is at $x = -8.69$**, which is not on an aisle centreline
  ($x_{15} = -10.75$, $x_{16} = -7.25$) but almost exactly on the **trunk line
  between them**, at $y = 43.5$ — 1.5 m *past* the row mouth. That row of trees
  runs further north than the row the machine was sitting in.
- **It is hit 9.5 m into an 11.8 m manoeuvre**, i.e. in the second quarter of
  the U-turn, when the machine is travelling sideways relative to the direction
  the probe looked.
- **The machine's own footprint costs 0**, so this is real geometry, not a
  costmap artefact.

A probe along the heading could not have found this obstacle and no amount of
tuning its length or its lateral samples would have. The swept check found it on
the first attempt, and refused — where the previous version drove into it.

### The open problem

Refusing correctly is necessary but not sufficient. Having refused, the machine
still has to get out of a row whose headland is obstructed on the side it wants
to go. As of the last run it does not reliably manage that: the leg is written
off and the following legs inherit an awkward pose, and coverage stops at 7 of
18. **This is the live piece of work.** The fix in
[mission planning §5](mission-planning.md#5-the-two-forcesuccess-decorators)
removes one cause (a silently swallowed failure); whether the search-planner
fallback is enough to recover from a genuinely blocked headland is not yet
demonstrated.

### The controller judges by the nose, not the axle

The same argument applies while driving, not just while planning. The path is
drawn for the rear axle, so the axle can sit on it while the front does not:
heading error swings the nose out by `front_overhang * sin(drift)`, which is
0.43 m at ten degrees on a 2.5 m overhang. At the 2.61 m axle radius the outer
front corner already sweeps 3.95 m, 1.34 m wider, for a swept width of 1.79 m in
a 3.5 m aisle.

So `max_cross_track` is applied to `noseOffset(across, drift, front_overhang)`
rather than to `across`. It is a free function with its own tests because the
sign is the whole point — reversed, it reports the nose swinging *in* when it
swings *out*, which reads as a machine tracking better under heading error
rather than worse. Reversing it fails four of the five tests.

One case is deliberately not treated as worse than the axle: off to one side
while pointing back the other is a machine recovering, its nose already over the
path, and aborting that would stop runs that were about to succeed.

This is **not** front-axle tracking, and the distinction matters. Tracking the
front against a plan drawn for the rear would have the machine faithfully
following a curve nobody planned; doing it properly means changing the planner
too, and re-deriving a correction law whose sign already cost 0.77 m of drift
once (§2).

---

## 7. The geofence

An autonomous machine in a real orchard must not leave the block, whatever the
planner thinks. This is a policy boundary, not a perception result, so it is a
**costmap filter** rather than a costmap layer — filters exist precisely for
constraints that come from outside the sensor stream.

`nav2_costmap_2d::KeepoutFilter` on **both** costmaps:

```yaml
filters: ["keepout_filter"]
keepout_filter:
  plugin: "nav2_costmap_2d::KeepoutFilter"
  enabled: true
  filter_info_topic: costmap_filter_info
```

Both matters. On the global costmap it stops the planner ever proposing a path
that leaves the block; on the local costmap it stops the controller executing
one if it somehow gets one.

The mask is derived from the survey rather than drawn by hand
(`maps/make_keepout_mask.py`), so it cannot drift out of step with the field:

$$
x \in [x_0 - \tfrac{p}{2},\ x_{17} + \tfrac{p}{2}] = [-65.0,\ -2.0],
\qquad
y \in [y_{\text{far}} - H,\ y_{\text{mouth}} + H] = [-11.0,\ 52.0]
$$

with headland allowance $H = 10$ m — the working minimum, since a U-turn onto a
row two across reaches 5.5 m past the row end and the machine is 3.1 m long.
The result is a 226 × 226 grid at 0.5 m, 51 kB, black outside the block and
white inside, with a 25 m lethal border so the boundary is real rather than an
edge-of-map artefact.

Serving it needs two more lifecycle nodes, and **they must come up before the
costmaps**, which will not activate while waiting for filter info:

```python
SERVERS = ['filter_mask_server', 'costmap_filter_info_server',
           'controller_server', 'planner_server', 'behavior_server', 'bt_navigator']
```

> **Trap.** `filter_info_topic` is resolved relative to the *costmap* node, which
> lives one level below the robot namespace — left relative it becomes
> `/<ns>/global_costmap/costmap_filter_info` and never connects. The launch file
> absolutises it alongside the observation topics, for the same reason.

The manager runs with `autostart: False`, and `nav_bringup` starts it instead.
Nav2's own autostart begins transitioning as soon as it starts; it waits for
each server's transition service to be *discovered* — `nav2_util`'s service
client loops on `wait_for_service` indefinitely — but the reply to
`change_state` has a finite timeout, and a server discovered while still
constructing can miss it. One failed transition abandons the whole bringup,
leaving a stack that looks alive and aborts every goal instantly.

That was previously papered over with an eight-second timer before Nav2 started.
A duration cannot express "ready", so `nav_bringup` requires every server to
*answer* `get_state` before asking the manager to start anything, then confirms
`is_active`. Measured against O3DE it reports active in 4.7 s, and it is the
same `is_active` that scripts should gate on rather than sleeping:

```bash
ros2 service call /kraken1/lifecycle_manager_navigation/is_active std_srvs/srv/Trigger
```

### Slowing at the row ends

The ends of the rows open onto the headlands, which is where people and vehicles
cross, and the machine cannot see round a row of trees. Slowing has to happen
*before* it arrives, which makes it a property of the map rather than of
anything the sensors can report — so it is a second mask, served by a second
`map_server` and `costmap_filter_info_server` pair. A filter info message names
exactly one mask, and the type differs: `0` is a keepout, `1` is a speed limit
in percent.

`SpeedFilter` runs on the **global costmap only**. Two of them publish two
limits on one topic and the controller obeys whichever arrived last.

The band is 8 m of approach inside the row and 2 m past the end — not the whole
headland. Carrying it further would slow the turn itself, and `ArcTracker` has
already dropped to `turn_linear_vel` by then. The two multiply:

```
wanted = |curvature| > straight_curvature ? turn_linear_vel : desired_linear_vel;
wanted *= speed_fraction_;
```

which took a 0.4 m/s turn down to 0.16 before the band was trimmed.

> **Trap.** The mask value is *not* the speed percent. nav2 rescales a
> scale-mode mask between its thresholds,
> `value = round(1 + 98 * (occ - free) / (occupied - free))`, so a band written
> as 40 comes back as 45. `make_speed_mask.py` solves for the pixel instead of
> assuming it, and the formula is calibrated against two live readings — pixel
> 153 read 45, pixel 158 read 41.

> **Trap.** `speed_limit_topic` must be absolutised. Left relative it resolves
> against the *costmap* node, `/<ns>/global_costmap/speed_limit`, while the
> controller listens on `/<ns>/speed_limit`. Nothing errors: the servers
> activate, the filter runs, and no limit ever arrives.

Neither mask is derived from anything the machine sees. Both read the block
geometry from `maps/orchard_survey.py`, which is a transcription of
`Main.prefab` — `CoverRows` takes aisles, pitch and row extents as goal fields,
so the mission is parameterised while the masks are not.

### The trees are in the mask too

The global costmap is 100 m wide with `track_unknown_space: false`, and the
lidar marks to 20 m. Everything beyond that reads as **free**, so the planner —
`allow_unknown: true` — will draw a straight line diagonally across fifteen rows
it has never seen. That was observed on a real run.

The rows are map knowledge, not a perception result, so the mask is where they
belong. `tree_lines()` marks a 0.5 m band on each of the seventeen lines between
the eighteen aisles, from the row mouth to the row end only — the headland is
where the machine turns round and there are no trunks in it.

> **Trap.** `FIRST_ROW_X + k * ROW_PITCH` is an **aisle centre**, not a trunk
> line. It is where `RowSpawnPoint` puts the machine. Trees are on the
> midpoints. Marking the survey values themselves would make every aisle a
> keepout zone. Measured off the costmap after driving a row, lethal cells peak
> at −61.5, −58.0, −54.5, −51.0 and trough at −63.25, −59.75, −56.25, −52.75.

The mask resolution is 0.25 m rather than 0.5 for this: tree lines fall on cell
boundaries, so both neighbouring cells qualify and the band comes out one cell
wider than the half-width suggests. At 0.5 m that made a 1.0 m band and left
1.40 m to drive in — against a machine 0.9 m wide, 0.25 m of clearance a side.
At 0.25 m the band is 0.5 m, the aisle keeps 3.0 m, and inflation leaves the
1.9 m free band this file is tuned around. A mask cannot represent a feature
narrower than twice its cell.

Checked on the generated mask rather than reasoned about: all eighteen aisle
centres free at y = 5, 20 and 41; all seventeen tree lines blocked at y = 20;
every tree-line x free at y = −5, +45 and +50; and a diagonal across six aisles
blocked at 58 of 401 samples.

---

## 8. Results

Single runs against O3DE with real trees and real physics. The harness is not
reproducible to the decimal place — the simulator steps on a wall-clock timer
while everything else consumes its output over DDS — so read the order of
magnitude, not the second digit.

| Run | Aisles | Covered | Worst cross-track |
| --- | ---: | ---: | --- |
| C++ navigator, 4 aisles | 4 | **4** | 0.03, 0.10, 0.09, 0.11 m |
| C++ navigator, 4 aisles, geofenced | 4 | 3 | 0.06, 0.06, 0.11 m |
| C++ navigator, 18 aisles | 18 | 7 | 0.05–0.11 m on the legs that ran |
| Python mission, 10 aisles | 10 | **10** | 0.01–0.18 m; every row entered within 0.03 m of centre |

Every one of the seven legs completed in the 18-aisle run took the same turn —
`u-turn 7.00 m, radius 3.50 m at 0.56 rad of steering, 11.8 m of path` — in
headlands measured at 6.7, 7.6, 8.0, 25.0, 9.8, 25.0 and 10.8 m, needing 5.5 m
of them. The ladder never had to reach for the bulb, which is the skip rule of
§4 working: it arranges for all but one turn in the field to be the easy case.

That one turn is where the geofenced run lost its aisle:

```
leg 2/4: aisle 2
3-point 3.50 m to its right in 7.5 m of headland, needing 6.2 m: radius 3.53 m
leg 2 failed: aisle 2
```

7.5 m of headland is not enough for the bulb (which wants 10.8 m at this
radius), so the ladder correctly dropped to the reversing manoeuvre — and the
reversing manoeuvre is the one the machine executes least reliably. **The one
neighbour-to-neighbour turn per field is the whole remaining problem.**

The keepout filter was active throughout and cost nothing: the mask was received
by both costmaps and no leg was affected by it.

Steering smoothness: 0.169 rad between consecutive commands on the cleanest leg,
0.40–0.49 rad on the others. The larger figures are the U-turn's entry, where
the reference curvature steps from zero to $1/3.5$ in one command — a real
discontinuity in the plan rather than controller noise, and the obvious next
thing to smooth.

---

## 9. Bugs only a real simulator finds

Every one of these passed unit tests. Four of them live in
[control](control.md), one in the planner above and one in
[mission planning](mission-planning.md) — which is the point: none of them is a
subsystem being wrong on its own.

1. **Vetoing on cost 253.** `INSCRIBED_INFLATED_OBSTACLE` is inflation, not an
   obstacle. In a corridor barely wider than twice the inflation radius, this
   aborts every leg. *Symptom: total failure that looked like a planner bug.*

2. **Flipping the wrong term in reverse.** The cross-track error's sense does
   not invert when reversing; the heading error's does. *Symptom: −0.77 m and
   diverging, indistinguishable from bad gains.*

3. **Reading arcs out of a pose list.** 0.4 m pose spacing plus orientation
   noise made a straight row read as two direction changes at 1.47 m radius.
   *Fix: run-length filter and a sample-counted curvature window.*

4. **Certifying a turn with a straight-line probe.** A depth probe along the
   heading says nothing about a manoeuvre that leaves the heading. *Symptom:
   seven turns fine, the eighth into the trees.*

5. **Replanning the turn the machine is already driving.** The "near the end"
   condition kept re-firing as the machine drove into the turn, replanning ten
   times per leg under a controller already following it. *Fix: latch on the leg
   index.*

6. **Parking on a cusp for good.** The controller must stop at a direction
   change, and it walks the plan by dropping poses it has driven past — so at
   the cusp it commands zero, never passes the pose, and holds zero. Both rules
   are right on their own. *Symptom: a plan repeating unchanged every second
   under a motionless robot, which reads exactly like a stuck planner. The
   machine could not complete a three point turn anywhere, including the
   headland turns laid out for it above. Fix: [step over the
   cusp](control.md#stepping-over-a-cusp) once stopped on it.*

And two environment traps worth writing down:

- `bt_navigator`'s `plugin_lib_names` must list **only** custom BT node
  libraries. Jazzy loads the stock ones itself; naming one gives a fatal
  `ID [ComputePathToPose] already registered`.
- Every plugin parameter here is declared as a `double`. Writing
  `curvature_samples: 4` instead of `4.0` throws
  `InvalidParameterTypeException` and aborts bringup with a message that names
  no parameter.

---

## 10. Running it

See [getting started §4](getting-started.md#4-the-orchard-coverage-run) for the
full O3DE bringup, and §5 there for the unit tests, which need no simulator and
no GPU.