feat: add trajectory output demo

ClumsyPilot/ClumsyPilot.csproj

@@ -20,6 +20,7 @@
     <Compile Remove="tests\TrajectoryPlanningVisualizationVerificationHost\**\*.cs" />
     <Compile Remove="tests\PathSmoothingPngVerificationHost\**\*.cs" />
     <Compile Remove="tests\EMPlannerVerificationHost\**\*.cs" />
+    <Compile Remove="ParkrobTrajplanner\Trajplanner_output\**\*.cs" />
     <Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
              Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
   </ItemGroup>

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/ControlModuleTrajectoryAdapter.cs

@@ -0,0 +1,68 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+namespace TrajectoryOutputDemo;
+
+/// <summary>供外部控制模块消费的单个只读轨迹点；全部字段直接来自已验证的 <see cref="EmTrajectoryPoint"/>。</summary>
+public sealed class ControlTrajectoryPoint
+{
+    internal ControlTrajectoryPoint(EmTrajectoryPoint source)
+    {
+        TimeFromStartSeconds = source.TimeFromStart;
+        XMeters = source.X;
+        YMeters = source.Y;
+        YawRadians = source.Yaw;
+        SignedLongitudinalVelocityMetersPerSecond = source.SignedLongitudinalVelocity;
+        YawRateRadiansPerSecond = source.YawRate;
+        CurvaturePerMeter = source.VehicleCurvature;
+        Direction = source.Direction;
+        SegmentIndex = source.SegmentIndex;
+        PathSMeters = source.PathS;
+        BoundaryType = source.BoundaryType;
+    }
+
+    public double TimeFromStartSeconds { get; }
+    public double XMeters { get; }
+    public double YMeters { get; }
+    public double YawRadians { get; }
+    public double SignedLongitudinalVelocityMetersPerSecond { get; }
+    public double YawRateRadiansPerSecond { get; }
+    public double CurvaturePerMeter { get; }
+    public TravelDirection Direction { get; }
+    public int SegmentIndex { get; }
+    public double PathSMeters { get; }
+    public EmBoundaryType BoundaryType { get; }
+}
+
+/// <summary>轨迹元数据与控制点序列的不可变组合；它不包含也不发送任何硬件命令。</summary>
+public sealed class ControlTrajectorySequence
+{
+    internal ControlTrajectorySequence(EmTrajectoryMetadata metadata, IReadOnlyList<ControlTrajectoryPoint> points)
+    {
+        Metadata = metadata ?? throw new ArgumentNullException(nameof(metadata));
+        Points = points ?? throw new ArgumentNullException(nameof(points));
+    }
+
+    public EmTrajectoryMetadata Metadata { get; }
+    public IReadOnlyList<ControlTrajectoryPoint> Points { get; }
+}
+
+/// <summary>将不可变 EM 轨迹投影为控制模块可引用的只读序列，不进行采样、插值或底盘协议转换。</summary>
+public sealed class ControlModuleTrajectoryAdapter
+{
+    /// <summary>逐点复制公开控制字段；调用方只能在完整非空轨迹上调用此方法。</summary>
+    public ControlTrajectorySequence Create(EmTrajectory trajectory)
+    {
+        if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
+            throw new ArgumentException("必须提供完整非空的 EM 轨迹。", nameof(trajectory));
+
+        var points = new List<ControlTrajectoryPoint>(trajectory.Points.Count);
+        for (int index = 0; index < trajectory.Points.Count; index++)
+            points.Add(new ControlTrajectoryPoint(trajectory.Points[index]));
+        return new ControlTrajectorySequence(trajectory.Metadata,
+            new ReadOnlyCollection<ControlTrajectoryPoint>(points));
+    }
+}

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/Program.cs

