# 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 + 完整流程示例 + 控制器替换示例”的结构：

```text
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 开头直接说明两个类型边界：

```text
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。

必须明确：

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

### 5. 单方向、换向和终点

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

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

### 6. 失败和安全处理

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

## 完整流程示例设计

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

示例包含：

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

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

## 控制器替换示例设计

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

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

// 替换后：从规划服务结果索取并适配
EmPlanningResult result = planningService.Plan(request, cancellationToken);
if (result.Status != EmPlanningStatus.Success || 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 项目；
- 不直接运行车辆或发送任何底盘命令；
- 不在本次工作中设计完整的多方向自动换向执行器。
