feat: observe one full EM direction segment

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs

@@ -32,22 +32,23 @@ public sealed class TrajectoryObservationMovementTest : MovementTest
     public float MapResolutionMm = 50f;
     public double ReplanPeriodSeconds = 0.20d;
     public double ObserverPeriodSeconds = 0.05d;
+    public bool UseFullDirectionSegmentPlanning = true;
     public double SolverTimeoutSeconds = 5.0d;
     public int MaximumOsqpIterations = 100000;
-    /// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；默认 2s，不等于观察循环周期。</summary>
-    public double TimeHorizonSeconds = 2d;
+    /// <summary>仅滚动兼容字段；完整方向段模式不用于截断。默认 2s，不等于观察循环周期。</summary>
+    public double TimeHorizonSeconds = 20d;
     /// <summary>Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；默认 0.10s，不等于观察循环周期。</summary>
     public double OutputTimeStepSeconds = 0.10d;
     public double VehicleLengthMeters = 0.80d;
     public double VehicleWidthMeters = 0.60d;
     public double SafetyMarginMeters = 0.05d;
     public double MaximumCurvaturePerMeter = 1d / 1.20d;
-    public bool EnableWebVisualization = false;
+    public bool EnableWebVisualization = true;
     public bool AutoOpenWebVisualization = true;
     public int WebVisualizationPort = 0;
     public double WebRefreshRateHz = 10d;
     public int VisualizationHistoryCycleLimit = 60;
-    public bool EnableNativePainterVisualization = false;
+    public bool EnableNativePainterVisualization = true;
     public double DirectionConfirmationSpeedMetersPerSecond = 0.02d;
     public int DirectionConfirmationSamples = 3;
     public double GearSwitchProjectionToleranceMeters = 0.50d;
@@ -76,6 +77,7 @@ public sealed class TrajectoryObservationMovementTest : MovementTest
                 MapResolutionMillimeters = MapResolutionMm,
                 ReplanPeriodSeconds = ReplanPeriodSeconds,
                 ObserverPeriodSeconds = ObserverPeriodSeconds,
+                PlanningScope = UseFullDirectionSegmentPlanning ? EmPlanningScope.FullDirectionSegment : EmPlanningScope.RollingHorizon,
                 SolverTimeoutSeconds = SolverTimeoutSeconds,
                 MaximumOsqpIterations = MaximumOsqpIterations,
                 TimeHorizonSeconds = TimeHorizonSeconds,

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs

@@ -2,19 +2,21 @@ using System;
 using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
 using MultiWheelC.TrajectoryPlanning.Mapping;
 
 namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
 
 public sealed class TrajectoryObservationSettings
 {
+    public EmPlanningScope PlanningScope { get; set; } = EmPlanningScope.FullDirectionSegment;
     public double MapPaddingMeters { get; set; } = 2d;
     public float MapResolutionMillimeters { get; set; } = 50f;
     public double ReplanPeriodSeconds { get; set; } = 0.20d;
     public double ObserverPeriodSeconds { get; set; } = 0.05d;
     public double SolverTimeoutSeconds { get; set; } = 5d;
     public int MaximumOsqpIterations { get; set; } = 100000;
-    /// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；默认 2 s，不等于观察循环周期。</summary>
+    /// <summary>仅滚动视窗兼容字段；完整方向段模式不用于截断。单次 ST 轨迹覆盖的未来时长，单位 s；默认 2 s，不等于观察循环周期。</summary>
     public double TimeHorizonSeconds { get; set; } = 2d;
     /// <summary>发布 Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；不等于观察循环周期。</summary>
     public double OutputTimeStepSeconds { get; set; } = 0.10d;
@@ -22,7 +24,7 @@ public sealed class TrajectoryObservationSettings
     public double VehicleWidthMeters { get; set; } = 0.60d;
     public double SafetyMarginMeters { get; set; } = 0.05d;
     public double MaximumCurvaturePerMeter { get; set; } = 1d / 1.20d;
-    public bool EnableWebVisualization { get; set; } = false;
+    public bool EnableWebVisualization { get; set; } = true;
     public bool AutoOpenWebVisualization { get; set; } = true;
     public int WebVisualizationPort { get; set; } = 0;
     public double WebRefreshRateHz { get; set; } = 10d;
@@ -37,6 +39,7 @@ public sealed class TrajectoryObservationSettings
     {
         var snapshot = new TrajectoryObservationSettings
         {
+            PlanningScope = PlanningScope,
             MapPaddingMeters = MapPaddingMeters,
             MapResolutionMillimeters = MapResolutionMillimeters,
             ReplanPeriodSeconds = ReplanPeriodSeconds,
@@ -66,6 +69,8 @@ public sealed class TrajectoryObservationSettings
 
     public void Validate()
     {
+        if (!Enum.IsDefined(typeof(EmPlanningScope), PlanningScope))
+            throw new ArgumentOutOfRangeException(nameof(PlanningScope), "Value must be a defined planning scope.");
         EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters));
         EnsurePositiveFinite(MapResolutionMillimeters, nameof(MapResolutionMillimeters));
         EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds));

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs

