EM Longitudinal Rolling Planning — Phase 3 Handoff

Scope and commits

Phase 3 implementation baseline: 59d13e5 (feat: seed rolling and exact-stop ST profiles)
Task 6: 4159ae0 (feat: publish rolling trajectories without stop tails)
Task 7 / phase 3 code end: 6cfbaf6 (feat: reuse prior trajectory in longitudinal planning)

Only original-plan Tasks 6 and 7 were implemented. Task 8, Task 9, and Task 10 have not been started. This handoff and the companion phase-4 prompt are documentation-only follow-up artifacts; they do not change the phase-3 code end above.

The preceding Task 3 and Task 4 fixups remain separate commits, dbc7b7c and 26bd822, because the shared dirty worktree prevented autosquash. Preserve them as part of the actual history; do not rebase, reset, clean, or stash user files merely to rewrite it.

Delivered behavior

Task 6: mode-aware publication and validation

EmTrajectoryMetadata now carries EmLongitudinalMode, supplied by EmPlanningService. TrajectorySampleSchedule is mode-aware:

RollingContinuation and ApproachStopBoundary publish exactly the ST candidate knots, without an artificial zero-speed hold or a terminal boundary anchor.
ExactStopAtBoundary locates the internal stable tail with LongitudinalTerminalSchedule.GetStabilizationStartIndex, requires its constant PathS, U=0, A=0, and zero tail jerk, marks only its first point as the boundary anchor, then appends ZeroSpeedHoldSeconds after the QP horizon with strictly increasing timestamps.

EmTrajectoryAssembler marks an anchor only for the first exact stable-tail knot. EmTrajectoryValidator retains finite-value, ordinary kinematics, jerk, and collision checks for every mode, but applies terminal-anchor, zero terminal speed/acceleration/yaw, and static-tail requirements only to ExactStopAtBoundary. It now reports TerminalAccelerationNotZero when appropriate. No validation, solver, acceleration, or jerk threshold was relaxed, and OBSERVE_ONLY behavior is unchanged.

Task 6 changed only:

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs
ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs

Task 7: previous-trajectory longitudinal soft reference

LongitudinalPreviousTrajectorySeed and LongitudinalPreviousTrajectorySeedBuilder were added. The builder samples a compatible previous EmTrajectory at newEffectiveAtUtc + knotTime, linearly interpolates world position and signed speed, monotonically projects the position onto the current LateralPath, and publishes non-negative progress speed. Missing trajectories, incompatible segment/direction, malformed input, and out-of-range sampling return LongitudinalPreviousTrajectorySeed.Empty; none of those conditions throw into the service publication path.

After lateral planning succeeds, EmPlanningService creates the current ST knot times, builds the seed with the prior trajectory/current path/effective time/segment/direction, and passes its PathS and ProgressSpeedMetersPerSecond arrays into the existing explicit-mode LongitudinalPlanningInput. These arrays activate the existing previous-S and previous-U objective terms only as soft references. They are not terminal constraints. LS slicing remains based on WindowEndReferenceS.

Task 7 changed only:

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs (new)
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

The service test fixture now captures the emitted ST quadratic program to prove that valid seeds add nonzero previous-S/previous-U objective terms, while an invalid seed adds neither and planning still succeeds. Its scripted solver now returns the supplied warm start for normal longitudinal QPs, allowing phase-1/2 constraints and the phase-3 soft reference to be exercised without fabricating an incompatible primal solution.

Interfaces the next phase must preserve

EmLongitudinalMode: RollingContinuation, ApproachStopBoundary, ExactStopAtBoundary
LongitudinalTerminalSchedule.GetStabilizationStartIndex
Explicit-mode LongitudinalPlanningInput, including PreviousPathS and PreviousProgressSpeedMetersPerSecond
PathUpperBoundS, StopBoundaryPathS, and PathSpeedLimit.HasStopBoundary
EmTrajectoryMetadata.LongitudinalMode
LongitudinalPreviousTrajectorySeed.PathS, ProgressSpeedMetersPerSecond, and Empty
LongitudinalPreviousTrajectorySeedBuilder.Build(EmTrajectory, LateralPath, DateTimeOffset, IReadOnlyList<double>, int, TravelDirection)

TerminalPathS remains compatibility-only. Use PathUpperBoundS for the LS/path-window upper bound; use StopBoundaryPathS only for an actual Goal or GearSwitch stop boundary.

Verification evidence

All commands below exited 0 at the stated checkpoint:

# Before Task 6 commit 4159ae0
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
# PASS trajectory
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
# PASS coordinator
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- executor
# PASS executor
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
# PASS trajectory-observation

# Before Task 7 commit 6cfbaf6
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
# PASS longitudinal-model
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-planning-service
# PASS em-planning-service

# Fresh phase-end checks after Task 7
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
# PASS trajectory
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-planning-service
# PASS em-planning-service
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
# PASS coordinator
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- executor
# PASS executor

git diff --check
# exit 0

The fresh trajectory run emitted two pre-existing obsolete-API warnings from MovementTests.TireFollowing.cs and TireFollowing.cs; no failure resulted. git diff --check returned success but printed CRLF conversion notices for extensive unrelated dirty user files.

At phase end, git log -5 --oneline began with 6cfbaf6, 4159ae0, 72592f0, 59d13e5, and 26bd822. The worktree still contains many unrelated modified, deleted, and untracked files in PathSmoothing, CoarsePath, Map, reports, configuration, and other user work. They were not staged or committed. Preserve them exactly: do not run reset, checkout, clean, stash, git add ., or git add -A.

Phase 4 boundary

Phase 4 is original-plan Tasks 8, 9, and 10 only:

Add service-level multi-cycle regressions for RollingContinuation -> ApproachStopBoundary -> ExactStopAtBoundary, including Goal/GearSwitch and independent 5 m / 2 s horizon semantics.
Add observation diagnostics and documentation for mode, terminal state, and jerk-limited stopping capability while retaining OBSERVE_ONLY.
Run the complete regression/build set and the target-machine real-OSQP probe. If the current machine lacks the native dependency, record that as pending target-machine verification rather than changing longitudinal logic or thresholds.

Do not alter the phase-3 behavior to make the new Task 8 fixtures easier. In particular, rolling and approach are allowed to end with nonzero speed, only exact stops carry a boundary anchor/static tail, ZeroSpeedHoldSeconds is outside QP time, and previous-trajectory references remain soft only.

8.5 KiB Raw Blame History