fix: complete EM observation final review

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -6,6 +6,7 @@ using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
 using MultiWheelC.TrajectoryPlanning.EMPlanner;
+using MultiWheelC.TrajectoryPlanning.Mapping;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
 using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
@@ -19,8 +20,14 @@ internal static class TrajectoryObservationChecks
         VerifiesObservationSourceHasNoActuatorCalls();
         VerifiesObservationSourceUsesRequiredOperatorText();
         VerifiesOperatorDocumentationUsesExactUiEntry();
+        VerifiesMovementTestVehicleInputsAndSettingsSnapshot();
+        RejectsInvalidObservationSettings();
         VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
+        VerifiesValidNonEmptyObstacleSource();
         RejectsObstacleOutsideConfiguredBounds();
+        VerifiesObserverTicksWhilePlanningIsDelayed();
+        VerifiesSessionLayerCleanupDecisions();
+        VerifiesGearSwitchWaitStateForWorldPresentation();
         VerifiesLsAndStUsePublishedTrajectoryData();
         VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
         VerifiesLsPresentationUsesPathSOnHorizontalAxis();
@@ -61,8 +68,9 @@ internal static class TrajectoryObservationChecks
 
         Verification.True(source.Contains("[MovementTest(name = \"EM轨迹规划观察闭环测试\")]"),
             "observation MovementTest uses required Chinese display name");
-        Verification.True(source.Contains("\"等待真实档位/方向确认；观察模式不会推进下一方向段\""),
-            "observation MovementTest uses required gear-switch notice");
+        Verification.Equal("等待真实档位/方向确认；观察模式不会推进下一方向段",
+            TrajectoryObservationRuntimeState.GearSwitchWaitingNotice,
+            "observation runtime uses required gear-switch notice");
     }
 
     private static void VerifiesOperatorDocumentationUsesExactUiEntry()
@@ -81,6 +89,82 @@ internal static class TrajectoryObservationChecks
             "observation README has the exact UI entry");
     }
 
