EM Longitudinal Rolling Planning — Phase 2 Handoff

Scope and commits

Phase start baseline: 21b20d0 (feat: separate rolling horizons from stop boundaries)
Task 3: 94a9be9 (feat: keep rolling speed envelopes open)
Task 4: efa03c1 (feat: apply ST stop constraints only at real boundaries)
Task 3 compatibility fixup: dbc7b7c
Task 4 initial-state/envelope fixup: 26bd822
Task 5 / phase code end: 59d13e5 (feat: seed rolling and exact-stop ST profiles)

Only original-plan Tasks 3, 4, and 5 were implemented. The two fixups could not be autosquashed because the shared worktree has unrelated unstaged user changes and Git refused rebase. They are intentionally retained as separate local commits; their changes belong to Tasks 3 and 4 respectively.

No trajectory publication code, service-flow work, external hold behavior, chassis command, or Task 6+ production code was started. OBSERVE_ONLY remains unchanged.

Delivered behavior

PathSpeedLimit now exposes HasStopBoundary and PathUpperBoundS; TerminalPathS remains a warning-free compatibility alias. PathSpeedLimitBuilder gives RollingContinuation a nonzero open end and no stop boundary. ApproachStopBoundary and ExactStopAtBoundary use JerkLimitedStoppingMath.MaximumInitialSpeedForDistance at every PathS sample against StopBoundaryPathS; only the actual stop-boundary sample is exactly zero. The old square-root limit and discrete deceleration tail approximation were removed.

LongitudinalConstraintBuilder applies no terminal S/U/A equality to Rolling or Approach. Exact mode uses LongitudinalTerminalSchedule.GetStabilizationStartIndex and pins every internal tail knot to StopBoundaryPathS, zero speed, and zero acceleration. Its row count is 8*K-2+3*M; the other modes remain 8*K-2.

LongitudinalSolutionValidator always checks knot layout, finite values, PathS monotonicity/bounds, jerk, and exact constant-jerk dynamics. Approach and Exact independently test each knot against its complete jerk-limited stopping distance; Exact also canonicalizes the entire internal static tail. The Task 4 fixup preserves the supplied physical state at knot zero: the conservative maximum-positive-acceleration envelope applies from the following knot, while the actual-state stopping check still applies at knot zero.

SequentialLongitudinalOptimizer dispatches seed construction by EmLongitudinalMode. Rolling forward-integrates without forcing the LS path end. Approach uses the stopping-speed envelope while producing only this ST time prefix. Exact uses the stabilization schedule, tries a cruise-plus-jerk-brake profile, and otherwise constructs a terminal-equality seed before filling the QP-internal static tail. Exact-only outer iteration pins tail progress; ordinary knots use PathUpperBoundS. The final-knot forward-probe index guard remains, without restoring an unconditional terminal-position pin.

LongitudinalObjectiveBuilder uses PathUpperBoundS for normalization and omits the terminal-acceleration soft residual only in Exact mode. Integration fixtures now explicitly distinguish generic Approach OSQP scenarios from the independent Exact-stop tail scenario; this avoids incorrectly demanding an exact stop for a distance that cannot fit inside the ST motion horizon.

Interfaces the next phase must preserve

JerkLimitedStoppingMath, JerkLimitedStoppingProfile
EmLongitudinalMode: RollingContinuation, ApproachStopBoundary, ExactStopAtBoundary
LongitudinalTerminalSchedule.GetStabilizationStartIndex
PlanningHorizonSelection.WindowEndReferenceS, WindowEndBoundaryType, LongitudinalMode, StopBoundaryReferenceS, HasStopBoundary
Mode-explicit LongitudinalPlanningInput, PathUpperBoundS, StopBoundaryPathS, HasStopBoundary
PathSpeedLimit.HasStopBoundary and PathSpeedLimit.PathUpperBoundS

TerminalPathS is compatibility-only. Use PathUpperBoundS for every path/window upper bound and StopBoundaryPathS only for a real Goal/GearSwitch stop.

Verification evidence

The following were rerun at phase end and exited 0:

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
# PASS longitudinal-model

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration
# PASS longitudinal-integration

git diff --check
# exit 0

The .NET runs report two pre-existing obsolete warnings from MovementTests.TireFollowing.cs and TireFollowing.cs; phase 2 introduced none. git diff --check also prints CRLF notices for unrelated pre-existing dirty files but returned success.

git log -5 --oneline ended with 59d13e5, 26bd822, dbc7b7c, efa03c1, and 94a9be9. The worktree still contains extensive unrelated modified, deleted, and untracked PathSmoothing, CoarsePath, Map, report, and configuration files. Preserve them; never reset, clean, broadly stage, or commit them.

Phase 3 boundary

Phase 3 is only original-plan Tasks 6 and 7: mode-aware trajectory sampling/publication validation, followed by previous-trajectory longitudinal soft references. It must not begin Task 8, 9, or 10. ZeroSpeedHoldSeconds remains outside the QP horizon and becomes an external hold only for Exact mode. Rolling and Approach must publish their ST horizon unchanged, with no synthetic terminal stop or boundary marker.

5.3 KiB Raw Blame History