@@ -0,0 +1,45 @@
+using System;
+using System.IO;
+
+namespace TrajectoryOutputDemo;
+
+/// <summary>Trajplanner_output 控制台入口；只输出规划诊断、轨迹摘要和 CSV 路径。</summary>
+internal static class Program
+{
+    private static int Main(string[] args)
+    {
+        if (args.Length != 0)
+        {
+            Console.Error.WriteLine("用法：dotnet run --project TrajectoryOutputDemo.csproj");
+            return 2;
+        }
+
+        string csvPath = Path.Combine(AppContext.BaseDirectory, "output", "trajectory.csv");
+        try
+        {
+            TrajectoryOutputDemoResult result = new TrajectoryOutputDemoRunner().Run(
+                TrajectoryOutputDemoConfiguration.CreateDefault(csvPath));
+            if (!result.Succeeded || result.Trajectory == null || result.ControlTrajectory == null || result.CsvPath == null)
+            {
+                Console.Error.WriteLine(result.Diagnostic);
+                return 1;
+            }
+
+            ControlTrajectoryPoint first = result.ControlTrajectory.Points[0];
+            ControlTrajectoryPoint last = result.ControlTrajectory.Points[result.ControlTrajectory.Points.Count - 1];
+            Console.WriteLine("轨迹 ID：" + result.Trajectory.Metadata.TrajectoryId);
+            Console.WriteLine("生效时间：" + result.Trajectory.Metadata.EffectiveAtUtc.ToString("O"));
+            Console.WriteLine("方向段：" + result.Trajectory.Metadata.SegmentIndex + "，终端类型：" + result.Trajectory.Metadata.TerminalType);
+            Console.WriteLine("轨迹点数：" + result.ControlTrajectory.Points.Count);
+            Console.WriteLine("首点：t=" + first.TimeFromStartSeconds + "s, X=" + first.XMeters + "m, Y=" + first.YMeters + "m");
+            Console.WriteLine("末点：t=" + last.TimeFromStartSeconds + "s, X=" + last.XMeters + "m, Y=" + last.YMeters + "m");
+            Console.WriteLine("CSV：" + result.CsvPath);
+            return 0;
+        }
+        catch (Exception exception)
+        {
+            Console.Error.WriteLine("轨迹 Demo 运行异常：" + exception.Message);
+            return 1;
+        }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/README.md

@@ -0,0 +1,158 @@
+# 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 仍使用显式空地图 | 先接入真实障碍物来源、状态快照和硬件安全审查 |
+
+本项目是“如何得到并交给他人轨迹序列”的学习/接口示例，不是经过现场认证的车辆控制器。

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/Tests/Program.cs

@@ -0,0 +1,45 @@
+using System;
+using System.IO;
+using TrajectoryOutputDemo;
+
+namespace TrajectoryOutputDemoTests;
+
+internal static class Program
+{
+    private static int Main()
+    {
+        string temporaryDirectory = Path.Combine(Path.GetTempPath(), "trajectory-output-demo-tests", Guid.NewGuid().ToString("N"));
+        try
+        {
+            Directory.CreateDirectory(temporaryDirectory);
+            string csvPath = Path.Combine(temporaryDirectory, "trajectory.csv");
+            var configuration = TrajectoryOutputDemoConfiguration.CreateDefault(csvPath);
+            TrajectoryOutputDemoResult result = new TrajectoryOutputDemoRunner().Run(configuration);
+
+            if (!result.Succeeded)
+                throw new InvalidOperationException("真实 OSQP 规划未成功：" + result.Diagnostic);
+            if (!File.Exists(csvPath))
+                throw new InvalidOperationException("成功规划必须导出 CSV。");
+            ControlTrajectorySequence sequence = result.ControlTrajectory ??
+                throw new InvalidOperationException("成功规划必须提供控制模块序列。");
+            var trajectory = result.Trajectory ??
+                throw new InvalidOperationException("成功规划必须提供 EM 轨迹。");
+            if (sequence.Points.Count != trajectory.Points.Count)
+                throw new InvalidOperationException("控制序列必须逐点对应 EM 轨迹。");
+
+            string[] lines = File.ReadAllLines(csvPath);
+            if (lines.Length < 2)
+                throw new InvalidOperationException("CSV 必须包含表头和至少一个轨迹点。");
+            if (lines[0] != "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")
+                throw new InvalidOperationException("CSV 表头不符合控制模块契约。");
+
+            Console.WriteLine("PASS trajectory-output-demo");
+            return 0;
+        }
+        finally
+        {
+            if (Directory.Exists(temporaryDirectory))
+                Directory.Delete(temporaryDirectory, true);
+        }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/Tests/TrajectoryOutputDemo.Tests.csproj

@@ -0,0 +1,11 @@
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net10.0-windows</TargetFramework>
+    <ImplicitUsings>disable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\TrajectoryOutputDemo.csproj" />
+  </ItemGroup>
+</Project>

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemo.csproj

@@ -0,0 +1,15 @@
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net10.0-windows</TargetFramework>
+    <ImplicitUsings>disable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\..\ClumsyPilot.csproj"
+                      AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Remove="Tests\**\*.cs" />
+  </ItemGroup>
+</Project>

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemoConfiguration.cs

@@ -0,0 +1,63 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace TrajectoryOutputDemo;
+
+/// <summary>
+/// Trajplanner_output 的唯一演示调参入口。
+/// 地图边界与分辨率使用 mm；车辆、起终点位置和安全余量使用 m；航向使用 rad；输出路径为 CSV 文件位置。
+/// </summary>
+public sealed class TrajectoryOutputDemoConfiguration
+{
+    /// <summary>演示地图的世界边界；空地图只允许用于算法学习，不能替代真实障碍物来源。</summary>
+    public MapBoundsMm MapBounds { get; init; } = new MapBoundsMm(0f, 6000f, 0f, 4000f);
+
+    /// <summary>演示占据栅格分辨率，单位 mm。</summary>
+    public float MapResolutionMillimeters { get; init; }
+
+    /// <summary>车辆几何中心起点，位置单位 m、航向单位 rad。</summary>
+    public Pose2D Start { get; init; } = new Pose2D(1d, 1d, 0d);
+
+    /// <summary>车辆几何中心目标点，位置单位 m、航向单位 rad。</summary>
+    public Pose2D Goal { get; init; } = new Pose2D(3d, 1d, 0d);
+
+    /// <summary>车辆长度，单位 m。</summary>
+    public double VehicleLengthMeters { get; init; }
+
+    /// <summary>车辆宽度，单位 m。</summary>
+    public double VehicleWidthMeters { get; init; }
+
+    /// <summary>车辆矩形外的附加安全余量，单位 m。</summary>
+    public double SafetyMarginMeters { get; init; }
+
+    /// <summary>车辆最大曲率，单位 1/m。</summary>
+    public double MaximumCurvaturePerMeter { get; init; }
+
+    /// <summary>轨迹开始时的带符号纵向速度，单位 m/s；前进为正、倒车为负。</summary>
+    public double InitialSignedSpeedMetersPerSecond { get; init; }
+
+    /// <summary>输出 CSV 的绝对或相对路径。</summary>
+    public string CsvOutputPath { get; init; } = string.Empty;
+
+    /// <summary>创建可直线通行的最小真实 EM 演示配置。</summary>
+    public static TrajectoryOutputDemoConfiguration CreateDefault(string csvOutputPath)
+    {
+        if (string.IsNullOrWhiteSpace(csvOutputPath))
+            throw new ArgumentException("CSV 输出路径不能为空。", nameof(csvOutputPath));
+
+        return new TrajectoryOutputDemoConfiguration
+        {
+            MapBounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
+            MapResolutionMillimeters = 50f,
+            Start = new Pose2D(1d, 1d, 0d),
+            Goal = new Pose2D(3d, 1d, 0d),
+            VehicleLengthMeters = 0.80d,
+            VehicleWidthMeters = 0.60d,
+            SafetyMarginMeters = 0.05d,
+            MaximumCurvaturePerMeter = 1d / 1.20d,
+            InitialSignedSpeedMetersPerSecond = 0d,
+            CsvOutputPath = csvOutputPath,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemoRunner.cs

@@ -0,0 +1,99 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+namespace TrajectoryOutputDemo;
+
+/// <summary>一次 Demo 运行的结果；成功时同时提供原始 EM 轨迹、控制只读序列和已完成的 CSV 路径。</summary>
+public sealed class TrajectoryOutputDemoResult
+{
+    private TrajectoryOutputDemoResult(bool succeeded, string diagnostic, EmTrajectory? trajectory,
+        ControlTrajectorySequence? controlTrajectory, string? csvPath)
+    {
+        Succeeded = succeeded;
+        Diagnostic = diagnostic ?? string.Empty;
+        Trajectory = trajectory;
+        ControlTrajectory = controlTrajectory;
+        CsvPath = csvPath;
+    }
+
+    public bool Succeeded { get; }
+    public string Diagnostic { get; }
+    public EmTrajectory? Trajectory { get; }
+    public ControlTrajectorySequence? ControlTrajectory { get; }
+    public string? CsvPath { get; }
+
+    internal static TrajectoryOutputDemoResult Failure(string diagnostic) => new(false, diagnostic, null, null, null);
+    internal static TrajectoryOutputDemoResult Success(EmTrajectory trajectory, ControlTrajectorySequence sequence, string csvPath) =>
+        new(true, string.Empty, trajectory, sequence, csvPath);
+}
+
+/// <summary>
+/// 编排一次真实轨迹输出：粗路径、Local G2 平滑、真实 OSQP EM 规划、控制序列投影和 CSV 导出。
+/// 它只处理冻结的演示输入，绝不读取硬件状态或发送控制命令。
+/// </summary>
+public sealed class TrajectoryOutputDemoRunner
+{
+    /// <summary>在配置定义的演示场景运行一次完整规划；失败时不导出任何轨迹文件。</summary>
+    public TrajectoryOutputDemoResult Run(TrajectoryOutputDemoConfiguration configuration)
+    {
+        if (configuration == null) throw new ArgumentNullException(nameof(configuration));
+
+        CoarsePathPlanningJob job = CreateCoarseJob(configuration);
+        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper().Bootstrap(job, CancellationToken.None);
+        if (!bootstrap.Succeeded)
+            return TrajectoryOutputDemoResult.Failure("粗路径或平滑阶段失败：" + bootstrap.FailureReason);
+        if (bootstrap.Segments.Count == 0)
+            return TrajectoryOutputDemoResult.Failure("平滑路径没有可供 EM 规划的方向段。");
+
+        DateTimeOffset now = DateTimeOffset.UtcNow;
+        DirectionSegmentView segment = bootstrap.Segments[0];
+        var state = new VehicleMotionState(configuration.Start, configuration.InitialSignedSpeedMetersPerSecond,
+            0d, now, 1L);
+        var request = new EmPlanningRequest(bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state,
+            EmPlannerConfiguration.CreateDefault(), segment.SegmentIndex, null, now, now,
+            "trajectory-output-demo-" + now.ToUnixTimeMilliseconds(), "trajectory-output-demo-reference", string.Empty,
+            EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.FullDirectionSegment);
+        EmPlanningResult result = new EmPlanningService(new OsqpNativeSolver()).Plan(request, CancellationToken.None);
+        bool accepted = (result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback) &&
+            result.Trajectory != null && result.Trajectory.Points.Count > 0;
+        if (!accepted)
+            return TrajectoryOutputDemoResult.Failure("EM 规划失败：" + result.Status + "；" + result.FailureReason);
+
+        EmTrajectory trajectory = result.Trajectory!;
+        ControlTrajectorySequence sequence = new ControlModuleTrajectoryAdapter().Create(trajectory);
+        string csvPath = new TrajectorySequenceExporter().Export(sequence, configuration.CsvOutputPath);
+        return TrajectoryOutputDemoResult.Success(trajectory, sequence, csvPath);
+    }
+
+    private static CoarsePathPlanningJob CreateCoarseJob(TrajectoryOutputDemoConfiguration configuration)
+    {
+        return new CoarsePathPlanningJob
+        {
+            MapRequest = new PlanningMapRequest
+            {
+                Bounds = configuration.MapBounds,
+                ResolutionMm = configuration.MapResolutionMillimeters,
+                ObstacleSources = Array.Empty<IMapObstacleSource>(),
+                AllowExplicitEmptyMap = true,
+            },
+            Start = configuration.Start,
+            Goal = configuration.Goal,
+            Vehicle = new VehicleParameters
+            {
+                LengthMeters = configuration.VehicleLengthMeters,
+                WidthMeters = configuration.VehicleWidthMeters,
+                SafetyMarginMeters = configuration.SafetyMarginMeters,
+                MaximumCurvaturePerMeter = configuration.MaximumCurvaturePerMeter,
+            },
+            Configuration = new HybridAStarConfiguration(),
+            StartDirection = TravelDirection.Forward,
+            GoalDirection = GoalDirectionConstraint.Forward,
+        };
+    }
+}

ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectorySequenceExporter.cs

@@ -0,0 +1,46 @@
+using System;
+using System.Globalization;
+using System.IO;
+using System.Text;
+
+namespace TrajectoryOutputDemo;
+
+/// <summary>将完整控制轨迹以稳定字段顺序导出为 UTF-8 CSV；只在完整内容写完后替换目标文件。</summary>
+public sealed class TrajectorySequenceExporter
+{
+    /// <summary>控制模块 CSV 的固定表头；数值字段均按不受区域设置影响的点号格式写出。</summary>
+    public const string Header = "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";
+
+    /// <summary>将完整序列原子写入目标路径，防止读方看到半写入的 CSV。</summary>
+    public string Export(ControlTrajectorySequence sequence, string outputPath)
+    {
+        if (sequence == null) throw new ArgumentNullException(nameof(sequence));
+        if (string.IsNullOrWhiteSpace(outputPath)) throw new ArgumentException("输出路径不能为空。", nameof(outputPath));
+
+        string fullPath = Path.GetFullPath(outputPath);
+        string? directory = Path.GetDirectoryName(fullPath);
+        if (!string.IsNullOrEmpty(directory)) Directory.CreateDirectory(directory);
+        string temporaryPath = fullPath + ".tmp";
+
+        using (var writer = new StreamWriter(temporaryPath, false, new UTF8Encoding(false)))
+        {
+            writer.WriteLine(Header);
+            for (int index = 0; index < sequence.Points.Count; index++)
+            {
+                ControlTrajectoryPoint point = sequence.Points[index];
+                writer.WriteLine(string.Join(",", new[]
+                {
+                    Number(point.TimeFromStartSeconds), Number(point.XMeters), Number(point.YMeters), Number(point.YawRadians),
+                    Number(point.SignedLongitudinalVelocityMetersPerSecond), Number(point.YawRateRadiansPerSecond),
+                    Number(point.CurvaturePerMeter), point.Direction.ToString(), point.SegmentIndex.ToString(CultureInfo.InvariantCulture),
+                    Number(point.PathSMeters), point.BoundaryType.ToString(),
+                }));
+            }
+        }
+
+        File.Move(temporaryPath, fullPath, true);
+        return fullPath;
+    }
+
+    private static string Number(double value) => value.ToString("G17", CultureInfo.InvariantCulture);
+}

docs/superpowers/plans/2026-08-09-trajplanner-output-demo.md

@@ -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 与实施计划。