feat: observe EM planning across gear segments

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs

@@ -189,8 +189,11 @@ public sealed class TrajectoryObservationController
 {
     private readonly TrajectoryObservationBootstrapResult bootstrap;
     private readonly EmPlannerConfiguration configuration;
-    private readonly EmPlanningCoordinator coordinator;
-    private readonly TrajectoryExecutor executor;
+    private readonly IEmPlanningService planningService;
+    private readonly TrajectoryObservationSegmentTracker segmentTracker;
+    private EmPlanningCoordinator coordinator;
+    private TrajectoryExecutor executor;
+    private EmTrajectory previousTrajectoryForVisualization;
     private readonly string sessionId;
     private long cycleId;
 
@@ -211,13 +214,27 @@ public sealed class TrajectoryObservationController
         configuration.Solver.MaximumOsqpIterations = settings.MaximumOsqpIterations;
         configuration.Scheduling.TimeHorizonSeconds = settings.TimeHorizonSeconds;
         configuration.Scheduling.OutputTimeStepSeconds = settings.OutputTimeStepSeconds;
+        this.planningService = planningService;
         coordinator = new EmPlanningCoordinator(planningService);
         executor = new TrajectoryExecutor(configuration);
+        segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, settings,
+            configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
         this.sessionId = sessionId;
     }
 
     public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory;
 
+    public DirectionSegmentView ActiveSegment => bootstrap.Segments[segmentTracker.State.ActiveSegmentIndex];
+
+    public TrajectoryObservationSegmentState SegmentState => segmentTracker.State;
+
+    public EmTrajectory PreviousTrajectoryForVisualization => previousTrajectoryForVisualization;
+
+    internal EmPlannerConfiguration CreateEffectiveConfigurationSnapshot()
+    {
+        return configuration.Copy();
+    }
+
     public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
     {
         return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
@@ -233,18 +250,31 @@ public sealed class TrajectoryObservationController
     {
         if (state == null) throw new ArgumentNullException(nameof(state));
 
-        const int segmentIndex = 0;
         long currentCycleId = Interlocked.Increment(ref cycleId);
         EmTrajectory previousTrajectory = coordinator.PublishedTrajectory;
         var request = new EmPlanningRequest(
             bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, configuration,
-            segmentIndex, previousTrajectory, now, now,
+            ActiveSegment.SegmentIndex, previousTrajectory, now, now,
             sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
             previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
             EmMotionModel.NonholonomicForwardReverse);
         return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken);
     }
 
+    public bool TryAdvanceSegment(DateTimeOffset now, VehicleMotionState state)
+    {
+        if (state == null) throw new ArgumentNullException(nameof(state));
+
+        TrajectoryObservationSegmentUpdate update = segmentTracker.Update(now, state, coordinator.PublishedTrajectory);
+        if (!update.Advanced)
+            return false;
+
+        previousTrajectoryForVisualization = coordinator.PublishedTrajectory;
+        coordinator = new EmPlanningCoordinator(planningService);
+        executor = new TrajectoryExecutor(configuration);
+        return true;
+    }
+
     public TrajectoryObservationObservation Observe(DateTimeOffset now, VehicleMotionState state)
     {
         if (state == null) throw new ArgumentNullException(nameof(state));
@@ -253,8 +283,15 @@ public sealed class TrajectoryObservationController
         if (trajectory == null)
             return new TrajectoryObservationObservation(now, state, null, null, null, null);
 
+        TrajectoryObservationSegmentState segmentState = segmentTracker.State;
+        TravelDirection currentDirection = ActiveSegment.Direction;
+        bool waitingForDirection = segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForStop ||
+            segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection;
+        TravelDirection desiredDirection = waitingForDirection && segmentState.ExpectedDirection.HasValue
+            ? segmentState.ExpectedDirection.Value
+            : currentDirection;
         TrajectoryControlCommand command = executor.UpdateCommand(now, state, trajectory,
-            trajectory.Metadata.Direction, trajectory.Metadata.Direction, true);
+            desiredDirection, currentDirection, !waitingForDirection);
         TrajectoryExecutionState executorState = executor.State;
         return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint,
             command, executorState);
@@ -265,7 +302,8 @@ public sealed class TrajectoryObservationLoopTick
 {
     internal TrajectoryObservationLoopTick(TrajectoryObservationObservation observation,
         PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed, bool planningInFlight,
-        bool planningStarted, bool planningCompleted)
+        bool planningStarted, bool planningCompleted, bool segmentAdvanced,
+        TrajectoryObservationSegmentState segmentState)
     {
         Observation = observation ?? throw new ArgumentNullException(nameof(observation));
         LatestCycle = latestCycle;
@@ -273,6 +311,8 @@ public sealed class TrajectoryObservationLoopTick
         PlanningInFlight = planningInFlight;
         PlanningStarted = planningStarted;
         PlanningCompleted = planningCompleted;
+        SegmentAdvanced = segmentAdvanced;
+        SegmentState = segmentState ?? throw new ArgumentNullException(nameof(segmentState));
     }
 
     public TrajectoryObservationObservation Observation { get; }
@@ -287,6 +327,10 @@ public sealed class TrajectoryObservationLoopTick
 
     public bool PlanningCompleted { get; }
 
+    public bool SegmentAdvanced { get; }
+
+    public TrajectoryObservationSegmentState SegmentState { get; }
+
     public bool ShouldLog => true;
 }
 
@@ -310,6 +354,9 @@ public sealed class TrajectoryObservationLoop
         cancellationToken.ThrowIfCancellationRequested();
 
         bool planningCompleted = ConsumeCompletedPlanning(now);
+        bool segmentAdvanced = planningTask == null && controller.TryAdvanceSegment(now, state);
+        if (segmentAdvanced)
+            latestCycle = null;
         bool planningStarted = false;
         if (planningTask == null && controller.ShouldStartCycle(now))
         {
@@ -321,7 +368,7 @@ public sealed class TrajectoryObservationLoop
 
         TrajectoryObservationObservation observation = controller.Observe(now, state);
         return new TrajectoryObservationLoopTick(observation, latestCycle, latestPlanningElapsed,
-            planningTask != null, planningStarted, planningCompleted);
+            planningTask != null, planningStarted, planningCompleted, segmentAdvanced, controller.SegmentState);
     }
 
     private bool ConsumeCompletedPlanning(DateTimeOffset observedAtUtc)