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

EM Planner 控制器交接指南设计

目标

在 ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide 中提供一套无需编译、可以直接复制阅读的中文指南和 C# 示例，使闭环控制器维护者能够完整理解：

1. 固定离线场景中的起点、终点、车辆参数和障碍物如何形成规划输入；
2. Hybrid A* 粗路径、Local G2 平滑路径和 EM 时间轨迹依次如何产生；
3. EmPlanningService 如何调用，调用方何时可以从 EmPlanningResult 索取完整轨迹；
4. EmTrajectory 输出包含什么、字段单位和坐标语义是什么；
5. 如何把 EM 输出转换成闭环测试消费的 Trajectory2D，替换当前 TestTrajectoryFactory 人工轨迹。

交付物仅为指南和示例代码，不新增可执行项目，不改变规划器或闭环控制器行为。

读者与使用边界

主要读者是维护 停车机器人-合并测试/parkr_shen/MultiWheelC/Movements 和对应 Experiments 闭环测试的控制器开发者。指南假设读者了解 C#，但不了解本仓库规划模块。

示例采用固定、离线、静态场景，目的是展示真实 API 的完整调用关系，而不是提供可以直接启动车辆的测试入口。示例不得读取实时定位、直接发送底盘命令或暗示规划成功即可无条件接管车辆。

方案与文件结构

采用“详细 README + 完整流程示例 + 控制器替换示例”的结构：

ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/
├── README.md
├── EmPlannerFullPipelineDemo.cs
├── EmPlannerTrajectoryReplacementExample.cs
└── trajectory-planning-flow-demo.html

• README.md 是主指南，负责步骤解释、配置索引、输出契约、错误处理和集成边界。
• EmPlannerFullPipelineDemo.cs 是一份连续的固定离线 C# 示例，从场景输入一直写到 Trajectory2D。
• EmPlannerTrajectoryReplacementExample.cs 聚焦闭环测试中的轨迹索取和替换点，提供替换前后对照。
• 现有 trajectory-planning-flow-demo.html 保留为辅助流程图；README 会链接它，但完整理解不依赖浏览器或 HTML。

示例代码无需独立编译，但所有命名、构造参数、状态判断和字段访问必须与当前仓库真实 API 对齐。为了阅读连续性，允许在示例中使用少量明确标注为“宿主提供”的辅助方法；每个辅助方法都必须说明其输入、输出和真实系统替换来源，不能以省略号隐藏规划步骤。

README 内容设计

1. 首屏结论

README 开头直接说明两个类型边界：

EMplanner 正式输出：EmTrajectory
闭环控制器正式输入：Trajectory2D
桥接方法：EmControlTrajectoryAdapter.Create(EmTrajectory)

同时明确 EmPlanningService 是进程内同步服务，不是 HTTP、RPC、消息队列或后台轨迹仓库。调用方构造冻结的 EmPlanningRequest，调用 Plan(request, cancellationToken)，并从本次返回的 EmPlanningResult 中索取轨迹。

2. 完整数据流

按以下顺序逐步解释，每一步与完整示例中的编号注释一致：

1. 定义世界坐标系固定起点和终点；
2. 定义车辆尺寸、曲率约束、安全边界和规划预算；
3. 建立带固定障碍物的 PlanningGridMap；
4. 建立粗路径 PlanningRequest；
5. 调用 HybridAStarPlanner.Plan 并拒绝非成功结果；
6. 建立 PathSmoothingRequest 并调用 Local G2 平滑；
7. 从平滑结果选择当前方向段，禁止跨换向边界混合；
8. 建立初始 VehicleMotionState；
9. 建立并冻结 EmPlannerConfiguration；
10. 建立 EmPlanningRequest；
11. 使用 EmPlanningService(new OsqpNativeSolver()).Plan(request, cancellationToken)；
12. 检查状态、失败原因、轨迹非空和点数；
13. 读取 EmTrajectory.Metadata 和 EmTrajectory.Points；
14. 使用 EmControlTrajectoryAdapter.Create(result.Trajectory) 转换为 Trajectory2D；
15. 把结果赋给 TrajectoryTrackingMovement.Trajectory 或包装为 TrackMotionPlanSegment。

每一步都说明“输入来自哪里、调用哪个函数、返回什么、失败后做什么、下一步消费什么”。

3. 配置索引

README 按模块列出配置的源码位置和作用：

• CoarsePath：车辆外廓、地图分辨率、运动原语、目标容差、搜索预算；
• PathSmoothing：采样间距、Local G2 窗口和连续性相关参数；
• EMPlanner/Configuration：走廊、Frenet、横向、纵向、求解器、调度和验证参数；
• TrajectoryTrackingMovement：Stanley、纵向 PID、速度上限、终点容差、偏离保护和执行超时。

每个对集成有影响的参数表必须包含：字段名、源码文件、单位、示例值、含义、调大/调小的主要影响。README 还要区分：

• 规划安全约束与控制器执行参数；
• 米和毫米；
• 弧度和角度；
• 世界坐标速度分量与车体纵向有符号速度；
• 滚动视界字段与完整方向段规划字段。

