61 lines
5.3 KiB
Markdown
61 lines
5.3 KiB
Markdown
# 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`:
|
|
|
|
```powershell
|
|
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.
|