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