4. 输出契约

README 逐项说明：

• EmPlanningResult.Status、FailureReason、Trajectory；
• EmTrajectory.Metadata 中的轨迹身份、生效时间、方向段、方向、终端类型和规划范围；
• EmTrajectoryPoint 中时间、世界坐标位置、航向、曲率、路径弧长、带符号纵向速度、世界速度分量、加速度、jerk 和 yaw rate。

必须明确：

• 只有 Success 或 SuccessWithFallback 结果中的完整非空轨迹可以交给控制适配器；
• VelocityX/VelocityY 是世界坐标预测分量，不能直接当底盘命令；
• 前进速度为正、倒车速度为负；
• 控制适配器使用世界位姿、车辆曲率和带符号纵向速度，并按世界位置重建从零开始的控制弧长；
• 轨迹是不可变结果，调用方不应在交给控制器前原地修改点列。

5. 单方向、换向和终点

固定 demo 使用单方向段，保证其最终结果可以直接转换成一个 Trajectory2D。README 仍需完整解释多方向粗路径：

• EM 每次规划一个活动方向段；
• 换向边界必须先精确停车；
• 不得跨方向或边界对轨迹点插值；
• 多方向任务应按段规划和执行，并由安全审查后的换向状态机确认方向，再请求下一段；
• 不得把多个正负方向段简单拼成一个供当前几何控制器连续跟踪的 Trajectory2D。

6. 失败和安全处理

README 为以下情况给出明确处理：粗路径失败、平滑失败、EM 求解失败、取消、超时、空轨迹、少于两个不同位置的控制点、单位错误、方向段不匹配。失败示例必须停止在控制输入边界，不创建或下发替代移动命令。

完整流程示例设计

EmPlannerFullPipelineDemo.cs 使用一个固定单方向停车场景。文件采用连续编号的区域和中文注释，注释必须同时回答“为什么”和“单位是什么”，而不是简单复述代码。

示例包含：

• 所有必需的 using；
• 固定场景常量；
• 场景、地图、车辆、粗规划、平滑和 EM 配置的建立；
• 每一层结果的显式状态检查；
• EmPlanningService 调用；
• 至少一段遍历输出点并解释字段的代码；
• EmControlTrajectoryAdapter 转换；
• 返回一个包含原始 EmTrajectory 与控制 Trajectory2D 的只读示例结果对象，以便读者看清两种输出并存而非互相替代。

示例必须完整展开所有步骤，不得使用省略号或待办标记。如果真实 API 需要由宿主提供地图栅格写入或固定障碍物离散化，示例必须给出完整辅助函数，或明确引用仓库中已有的完整公共入口。

控制器替换示例设计

EmPlannerTrajectoryReplacementExample.cs 以对方现有测试为上下文，直接展示：

// 替换前：人工测试轨迹
Trajectory2D trajectory = TestTrajectoryFactory.CreateStraight4Meters(
    trajectoryStartPose,
    CruiseSpeedMetersPerSecond,
    AccelerationMetersPerSecondSquared,
    DecelerationMetersPerSecondSquared,
    PointSpacingMeters);

// 替换后：从规划服务结果索取并适配
EmPlanningResult result = planningService.Plan(request, cancellationToken);
bool succeeded = result.Status == EmPlanningStatus.Success ||
    result.Status == EmPlanningStatus.SuccessWithFallback;
if (!succeeded || result.Trajectory == null)
    throw new InvalidOperationException(result.FailureReason);

Trajectory2D trajectory = adapter.Create(result.Trajectory);

文件随后分别展示两种消费方式：

1. new TrajectoryTrackingMovement { Trajectory = trajectory, StateProvider = stateProvider }；
2. new TrackMotionPlanSegment(trajectory)。

注释会指出对方测试中当前人工轨迹建立位置，并说明真正合并时需要替换的仅是“轨迹来源”，控制器参数和车辆状态源不应被示例暗中改写。

一致性与验证

由于交付物不要求独立编译，验证重点是静态真实性和可追踪性：

1. 逐个核对示例中的类型、构造函数、枚举、属性和方法与仓库源代码一致；
2. 检查 README 的步骤编号与 C# 注释编号一一对应；
3. 检查配置表中的路径、字段和单位均可在源码定位；
4. 检查替换示例准确对应外部闭环测试的 TestTrajectoryFactory 调用和 TrajectoryTrackingMovement.Trajectory；
5. 扫描交付物，确保没有省略号、占位符或未解释的伪 API；
6. 检查工作区差异，确保只修改指南范围内文件及本设计/计划文档，不覆盖用户现有改动。

非目标

• 不修改 EmPlanningService、规划算法或配置默认值；
• 不修改外部 停车机器人-合并测试 目录；
• 不创建网络服务或新的跨进程传输协议；
• 不创建可执行 demo 项目；
• 不直接运行车辆或发送任何底盘命令；
• 不在本次工作中设计完整的多方向自动换向执行器。