Navigation

Docs index · Control · Mission planning

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; deciding which row is next is mission planning.

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:

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

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.

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.

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 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

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

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:

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.

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

  1. This is the live piece of work. The fix in mission planning §5 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:

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:

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:

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, one in the planner above and one in mission planning — 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 once stopped on it.

And two environment traps worth writing down:

10. Running it

See getting started §4 for the full O3DE bringup, and §5 there for the unit tests, which need no simulator and no GPU.