chore: save current workspace progress

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -39,7 +39,8 @@ internal static class TrajectoryObservationChecks
         VerifiesObserverTicksWhilePlanningIsDelayed();
         VerifiesFullDirectionSegmentPlansOncePerActiveSegment();
         VerifiesFullDirectionRequestCarriesValidatedScope();
-        VerifiesFullDirectionPlansAgainOnlyAfterConfirmedTransition();
+        VerifiesFullDirectionPreplansAfterStopAndActivatesAfterConfirmation();
+        VerifiesDeterministicSingleForwardObservationSimulation();
         VerifiesFailedFullPlanDoesNotAutoRetry();
         VerifiesStaticSnapshotExportsPlanningScope();
         VerifiesSessionLayerCleanupDecisions();
@@ -114,7 +115,7 @@ internal static class TrajectoryObservationChecks
 
         Verification.True(source.Contains("[MovementTest(name = \"EM轨迹规划观察闭环测试\")]"),
             "observation MovementTest uses required Chinese display name");
-        Verification.Equal("等待真实档位/方向确认；观察模式不会推进下一方向段",
+        Verification.Equal("等待真实档位/方向确认；观察模式不会激活下一方向段",
             TrajectoryObservationRuntimeState.GearSwitchWaitingNotice,
             "observation runtime uses required gear-switch notice");
         Verification.True(source.Contains("Console.WriteLine(\"[TrajectoryObserver] \" + text);"),
@@ -180,6 +181,10 @@ internal static class TrajectoryObservationChecks
         Verification.True(normalizedSource.Contains(
                 "VehicleMotionState state = ReadVehicleState();\n            DateTimeOffset now = state.CapturedAtUtc;"),
             "observer host uses the fresh state snapshot time for each observation tick");
+        Verification.True(source.Contains("effectiveConfiguration.Frenet.MaximumProjectionDistanceMeters"),
+            "native observation charts use the EM Frenet projection tolerance rather than map padding");
+        Verification.True(source.Contains("tick.SegmentState.Phase == TrajectoryObservationSegmentPhase.Completed"),
+            "observer session terminates after the final direction segment completes");
         Verification.NearlyEqual(0.10d, new TrajectoryObservationSettings().OutputTimeStepSeconds,
             "observer settings use the ST timestamp-spacing default");
 
@@ -524,7 +529,7 @@ internal static class TrajectoryObservationChecks
             "full scope blocks coordinator-cadence restart after the first plan");
     }
 
-    private static void VerifiesFullDirectionPlansAgainOnlyAfterConfirmedTransition()
+    private static void VerifiesFullDirectionPreplansAfterStopAndActivatesAfterConfirmation()
     {
         DateTimeOffset t0 = new DateTimeOffset(2026, 8, 7, 2, 0, 0, TimeSpan.Zero);
         var settings = new TrajectoryObservationSettings
@@ -550,28 +555,90 @@ internal static class TrajectoryObservationChecks
         var stoppedState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0, 1L);
         controller.StartCycle(t0, stoppedState, CancellationToken.None).GetAwaiter().GetResult();
 
-        Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(1d)),
-            "no replan is armed before a confirmed segment transition");
         controller.TryAdvanceSegment(t0, stoppedState);