+    private static void VerifiesMovementTestVehicleInputsAndSettingsSnapshot()
+    {
+        string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
+            "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
+        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestSource));
+        Verification.True(source.Contains("public double VehicleLengthMeters = 0.80d;"),
+            "observer MovementTest exposes vehicle length with Task-1 default");
+        Verification.True(source.Contains("public double VehicleWidthMeters = 0.60d;"),
+            "observer MovementTest exposes vehicle width with Task-1 default");
+        Verification.True(source.Contains("public double SafetyMarginMeters = 0.05d;"),
+            "observer MovementTest exposes safety margin with Task-1 default");
+        Verification.True(source.Contains("public double MaximumCurvaturePerMeter = 1d / 1.20d;"),
+            "observer MovementTest exposes maximum curvature with Task-1 default");
+        Verification.True(source.Contains("VehicleLengthMeters = VehicleLengthMeters,"),
+            "observer MovementTest copies vehicle length into settings");
+        Verification.True(source.Contains("VehicleWidthMeters = VehicleWidthMeters,"),
+            "observer MovementTest copies vehicle width into settings");
+        Verification.True(source.Contains("SafetyMarginMeters = SafetyMarginMeters,"),
+            "observer MovementTest copies safety margin into settings");
+        Verification.True(source.Contains("MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,"),
+            "observer MovementTest copies maximum curvature into settings");
+        string normalizedSource = source.Replace("\r\n", "\n");
+        Verification.True(normalizedSource.Contains(
+                "VehicleMotionState state = ReadVehicleState();\n            DateTimeOffset now = state.CapturedAtUtc;"),
+            "observer host uses the fresh state snapshot time for each observation tick");
+
+        var configured = new TrajectoryObservationSettings
+        {
+            VehicleLengthMeters = 1.10d,
+            VehicleWidthMeters = 0.70d,
+            SafetyMarginMeters = 0.08d,
+            MaximumCurvaturePerMeter = 0.55d,
+        };
+        TrajectoryObservationSettings snapshot = configured.CreateValidatedSnapshot();
+        configured.VehicleLengthMeters = 9.10d;
+        configured.VehicleWidthMeters = 9.20d;
+        configured.SafetyMarginMeters = 9.30d;
+        configured.MaximumCurvaturePerMeter = 9.40d;
+
+        Verification.NearlyEqual(1.10d, snapshot.VehicleLengthMeters,
+            "observer settings snapshot freezes vehicle length");
+        Verification.NearlyEqual(0.70d, snapshot.VehicleWidthMeters,
+            "observer settings snapshot freezes vehicle width");
+        Verification.NearlyEqual(0.08d, snapshot.SafetyMarginMeters,
+            "observer settings snapshot freezes safety margin");
+        Verification.NearlyEqual(0.55d, snapshot.MaximumCurvaturePerMeter,
+            "observer settings snapshot freezes maximum curvature");
+
+        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
+            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), snapshot,
+            Array.Empty<TrajectoryObservationObstacle>(), 0L);
+        AssertVehicleSnapshot(job.Vehicle, 1.10d, 0.70d, 0.08d, 0.55d, null,
+            "observer configured bootstrap vehicle");
+    }
+
+    private static void RejectsInvalidObservationSettings()
+    {
+        AssertInvalidSetting(settings => settings.MapPaddingMeters = 0d, "map padding");
+        AssertInvalidSetting(settings => settings.MapResolutionMillimeters = float.NaN, "map resolution");
+        AssertInvalidSetting(settings => settings.ReplanPeriodSeconds = 0d, "replan period");
+        AssertInvalidSetting(settings => settings.ObserverPeriodSeconds = double.PositiveInfinity,
+            "observer period");
+        AssertInvalidSetting(settings => settings.VehicleLengthMeters = 0d, "vehicle length");
+        AssertInvalidSetting(settings => settings.VehicleWidthMeters = double.NaN, "vehicle width");
+        AssertInvalidSetting(settings => settings.SafetyMarginMeters = 0d, "safety margin");
+        AssertInvalidSetting(settings => settings.MaximumCurvaturePerMeter = double.PositiveInfinity,
+            "maximum curvature");
+    }
+
+    private static void AssertInvalidSetting(Action<TrajectoryObservationSettings> mutate, string name)
+    {
+        var settings = new TrajectoryObservationSettings();
+        mutate(settings);
+        Verification.True(Throws(settings.Validate), "observer rejects invalid " + name);
+    }
+
     private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
     {
         var settings = new TrajectoryObservationSettings
@@ -99,6 +183,29 @@ internal static class TrajectoryObservationChecks
         Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution");
     }
 
