feat: observe one full EM direction segment

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