ParkingRobot/docs/superpowers/plans/2026-08-11-em-planner-controller-handoff-guide.md

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:

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:

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

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

| 边界 | 类型/方法 | 含义 |
| --- | --- | --- |
| 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:

固定场景
  → 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:

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:

`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:

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:

## 推荐阅读顺序

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:

$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

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:

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:

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:

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:

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:

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:

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

return new FixedOfflinePlanningOutput(
    emTrajectory,
    controllerTrajectory);

• Step 7: Verify numbered coverage and banned placeholders

Run:

$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:

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

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:

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:

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:

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:

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

Composite movement:

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:

$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

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:

$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:

$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:

$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:

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:

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:

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"