+    private static void VerifiesValidNonEmptyObstacleSource()
+    {
+        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
+            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(),
+            new[] { TrajectoryObservationObstacle.Circle(500d, 500d, 100d) }, 23L);
+
+        Verification.True(!job.MapRequest.AllowExplicitEmptyMap,
+            "observer non-empty obstacle map is not explicitly empty");
+        Verification.Equal(1, job.MapRequest.ObstacleSources.Count,
+            "observer non-empty map has exactly one obstacle source");
+        IMapObstacleSource source = job.MapRequest.ObstacleSources[0];
+        Verification.Equal("trajectory-observer-manual", source.SourceId,
+            "observer manual obstacle source ID");
+        Verification.Equal(23L, source.SourceVersion,
+            "observer manual obstacle source version");
+        Verification.True(source.IsRequired, "observer manual obstacle source is required");
+        ObstacleProjectionResult projection = source.ProjectToWorld();
+        Verification.Equal(ObstacleSourceStatus.Applied, projection.Status,
+            "observer valid manual obstacle source applies");
+        Verification.Equal(1, projection.Obstacles.Count,
+            "observer valid manual obstacle source preserves geometry");
+    }
+
     private static void RejectsObstacleOutsideConfiguredBounds()
     {
         Verification.True(Throws(() => TrajectoryObservationSetupFactory.CreateBootstrapJob(
@@ -107,6 +214,144 @@ internal static class TrajectoryObservationChecks
             "observer obstacle outside configured bounds");
     }
 
+    private static void VerifiesObserverTicksWhilePlanningIsDelayed()
+    {
+        DateTimeOffset startedAt = new DateTimeOffset(2026, 8, 4, 3, 0, 0, TimeSpan.Zero);
+        var settings = new TrajectoryObservationSettings();
+        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
+            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
+            Array.Empty<TrajectoryObservationObstacle>(), 0L);
+        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
+            .Bootstrap(job, CancellationToken.None);
+        Verification.True(bootstrap.Succeeded, "observer delayed-planner bootstrap succeeds");
+
+        EmTrajectory published = CreatePublishedTrajectory(startedAt);
+        var planningService = new DelayedTrajectoryPlanningService(published, 2);
+        try
+        {
+            var controller = new TrajectoryObservationController(
+                bootstrap, settings, planningService, "cadence-check");
+            var loop = new TrajectoryObservationLoop(controller);
+            var firstState = new VehicleMotionState(
+                new Pose2D(0.10d, 0d, 0d), 0.10d, null, startedAt, 10L);
+
+            TrajectoryObservationLoopTick firstTick = loop.Tick(startedAt, firstState, CancellationToken.None);
+
+            Verification.True(firstTick.ShouldLog,
+                "observer first cadence tick is eligible for session-guarded logging");
+            Verification.Equal(10L, firstTick.Observation.VehicleState.SequenceId,
+                "observer first tick uses first fresh state");
+
+            TrajectoryObservationLoopTick? initialPublicationTick = null;
+            bool initiallyPublished = SpinWait.SpinUntil(() =>
+            {
+                var currentState = new VehicleMotionState(
+                    new Pose2D(0.15d, 0d, 0d), 0.15d, null,
+                    startedAt.AddSeconds(0.10d), 11L);
+                initialPublicationTick = loop.Tick(startedAt.AddSeconds(0.10d), currentState,
+                    CancellationToken.None);
+                return !initialPublicationTick.PlanningInFlight;
+            }, TimeSpan.FromSeconds(5d));
+            Verification.True(initiallyPublished && initialPublicationTick != null &&
+                    ReferenceEquals(published, initialPublicationTick.Observation.PublishedTrajectory),
+                "observer establishes a published trajectory before delayed rolling planning");
+
+            DateTimeOffset replanAt = startedAt.AddSeconds(settings.ReplanPeriodSeconds);
+            var replanState = new VehicleMotionState(
+                new Pose2D(0.20d, 0d, 0d), 0.20d, null, replanAt, 12L);
+            TrajectoryObservationLoopTick replanTick = loop.Tick(replanAt, replanState,
+                CancellationToken.None);
+            Verification.True(replanTick.PlanningInFlight,
+                "observer delayed rolling planning remains in flight after replan tick");
+            Verification.True(planningService.WaitUntilEntered(TimeSpan.FromSeconds(5d)),
+                "observer delayed planner enters planning service");
+
+            DateTimeOffset secondAt = replanAt.AddSeconds(settings.ObserverPeriodSeconds);
+            var secondState = new VehicleMotionState(
+                new Pose2D(0.25d, 0d, 0d), 0.25d, null, secondAt, 13L);
+            TrajectoryObservationLoopTick secondTick = loop.Tick(secondAt, secondState, CancellationToken.None);
+
+            Verification.True(secondTick.PlanningInFlight,
+                "observer second tick does not wait for delayed planning");
+            Verification.True(secondTick.ShouldLog,
+                "observer second cadence tick is eligible for session-guarded logging");
+            Verification.True(ReferenceEquals(published, secondTick.Observation.PublishedTrajectory),
+                "observer keeps observing the existing publication during delayed rolling planning");
+            Verification.Equal(13L, secondTick.Observation.VehicleState.SequenceId,
+                "observer second tick uses second fresh state");
+            Verification.Equal(secondAt, secondTick.Observation.ObservedAtUtc,
+                "observer second tick observes at its own time");
+
+            planningService.Release();
+            TrajectoryObservationLoopTick? completedTick = null;
+            bool completed = SpinWait.SpinUntil(() =>
+            {
+                var currentState = new VehicleMotionState(
+                    new Pose2D(0.30d, 0d, 0d), 0.30d, null,
+                    replanAt.AddSeconds(0.10d), 14L);
+                completedTick = loop.Tick(replanAt.AddSeconds(0.10d), currentState,
+                    CancellationToken.None);
+                return !completedTick.PlanningInFlight;
+            }, TimeSpan.FromSeconds(5d));
+
+            Verification.True(completed, "observer delayed planning completes deterministically");
+            Verification.True(completedTick != null, "observer delayed planning produces a completion tick");
+            TrajectoryObservationLoopTick finalTick = completedTick!;
+            Verification.True(finalTick.LatestCycle != null && finalTick.LatestCycle.Published,
+                "observer delayed planning result is consumed without a continuation");
+            Verification.Equal(14L, finalTick.Observation.VehicleState.SequenceId,
+                "observer post-plan observation does not reuse pre-plan state");
+            Verification.Equal(replanAt.AddSeconds(0.10d), finalTick.Observation.ObservedAtUtc,
+                "observer post-plan observation does not reuse pre-plan time");
+        }
+        finally
+        {
+            planningService.Release();
+        }
+    }
+
+    private static void VerifiesSessionLayerCleanupDecisions()
+    {
+        Verification.True(!TrajectoryObservationSessionLifecycle.ShouldClearLayers(
+                TrajectoryObservationSessionEndReason.BootstrapFailure),
+            "observer bootstrap failure preserves the diagnostic world view");
+        Verification.True(TrajectoryObservationSessionLifecycle.ShouldClearLayers(
+                TrajectoryObservationSessionEndReason.RuntimeFault),
+            "observer runtime fault clears all painter layers");
+        Verification.True(TrajectoryObservationSessionLifecycle.ShouldClearLayers(
+                TrajectoryObservationSessionEndReason.Cancellation),
+            "observer cancellation clears all painter layers");
+    }
+
+    private static void VerifiesGearSwitchWaitStateForWorldPresentation()
+    {
+        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 4, 0, 0, TimeSpan.Zero);
+        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt, EmTerminalType.GearSwitch);
+
+        TrajectoryObservationRuntimeState beforeFinal = TrajectoryObservationRuntimeState.Create(
+            effectiveAt.AddSeconds(0.99d), trajectory);
+        Verification.True(!beforeFinal.WaitingAtGearSwitch,
+            "observer does not paint gear-switch wait state before final time");
+        Verification.Equal(string.Empty, beforeFinal.WorldNotice,
+            "observer has no gear-switch world notice before final time");
+
+        TrajectoryObservationRuntimeState atFinal = TrajectoryObservationRuntimeState.Create(
+            effectiveAt.AddSeconds(1d), trajectory);
+        Verification.True(atFinal.WaitingAtGearSwitch,
+            "observer enters gear-switch wait state at final time");
+        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");
+
+        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
+            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
+        string presentationSource = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
+        Verification.True(presentationSource.Contains(
+                "worldPainter.DrawText(Color.OrangeRed, runtimeState.WorldNotice"),
+            "observer world painter draws the exact runtime wait state");
+    }
+
     private static void VerifiesLsAndStUsePublishedTrajectoryData()
     {
         DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
@@ -333,10 +578,11 @@ internal static class TrajectoryObservationChecks
             new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 12d), 10d);
     }
 
