189 lines
9.8 KiB
Markdown
189 lines
9.8 KiB
Markdown
# 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。
|
||
|
||
必须明确：
|
||
|
||
- 只有 `Success` 或 `SuccessWithFallback` 结果中的完整非空轨迹可以交给控制适配器；
|
||
- `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);
|
||
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 项目；
|
||
- 不直接运行车辆或发送任何底盘命令；
|
||
- 不在本次工作中设计完整的多方向自动换向执行器。
|