feat: observe EM planning across gear segments

2026-08-06 11:10:35 +08:00
parent e141a76fdb
commit 51f744bc00
3 changed files with 249 additions and 7 deletions
@@ -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)
@@ -38,6 +38,8 @@ internal static class TrajectoryObservationChecks
        VerifiesEmptyChartsReceivePersistentPlanningDiagnostic();
        VerifiesLsPresentationUsesPathSOnHorizontalAxis();
        VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
+        VerifiesActiveSegmentControllerDoesNotCrossSeedTrajectories();
+        VerifiesLoopDefersSegmentAdvanceUntilPlanningIsConsumed();
        FreezesBootstrapVehicleForRollingRequests();
        VerifiesEmptyMapBootstrapProducesPublishableReference();
    }
@@ -543,6 +545,125 @@ internal static class TrajectoryObservationChecks
            "observer rolling request trajectory object and ID use one publication snapshot");
    }

+    private static void VerifiesActiveSegmentControllerDoesNotCrossSeedTrajectories()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 4, 0, 0, TimeSpan.Zero);
+        var settings = new TrajectoryObservationSettings
+        {
+            DirectionConfirmationSamples = 1,
+        };
+        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
+            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
+            Array.Empty<TrajectoryObservationObstacle>(), 0L);
+        TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper()
+            .Bootstrap(job, CancellationToken.None);
+        Verification.True(baseBootstrap.Succeeded, "active-segment controller bootstrap succeeds");
+
+        TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success(
+            baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, CreateDirectionalSegments());
+        EmTrajectory forwardGearTrajectory = CreateSegmentTrajectory(t0, "active-forward", 0,
+            TravelDirection.Forward, EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d);
+        EmTrajectory reverseTrajectory = CreateSegmentTrajectory(t0.AddSeconds(1d), "active-reverse", 1,
+            TravelDirection.Reverse, EmTerminalType.Goal, EmBoundaryType.Goal, 0d);
+        var planningService = new SequenceTrajectoryPlanningService(forwardGearTrajectory, reverseTrajectory,
+            reverseTrajectory);
+        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "active-segment");
+
+        VehicleMotionState stopped = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L);
+        controller.StartCycle(t0, stopped, CancellationToken.None).GetAwaiter().GetResult();
+        Verification.Equal(0, planningService.Requests[0].SegmentIndex,
+            "first active-segment request uses segment zero");
+        Verification.Equal(0, controller.ActiveSegment.SegmentIndex, "controller exposes initial active segment");
+
+        controller.TryAdvanceSegment(t0, stopped);
+        controller.TryAdvanceSegment(t0.AddSeconds(0.21d), new VehicleMotionState(
+            new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 2L));
+        bool advanced = controller.TryAdvanceSegment(t0.AddSeconds(0.22d), new VehicleMotionState(
+            new Pose2D(1d, 0d, 0d), -0.03d, null, t0.AddSeconds(0.22d), 3L));
+
+        Verification.True(advanced, "confirmed reverse direction advances the controller");
+        Verification.Equal(1, controller.ActiveSegment.SegmentIndex, "controller uses tracker-confirmed segment");
+        Verification.True(ReferenceEquals(forwardGearTrajectory, controller.PreviousTrajectoryForVisualization),
+            "old direction trajectory is retained only for visualization");
+
+        DateTimeOffset reverseAt = t0.AddSeconds(1d);
+        controller.StartCycle(reverseAt, new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null,
+            reverseAt, 4L), CancellationToken.None).GetAwaiter().GetResult();
+        Verification.Equal(1, planningService.Requests[1].SegmentIndex,
+            "next active-segment request uses segment one");
+        Verification.True(planningService.Requests[1].PreviousTrajectory == null,
+            "new direction does not reuse the old trajectory as an EM seed");
+        Verification.Equal("active-segment-trajectory-2", planningService.Requests[1].OutputTrajectoryId,
+            "session cycle IDs remain monotonic after coordinator replacement");
+
+        controller.StartCycle(reverseAt.AddSeconds(settings.ReplanPeriodSeconds), new VehicleMotionState(
+            new Pose2D(0.9d, 0d, 0d), -0.03d, null, reverseAt.AddSeconds(settings.ReplanPeriodSeconds), 5L),
+            CancellationToken.None).GetAwaiter().GetResult();
+        Verification.True(ReferenceEquals(reverseTrajectory, planningService.Requests[2].PreviousTrajectory),
+            "same direction rolling request retains its published trajectory as the seed");
+    }
+
+    private static void VerifiesLoopDefersSegmentAdvanceUntilPlanningIsConsumed()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 5, 0, 0, TimeSpan.Zero);
+        var settings = new TrajectoryObservationSettings
+        {
+            DirectionConfirmationSamples = 1,
+        };
+        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
+            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
+            Array.Empty<TrajectoryObservationObstacle>(), 0L);
+        TrajectoryObservationBootstrapResult baseBootstrap = new TrajectoryObservationBootstrapper()
+            .Bootstrap(job, CancellationToken.None);
+        TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success(
+            baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, CreateDirectionalSegments());
+        EmTrajectory forwardGearTrajectory = CreateSegmentTrajectory(t0, "loop-forward", 0,
+            TravelDirection.Forward, EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d);
+        var planningService = new DelayedTrajectoryPlanningService(forwardGearTrajectory, 2);
+        try
+        {
+            var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "loop-segment");
+            controller.StartCycle(t0, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L),
+                CancellationToken.None).GetAwaiter().GetResult();
+            var loop = new TrajectoryObservationLoop(controller);
+
+            loop.Tick(t0, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 2L),
+                CancellationToken.None);
+            TrajectoryObservationLoopTick planningTick = loop.Tick(t0.AddSeconds(0.21d),
+                new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 3L),
+                CancellationToken.None);
+            Verification.True(planningTick.PlanningInFlight, "new cycle is in flight before direction evidence");
+            Verification.True(planningService.WaitUntilEntered(TimeSpan.FromSeconds(5d)),
+                "delayed cycle entered planning service");
+
+            TrajectoryObservationLoopTick blockedTick = loop.Tick(t0.AddSeconds(0.22d),
+                new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null, t0.AddSeconds(0.22d), 4L),
+                CancellationToken.None);
+            Verification.True(!blockedTick.SegmentAdvanced, "in-flight planning prevents a segment transition");
+            Verification.Equal(0, blockedTick.SegmentState.ActiveSegmentIndex,
+                "in-flight planning retains the immutable old segment state");
+
+            planningService.Release();
+            TrajectoryObservationLoopTick? advancedTick = null;
+            int completedSequence = 5;
+            bool advanced = SpinWait.SpinUntil(() =>
+            {
+                DateTimeOffset now = t0.AddSeconds(0.23d + (completedSequence - 5) * 0.01d);
+                advancedTick = loop.Tick(now, new VehicleMotionState(
+                    new Pose2D(1d, 0d, 0d), -0.03d, null, now, completedSequence++), CancellationToken.None);
+                return advancedTick.SegmentAdvanced;
+            }, TimeSpan.FromSeconds(5d));
+            Verification.True(advanced,
+                "loop advances only after it has consumed the completed planning task");
+            Verification.Equal(1, advancedTick!.SegmentState.ActiveSegmentIndex,
+                "loop tick exposes the advanced immutable segment state");
+        }
+        finally
+        {
+            planningService.Release();
+        }
+    }
+
    private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
