# EM Closed-Loop Movement Test 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 an `EM闭环测试` MovementTest that plans one complete EM direction segment once, freezes it, adapts it to `Trajectory2D`, and tracks it with the imported Stanley/PID/GCP controller until that segment stops.

**Architecture:** Keep the planning and controller models separate. Restore the reference controller's smallest portable runtime dependency set inside `ClumsyPilot`, add a pure `EmTrajectory`-to-`Trajectory2D` adapter, then make a session runner reuse the existing trajectory-observation bootstrap and one `TrajectoryObservationController.StartCycle` call before handing the frozen trajectory to `TrajectoryTrackingMovement`.

**Tech Stack:** C# 10, .NET Standard 2.0 plugin, .NET 10 Windows verification host, Clumsy `MovementTest`/`DriveTask`, existing EM planner and OSQP runtime.

## Global Constraints

- The MovementTest registration name is exactly `EM闭环测试`.
- Plan exactly once in `EmPlanningScope.FullDirectionSegment`; do not replan or replace the trajectory while driving.
- Execute only the first direction segment. A gear-switch terminal stops and ends this test.
- Shared/control boundaries use SI units: m, m/s, rad, rad/s; body X is forward, body Y is left, counter-clockwise is positive.
- Preserve signed longitudinal velocity: forward positive and reverse negative.
- Do not change velocity/steering signs, CAN IDs, remote mappings, mechanical limits, or mode-switch policy.
- Do not overwrite unrelated dirty files. Stage only the exact files named by each task.
- Real-vehicle validation must remain low-speed, short-distance, in an open area, with hardware emergency stop available.

---

## File Map

### Existing user files retained and brought under version control

- `ClumsyPilot/Control/**/*.cs` — Stanley/PID/GCP controller implementation already copied from the reference project.
- `ClumsyPilot/Trajectory/**/*.cs` — controller-side trajectory and projection implementation already copied from the reference project.

### Portable runtime dependencies copied from the reference project

- `ClumsyPilot/Shared/Mathematics/AngleMath.cs` — angle normalization and conversion.
- `ClumsyPilot/Shared/Mathematics/InterpolationMath.cs` — scalar interpolation.
- `ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs` — world/body transforms.
- `ClumsyPilot/Shared/Models/ChassisCommand.cs` — `Pose2D`, `Twist2D`, and chassis command value types.
- `ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs` — SI/GCP boundary over the existing `MultiWheelChassis`.
- `ClumsyPilot/StateEstimation/IVehicleStateProvider.cs` — controller state-source boundary.
- `ClumsyPilot/StateEstimation/VehicleState.cs` — immutable controller state.
- `ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs` — velocity filter primitive.
- `ClumsyPilot/StateEstimation/VelocityEstimator2D.cs` — Detour velocity estimation.
- `ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs` — live Detour state source.
- `ClumsyPilot/Movements/TrajectoryTrackingMovement.cs` — `DriveTask` movement wrapper around the geometric controller.

### New EM closed-loop files

- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs` — pure model adapter.
- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs` — UI input, session ownership, one-shot planning, controller handoff, and stop behavior.
- `ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs` — adapter behavior checks.
- `ClumsyPilot/tests/verify_em_closed_loop_movement.ps1` — MovementTest registration and one-shot lifecycle checks.
- `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs` — add the `em-control-adapter` verification command.
- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md` — operator procedure and test boundary.

---

### Task 1: Restore the controller's portable runtime dependency chain

**Files:**

- Add and track: `ClumsyPilot/Control/**/*.cs`
- Add and track: `ClumsyPilot/Trajectory/**/*.cs`
- Create: `ClumsyPilot/Shared/Mathematics/AngleMath.cs`
- Create: `ClumsyPilot/Shared/Mathematics/InterpolationMath.cs`
- Create: `ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs`
- Create: `ClumsyPilot/Shared/Models/ChassisCommand.cs`
- Create: `ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs`
- Create: `ClumsyPilot/StateEstimation/IVehicleStateProvider.cs`
- Create: `ClumsyPilot/StateEstimation/VehicleState.cs`
- Create: `ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs`
- Create: `ClumsyPilot/StateEstimation/VelocityEstimator2D.cs`
- Create: `ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs`
- Create: `ClumsyPilot/Movements/TrajectoryTrackingMovement.cs`

**Interfaces:**

- Produces: `MyParking.Shared.Pose2D`, `Twist2D`, `AngleMath`, `MultiWheelChassisAdapter`.
- Produces: `MultiWheelC.StateEstimation.IVehicleStateProvider` and `DetourVehicleStateProvider`.
- Produces: `MultiWheelC.Trajectory.Trajectory2D` and `TrajectoryPoint`.
- Produces: `MultiWheelC.TrajectoryTrackingMovement : MovementDefinition` with public `Trajectory`, `StateProvider`, `CycleObserver`, and safety-limit fields.

- [ ] **Step 1: Re-run the failing integration build**

Run:

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```