-    private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt)
+    private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt,
+        EmTerminalType terminalType = EmTerminalType.Goal)
     {
         var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
-            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal);
+            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, terminalType);
         return new EmTrajectory(metadata, new[]
         {
             new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,
@@ -364,6 +610,41 @@ internal static class TrajectoryObservationChecks
         }
     }
 
+    private sealed class DelayedTrajectoryPlanningService : IEmPlanningService
+    {
+        private readonly EmTrajectory trajectory;
+        private readonly int delayedCall;
+        private readonly ManualResetEventSlim entered = new ManualResetEventSlim(false);
+        private readonly ManualResetEventSlim release = new ManualResetEventSlim(false);
+        private int callCount;
+
+        public DelayedTrajectoryPlanningService(EmTrajectory trajectory, int delayedCall)
+        {
+            this.trajectory = trajectory;
+            this.delayedCall = delayedCall;
+        }
+
+        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
+        {
+            if (Interlocked.Increment(ref callCount) == delayedCall)
+            {
+                entered.Set();
+                release.Wait(cancellationToken);
+            }
+            return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
+        }
+
+        public bool WaitUntilEntered(TimeSpan timeout)
+        {
+            return entered.Wait(timeout);
+        }
+
+        public void Release()
+        {
+            release.Set();
+        }
+    }
+
     private static bool Throws(Action action)
     {
         try { action(); return false; }