# 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 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 ``` 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.