# EM Planner Controller Handoff Guide 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 a detailed Chinese README and two fully annotated, copy-readable C# examples that explain the fixed offline planning pipeline and replace the controller test's artificial `Trajectory2D` with an EM-planned trajectory.

**Architecture:** The README owns concepts, configuration locations, units, output contracts, failure semantics, and the numbered walkthrough. `EmPlannerFullPipelineDemo.cs` mirrors those numbers as one continuous offline data-flow example. `EmPlannerTrajectoryReplacementExample.cs` narrows the same contract to the exact `TestTrajectoryFactory` replacement boundary used by the external closed-loop tests.

**Tech Stack:** Markdown, C# source examples, existing `MultiWheelC.TrajectoryPlanning` contracts, Hybrid A*, Local G2 smoothing, `EmPlanningService`, OSQP, `EmControlTrajectoryAdapter`, and `TrajectoryTrackingMovement`.

## Global Constraints

- Deliver copy-readable C# only; do not add a project file or claim the examples are standalone executables.
- Use a fixed offline start, goal, vehicle, and obstacle scene; do not read live localization or send chassis commands.
- Keep every planning step explicit and fully annotated in Chinese; do not use ellipses or placeholder APIs.
- Match current repository type names, constructor signatures, enum values, properties, units, and status semantics.
- Treat `EmPlanningService` as an in-process synchronous service returning `EmPlanningResult` from `Plan(request, cancellationToken)`.
- Publish to the controller only when status is `Success` or `SuccessWithFallback` and the result contains a complete non-null trajectory.
- Keep single-direction `Trajectory2D` consumption separate from multi-direction gear-switch orchestration.
- Preserve the existing `trajectory-planning-flow-demo.html`; reference it as an optional visual companion.
- Do not modify the external `停车机器人-合并测试` tree or unrelated dirty-worktree files.

---

### Task 1: Write the main Chinese handoff guide

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/README.md`
- Reference: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
- Reference: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/README.md`
- Reference: `D:/Users/Desktop/项目/prakrobot/停车机器人-合并测试/parkr_shen/MultiWheelC/Movements/TrajectoryTrackingMovement.cs`
- Reference: `D:/Users/Desktop/项目/prakrobot/停车机器人-合并测试/parkr_shen/MultiWheelC/Experiments/NewControllerTrackingTests.cs`

**Interfaces:**
- Consumes: the approved design in `docs/superpowers/specs/2026-08-11-em-planner-controller-handoff-guide-design.md` and current public planner/controller contracts.
- Produces: numbered sections `步骤 1` through `步骤 15`, configuration tables, output-field tables, status handling, and exact links to both C# examples.

- [ ] **Step 1: Record the failing documentation-presence check**

Run:

```powershell
Test-Path 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide\README.md'
```

Expected before implementation: `False`.

- [ ] **Step 2: Create the README with the integration boundary first**

Start the document with this exact contract:

```markdown
# 轨迹规划服务调用与闭环控制器交接指南

> 本指南展示固定离线场景的完整调用顺序。代码只用于复制阅读和接口交接，不会读取实车定位，也不会向底盘发送命令。

| 边界 | 类型/方法 | 含义 |
| --- | --- | --- |
| EMplanner 正式输出 | `EmTrajectory` | 带时间、位姿、曲率、速度、加速度、jerk 与方向边界的不可变轨迹 |
| 闭环控制器正式输入 | `Trajectory2D` | 几何控制器消费的世界位姿、弧长、曲率和有符号参考速度 |
| 唯一适配入口 | `EmControlTrajectoryAdapter.Create(EmTrajectory)` | 将单方向 EM 轨迹转换为控制器轨迹 |

`EmPlanningService` 是进程内同步服务。调用方创建冻结的 `EmPlanningRequest`，调用 `Plan(request, cancellationToken)`，并仅从本次返回的成功 `EmPlanningResult` 中索取 `Trajectory`。
```

- [ ] **Step 3: Add the complete numbered data flow**

Add sections `步骤 1` through `步骤 15` in the exact order from the approved design. Each section must include four labeled paragraphs: `本步输入`、`调用`、`本步输出`、`失败处理`. Link every step to the matching numbered region in `EmPlannerFullPipelineDemo.cs`.

The flow summary must be:

```text
固定场景
  → PlanningGridMap
  → PlanningRequest
  → HybridAStarPlanner.Plan
  → PathSmoothingRequest
  → PathSmoothingService.Smooth
  → 当前 DirectionSegmentView
  → EmPlanningRequest
  → EmPlanningService.Plan
  → EmPlanningResult.Trajectory
  → EmControlTrajectoryAdapter.Create
  → Trajectory2D
  → TrajectoryTrackingMovement
```