Expected: FAIL with missing `MyParking.Shared`, `MultiWheelC.StateEstimation`, `VehicleState`, and `MultiWheelChassisAdapter`. This proves the partial controller import is not silently excluded from the plugin build.

- [ ] **Step 2: Add the exact portable reference sources**

Read each source from:

```text
D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\Shared\...
D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC\StateEstimation\...
D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC\Movements\TrajectoryTrackingMovement.cs
```

Add the files at the destination paths in this task without changing namespaces or logic. Do not import `FleetKinematics`, experiment recorders, composite movements, rotation movements, or wheel-feedback providers because `TrajectoryTrackingMovement` does not consume them.

- [ ] **Step 3: Prove existing controller/trajectory files still match the reference implementation**

Run:

```powershell
$reference = 'D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC'
$local = (Resolve-Path '.\ClumsyPilot').Path
Get-ChildItem '.\ClumsyPilot\Control','.\ClumsyPilot\Trajectory' -File -Recurse | ForEach-Object {
    $relative = $_.FullName.Substring($local.Length + 1)
    $source = Join-Path $reference $relative
    if (-not (Test-Path $source) -or (Get-FileHash $_.FullName).Hash -ne (Get-FileHash $source).Hash) {
        throw "Reference mismatch: $relative"
    }
}
```

Expected: exit 0 and no mismatch.

- [ ] **Step 4: Run the build to verify the dependency chain compiles**

Run:

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```

Expected: PASS with zero errors. Record warnings verbatim; do not hide them.

- [ ] **Step 5: Commit only the controller runtime integration**

```powershell
git add -- ClumsyPilot/Control ClumsyPilot/Trajectory ClumsyPilot/Shared ClumsyPilot/StateEstimation ClumsyPilot/Movements/TrajectoryTrackingMovement.cs
git diff --cached --check
git commit -m "feat: integrate trajectory tracking controller runtime"
```

---

### Task 2: Adapt a frozen EM trajectory to `Trajectory2D`

**Files:**

- Create: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**

- Consumes: `EmTrajectory` with validated SI-unit `EmTrajectoryPoint` values.
- Produces: `public sealed class EmControlTrajectoryAdapter`.
- Produces: `public Trajectory2D Create(EmTrajectory trajectory)`.
- Duplicate-position threshold: `1e-6 m`, matching `Trajectory2D`'s minimum valid projection segment.

- [ ] **Step 1: Register the adapter check before implementing the adapter**

Extend `Program.cs` argument validation with `em-control-adapter`, and add:

```csharp
if (args[0] == "em-control-adapter" || args[0] == "em-all")
{
    EmControlTrajectoryAdapterChecks.Run();
    Console.WriteLine("PASS em-control-adapter");
}
```

Create `EmControlTrajectoryAdapterChecks.cs` with checks that build real `EmTrajectory` objects and assert:

```csharp
internal static void Run()
{
    MapsFieldsAndRebuildsGeometricArcLength();
    KeepsReverseVelocityNegative();
    CollapsesTerminalHoldAndKeepsItsZeroSpeed();
    RejectsFewerThanTwoDistinctPositions();
}
```

The forward mapping check must assert all of these exact relations:

```csharp
Trajectory2D actual = new EmControlTrajectoryAdapter().Create(source);
Require(actual.Count == 3, "adapter point count");
RequireClose(actual[0].ArcLengthMeters, 0d, "first arc");
RequireClose(actual[1].ArcLengthMeters, 0.5d, "second arc");
RequireClose(actual[2].ArcLengthMeters, 1.0d, "third arc");
RequireClose(actual[1].PoseInWorld.XMeters, source.Points[1].X, "x");
RequireClose(actual[1].PoseInWorld.YMeters, source.Points[1].Y, "y");
RequireClose(actual[1].PoseInWorld.YawRadians, source.Points[1].Yaw, "yaw");
RequireClose(actual[1].CurvaturePerMeter, source.Points[1].VehicleCurvature, "curvature");
RequireClose(actual[1].ReferenceSpeedMetersPerSecond,
    source.Points[1].SignedLongitudinalVelocity, "signed speed");
