# 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。

## 运行（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 仍使用显式空地图 | 先接入真实障碍物来源、状态快照和硬件安全审查 |

本项目是“如何得到并交给他人轨迹序列”的学习/接口示例，不是经过现场认证的车辆控制器。