73 lines
6.2 KiB
Markdown
73 lines
6.2 KiB
Markdown
# EM Longitudinal Rolling Planning — Phase 1 Handoff
|
||
|
||
## Scope and commits
|
||
|
||
- Phase start baseline: `6c2f406` (`docs: split EM rolling plan into four phases`)
|
||
- Task 1 commit: `e56220f` (`feat: add complete jerk-limited stopping math`)
|
||
- Task 2 commit / phase code end: `21b20d0` (`feat: separate rolling horizons from stop boundaries`)
|
||
|
||
This phase implemented only original-plan Tasks 1 and 2. It did not implement the Task 3 speed-envelope behavior, Task 4 conditional QP constraints, Task 5 optimizer seeding, trajectory publication behavior, or any chassis command. `OBSERVE_ONLY` remains unchanged.
|
||
|
||
## Delivered changes
|
||
|
||
Task 1 added `JerkLimitedStoppingMath` and `JerkLimitedStoppingProfile` in `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs`. The public API is:
|
||
|
||
```csharp
|
||
JerkLimitedStoppingMath.TryCalculate(double speed, double acceleration,
|
||
double maximumDeceleration, double maximumJerk,
|
||
out JerkLimitedStoppingProfile profile, out string failureReason)
|
||
JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(double availableDistance,
|
||
double conservativeInitialAcceleration, double maximumDeceleration,
|
||
double maximumJerk, double directionMaximumSpeed)
|
||
JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(double initialSpeed,
|
||
double initialAcceleration, double maximumSpeed, double maximumAcceleration,
|
||
double maximumDeceleration, double maximumJerk, double timeHorizon)
|
||
```
|
||
|
||
The profile exposes `DistanceMeters`, `DurationSeconds`, `FinalSpeedMetersPerSecond`, and `FinalAccelerationMetersPerSecondSquared`. The model uses the ST constant-jerk integration equations and releases both speed and acceleration to zero. `PathSpeedLimitBuilder` now uses it for its existing stopping precheck; the old duplicate stopping classes were removed.
|
||
|
||
Task 1 files were `JerkLimitedStoppingMath.cs`, `PathSpeedLimitBuilder.cs`, and `LongitudinalModelChecks.cs`. Two existing direct callers of the removed internal types also required minimal compilation adaptation: `PlanningHorizonSelector.cs` and `SequentialLongitudinalOptimizer.cs`. They now call the new math but otherwise preserve their pre-Task-2 behavior.
|
||
|
||
Task 2 added `EmLongitudinalMode` (`RollingContinuation`, `ApproachStopBoundary`, `ExactStopAtBoundary`) and `LongitudinalTerminalSchedule.GetStabilizationStartIndex(IReadOnlyList<double>, double)`.
|
||
|
||
`PlanningHorizonSelection` now records `WindowEndReferenceS`, `WindowEndBoundaryType`, `TerminalType`, `LongitudinalMode`, `StopBoundaryReferenceS`, and `HasStopBoundary`. `TerminalReferenceS` remains as a read-only compatibility alias for the window end.
|
||
|
||
`PlanningHorizonSelector` now:
|
||
|
||
- rejects an already insufficient real segment stopping distance using the complete jerk model;
|
||
- sets the LS window with only `DistanceHorizonMeters`;
|
||
- uses only real `Goal` and `GearSwitchApproach` segment ends as stop boundaries;
|
||
- selects rolling when the boundary is outside the window, approach when visible but not time-reachable with the mandatory internal stabilization step, and exact stop otherwise;
|
||
- does not deduct `ZeroSpeedHoldSeconds` from the ST horizon.
|
||
|
||
`LongitudinalPlanningInput` now accepts `EmLongitudinalMode` and exposes `PathUpperBoundS`, `HasStopBoundary`, `StopBoundaryPathS`, `StopBoundaryType`, and `Mode`. Its original constructor remains as a compatibility overload, mapping rolling terminals to `RollingContinuation` and Goal/GearSwitch terminals to `ExactStopAtBoundary`; new code must use the mode-explicit constructor. `TerminalPathS` remains a compatibility alias for now, because Tasks 3–5 must migrate its upper-bound and stop-boundary uses deliberately.
|
||
|
||
`EmPlanningRequestValidator` checks the worst forward/reverse full jerk-limited stop at configured maximum positive acceleration plus one replan-period reserve against `DistanceHorizonMeters`, and checks that the ST knot horizon can reserve a full internal stabilization interval. `EmPlanningService` slices with `WindowEndReferenceS` and passes the selected longitudinal mode into the ST input.
|
||
|
||
Task 2 files were `EmLongitudinalMode.cs`, `LongitudinalTerminalSchedule.cs`, `PlanningHorizonSelector.cs`, `LongitudinalPlanningInput.cs`, `EmPlanningRequestValidator.cs`, `EmPlanningService.cs`, `LongitudinalModelChecks.cs`, and `FoundationChecks.cs`. `LongitudinalIntegrationChecks.cs` was additionally updated only to pass `Mode` at every direct `LongitudinalPlanningInput` construction.
|
||
|
||
## Verification evidence
|
||
|
||
The phase-end commands were run from commit `21b20d0`; each exited `0`:
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation
|
||
# PASS foundation
|
||
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
# PASS longitudinal-model
|
||
|
||
git diff --check
|
||
# exit 0
|
||
```
|
||
|
||
The .NET commands still print two pre-existing obsolete warnings from `MovementTests.TireFollowing.cs` and `TireFollowing.cs`; phase 1 added no warning source. The staged-file checks before each commit contained only the listed phase files and the explicitly noted direct-caller adaptations.
|
||
|
||
## Remaining work and constraints for phase 2
|
||
|
||
Phase 2 is original-plan Tasks 3–5 only. It must make speed envelopes, QP terminal constraints, solution validation, and optimizer seeds conditional on `LongitudinalPlanningInput.Mode`. It must replace legacy `TerminalPathS` semantics with `PathUpperBoundS` versus `StopBoundaryPathS` at the relevant use sites, without changing trajectory publication (Task 6+) or adding any chassis command.
|
||
|
||
Do not loosen jerk, acceleration, velocity, QP residual, or trajectory-validation tolerances. `DistanceHorizonMeters` remains the LS spatial look-ahead; `TimeHorizonSeconds` remains one ST solve duration. A rolling window end is not a stop boundary. Exact stops must reserve at least one full `OutputTimeStepSeconds` static interval inside the QP horizon. `ZeroSpeedHoldSeconds` stays outside that horizon and is not phase-2 work.
|
||
|
||
The working tree still contains extensive unrelated user PathSmoothing, CoarsePath, Map, configuration, report, and untracked-file changes. Preserve them; do not reset, clean, broadly stage, or commit them. No target-machine OSQP probe was run in phase 1; it belongs to the later roadmap phase.
|