```

For the terminal-hold case, provide two final EM samples at the same pose where the later sample has zero speed, then assert that the output contains one terminal position and `EndPoint.ReferenceSpeedMetersPerSecond == 0d`.

- [ ] **Step 2: Run the new check and verify RED**

Run:

```powershell
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter
```

Expected: FAIL to compile because `EmControlTrajectoryAdapter` does not exist.

- [ ] **Step 3: Implement the minimum adapter**

Implement this public surface and algorithm:

```csharp
public sealed class EmControlTrajectoryAdapter
{
    private const double MinimumSegmentLengthMeters = 1e-6;

    public Trajectory2D Create(EmTrajectory trajectory)
    {
        if (trajectory == null)
            throw new ArgumentNullException(nameof(trajectory));

        var points = new List<TrajectoryPoint>(trajectory.Points.Count);
        double arcLength = 0d;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint source = trajectory.Points[index];
            var pose = new MyParking.Shared.Pose2D(source.X, source.Y, source.Yaw);
            var converted = new TrajectoryPoint(
                arcLength, pose, source.VehicleCurvature, source.SignedLongitudinalVelocity);

            if (points.Count == 0)
            {
                points.Add(converted);
                continue;
            }

            TrajectoryPoint previous = points[points.Count - 1];
            double dx = pose.XMeters - previous.PoseInWorld.XMeters;
            double dy = pose.YMeters - previous.PoseInWorld.YMeters;
            double distance = Math.Sqrt(dx * dx + dy * dy);
            if (distance < MinimumSegmentLengthMeters)
            {
                points[points.Count - 1] = new TrajectoryPoint(
                    previous.ArcLengthMeters, pose, source.VehicleCurvature,
                    source.SignedLongitudinalVelocity);
                continue;
            }

            arcLength += distance;
            points.Add(new TrajectoryPoint(
                arcLength, pose, source.VehicleCurvature,
                source.SignedLongitudinalVelocity));
        }

        if (points.Count < 2)
            throw new ArgumentException(
                "EM轨迹至少需要包含两个不同位置的有效控制点。", nameof(trajectory));

        return new Trajectory2D(points);
    }
}
```

Include `/// <summary>` comments on the class and public method, explicitly stating the unit and signed-speed boundary.

- [ ] **Step 4: Run adapter checks and the existing EM suite**

Run:

```powershell
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- trajectory-observation
```

Expected: both commands PASS.

- [ ] **Step 5: Commit the adapter and checks**

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
git diff --cached --check
git commit -m "feat: adapt EM trajectory for geometric controller"
```

---

### Task 3: Add the one-shot, single-direction `EM闭环测试`

**Files:**

- Create: `ClumsyPilot/tests/verify_em_closed_loop_movement.ps1`
- Create: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs`

**Interfaces:**

