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 路径	逐行打印轨迹或修改规划结果

唯一的轨迹生成入口是：

TrajectoryOutputDemoResult result =
    new TrajectoryOutputDemoRunner().Run(configuration);

文件结构（File Structure）

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）

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，可在同一米制地图中按阶段查看地图、Hybrid A* 粗路径、Local G2 平滑路径、EmTrajectory 和控制层采样命令。该页面使用内置说明数据，帮助理解版本身份与数据边界；它不调用 OSQP、定位、硬件或真实控制器。

控制器只消费已验证完整轨迹的采样结果或执行层导出的 TrajectoryControlCommand。粗路径、平滑路径、地图和规划诊断属于观测、回放和问题定位证据链，不是逐周期的底盘命令。

运行（Run）

在仓库根目录执行：

dotnet run --project ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj

成功时控制台会输出轨迹 ID、生效时间、方向段、终端类型、轨迹点数量、首末点和 CSV 绝对路径。默认 CSV 位于 Demo 程序输出目录下的 output/trajectory.csv。

运行自检：

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）

首行固定为：

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）

控制模块优先以项目引用依赖规划库：

<ItemGroup>
  <ProjectReference Include="..\ClumsyPilot\ClumsyPilot.csproj"
                    AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
</ItemGroup>

对接方应接收上层规划服务提供的完整 EmTrajectory，再使用适配器读取不可变序列：

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

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

7.8 KiB Raw Blame History Unescape Escape