-        controller.TryAdvanceSegment(t0.AddSeconds(0.21d),
-            new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, t0.AddSeconds(0.21d), 2L));
-        Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(1d)),
-            "stop hold and direction waiting do not reset the one-shot flag");
+        DateTimeOffset stoppedAt = t0.AddSeconds(0.21d);
+        var heldStoppedState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null, stoppedAt, 2L);
+        controller.TryAdvanceSegment(stoppedAt, heldStoppedState);
+        Verification.True(controller.ShouldStartCycle(stoppedAt),
+            "the next full direction segment is planned after the real stop hold, before direction evidence");
+
+        PlanningCycleResult pendingCycle = controller.StartCycle(stoppedAt, heldStoppedState, CancellationToken.None)
+            .GetAwaiter().GetResult();
+        Verification.True(pendingCycle.Published, "the stopped-state pending segment plan is independently publishable");
+        Verification.Equal(2, planningService.Requests.Count,
+            "the next segment is planned exactly once from the stopped state");
+        Verification.Equal(1, planningService.Requests[1].SegmentIndex,
+            "the stopped-state pending request targets segment N+1");
+        Verification.Equal(heldStoppedState.SequenceId, planningService.Requests[1].VehicleState.SequenceId,
+            "the pending segment request uses the real stopped vehicle state");
+        Verification.True(planningService.Requests[1].PreviousTrajectory == null,
+            "a pending direction segment never reuses the old direction trajectory as an EM seed");
+        Verification.Equal(0, controller.ActiveSegment.SegmentIndex,
+            "planning N+1 does not replace the old segment before direction confirmation");
+        Verification.True(ReferenceEquals(forwardGearTrajectory, controller.PublishedTrajectory),
+            "the old segment trajectory remains active while N+1 waits for confirmation");
 
         var reverseState = new VehicleMotionState(new Pose2D(1d, 0d, 0d), -0.03d, null,
             t0.AddSeconds(0.22d), 3L);
         Verification.True(controller.TryAdvanceSegment(t0.AddSeconds(0.22d), reverseState),
             "confirmed N to N+1 transition succeeds");
-        Verification.True(controller.ShouldStartCycle(t0.AddSeconds(0.23d)),
-            "one-shot planning is rearmed only after the confirmed transition");
-
-        controller.StartCycle(t0.AddSeconds(0.23d), reverseState, CancellationToken.None).GetAwaiter().GetResult();
+        Verification.Equal(1, controller.ActiveSegment.SegmentIndex,
+            "the pending plan becomes active only after direction confirmation");
+        Verification.True(!controller.ShouldStartCycle(t0.AddSeconds(0.23d)),
+            "confirmed activation does not trigger a duplicate full-segment plan");
         Verification.Equal(2, planningService.Requests.Count,
-            "the next active direction segment receives exactly one new plan");
-        Verification.Equal(1, planningService.Requests[1].SegmentIndex,
-            "the new full-direction plan targets segment N+1");
-        Verification.Equal(EmPlanningScope.FullDirectionSegment, planningService.Requests[1].PlanningScope,
-            "the new full-direction plan keeps the full scope");
+            "the pending plan is reused at activation instead of planning N+1 twice");
+        Verification.Equal(reverseState.CapturedAtUtc, controller.PublishedTrajectory.Metadata.EffectiveAtUtc,
+            "pending trajectory effective time is rebased to its real activation time");
+    }
+
+    private static void VerifiesDeterministicSingleForwardObservationSimulation()
+    {
+        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 8, 0, 0, 0, TimeSpan.Zero);
+        var settings = new TrajectoryObservationSettings
+        {
+            PlanningScope = EmPlanningScope.FullDirectionSegment,
+        };
+        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, "deterministic single-forward simulation bootstrap succeeds");
+        TrajectoryObservationBootstrapResult bootstrap = TrajectoryObservationBootstrapResult.Success(
+            baseBootstrap.Job, baseBootstrap.CoarseResult, baseBootstrap.SmoothedPath, new[]
+            {
+                CreateDirectionalSegment(0, TravelDirection.Forward, 0d, 1d, false,
+                    EmBoundaryType.None, EmBoundaryType.Goal),
+            });
+        EmTrajectory trajectory = CreateSingleForwardSimulationTrajectory(t0);
+        var controller = new TrajectoryObservationController(bootstrap, settings,
+            new FixedTrajectoryPlanningService(trajectory), "single-forward-simulation");
+        var loop = new TrajectoryObservationLoop(controller);
+
+        double x = 0d;
+        double heading = 0d;
+        var initialState = new VehicleMotionState(new Pose2D(x, 0d, heading), 0d, null, t0, 1L);
+        controller.StartCycle(t0, initialState, CancellationToken.None).GetAwaiter().GetResult();
+        TrajectoryControlCommand command = controller.Observe(t0, initialState).Command;
+        TrajectoryObservationLoopTick? finalTick = null;
+        const double deltaSeconds = 0.10d;
+        for (int step = 1; step <= 10; step++)
+        {
+            x += command.SignedLongitudinalVelocity * Math.Cos(heading) * deltaSeconds;
+            heading += command.YawRate * deltaSeconds;
+            DateTimeOffset now = t0.AddSeconds(step * deltaSeconds);
+            var state = new VehicleMotionState(new Pose2D(x, 0d, heading), command.SignedLongitudinalVelocity,
+                null, now, step + 1L);
+            finalTick = loop.Tick(now, state, CancellationToken.None);
+            command = finalTick.Observation.Command;
+        }
+
+        Verification.NearlyEqual(1d, x, "simulated forward vehicle reaches the real Goal pose from executor commands");
+        Verification.Equal(TrajectoryObservationSegmentPhase.Completed, finalTick!.SegmentState.Phase,
+            "simulated forward Goal completes the observation lifecycle");
+        Verification.NearlyEqual(0d, command.SignedLongitudinalVelocity,
+            "simulated forward Goal receives the terminal zero-speed command");
     }
 
     private static void VerifiesFailedFullPlanDoesNotAutoRetry()
