Design changes

Log /cmd_vel — the controller’s output — and /odom at the same station, and cross-correlate the commanded angular rate against the measured one.

The chain is MPPI at 20 Hz → velocity_smoother at 20 Hz → collision_monitor → base driver, so expect 60–150 ms.

Record the map-frame pose reported at goal-reached and the physical position, measured with tape or a laser. Also record AMCL pose-jump magnitude and rate while driving a straight aisle.

If AMCL’s lateral 1σ at a goal is 3 cm, no controller change reaches 5 cm and the answer becomes a local-feature fine-positioning stage instead — the QR/dock path already in FollowPathWithBaseController. Everything in Phase 2 assumes this number came back under 2 cm.

From a straight-lane run, log lateral error — robot pose against the nearest point on /controller_path_bt — and take its FFT. The peak tells you which of the changes below to expect a result from:

• ~10 Hz → costmap or critic ripple: the costmap-rate and rollout-overrun items.
• 1–3 Hz → a dead-time limit cycle: the latency model.
• matches the AMCL update rate → localization, not control, and the repeatability measurement above is the whole answer.

Existing instrumentation covers most of this: publish_critics_stats: true, publish_optimal_trajectory: true, and the ~/path_follow_debug_markers publisher in path_follow_critic.cpp.

model_delay_vx / vy / wz are all 0.0, but the fork already implements them — motion_models.hpp maintains a command history and offsets it by round(delay / model_dt) steps.

Lateral error is corrected through wz, so the alignment loop is closed around an unmodelled pure dead time. At 1 m/s with an alignment loop crossing near 2 rad/s, τ = 0.1 s costs about 11° of phase margin — enough to turn a well-damped loop into a limit cycle. That is the classic signature of the reported weave.

Set the delays to the τ measured in Phase 0, rounded to a multiple of model_dt (0.05). Start at the measured value.

The parameter file’s comments already diagnose one relay and work around it by moving PathAngleCritic.offset_from_furthest from 12 to 25. These remain:

The max_path_occupancy_ratio gate matters most in a factory. A path point is invalid at cost ≥ INSCRIBED_INFLATED_OBSTACLE (253) — see findPathCosts. With footprint_padding: 0.25 the inflation layer’s inscribed radius is 0.38 + 0.25 = 0.63 m, so every authored-lane point within 0.63 m of a rack, a wall or a parked pallet is invalid. In a narrow aisle the ratio hovers right at the 0.10 / 0.07 thresholds, and the weight-25 alignment term blinks on and off as the robot advances.

Worse, invalid points are also skipped inside the per-sample accumulation — if (path_pts_valid[path_pt]) — so the alignment signal’s composition changes on every 10 Hz costmap update even while the ratio stays below the threshold.

The change: replace each hard return with a continuous weight ramp. Add one shared helper — mppi::utils::gateRamp(value, threshold, band), returning a smoothstep in [0, 1] — and multiply the critic’s contribution by it, behind a new per-critic gate_ramp_band parameter defaulting to 0. Zero is today’s behaviour, so the change is opt-in and bisectable.

Starting bands: 5 indices for the offset_from_furthest gate, 0.05 for max_path_occupancy_ratio, 0.15 rad for max_angle_to_furthest, 0.3 m for CostCritic.near_goal.

Independently of the gate ramps, the padding inflates every obstacle by an extra 25 cm on both costmaps. Drop it to 0.05 and carry the real uncertainty in inflation_layer instead.

Then revisit inflation_radius: 1.7. With cost_scaling_factor: 5.0 it spans a whole factory aisle, so CostCritic (weight 3.0) applies a lateral bias almost everywhere and fights PathAlignCritic. 1.0–1.2 m is the range the BT’s own comment assumes — “inflation_radius is 1.2 m”, line 268. The parameter file and the BT documentation have drifted apart, which is worth reconciling either way.

CostCritic reads the costmap directly, so a 10 Hz ripple is injected into a 20 Hz loop. Raise the local costmap’s update_frequency to 20.0. A 15 × 15 m rolling window at 5 cm is 300 × 300 cells; this is cheap.

If the FFT peak from the weave measurement is at 10 Hz, do this one first.

Horizon = time_steps 70 × model_dt 0.05 = 3.5 s, which at vx_max 1.0 covers 3.5 m — but PathHandler.prune_distance is 3.0 m. findClosestPathPt clamps to size - 1, so the last ~14% of every rollout maps onto the same final path point. That both dilutes the lateral-error signal and couples longitudinal to lateral motion in the cost.

Set prune_distance to 4.0 — at least vx_max × time_steps × model_dt, plus margin. It stays well inside the 15 m local costmap and inside transformLocalPlan’s costmap-bounds break.