- [ ] **Step 4: Add configuration source and meaning tables**

Create four tables: `粗路径配置`、`Local G2 平滑配置`、`EM 配置`、`闭环控制配置`. Every row must include `参数`、`源码位置`、`单位`、`固定 demo 值/当前默认值`、`含义`、`调大/调小影响`.

At minimum cover:

```text
VehicleParameters.LengthMeters / WidthMeters / SafetyMarginMeters / MaximumCurvaturePerMeter
HybridAStarConfiguration primitive length, heading bins, goal tolerances, search limits
PathSmoothingConfiguration.OutputSpacingMeters / MaximumCollisionCheckStepMeters / MinimumClearanceReserveMeters
LocalG2QuinticOptions window lengths, deviation, curvature jump, improvement and candidate limits
EmPlannerConfiguration.Corridor / Frenet / Lateral / Longitudinal / Solver / Scheduling / Validation
TrajectoryTrackingMovement Stanley gains, PID gains, speed limit, finish tolerances, deviation limit, timeout
```

Add a dedicated unit table that explicitly distinguishes `m` vs `mm`, `rad` vs `deg`, `1/m`, `m/s`, `m/s²`, `m/s³`, `rad/s`, world frame, body frame, and signed longitudinal speed.

- [ ] **Step 5: Add complete result and field contracts**

Document `EmPlanningResult`, `EmTrajectoryMetadata`, and every public `EmTrajectoryPoint` field that affects downstream interpretation. Include this warning verbatim:

```markdown
`VelocityX` 和 `VelocityY` 是世界坐标系中的预测速度分量，不是底盘纵向/横向命令；闭环测试不得把它们直接发送给车辆。
```

Explain that `EmControlTrajectoryAdapter` maps `X/Y/Yaw`, `VehicleCurvature`, and `SignedLongitudinalVelocity`, while rebuilding controller arc length from world positions.

- [ ] **Step 6: Add service request, success, failure, and gear-switch guidance**

Show the exact retrieval pattern:

```csharp
var planningService = new EmPlanningService(new OsqpNativeSolver());
EmPlanningResult result = planningService.Plan(request, cancellationToken);

bool succeeded = result.Status == EmPlanningStatus.Success ||
    result.Status == EmPlanningStatus.SuccessWithFallback;
if (!succeeded || result.Trajectory == null)
{
    throw new InvalidOperationException(
        "EM 规划未返回可交给控制器的完整轨迹：" + result.FailureReason);
}

EmTrajectory emTrajectory = result.Trajectory;
Trajectory2D controllerTrajectory =
    new EmControlTrajectoryAdapter().Create(emTrajectory);
```

Add a status decision table covering invalid input, infeasible planning, cancellation, deadline/timeout, empty trajectory, direction mismatch, and adapter rejection. Explain that the fixed demo is single-direction; a gear-switch boundary requires a full stop and a separately confirmed next-direction plan rather than concatenation into one `Trajectory2D`.

- [ ] **Step 7: Add the external-test replacement map and reading order**

Identify the current artificial source as `TestTrajectoryFactory.CreateStraight4Meters` in `NewControllerTrackingTests.cs`, and the controller sink as `TrajectoryTrackingMovement.Trajectory`. State that only the trajectory-source block changes; the existing state provider and controller tuning remain owned by the test.

End with:

```markdown
## 推荐阅读顺序

1. 先完整阅读本 README。
2. 顺序阅读 `EmPlannerFullPipelineDemo.cs` 的步骤 1–15。
3. 最后复制 `EmPlannerTrajectoryReplacementExample.cs` 中与目标测试相符的替换方式。
4. 如需流程图，再打开 `trajectory-planning-flow-demo.html`；HTML 不是接口规范。
```

- [ ] **Step 8: Run the README structural check**

Run:

```powershell
$guide = 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide\README.md'
Test-Path $guide
rg -n "步骤 1|步骤 15|EmPlanningService|EmPlanningResult|EmTrajectoryPoint|Trajectory2D|TestTrajectoryFactory|VelocityX|换向|参数" $guide
```

Expected: `True`; every required concept has at least one match, and steps 1 and 15 are present.

- [ ] **Step 9: Commit the README**

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/README.md
git commit -m "docs: add EM planner controller handoff guide"
```

### Task 2: Add the fully annotated fixed offline pipeline example

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerFullPipelineDemo.cs`
- Reference: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs`
- Reference: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs`

