ParkingRobot/docs/superpowers/plans/2026-08-03-em-planner-rolling-execution-implementation.md

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

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

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

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

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

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

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:

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

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

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:

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:

<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

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

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

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

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.