The difference between the optimised curvature and the executed one shows up directly as lateral error. Set it True, so the v/ω ratio is preserved under limiting.

PathAlignCritic runs at weight 25, step 4, offset 10; CurvatureOnlyPathAlignCritic at weight 8, step 2, offset 8. Both score lateral deviation.

After the gate ramps land, re-measure costs_std / cost_weight for both. If they are still fighting, fold the curvature term into PathAlignCritic as a curvature-dependent weight and disable the second critic, rather than keeping two gated terms on one physical quantity.

wz_std is 0.25, with the note that “0.14 is the magic number”, while advanced.wz_std_decay_strength is -1.0 (disabled) and wz_std_decay_to is 0.06. Try a decay strength of 2.0, so angular exploration decays on the straights while corner authority is retained.

Today the last mile is planned by AstarGridBased (NavFn) — an 8-connected grid path with 45°-quantised headings and a staircase — then passed through CornerFilletSmoother with simplify_tolerance: 0.10 (RDP) and smoothing_radius: 2.0. The BT itself flags this at line 524: “SmoothPath will cause the path slightly deviated from route_graph (about max 5cm) … lower the RDP tolerance”.

Lower the tolerance to 0.02, as that comment already recommends. Verify the A* connector staircase does not survive as false corners; if it does, straighten the graph rather than raise ε back up.

Author a straight approach segment into the last mile. In ComputeAndPublishRoute, back-project a fixed length L — start with 1.5 m — from the goal along the goal yaw to form a goal_approach_start pose; plan last_mile_path to that pose, then append the straight segment verbatim. The approach line must be excluded from filleting: the smoother already has the preserve_authored_curves machinery and the soft vertex concept to build on.

The final approach is then a straight line at exactly the goal heading, so the goal yaw is achieved by driving rather than by spinning in place — which removes the terminal-rotation term almost entirely.

GoalCritic takes .rowwise().mean() over the horizon and GoalAngleCritic does the same for yaw error. Near the goal, with near_goal_vx_max: 0.20 and a 3.5 s horizon, most of the horizon sits past the goal, so both means are dominated by the overshoot region. No amount of weight tuning creates a gradient that is not there.

Add a TerminalGoalCritic, modelled on goal_critic.cpp, that when local_path_length falls below a threshold:

• scores the closest approach to the goal over the rollout — .rowwise().minCoeff() rather than the mean — so parking on the goal is strictly cheaper than passing near it;
• adds a stop term penalising residual speed at the point of closest approach, so the optimum is stopped at the goal rather than passing through it;
• keeps symmetric_yaw_tolerance semantics, so it stays compatible with BidirectionalGoalChecker.

Keep the existing GoalCritic for the 1.4 m → 0.3 m band and hand over to the terminal critic inside about 0.3 m. Balance it the way the others are balanced: costs_std / cost_weight against T = 0.4608.

The GA-MPC controller carries a terminal pose regulator: k_phi: 6.0, k_delta: 2.0, final_rotation: true, a slowdown_radius, and it pairs with BaseGoal at a 0.001 m tolerance.

The BT declares a ControllerSelector at line 86, but FollowPath hardcodes goal_checker_id="general_goal_checker" at line 647. Add a GoalCheckerSelector — the node already exists and is registered in nav2_tree_nodes.xml:258 — and bind goal_checker_id="{selected_goal_checker}". A fleet manager or a route operation can then select {FollowPathWithBaseController, BaseGoal} for the last ~1.5 m of a precise goal, and {FollowPath, general_goal_checker} everywhere else.

Prefer this over the new critic if it proves sufficient in trials — reusing the tuned GA-MPC regulator is less new code than a new critic. Run both in the goal-accuracy trials and keep whichever closes the budget.

The stopped-velocity terms matter at this scale: at 0.08 m/s the robot is still covering 4 mm per control cycle while it is called stopped.

Watch for a hunting mode here. BidirectionalGoalChecker::isGoalReached re-checks within_xy after the stop check and resets check_xy_ = true when it fails. If the shim’s in-place terminal rotation (rotate_to_goal_heading: true, rotate_to_heading_angular_vel: 0.25) drags the robot even 2 cm through wheel slip under payload, the goal is rejected and control returns to MPPI — a cycle. At 15 cm this is masked; at 5 cm it becomes the dominant failure. The straight approach segment is the real fix, since it arrives already aligned; log terminal-rotation xy drift explicitly to confirm.

Leave bypass_y_axis: False. The parameter file already marks it “Disable this during deployment”, and at a 5 cm target, accepting a 0.5 m lateral error would defeat the purpose.