111 lines
8.5 KiB
Markdown
111 lines
8.5 KiB
Markdown
# 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:
|
|
|
|
```powershell
|
|
# 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:
|
|
|
|
1. Add service-level multi-cycle regressions for `RollingContinuation -> ApproachStopBoundary -> ExactStopAtBoundary`, including Goal/GearSwitch and independent 5 m / 2 s horizon semantics.
|
|
2. Add observation diagnostics and documentation for mode, terminal state, and jerk-limited stopping capability while retaining `OBSERVE_ONLY`.
|
|
3. 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.
|