- Consumes: `TrajectoryObservationSetupFactory.CreateBootstrapJob(...)`.
- Consumes: `TrajectoryObservationBootstrapper.Bootstrap(...)`.
- Consumes exactly one call to `TrajectoryObservationController.StartCycle(...)`.
- Consumes: `EmControlTrajectoryAdapter.Create(...)`.
- Produces: `[MovementTest(name = "EM闭环测试")] public sealed class EmClosedLoopMovementTest : MovementTest`.
- Produces: `Test()` for session start and `TestStop()` for idempotent cancellation/stop.

- [ ] **Step 1: Write the structural/lifecycle verification first**

Create `verify_em_closed_loop_movement.ps1` so it reads the new source file and fails unless all conditions hold:

```powershell
$sourcePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\tarjplanner_movementtest\MovementTest.EmClosedLoopTest.cs'
if (-not (Test-Path -LiteralPath $sourcePath)) { throw 'EM closed-loop MovementTest source is missing.' }
$source = Get-Content -Raw -Encoding UTF8 -LiteralPath $sourcePath

$required = @(
    '\[MovementTest\(name = "EM闭环测试"\)\]',
    'EmPlanningScope\.FullDirectionSegment',
    'TrajectoryObservationBootstrapper',
    '\.StartCycle\(',
    'EmControlTrajectoryAdapter',
    'TrajectoryTrackingMovement',
    'public override void TestStop\(\)',
    '\.Cancel\(\)',
    '\.Stop\(\)'
)
foreach ($pattern in $required) {
    if ($source -notmatch $pattern) { throw "Missing required pattern: $pattern" }
}

if ([regex]::Matches($source, '\.StartCycle\(').Count -ne 1) {
    throw 'EM closed-loop test must contain exactly one planning-cycle call site.'
}
if ($source -match 'TrajectoryObservationLoop|ReplanPeriod|while\s*\(true\).*StartCycle') {
    throw 'EM closed-loop test must not contain a replanning loop.'
}

Write-Output 'EM closed-loop MovementTest checks passed.'
```

- [ ] **Step 2: Run the verification and confirm RED**

Run:

```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1
```

Expected: FAIL with `EM closed-loop MovementTest source is missing.`

- [ ] **Step 3: Implement the MovementTest input surface**

Add `EmClosedLoopMovementTest` with the same goal, map, obstacle, vehicle, solver, and output-step defaults used by `TrajectoryObservationMovementTest`. Set control defaults explicitly:

```csharp
public double MaximumCommandSpeedMetersPerSecond = 0.20d;
public double MaximumDistanceToTrajectoryMeters = 0.30d;
public double ExecutionTimeoutSeconds = 120d;
public float WheelAlignmentToleranceDegrees = 2f;
```

`Test()` must validate inputs, create a validated settings snapshot with:

```csharp
PlanningScope = EmPlanningScope.FullDirectionSegment
```

and start one session through `EmClosedLoopMovementTestRunner.Start(...)`. `TestStop()` must call only:

```csharp
EmClosedLoopMovementTestRunner.Stop();
```

Use the existing observation test's culture-aware finite-number and bounded-obstacle input behavior. Keep those helpers private to the new test so the user's modified observation file is not edited.

- [ ] **Step 4: Implement idempotent session ownership and one-shot planning**

Use a static runner guarded by one lock. Store one `CancellationTokenSource`, one background `Task`, one active `DriveTask`, and a monotonically increasing session ID.

The planning body must follow this exact sequence:

```csharp
VehicleMotionState initialState = ReadPlanningState();
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
    initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
TrajectoryObservationBootstrapResult bootstrap =
    new TrajectoryObservationBootstrapper().Bootstrap(job, token);
if (!bootstrap.Succeeded)
    throw new InvalidOperationException(bootstrap.FailureReason);

var controller = new TrajectoryObservationController(
    bootstrap, settings, new EmPlanningService(new OsqpNativeSolver()),
    "em-closed-loop-" + sessionId.ToString(CultureInfo.InvariantCulture));
PlanningCycleResult cycle = await controller.StartCycle(
    initialState.CapturedAtUtc, initialState, token).ConfigureAwait(false);
if (!cycle.Published || controller.PublishedTrajectory == null)
    throw new InvalidOperationException(
        string.IsNullOrWhiteSpace(cycle.Result.FailureReason)
            ? cycle.Diagnostic
            : cycle.Result.FailureReason);

Trajectory2D trajectory =
    new EmControlTrajectoryAdapter().Create(controller.PublishedTrajectory);
```