**Interfaces:**
- Consumes: real coarse-path, Local G2, EM, and control-adapter APIs; README steps 1–15.
- Produces: `EmPlannerFullPipelineDemo.CreateFixedOfflineTrajectory(CancellationToken)` returning a `FixedOfflinePlanningOutput` that exposes both `EmTrajectory` and `Trajectory2D`.

- [ ] **Step 1: Record the failing example-presence check**

Run:

```powershell
Test-Path 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide\EmPlannerFullPipelineDemo.cs'
```

Expected before implementation: `False`.

- [ ] **Step 2: Create the example shell and explicit output contract**

Use this public shape:

```csharp
public sealed class FixedOfflinePlanningOutput
{
    public FixedOfflinePlanningOutput(
        EmTrajectory emTrajectory,
        Trajectory2D controllerTrajectory)
    {
        EmTrajectory = emTrajectory ??
            throw new ArgumentNullException(nameof(emTrajectory));
        ControllerTrajectory = controllerTrajectory ??
            throw new ArgumentNullException(nameof(controllerTrajectory));
    }

    public EmTrajectory EmTrajectory { get; }
    public Trajectory2D ControllerTrajectory { get; }
}

public static class EmPlannerFullPipelineDemo
{
    public static FixedOfflinePlanningOutput CreateFixedOfflineTrajectory(
        CancellationToken cancellationToken);
}
```

The declaration above defines the reader-facing signature. The final class must give that method the complete numbered body produced by steps 3–6; it must not remain declaration-only.

- [ ] **Step 3: Implement steps 1–5 with a complete fixed scene and coarse plan**

Use explicit SI-valued constants for start, goal, vehicle length/width, safety margin, curvature limit, map resolution, and at least one fixed obstacle. Construct the map with `MapBoundsMm`, `CircleObstacle` or `AxisAlignedRectangleObstacle`, `ManualObstacleSource`, `PlanningMapRequest`, and `new PlanningMapFactory().Create(mapRequest)`. Require `PlanningMapBuildResult.Succeeded`, `Map != null`, and `Map.PlanningReady`. Then construct `PlanningRequest` and call `new HybridAStarPlanner().Plan(request, cancellationToken)`.

Reject non-success with diagnostics:

```csharp
if (coarseResult.Status != PlanningStatus.Success)
{
    throw new InvalidOperationException(
        "Hybrid A* 粗路径失败：" +
        coarseResult.Diagnostics.TerminationReason);
}
```

- [ ] **Step 4: Implement steps 6–7 with Local G2 and a single active direction segment**

Construct `PathSmoothingRequest` from the successful coarse path, segments, frozen map, vehicle, and explicit `PathSmoothingConfiguration`. Call `new PathSmoothingService().Smooth(smoothingRequest, cancellationToken)`. Accept only `Complete`, `PartialImprovement`, `NotNeeded`, or `Unchanged`, and reject empty path/segment results.

Call `ReferencePathSegmenter.Create(smoothingResult)`, select segment index `0`, and assert that the fixed scene remains one direction. If the result contains more than one direction segment, throw an exception explaining that this fixed controller demo intentionally does not cross a gear-switch boundary.

- [ ] **Step 5: Implement steps 8–11 with state, configuration, request, and service call**

Use zero initial signed speed/acceleration and the fixed start pose for `VehicleMotionState`. Create `EmPlannerConfiguration` with explicit demo overrides, copy the repository's request-construction order, set `EmPlanningScope.FullDirectionSegment`, and invoke:

```csharp
var planningService = new EmPlanningService(new OsqpNativeSolver());
EmPlanningResult emResult = planningService.Plan(
    emRequest,
    cancellationToken);
```

Every constructor argument must have an adjacent Chinese comment giving its source and unit where applicable.

- [ ] **Step 6: Implement steps 12–15 with validation, field reading, adaptation, and return**

Use this publication gate:

```csharp
bool emSucceeded = emResult.Status == EmPlanningStatus.Success ||
    emResult.Status == EmPlanningStatus.SuccessWithFallback;
if (!emSucceeded ||
    emResult.Trajectory == null ||
    emResult.Trajectory.Points.Count < 2)
{
    throw new InvalidOperationException(
        "EM 规划没有返回完整可控轨迹：" + emResult.FailureReason);
}
```

Iterate through the points and assign local variables for time, `X/Y/Yaw`, curvature, `PathS`, signed speed, world velocity components, acceleration, jerk, and yaw rate. The comments must explain that this is inspection, not command output.

