docs: design EM planner controller handoff guide

2026-08-11 15:53:11 +08:00
parent c7deb169da
commit 99b15d0d96
1 changed files with 186 additions and 0 deletions
@@ -0,0 +1,186 @@
 # 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 项目；
 - 不直接运行车辆或发送任何底盘命令；
 - 不在本次工作中设计完整的多方向自动换向执行器。