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