feat: add trajectory output demo
This commit is contained in:
梁薄云
@@ -0,0 +1,136 @@
|
||||
# Trajplanner_output 真实轨迹输出 Demo Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use `executing-plans` to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** 提供一个可配置、可运行、可导出且可供控制模块学习引用的真实 EM 轨迹序列 Demo。
|
||||
|
||||
**Architecture:** 独立 `net10.0-windows` 控制台项目以 `ProjectReference` 调用已有规划库，在固定演示场景中依次得到粗路径、平滑路径和真实 OSQP EM 轨迹。输出层只消费不可变 `EmTrajectory`，将其写入 CSV 并投影为控制模块 DTO，不包含硬件调用。
|
||||
|
||||
**Tech Stack:** .NET 10、C#、`ClumsyPilot.csproj`、OSQP Windows x64、CSV、Markdown。
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- Demo 必须使用 `EmPlanningService(new OsqpNativeSolver())`，不得用假求解器伪造成功轨迹。
|
||||
- Demo 配置全部集中于 `TrajectoryOutputDemoConfiguration.cs`；位置 m、航向 rad、速度 m/s、曲率 1/m、时间 s。
|
||||
- 只接受 `Success` 或 `SuccessWithFallback` 的非空 `EmTrajectory`。
|
||||
- OSQP 或任一规划阶段失败时非零退出，不输出部分/伪造 CSV。
|
||||
- 控制模块适配器只提供只读序列，不驱动、转向、制动或换向设备。
|
||||
- 新增注释使用 CoarsePath 风格中文 XML 文档注释。
|
||||
|
||||
---
|
||||
|
||||
### Task 1: 创建可运行 Demo 项目和集中配置
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/Program.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemoConfiguration.cs`
|
||||
- Test: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `ClumsyPilot.csproj` 的 `CoarsePath`、`PathSmoothing` 与 `EMPlanner` 公共 API。
|
||||
- Produces: 一份可复制、单文件可调的演示配置和标准退出码入口。
|
||||
|
||||
- [ ] **Step 1: 写入项目引用**
|
||||
|
||||
创建 `net10.0-windows` 控制台项目，关闭隐式 using/启用 nullable，并引用 `../../ClumsyPilot.csproj`，同时设定 `ExcludeLegacyAutoAvoidance=true`。
|
||||
|
||||
- [ ] **Step 2: 写入默认演示配置**
|
||||
|
||||
配置包含 `MapBoundsMm(0, 6000, 0, 4000)`、`50 mm` 栅格、显式空地图、起点 `(1,1,0)`、终点 `(3,1,0)`、车辆 `0.80 m × 0.60 m`、`0.05 m` 安全余量、`1/1.20 1/m` 曲率上限、前进方向与 `output/trajectory.csv`。
|
||||
|
||||
- [ ] **Step 3: 编写失败入口测试**
|
||||
|
||||
Run: `dotnet run --project ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj -- --invalid-option`
|
||||
|
||||
Expected: 非零退出并打印使用说明；尚未实现时命令因项目不存在而失败。
|
||||
|
||||
### Task 2: 实现真实规划链路与成功/失败边界
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemoRunner.cs`
|
||||
- Modify: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/Program.cs`
|
||||
- Test: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `CoarsePathPlanningService.Plan`、`PathSmoothingService.Smooth`、`EmPlanningService.Plan`。
|
||||
- Produces: 成功时 `EmTrajectory`；失败时含阶段、状态和原因的非零结果。
|
||||
|
||||
- [ ] **Step 1: 构造冻结的 CoarsePath 与平滑请求**
|
||||
|
||||
使用配置创建 `CoarsePathPlanningJob` 和 `PathSmoothingRequest`，每一步仅在成功状态且输出非空时进入下一阶段；失败信息写入 Demo 结果。
|
||||
|
||||
- [ ] **Step 2: 构造真实 EM 请求**
|
||||
|
||||
以平滑路径、同一地图、车辆、`VehicleMotionState`、默认 `EmPlannerConfiguration`、方向段索引和唯一输出 ID 创建 `EmPlanningRequest`，并调用 `new EmPlanningService(new OsqpNativeSolver()).Plan(...)`。
|
||||
|
||||
- [ ] **Step 3: 拒绝非完整输出**
|
||||
|
||||
仅当 `result.Status` 为 `Success` 或 `SuccessWithFallback`、`result.Trajectory` 非空且点数大于零时返回成功；其他状态返回非零并输出 `FailureReason`。
|
||||
|
||||
- [ ] **Step 4: 运行真实链路**
|
||||
|
||||
Run: `dotnet run --project ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj`
|
||||
|
||||
Expected: OSQP 可用时输出轨迹 ID、点数和 CSV 路径；不可用时输出明确 OSQP/规划诊断且不产生成功 CSV。
|
||||
|
||||
### Task 3: 导出轨迹序列和控制模块只读适配器
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectorySequenceExporter.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/ControlModuleTrajectoryAdapter.cs`
|
||||
- Modify: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemoRunner.cs`
|
||||
- Test: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `EmTrajectory.Metadata` 和 `IReadOnlyList<EmTrajectoryPoint>`。
|
||||
- Produces: UTF-8 CSV，以及控制模块可枚举的只读 `ControlTrajectoryPoint` 序列。
|
||||
|
||||
- [ ] **Step 1: 实现 CSV 字段和原子写入**
|
||||
|
||||
首行固定为 `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`。成功轨迹写入临时文件后原子替换目标 CSV，避免控制模块读到半文件。
|
||||
|
||||
- [ ] **Step 2: 实现控制 DTO**
|
||||
|
||||
`ControlTrajectoryPoint` 提供时间、位置、航向、带符号速度、yaw rate、曲率、方向和边界类型；`ControlModuleTrajectoryAdapter.Create(EmTrajectory)` 返回只读列表和元数据，不产生任何硬件调用。
|
||||
|
||||
- [ ] **Step 3: 输出摘要**
|
||||
|
||||
控制台打印轨迹 ID、生效时间、方向段、终端类型、点数、首末点和 CSV 绝对路径，不逐行刷屏。
|
||||
|
||||
- [ ] **Step 4: 验证文件内容**
|
||||
|
||||
Run: `Import-Csv ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/output/trajectory.csv | Select-Object -First 1`
|
||||
|
||||
Expected: 首个数据行具有全部 11 个字段，时间字段为非负数。
|
||||
|
||||
### Task 4: 编写 README 和最终验证
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/README.md`
|
||||
- Modify: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/*.cs`
|
||||
- Test: `ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: 最终项目、配置、CSV 和控制 DTO。
|
||||
- Produces: 对外可复现的运行、调参和控制模块引用说明。
|
||||
|
||||
- [ ] **Step 1: 以 CoarsePath 结构编写 README**
|
||||
|
||||
写入模块职责、文件结构、数据流、单位、运行命令、配置表、CSV 契约、控制模块 `ProjectReference` 示例、失败语义和“演示空地图不得用于真实作业”的限制。
|
||||
|
||||
- [ ] **Step 2: 完成 XML 注释**
|
||||
|
||||
每个公开类型、配置字段、运行阶段、导出边界和控制 DTO 都说明职责、单位与失败/只读语义；不写逐行翻译式注释。
|
||||
|
||||
- [ ] **Step 3: 运行格式与构建验证**
|
||||
|
||||
Run: `dotnet build ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj; git diff --check`
|
||||
|
||||
Expected: 构建退出 0，格式检查退出 0；若现有 Visual Studio 锁定依赖 DLL，记录锁定文件和进程，不假称通过。
|
||||
|
||||
- [ ] **Step 4: 提交 Demo**
|
||||
|
||||
Run: `git add -- ClumsyPilot/ParkrobTrajplanner/Trajplanner_output docs/superpowers/plans/2026-08-09-trajplanner-output-demo.md; git commit -m "feat: add trajectory output demo"`
|
||||
|
||||
Expected: 本机提交只包含 Demo、README 与实施计划。
|
||||
Reference in New Issue
Block a user