feat: add trajectory output demo

2026-08-09 23:39:27 +08:00
parent d4818921ce
commit 3e3a031b9c
11 changed files with 687 additions and 0 deletions
@@ -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,
+        };
+    }
+}