@@ -188,6 +188,7 @@ public sealed class TrajectoryObservationObservation
 public sealed class TrajectoryObservationController
 {
     private readonly TrajectoryObservationBootstrapResult bootstrap;
+    private readonly TrajectoryObservationSettings settings;
     private readonly EmPlannerConfiguration configuration;
     private readonly IEmPlanningService planningService;
     private readonly TrajectoryObservationSegmentTracker segmentTracker;
@@ -196,6 +197,8 @@ public sealed class TrajectoryObservationController
     private EmTrajectory previousTrajectoryForVisualization;
     private readonly string sessionId;
     private long cycleId;
+    private int plannedSegmentIndex;
+    private bool planAttemptedForActiveSegment;
 
     public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap,
         TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId)
@@ -207,17 +210,18 @@ public sealed class TrajectoryObservationController
         if (planningService == null) throw new ArgumentNullException(nameof(planningService));
         if (string.IsNullOrWhiteSpace(sessionId)) throw new ArgumentException("A session ID is required.", nameof(sessionId));
 
-        settings.Validate();
+        TrajectoryObservationSettings snapshot = settings.CreateValidatedSnapshot();
+        this.settings = snapshot;
         configuration = EmPlannerConfiguration.CreateDefault();
-        configuration.Scheduling.ReplanPeriodSeconds = settings.ReplanPeriodSeconds;
-        configuration.Scheduling.SolverTimeoutSeconds = settings.SolverTimeoutSeconds;
-        configuration.Solver.MaximumOsqpIterations = settings.MaximumOsqpIterations;
-        configuration.Scheduling.TimeHorizonSeconds = settings.TimeHorizonSeconds;
-        configuration.Scheduling.OutputTimeStepSeconds = settings.OutputTimeStepSeconds;
+        configuration.Scheduling.ReplanPeriodSeconds = snapshot.ReplanPeriodSeconds;
+        configuration.Scheduling.SolverTimeoutSeconds = snapshot.SolverTimeoutSeconds;
+        configuration.Solver.MaximumOsqpIterations = snapshot.MaximumOsqpIterations;
+        configuration.Scheduling.TimeHorizonSeconds = snapshot.TimeHorizonSeconds;
+        configuration.Scheduling.OutputTimeStepSeconds = snapshot.OutputTimeStepSeconds;
         this.planningService = planningService;
         coordinator = new EmPlanningCoordinator(planningService);
         executor = new TrajectoryExecutor(configuration);
-        segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, settings,
+        segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, snapshot,
             configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
         this.sessionId = sessionId;
     }
@@ -242,6 +246,15 @@ public sealed class TrajectoryObservationController
 
     public bool ShouldStartCycle(DateTimeOffset now)
     {
+        if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment)
+        {
+            if (plannedSegmentIndex != ActiveSegment.SegmentIndex)
+            {
+                plannedSegmentIndex = ActiveSegment.SegmentIndex;
+                planAttemptedForActiveSegment = false;
+            }
+            return !planAttemptedForActiveSegment;
+        }
         return coordinator.ShouldStartCycle(now);
     }
 
@@ -250,6 +263,8 @@ public sealed class TrajectoryObservationController
     {
         if (state == null) throw new ArgumentNullException(nameof(state));
 
+        planAttemptedForActiveSegment = true;
+        plannedSegmentIndex = ActiveSegment.SegmentIndex;
         long currentCycleId = Interlocked.Increment(ref cycleId);
         EmTrajectory previousTrajectory = coordinator.PublishedTrajectory;
         var request = new EmPlanningRequest(
@@ -257,7 +272,7 @@ public sealed class TrajectoryObservationController
             ActiveSegment.SegmentIndex, previousTrajectory, now, now,
             sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
             previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
-            EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.RollingHorizon);
+            EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope);
         return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
     }
 
@@ -272,6 +287,8 @@ public sealed class TrajectoryObservationController
         previousTrajectoryForVisualization = coordinator.PublishedTrajectory;
         coordinator = new EmPlanningCoordinator(planningService);
         executor = new TrajectoryExecutor(configuration);
+        plannedSegmentIndex = segmentTracker.State.ActiveSegmentIndex;
+        planAttemptedForActiveSegment = false;
         return true;
     }
 

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationStaticSnapshotBuilder.cs

@@ -168,6 +168,7 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
                 V("行数", "Rows", I(map.Rows), ""),
                 V("列数", "Cols", I(map.Cols), "")),
             Group("可视化与换向确认",
+                V("规划范围", "PlanningScope", settings.PlanningScope.ToString(), ""),
                 V("启用网页", "EnableWebVisualization", B(settings.EnableWebVisualization), ""),
                 V("自动打开网页", "AutoOpenWebVisualization", B(settings.AutoOpenWebVisualization), ""),
                 V("网页端口", "WebVisualizationPort", I(settings.WebVisualizationPort), ""),