@@ -788,6 +909,47 @@ internal static class TrajectoryObservationChecks
        });
    }

+    private static IReadOnlyList<DirectionSegmentView> CreateDirectionalSegments()
+    {
+        return new[]
+        {
+            CreateDirectionalSegment(0, TravelDirection.Forward, 0d, 1d, false,
+                EmBoundaryType.None, EmBoundaryType.GearSwitchApproach),
+            CreateDirectionalSegment(1, TravelDirection.Reverse, 1d, 0d, true,
+                EmBoundaryType.GearSwitchDeparture, EmBoundaryType.Goal),
+        };
+    }
+
+    private static DirectionSegmentView CreateDirectionalSegment(int index, TravelDirection direction, double startX,
+        double endX, bool startIsGearSwitch, EmBoundaryType startBoundary, EmBoundaryType endBoundary)
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            new SmoothedPathPoint(startX, 0d, 0d, 0d, 0d, direction, 0d, 0d, 1d,
+                startIsGearSwitch, SmoothedPathPointSource.Anchor),
+            new SmoothedPathPoint(endX, 0d, 0d, 0d, 1d, direction, 0d, 0d, 1d,
+                false, SmoothedPathPointSource.Anchor),
+        };
+        return new DirectionSegmentView(index, direction, points,
+            new ReferenceBoundary(index, 0d, startBoundary, index),
+            new ReferenceBoundary(index, 1d, endBoundary, index + 1d), index);
+    }
+
+    private static EmTrajectory CreateSegmentTrajectory(DateTimeOffset effectiveAt, string trajectoryId, int segmentIndex,
+        TravelDirection direction, EmTerminalType terminalType, EmBoundaryType boundaryType, double x)
+    {
+        var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 1L,
+            "active-segment-reference", 1L, string.Empty, segmentIndex, direction, terminalType,
+            terminalType == EmTerminalType.GearSwitch
+                ? EmLongitudinalMode.ExactStopAtBoundary
+                : EmLongitudinalMode.RollingContinuation);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(x, 0d, 0d, 0d, 0d, 0d, segmentIndex, 1d, 1d,
+                direction, boundaryType, 0d, 0d),
+        });
+    }
+
    private sealed class FixedTrajectoryPlanningService : IEmPlanningService
    {
        private readonly EmTrajectory trajectory;
@@ -806,6 +968,27 @@ internal static class TrajectoryObservationChecks
        }
    }