There must be no observation loop and no second `StartCycle` call. Log the EM point count, controller point count, geometric length, segment index, direction, and terminal type before control starts.

Create `EmClosedLoopWheelSafety.AreWheelsForward(float toleranceDegrees)` in the same file using `MultiWheelChassisAdapter.AreParallelWheelsAligned(0d, toleranceRadians)`. If this check fails, throw before constructing `DriveTask`.

Create and run the controller movement as:

```csharp
var movement = new TrajectoryTrackingMovement
{
    Trajectory = trajectory,
    MaximumCommandSpeedMetersPerSecond = maximumCommandSpeedMetersPerSecond,
    MaximumDistanceToTrajectoryMeters = maximumDistanceToTrajectoryMeters,
    ExecutionTimeoutSeconds = executionTimeoutSeconds
};
var driveTask = new DriveTask(movement.Get());
```

Publish `driveTask` under the session lock only if the session is still current and not cancelled. Otherwise stop it immediately. For a current session call `driveTask.Wait()` on the background worker, then report that the first direction segment ended. A `GearSwitch` terminal is reported as “stopped at gear-switch boundary; next segment was not started.”

`Stop()` must atomically clear the active references, call `CancellationTokenSource.Cancel()`, and call `DriveTask.Stop()`. The completion continuation must clear only references that still belong to its own session.

- [ ] **Step 5: Run structural verification and compile**

Run:

```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```

Expected: structural check PASS; build PASS with zero errors.

- [ ] **Step 6: Commit the MovementTest**

```powershell
git add -- ClumsyPilot/tests/verify_em_closed_loop_movement.ps1 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs
git diff --cached --check
git commit -m "feat: add one-shot EM closed-loop movement test"
```

---

### Task 4: Document operation and run the complete non-hardware verification

**Files:**

- Modify: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md`

**Interfaces:**

- Documents: test selection, inputs, one-shot/single-direction boundary, `TestStop`, low-speed first-run procedure, and limits of automated validation.

- [ ] **Step 1: Add the operator-facing section**

Add a section named `EM闭环测试` containing these exact operational facts:

```text
- 启动时只规划一次，并冻结首次成功的完整方向段轨迹。
- 控制执行期间不重规划、不切换轨迹。
- 到达目标或换向边界后停车；本测试不启动下一方向段。
- 默认控制速度上限为 0.20 m/s。
- 规划、适配或车轮方向检查失败时不会启动底盘控制。
- TestStop 会同时取消规划并停止活动控制任务。
- 首次实车测试使用空旷环境、短距离单方向目标，并确保硬件急停可用。
```

- [ ] **Step 2: Run all targeted checks**

Run:

```powershell
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- trajectory-observation
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```

Expected: every command exits 0. Report warning counts from the build.

- [ ] **Step 3: Verify scope and source hygiene**

Run:

```powershell
git diff --check
git status --short
git diff --no-ext-diff -- ClumsyPilot/Control ClumsyPilot/Trajectory ClumsyPilot/Shared ClumsyPilot/StateEstimation ClumsyPilot/Movements ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest ClumsyPilot/tests
```

Expected: no whitespace errors; only files named by this plan are staged or changed by this work. Existing unrelated modifications remain untouched.

- [ ] **Step 4: Commit documentation**

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md
git diff --cached --check
git commit -m "docs: explain EM closed-loop test operation"
```

- [ ] **Step 5: Hand off real-vehicle validation without claiming it passed**

Report separately:

```text
Automated status: adapter checks, observation regressions, structural lifecycle check, and plugin build.
Hardware status: not run by automated verification; requires a low-speed, short, single-direction field test with emergency stop available.
```