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
@@ -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);