+    private sealed class SequenceTrajectoryPlanningService : IEmPlanningService
+    {
+        private readonly IReadOnlyList<EmTrajectory> trajectories;
+        private int nextTrajectoryIndex;
+
+        public SequenceTrajectoryPlanningService(params EmTrajectory[] trajectories)
+        {
+            this.trajectories = trajectories ?? throw new ArgumentNullException(nameof(trajectories));
+        }
+
+        public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();
+
+        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
+        {
+            Requests.Add(request);
+            EmTrajectory trajectory = trajectories[Math.Min(nextTrajectoryIndex, trajectories.Count - 1)];
+            nextTrajectoryIndex++;
+            return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
+        }
+    }
+
    private sealed class DelayedTrajectoryPlanningService : IEmPlanningService
    {
        private readonly EmTrajectory trajectory;
@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.IO;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.EMPlanner;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
@@ -11,12 +12,23 @@ internal static class TrajectoryObservationSegmentChecks
 {
    public static void Run()
    {
+        RejectsHardcodedActiveSegmentIndex();
        AdvancesOnlyAfterContinuousStopAndStableNextDirection();
        RejectsNonTerminalOrMismatchedGearTrajectory();
        ResetsConfirmationForInvalidDirectionEvidence();
        CompletesOneSegmentWithoutIndexingPastTheEnd();
    }

+    private static void RejectsHardcodedActiveSegmentIndex()
+    {
+        string pipelinePath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
+            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPipeline.cs");
+        string source = File.ReadAllText(pipelinePath);
+
+        Verification.True(source.IndexOf("segmentIndex = 0", StringComparison.Ordinal) < 0,
+            "active observation segment is not hardcoded to zero");
+    }
+
    private static void AdvancesOnlyAfterContinuousStopAndStableNextDirection()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 0, 0, 0, TimeSpan.Zero);