165 lines
7.8 KiB
Markdown
165 lines
7.8 KiB
Markdown
# Trajplanner_output 真实轨迹序列 Demo
|
||
|
||
`Trajplanner_output` 是面向学习、调参和控制模块对接的独立控制台示例。它在一个明确允许的空地图演示场景中依次执行粗路径规划、Local G2 平滑和真实 OSQP EM 规划，最终得到不可变 `EmTrajectory`，导出 CSV，并投影为控制模块可读取的只读轨迹序列。
|
||
|
||
它不读取真实定位、传感器或底盘状态，不驱动、转向、制动或换向车辆。演示空地图只用于理解接口与算法链路，不能替代真实作业地图。
|
||
|
||
## 模块说明（Module Overview）
|
||
|
||
| 模块 | 负责内容 | 不负责内容 |
|
||
| --- | --- | --- |
|
||
| `TrajectoryOutputDemoConfiguration` | 集中保存地图、起终点、车辆、曲率、初速和输出路径 | 运行时读取 UI 或硬件参数 |
|
||
| `TrajectoryOutputDemoRunner` | 串联 CoarsePath、PathSmoothing 与真实 OSQP EM 规划 | 发布硬件命令或伪造失败轨迹 |
|
||
| `ControlModuleTrajectoryAdapter` | 将 `EmTrajectory` 转为只读控制序列 | 插值、采样或底盘协议转换 |
|
||
| `TrajectorySequenceExporter` | 原子导出稳定字段顺序的 UTF-8 CSV | 将 CSV 当作车辆命令发送 |
|
||
| `Program` | 打印轨迹摘要、诊断和 CSV 路径 | 逐行打印轨迹或修改规划结果 |
|
||
|
||
唯一的轨迹生成入口是：
|
||
|
||
```csharp
|
||
TrajectoryOutputDemoResult result =
|
||
new TrajectoryOutputDemoRunner().Run(configuration);
|
||
```
|
||
|
||
## 文件结构（File Structure）
|
||
|
||
```text
|
||
Trajplanner_output/
|
||
├── TrajectoryOutputDemo.csproj # 独立 net10.0-windows 控制台项目
|
||
├── Program.cs # 运行入口和轨迹摘要
|
||
├── TrajectoryOutputDemoConfiguration.cs # 唯一调参位置
|
||
├── TrajectoryOutputDemoRunner.cs # 粗路径、平滑和真实 EM 规划编排
|
||
├── ControlModuleTrajectoryAdapter.cs # EM 轨迹到控制只读序列的映射
|
||
├── TrajectorySequenceExporter.cs # 原子 CSV 导出
|
||
├── README.md # 本说明
|
||
└── Tests/
|
||
├── TrajectoryOutputDemo.Tests.csproj # 真实 OSQP 自检项目
|
||
└── Program.cs # CSV 和逐点映射契约验证
|
||
```
|
||
|
||
## 轨迹数据流（Trajectory Data Flow）
|
||
|
||
```text
|
||
TrajectoryOutputDemoConfiguration
|
||
▼
|
||
PlanningMapRequest（显式允许的空地图）
|
||
▼
|
||
CoarsePathPlanningService
|
||
▼
|
||
PathSmoothingService（Local G2）
|
||
▼
|
||
EmPlanningService(new OsqpNativeSolver())
|
||
▼
|
||
EmPlanningResult
|
||
│ 仅 Success / SuccessWithFallback 且 Trajectory 非空
|
||
▼
|
||
EmTrajectory.Points + Metadata
|
||
├── ControlModuleTrajectoryAdapter
|
||
│ └── ControlTrajectorySequence
|
||
└── TrajectorySequenceExporter
|
||
└── output/trajectory.csv
|
||
```
|
||
|
||
中间任一阶段失败都会立刻停止，打印状态与诊断，且不会导出部分或伪造的 CSV。
|
||
|
||
## 全链路可视化（Offline Flow Demo）
|
||
|
||
双击打开 [trajectory-planning-flow-demo.html](trajectory-planning-flow-demo.html)，可在同一米制地图中按阶段查看地图、Hybrid A* 粗路径、Local G2 平滑路径、`EmTrajectory` 和控制层采样命令。该页面使用内置说明数据，帮助理解版本身份与数据边界；它不调用 OSQP、定位、硬件或真实控制器。
|
||
|
||
控制器只消费已验证完整轨迹的采样结果或执行层导出的 `TrajectoryControlCommand`。粗路径、平滑路径、地图和规划诊断属于观测、回放和问题定位证据链，不是逐周期的底盘命令。
|
||
|
||
## 运行（Run）
|
||
|
||
在仓库根目录执行：
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj
|
||
```
|
||
|
||
成功时控制台会输出轨迹 ID、生效时间、方向段、终端类型、轨迹点数量、首末点和 CSV 绝对路径。默认 CSV 位于 Demo 程序输出目录下的 `output/trajectory.csv`。
|
||
|
||
运行自检：
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/Tests/TrajectoryOutputDemo.Tests.csproj
|
||
```
|
||
|
||
自检使用真实 OSQP，不使用假求解器；它验证成功轨迹存在、CSV 存在、CSV 表头稳定，且控制序列点数与 `EmTrajectory.Points` 相同。
|
||
|
||
## 调参（Configuration）
|
||
|
||
所有 Demo 参数集中在 `TrajectoryOutputDemoConfiguration.CreateDefault(csvOutputPath)`：
|
||
|
||
| 参数 | 单位 | 默认值 | 作用 |
|
||
| --- | --- | ---: | --- |
|
||
| `MapBounds` | mm | `0..6000 × 0..4000` | 演示地图范围 |
|
||
| `MapResolutionMillimeters` | mm | `50` | 占据栅格分辨率 |
|
||
| `Start` / `Goal` | m, rad | `(1,1,0)` / `(3,1,0)` | 车辆几何中心位姿 |
|
||
| `VehicleLengthMeters` / `VehicleWidthMeters` | m | `0.80 / 0.60` | 车辆矩形尺寸 |
|
||
| `SafetyMarginMeters` | m | `0.05` | 车辆外扩安全余量 |
|
||
| `MaximumCurvaturePerMeter` | 1/m | `1 / 1.20` | 最大允许曲率 |
|
||
| `InitialSignedSpeedMetersPerSecond` | m/s | `0` | 初始带符号纵向速度 |
|
||
| `CsvOutputPath` | 文件路径 | 运行时指定 | 完整 CSV 输出位置 |
|
||
|
||
调整参数后应重新运行自检。若更换为真实作业场景，必须将 `ObstacleSources` 替换为有效地图来源，并取消演示空地图策略。
|
||
|
||
## CSV 契约（CSV Contract）
|
||
|
||
首行固定为：
|
||
|
||
```text
|
||
time_s,x_m,y_m,yaw_rad,signed_velocity_mps,yaw_rate_radps,curvature_per_m,direction,segment_index,path_s_m,boundary_type
|
||
```
|
||
|
||
每一行与一个不可变 `EmTrajectoryPoint` 一一对应。CSV 使用 UTF-8 无 BOM 和不受系统区域设置影响的小数点格式；导出过程先写临时文件，再替换最终文件，避免读取方获得半写入内容。
|
||
|
||
| 字段 | 单位 / 语义 |
|
||
| --- | --- |
|
||
| `time_s` | 自轨迹生效时刻起的秒数，严格递增 |
|
||
| `x_m`, `y_m`, `yaw_rad` | 世界位置和航向 |
|
||
| `signed_velocity_mps` | 带符号纵向速度；前进为正、倒车为负 |
|
||
| `yaw_rate_radps` | 世界航向角速度，不是转向角 |
|
||
| `curvature_per_m` | 车辆曲率 |
|
||
| `direction`, `segment_index`, `boundary_type` | 方向段与终端边界语义 |
|
||
|
||
## 控制模块对接（Control Module Integration）
|
||
|
||
控制模块优先以项目引用依赖规划库：
|
||
|
||
```xml
|
||
<ItemGroup>
|
||
<ProjectReference Include="..\ClumsyPilot\ClumsyPilot.csproj"
|
||
AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
|
||
</ItemGroup>
|
||
```
|
||
|
||
对接方应接收上层规划服务提供的完整 `EmTrajectory`，再使用适配器读取不可变序列：
|
||
|
||
```csharp
|
||
var sequence = new ControlModuleTrajectoryAdapter().Create(trajectory);
|
||
|
||
foreach (ControlTrajectoryPoint point in sequence.Points)
|
||
{
|
||
SendReference(
|
||
point.TimeFromStartSeconds,
|
||
point.XMeters,
|
||
point.YMeters,
|
||
point.YawRadians,
|
||
point.SignedLongitudinalVelocityMetersPerSecond,
|
||
point.YawRateRadiansPerSecond);
|
||
}
|
||
```
|
||
|
||
`SendReference` 是控制模块自己的协议适配函数，不是本项目 API。控制模块必须保留 `sequence.Metadata`，并依据方向、换向边界和生效时间实施自己的安全策略。不要把 `VelocityX`、`VelocityY` 当作底盘命令；本 Demo 也不实现换向确认、制动和硬件通信。
|
||
|
||
## 失败与限制（Failures and Limits）
|
||
|
||
| 现象 | 原因 | 处理 |
|
||
| --- | --- | --- |
|
||
| 没有 CSV 输出 | 粗路径、平滑、OSQP 或 EM 验证未成功 | 阅读控制台诊断；不能将失败当作部分轨迹 |
|
||
| OSQP 加载失败 | `osqp.dll` 未随运行输出部署，或 Windows x64 运行时不匹配 | 使用项目引用构建，检查输出目录的 OSQP 文件 |
|
||
| 控制模块轨迹跳变 | 忽略轨迹 ID、生效时间或方向段 | 持有 `EmTrajectoryMetadata` 并按控制周期安全接管 |
|
||
| 想用于真实车辆 | Demo 仍使用显式空地图 | 先接入真实障碍物来源、状态快照和硬件安全审查 |
|
||
|
||
本项目是“如何得到并交给他人轨迹序列”的学习/接口示例，不是经过现场认证的车辆控制器。
|