Finish with:

```csharp
EmTrajectory emTrajectory = emResult.Trajectory;
Trajectory2D controllerTrajectory =
    new EmControlTrajectoryAdapter().Create(emTrajectory);

return new FixedOfflinePlanningOutput(
    emTrajectory,
    controllerTrajectory);
```

- [ ] **Step 7: Verify numbered coverage and banned placeholders**

Run:

```powershell
$demo = 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide\EmPlannerFullPipelineDemo.cs'
1..15 | ForEach-Object {
    if (-not (Select-String -Path $demo -Pattern ("步骤 " + $_) -Quiet)) {
        throw "缺少步骤 $_"
    }
}
rg -n "TODO|TBD|[.][.][.]|NotImplementedException" $demo
```

Expected: all numbered checks complete without an exception; `rg` prints no matches and exits with code `1` because the banned patterns are absent.

- [ ] **Step 8: Cross-check every API token against source**

Run targeted searches for every constructed public type and method:

```powershell
rg -n "class (PlanningRequest|HybridAStarPlanner|PathSmoothingRequest|PathSmoothingService|EmPlanningRequest|EmPlanningService|EmControlTrajectoryAdapter)|Plan\(|Smooth\(|Create\(" ClumsyPilot/ParkrobTrajplanner -g '*.cs'
```

Expected: each type and call used by the example resolves to a current repository declaration; adjust the example to the source signature before continuing.

- [ ] **Step 9: Commit the full pipeline example**

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerFullPipelineDemo.cs
git commit -m "docs: add annotated EM planning pipeline example"
```

### Task 3: Add the closed-loop test replacement example

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerTrajectoryReplacementExample.cs`
- Reference: `D:/Users/Desktop/项目/prakrobot/停车机器人-合并测试/parkr_shen/MultiWheelC/Experiments/NewControllerTrackingTests.cs`
- Reference: `D:/Users/Desktop/项目/prakrobot/停车机器人-合并测试/parkr_shen/MultiWheelC/Movements/TrajectoryTrackingMovement.cs`
- Reference: `D:/Users/Desktop/项目/prakrobot/停车机器人-合并测试/parkr_shen/MultiWheelC/Movements/MotionPlanExecutor.cs`

**Interfaces:**
- Consumes: `FixedOfflinePlanningOutput`, `EmTrajectory`, `EmControlTrajectoryAdapter`, `Trajectory2D`, and the external test's existing state-provider/controller setup.
- Produces: exact before/after source blocks plus direct `TrajectoryTrackingMovement` and `TrackMotionPlanSegment` consumption examples.

- [ ] **Step 1: Record the failing replacement-example presence check**

Run:

```powershell
Test-Path 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide\EmPlannerTrajectoryReplacementExample.cs'
```

Expected before implementation: `False`.

- [ ] **Step 2: Add a fully parameterized before block**

Copy the current artificial trajectory call without abbreviating arguments:

```csharp
Trajectory2D trajectory =
    TestTrajectoryFactory.CreateStraight4Meters(
        trajectoryStartPose,
        CruiseSpeedMetersPerSecond,
        AccelerationMetersPerSecondSquared,
        DecelerationMetersPerSecondSquared,
        PointSpacingMeters);
```

Comment that this line generates controller test data and does not call the formal planner.

- [ ] **Step 3: Add the after block and explain how the test obtains the result**

Use:

```csharp
FixedOfflinePlanningOutput planningOutput =
    EmPlannerFullPipelineDemo.CreateFixedOfflineTrajectory(
        cancellationToken);

EmTrajectory emTrajectory = planningOutput.EmTrajectory;
Trajectory2D trajectory = planningOutput.ControllerTrajectory;
```

Explain that the full demo internally performs the synchronous `EmPlanningService.Plan` call and publication gate. Also show the equivalent direct retrieval block with `EmPlanningResult`, status check, `result.Trajectory`, and `EmControlTrajectoryAdapter.Create` so the reader understands how to extract the trajectory without using the demo wrapper.

- [ ] **Step 4: Add both controller consumption paths without changing tuning ownership**

Direct movement:

```csharp
var movement = new TrajectoryTrackingMovement
{
    Trajectory = trajectory,
    StateProvider = stateProvider
};
```

Composite movement:

```csharp
var segment = new TrackMotionPlanSegment(trajectory);
```

Comment that existing `ConfigureTrackingMovement`, finish tolerances, observers, state provider, and safety checks stay in the controller test; only the trajectory source changes.

