376 lines
18 KiB
Markdown
376 lines
18 KiB
Markdown
# EM Planner Rolling Execution and Plugin Deployment Implementation Plan
|
|
|
|
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
|
|
|
**Goal:** Add version-safe rolling replanning, trajectory handoff, gear-switch execution states, controller adaptation, and deterministic packaging of `ClumsyPilot.dll` with `osqp.dll` and licenses.
|
|
|
|
**Architecture:** Keep `EmPlanningService` pure and place scheduling, cancellation, stale-result suppression, previous-trajectory reuse, and command generation in a sibling `TrajectoryExecution` module. The coordinator publishes only a fully validated current-version trajectory; the executor samples that immutable trajectory, manages zero-speed gear transitions, and converts its fields into a generic controller command without coupling EM optimization to the hardware API.
|
|
|
|
**Tech Stack:** C# 10, .NET Standard 2.0, `Task`/`CancellationToken`, immutable EM trajectories, PowerShell packaging, Windows x64 plugin layout.
|
|
|
|
## Global Constraints
|
|
|
|
- This plan depends on all four preceding implementation plans.
|
|
- Default replan period is `0.20 s`; time horizon `6.0 s`; distance horizon `5.0 m`; handoff lookahead `0.30 s`.
|
|
- The coordinator accepts captured `VehicleMotionState`; it does not read localization, wheel speed, UI, or hardware directly.
|
|
- Each cycle binds `MapSnapshotId`, `ReferencePathId`, `VehicleState.SequenceId`, `PreviousTrajectoryId`, and `SegmentIndex`.
|
|
- A result is publishable only if all bound identities still match the latest cycle and the cycle version is current.
|
|
- Normal replans hand off from the previous trajectory only when tracking error and age are within configuration limits and no gear boundary is crossed.
|
|
- If a replan fails, the previously published trajectory remains executable and ends in its own zero-speed safety tail.
|
|
- Gear change occurs only after measured speed remains below `0.01 m/s` for at least `0.20 s`.
|
|
- The executor never commands lateral body velocity or in-place rotation.
|
|
- The generic control command uses signed longitudinal velocity and yaw rate; world `vx/vy`, speed, curvature, and pose remain available for monitoring.
|
|
- No dynamic-obstacle prediction or dynamic behavior state is introduced.
|
|
- Plugin runtime layout is `plugins/ClumsyPilot.dll`, `plugins/osqp.dll`, and `plugins/licenses/*`.
|
|
- Deployment refuses a non-x64 host/runtime package or an OSQP hash mismatch.
|
|
|
|
---
|
|
|
|
## Locked File Structure
|
|
|
|
```text
|
|
ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/
|
|
├── EmPlanningCoordinator.cs
|
|
├── GearSwitchStateMachine.cs
|
|
├── GearSwitchState.cs
|
|
├── IEmPlanningCycleSink.cs
|
|
├── IVehicleStateProvider.cs
|
|
├── PlanningCycleIdentity.cs
|
|
├── PlanningCycleInput.cs
|
|
├── PlanningCycleResult.cs
|
|
├── TrajectoryControlAdapter.cs
|
|
├── TrajectoryControlCommand.cs
|
|
├── TrajectoryExecutionState.cs
|
|
├── TrajectoryExecutor.cs
|
|
├── TrajectoryHandoffSelector.cs
|
|
└── TrajectorySampler.cs
|
|
|
|
ClumsyPilot/scripts/
|
|
└── Publish-ClumsyPilotPlugin.ps1
|
|
|
|
ClumsyPilot/tests/EMPlannerVerificationHost/
|
|
├── CoordinatorChecks.cs
|
|
├── ExecutorChecks.cs
|
|
└── PluginPackagingChecks.cs
|
|
```
|
|
|
|
## Shared Interfaces
|
|
|
|
```csharp
|
|
public interface IVehicleStateProvider
|
|
{
|
|
VehicleMotionState Capture();
|
|
}
|
|
|
|
public sealed class EmPlanningCoordinator
|
|
{
|
|
public EmPlanningCoordinator(IEmPlanningService planningService,
|
|
IEmPlanningCycleSink sink = null);
|
|
public Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input,
|
|
CancellationToken cancellationToken);
|
|
public EmTrajectory PublishedTrajectory { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryExecutor
|
|
{
|
|
public TrajectoryExecutionState State { get; }
|
|
public TrajectoryControlCommand Update(DateTimeOffset now,
|
|
VehicleMotionState measuredState, EmTrajectory trajectory);
|
|
}
|
|
|
|
public sealed class TrajectoryControlCommand
|
|
{
|
|
public double SignedLongitudinalVelocity { get; }
|
|
public double YawRate { get; }
|
|
public TravelDirection Direction { get; }
|
|
public bool RequestDirectionChange { get; }
|
|
public bool HoldBrake { get; }
|
|
public bool IsTrajectoryComplete { get; }
|
|
}
|
|
```
|
|
|
|
### Task 1: Cycle Identity, Scheduling Decision, and Stale-Result Suppression
|
|
|
|
**Files:**
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs`
|
|
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
|
|
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
|
|
|
**Interfaces:**
|
|
- Consumes: pure `IEmPlanningService`, captured request inputs, and configuration scheduling values.
|
|
- Produces: latest-wins asynchronous planning and a read-only published trajectory.
|
|
|
|
- [ ] **Step 1: Write failing coordinator concurrency checks**
|
|
|
|
Use a controllable fake planning service to start cycle A, then cycle B before A completes. Complete B first with success and A later with success. Assert B alone is published and A returns `Superseded`. Add checks for map ID, reference ID, state sequence, segment, and prior trajectory ID changes invalidating an otherwise successful result.
|
|
|
|
Also assert `ShouldStartCycle(now)` is false before `0.20 s` and true at exactly `0.20 s`; this decision uses caller-supplied time.
|
|
|
|
- [ ] **Step 2: Run and verify coordinator types are absent**
|
|
|
|
```powershell
|
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
|
|
```
|
|
|
|
Expected: build failure naming `EmPlanningCoordinator`.
|
|
|
|
- [ ] **Step 3: Implement latest-wins coordination**
|
|
|
|
Use an incrementing `long` cycle version, a private lock only around publication state, and a per-cycle linked cancellation source. Starting a newer cycle cancels the prior source. After planning, compare the complete identity and version again under the publication lock; map stale success to `Superseded` without exposing its trajectory.
|
|
|
|
Sink exceptions are caught and reported in the cycle diagnostic. Never hold the publication lock while running the planner or invoking a sink.
|
|
|
|
- [ ] **Step 4: Run concurrency checks 100 times**
|
|
|
|
```powershell
|
|
1..100 | ForEach-Object {
|
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
|
|
if ($LASTEXITCODE -ne 0) { throw "Coordinator verification failed on iteration $_" }
|
|
}
|
|
```
|
|
|
|
Expected: every iteration prints `PASS coordinator`.
|
|
|
|
- [ ] **Step 5: Commit coordinator identity logic**
|
|
|
|
```powershell
|
|
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost
|
|
git commit -m "feat: coordinate rolling EM replans"
|
|
```
|
|
|
|
### Task 2: Safe Previous-Trajectory Handoff
|
|
|
|
**Files:**
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs`
|
|
- Modify: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs`
|
|
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
|
|
|
|
**Interfaces:**
|
|
- Consumes: current published trajectory, measured state, latest segment identity, and `0.30 s` handoff lookahead.
|
|
- Produces: either a future trajectory-derived start state/seed or a measured-state start with no seed.
|
|
|
|
- [ ] **Step 1: Write failing handoff acceptance/rejection checks**
|
|
|
|
Assert acceptance only when trajectory age is valid, position/yaw/speed tracking errors are inside configured tolerances, the future sample remains on the same segment/direction, and the interval contains no gear boundary. Assert rejection for stale trajectory, large error, terminal proximity, segment mismatch, direction mismatch, and a handoff time beyond the trajectory.
|
|
|
|
- [ ] **Step 2: Run and verify selector is absent**
|
|
|
|
Expected: build failure naming `TrajectoryHandoffSelector`.
|
|
|
|
- [ ] **Step 3: Implement time interpolation and selection**
|
|
|
|
Binary-search `TimeFromStart`, interpolate X/Y, unwrapped yaw, signed speed, curvature, and PathS, then derive redundant fields through the trajectory-point constructor. Never interpolate across different boundary types, segment indices, or directions. Return a result object that states `PreviousTrajectory` or `MeasuredState` and includes a deterministic rejection reason.
|
|
|
|
- [ ] **Step 4: Run handoff checks**
|
|
|
|
Expected: `PASS coordinator`, including a reverse same-segment handoff.
|
|
|
|
- [ ] **Step 5: Commit handoff logic**
|
|
|
|
```powershell
|
|
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs
|
|
git commit -m "feat: select safe EM trajectory handoffs"
|
|
```
|
|
|
|
### Task 3: Gear-Switch State Machine and Trajectory Executor
|
|
|
|
**Files:**
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs`
|
|
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
|
|
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
|
|
|
**Interfaces:**
|
|
- Consumes: measured state, current time, sampled trajectory, and exact boundary types.
|
|
- Produces: zero-speed holding, one-shot direction-change request, confirmed departure, and completion states.
|
|
|
|
- [ ] **Step 1: Write failing state-transition checks**
|
|
|
|
Cover this exact sequence:
|
|
|
|
```text
|
|
Following -> ApproachingGearSwitch
|
|
ApproachingGearSwitch -> HoldingZero when command reaches boundary
|
|
HoldingZero remains while |measured speed| >= 0.01 m/s
|
|
HoldingZero timer resets if speed rises above tolerance
|
|
HoldingZero -> RequestingDirectionChange after continuous 0.20 s below tolerance
|
|
RequestingDirectionChange emits exactly one request
|
|
AwaitingDirectionConfirmation holds zero
|
|
confirmed direction -> Following next segment
|
|
goal/rolling terminal -> Completed while holding zero
|
|
```
|
|
|
|
Assert no transition can output nonzero signed speed during holding or direction confirmation.
|
|
|
|
- [ ] **Step 2: Run and verify executor types are absent**
|
|
|
|
Expected: build failure naming `GearSwitchStateMachine`.
|
|
|
|
- [ ] **Step 3: Implement explicit state and event inputs**
|
|
|
|
The state machine receives caller-supplied `now`, measured signed speed, desired/current directions, and a boolean direction-confirmation input. It does not call hardware. `TrajectoryExecutor` samples the trajectory, delegates boundary behavior to the state machine, and returns an immutable execution state containing the selected point and reason.
|
|
|
|
- [ ] **Step 4: Run executor state checks**
|
|
|
|
Expected: `PASS executor`, including forward-to-reverse and reverse-to-forward sequences.
|
|
|
|
- [ ] **Step 5: Commit executor state machine**
|
|
|
|
```powershell
|
|
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs
|
|
git commit -m "feat: execute EM gear-switch boundaries"
|
|
```
|
|
|
|
### Task 4: Generic Control Adapter
|
|
|
|
**Files:**
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs`
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs`
|
|
- Modify: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs`
|
|
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
|
|
|
|
**Interfaces:**
|
|
- Consumes: an `EmTrajectoryPoint` and gear-switch execution state.
|
|
- Produces: signed linear velocity, yaw rate, direction-change request, brake hold, and completion flags.
|
|
|
|
- [ ] **Step 1: Write failing command-mapping checks**
|
|
|
|
For forward and reverse points assert command signed velocity and yaw rate equal the point fields exactly. Assert `Speed`, world `vx/vy`, pose, and curvature remain available in execution telemetry but are not reinterpreted as body lateral velocity. Assert holding states always override both command velocities to zero.
|
|
|
|
- [ ] **Step 2: Run and verify adapter is absent**
|
|
|
|
Expected: build failure naming `TrajectoryControlAdapter`.
|
|
|
|
- [ ] **Step 3: Implement adapter without hardware coupling**
|
|
|
|
`TrajectoryControlAdapter.CreateCommand` copies signed longitudinal velocity and yaw rate in normal following. For hold, switch, invalid, or completed states it sets both to zero and sets the corresponding flags. Do not reference `MultiVehicleScriptVx`, `MultiVehicleScriptVy`, or `MultiVehicleScriptVth`; a later hardware-specific adapter may map this generic command after the existing controller field semantics are confirmed.
|
|
|
|
- [ ] **Step 4: Run adapter and executor checks**
|
|
|
|
Expected: `PASS executor`; add an invariant that ordinary following never requests nonzero body lateral velocity or `v=0, yawRate!=0`.
|
|
|
|
- [ ] **Step 5: Commit control adaptation**
|
|
|
|
```powershell
|
|
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs
|
|
git commit -m "feat: adapt EM trajectories to control commands"
|
|
```
|
|
|
|
### Task 5: Plugin Output and License Packaging
|
|
|
|
**Files:**
|
|
- Modify: `ClumsyPilot/ClumsyPilot.csproj`
|
|
- Create: `ClumsyPilot/scripts/Publish-ClumsyPilotPlugin.ps1`
|
|
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/PluginPackagingChecks.cs`
|
|
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
|
|
|
**Interfaces:**
|
|
- Consumes: a successfully built `ClumsyPilot.dll` plus the pinned OSQP package.
|
|
- Produces: the exact deployable `plugins` tree.
|
|
|
|
- [ ] **Step 1: Write failing packaging checks**
|
|
|
|
Create a temporary output directory, invoke the future script, and assert exactly:
|
|
|
|
```text
|
|
plugins/ClumsyPilot.dll
|
|
plugins/osqp.dll
|
|
plugins/licenses/OSQP-LICENSE.txt
|
|
plugins/licenses/OSQP-NOTICE.txt
|
|
plugins/licenses/OSQP-VERSION.txt
|
|
```
|
|
|
|
Assert the deployed native hash equals `SHA256SUMS`, `ClumsyPilot.dll` is a managed assembly, OSQP is x64, and rerunning packaging replaces files without leaving stale temporary files.
|
|
|
|
- [ ] **Step 2: Run and verify packaging script is absent**
|
|
|
|
Expected: `PluginPackagingChecks` fails because `Publish-ClumsyPilotPlugin.ps1` does not exist.
|
|
|
|
- [ ] **Step 3: Add build-output metadata and transactional publish script**
|
|
|
|
Add these items to `ClumsyPilot.csproj` without rewriting existing targets:
|
|
|
|
```xml
|
|
<None Include="ThirdParty\OSQP\win-x64\osqp.dll"
|
|
Link="osqp.dll" CopyToOutputDirectory="PreserveNewest" />
|
|
<None Include="ThirdParty\OSQP\LICENSE"
|
|
Link="licenses\OSQP-LICENSE.txt" CopyToOutputDirectory="PreserveNewest" />
|
|
<None Include="ThirdParty\OSQP\NOTICE"
|
|
Link="licenses\OSQP-NOTICE.txt" CopyToOutputDirectory="PreserveNewest" />
|
|
<None Include="ThirdParty\OSQP\VERSION"
|
|
Link="licenses\OSQP-VERSION.txt" CopyToOutputDirectory="PreserveNewest" />
|
|
```
|
|
|
|
The publish script takes mandatory `-ManagedDll` and `-OutputDirectory`, resolves both absolute paths, validates inputs and hash, stages the five files in a unique sibling temporary directory, then renames the completed `plugins` directory into place. It refuses to operate when the resolved output is a drive root or workspace root.
|
|
|
|
- [ ] **Step 4: Run packaging checks against a fresh directory**
|
|
|
|
```powershell
|
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- plugin-package
|
|
```
|
|
|
|
Expected: `PASS plugin-package`; no files exist outside the temporary test root.
|
|
|
|
- [ ] **Step 5: Commit packaging**
|
|
|
|
```powershell
|
|
git add ClumsyPilot/ClumsyPilot.csproj ClumsyPilot/scripts/Publish-ClumsyPilotPlugin.ps1 ClumsyPilot/tests/EMPlannerVerificationHost
|
|
git commit -m "build: package ClumsyPilot with OSQP"
|
|
```
|
|
|
|
### Task 6: Rolling End-to-End and Final Gate
|
|
|
|
**Files:**
|
|
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
|
|
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
|
|
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
|
|
|
|
**Interfaces:**
|
|
- Consumes: all core, coordinator, executor, and packaging components.
|
|
- Produces: a complete first-version EM Planner workflow with safe rolling fallback.
|
|
|
|
- [ ] **Step 1: Add deterministic rolling scenarios**
|
|
|
|
Simulate caller-supplied time and measured states for: normal repeated forward replans, reverse replans, a solver failure with old-trajectory continuation, a superseded slow cycle, tracking-error reset to measured state, forward/reverse gear switch, and final goal stop. Assert every executed command comes from a currently published validated trajectory or is a zero hold.
|
|
|
|
- [ ] **Step 2: Add failure-tail assertions**
|
|
|
|
Force all new plans to fail after a successful publication. Advance time through the old trajectory and assert it reaches its exact zero-speed terminal and stays zero; no extrapolated nonzero command is allowed after its final point.
|
|
|
|
- [ ] **Step 3: Run the full first-version gate**
|
|
|
|
```powershell
|
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all
|
|
git diff --check
|
|
```
|
|
|
|
Expected groups include foundation, OSQP, lateral, longitudinal, trajectory, facade, coordinator, executor, and plugin packaging, all with PASS output.
|
|
|
|
- [ ] **Step 4: Update README with ownership and deployment**
|
|
|
|
Document the pure-planner/coordinator/executor boundary, caller responsibilities, update cadence, handoff rules, gear state sequence, all trajectory fields, generic controller command, OSQP files, packaging command, and explicitly deferred dynamic-obstacle scope.
|
|
|
|
- [ ] **Step 5: Commit rolling integration evidence**
|
|
|
|
```powershell
|
|
git add ClumsyPilot/tests/EMPlannerVerificationHost ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md
|
|
git commit -m "test: verify rolling EM execution"
|
|
```
|
|
|
|
## Completion Gate
|
|
|
|
- One slow cycle can never overwrite a newer published trajectory.
|
|
- All handoffs remain within one segment/direction and use measured state when tracking is unsafe.
|
|
- Failed replans leave a complete prior trajectory that terminates safely at zero.
|
|
- Gear changes require measured zero-speed dwell and emit one explicit request.
|
|
- Generic control output never invents crab or in-place-rotation behavior.
|
|
- Packaging produces the exact DLL/license tree and validates the pinned native hash.
|