@@ -666,11 +733,24 @@ internal static class TrajectoryObservationChecks
             effectiveAt.AddSeconds(1d), trajectory);
         Verification.True(atFinal.WaitingAtGearSwitch,
             "observer enters gear-switch wait state at final time");
-        Verification.Equal("等待真实档位/方向确认；观察模式不会推进下一方向段", atFinal.WorldNotice,
+        Verification.Equal("等待真实档位/方向确认；观察模式不会激活下一方向段", atFinal.WorldNotice,
             "observer exposes the exact gear-switch state to the world painter");
         Verification.Equal(0, trajectory.Metadata.SegmentIndex,
             "observer gear-switch wait state remains on segment zero");
 
+        var tracker = new TrajectoryObservationSegmentTracker(CreateDirectionalSegments(),
+            new TrajectoryObservationSettings(), 0.01d);
+        TrajectoryObservationRuntimeState planningState = TrajectoryObservationRuntimeState.Create(tracker.State);
+        Verification.True(!planningState.WaitingAtGearSwitch,
+            "tracker-planning state does not show a gear-switch wait notice merely because planned time elapsed");
+        tracker.Update(effectiveAt.AddSeconds(1d), new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null,
+                effectiveAt.AddSeconds(1d), 1L),
+            CreateSegmentTrajectory(effectiveAt, "runtime-forward", 0, TravelDirection.Forward,
+                EmTerminalType.GearSwitch, EmBoundaryType.GearSwitchApproach, 1d));
+        TrajectoryObservationRuntimeState stoppedState = TrajectoryObservationRuntimeState.Create(tracker.State);
+        Verification.True(stoppedState.WaitingAtGearSwitch,
+            "actual tracker stop state shows the gear-switch wait notice");
+
         string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
             "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
         string presentationSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
@@ -780,6 +860,7 @@ internal static class TrajectoryObservationChecks
         DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 4, 0, 0, TimeSpan.Zero);
         var settings = new TrajectoryObservationSettings
         {
+            PlanningScope = EmPlanningScope.RollingHorizon,
             DirectionConfirmationSamples = 1,
         };
         CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
@@ -1244,6 +1325,20 @@ internal static class TrajectoryObservationChecks
         });
     }
 
+    private static EmTrajectory CreateSingleForwardSimulationTrajectory(DateTimeOffset effectiveAt)
+    {
+        var metadata = new EmTrajectoryMetadata("single-forward-simulation", effectiveAt, effectiveAt, 1L,
+            "single-forward-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal,
+            EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.FullDirectionSegment);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(0d, 0d, 0d, 1d, 0d, 0d, 0, 0d, 0d,
+                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(1d, 0d, 0d, 0d, 1d, 0d, 0, 1d, 1d,
+                TravelDirection.Forward, EmBoundaryType.Goal, 0d, 0d),
+        });
+    }
+
     private sealed class FixedTrajectoryPlanningService : IEmPlanningService
     {
         private readonly EmTrajectory trajectory;