- [ ] **Step 5: Add rejection examples for failure and gear switch**

Include explicit guards that reject null/failed EM results and state that a single `TrajectoryTrackingMovement` invocation consumes one direction segment. Show an exception for `EmTerminalType.GearSwitch` unless the surrounding test has implemented and safety-reviewed the stop-and-direction-confirmation sequence.

- [ ] **Step 6: Verify the replacement map**

Run:

```powershell
$example = 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide\EmPlannerTrajectoryReplacementExample.cs'
rg -n "TestTrajectoryFactory.CreateStraight4Meters|EmPlanningService|EmPlanningResult|result.Trajectory|EmControlTrajectoryAdapter|TrajectoryTrackingMovement|TrackMotionPlanSegment|GearSwitch" $example
rg -n "TestTrajectoryFactory.CreateStraight4Meters|Trajectory = trajectory" 'D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC' -g '*.cs'
```

Expected: the guide contains every acquisition and consumption boundary; the external tree still shows its original test source and sink because it was not modified.

- [ ] **Step 7: Commit the replacement example**

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerTrajectoryReplacementExample.cs
git commit -m "docs: show EM trajectory replacement in controller tests"
```

### Task 4: Verify the complete guide as one handoff artifact

**Files:**
- Verify: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/README.md`
- Verify: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerFullPipelineDemo.cs`
- Verify: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerTrajectoryReplacementExample.cs`
- Preserve: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/trajectory-planning-flow-demo.html`

**Interfaces:**
- Consumes: all three guide deliverables and the real planner/controller sources.
- Produces: fresh evidence that the guide is complete, cross-linked, placeholder-free, and limited to the intended files.

- [ ] **Step 1: Verify all expected files and cross-links**

Run:

```powershell
$root = 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide'
@(
    "$root\README.md",
    "$root\EmPlannerFullPipelineDemo.cs",
    "$root\EmPlannerTrajectoryReplacementExample.cs",
    "$root\trajectory-planning-flow-demo.html"
) | ForEach-Object {
    if (-not (Test-Path $_)) { throw "缺少指南文件：$_" }
}
rg -n "EmPlannerFullPipelineDemo.cs|EmPlannerTrajectoryReplacementExample.cs|trajectory-planning-flow-demo.html" "$root\README.md"
```

Expected: all files exist and README links to both C# examples and the HTML companion.

- [ ] **Step 2: Verify completeness and placeholder absence**

Run:

```powershell
$root = 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide'
rg -n "TODO|TBD|NotImplementedException|[.][.][.]" "$root\README.md" "$root\EmPlannerFullPipelineDemo.cs" "$root\EmPlannerTrajectoryReplacementExample.cs"
```

Expected: no matches; `rg` exits with code `1`.

- [ ] **Step 3: Verify documentation-source correspondence**

Run:

```powershell
$root = 'ClumsyPilot\ParkrobTrajplanner\Trajplanner_guide'
rg -n "PlanningGridMap|HybridAStarPlanner|PathSmoothingService|EmPlanningRequest|EmPlanningService|EmTrajectoryPoint|EmControlTrajectoryAdapter" "$root\README.md" "$root\EmPlannerFullPipelineDemo.cs"
rg -n "class (HybridAStarPlanner|PathSmoothingService|EmPlanningRequest|EmPlanningService|EmTrajectoryPoint|EmControlTrajectoryAdapter)" ClumsyPilot/ParkrobTrajplanner -g '*.cs'
```

Expected: every documented integration type is present in the guide and resolves to a real repository declaration.

- [ ] **Step 4: Run repository-level static checks**

Run:

```powershell
git diff --check
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
```

Expected: `git diff --check` exits `0`; build exits `0`. If the already-dirty worktree causes an unrelated build failure, record the exact failing project/file and separately report that the guide's static checks passed.

- [ ] **Step 5: Audit scope and final diff**

Run:

```powershell
git status --short
git diff --no-ext-diff -- ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide docs/superpowers/plans/2026-08-11-em-planner-controller-handoff-guide.md
```

Expected: guide changes are limited to README and the two new C# examples; the existing HTML remains preserved. Unrelated pre-existing changes remain untouched.

- [ ] **Step 6: Commit any final guide-only corrections**

If verification required guide corrections, stage only the three guide deliverables and commit:

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/README.md ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerFullPipelineDemo.cs ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerTrajectoryReplacementExample.cs
git commit -m "docs: finalize EM planner trajectory handoff guide"
```