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,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);
+}