fix: complete EM observation final review

2026-08-04 17:33:14 +08:00
parent d0e673b573
commit bd7170b611
7 changed files with 655 additions and 50 deletions
@@ -31,6 +31,10 @@ public sealed class TrajectoryObservationMovementTest : MovementTest
    public float MapResolutionMm = 50f;
    public double ReplanPeriodSeconds = 0.20d;
    public double ObserverPeriodSeconds = 0.05d;
+    public double VehicleLengthMeters = 0.80d;
+    public double VehicleWidthMeters = 0.60d;
+    public double SafetyMarginMeters = 0.05d;
+    public double MaximumCurvaturePerMeter = 1d / 1.20d;

    public override void Test()
    {
@@ -55,8 +59,12 @@ public sealed class TrajectoryObservationMovementTest : MovementTest
                MapResolutionMillimeters = MapResolutionMm,
                ReplanPeriodSeconds = ReplanPeriodSeconds,
                ObserverPeriodSeconds = ObserverPeriodSeconds,
+                VehicleLengthMeters = VehicleLengthMeters,
+                VehicleWidthMeters = VehicleWidthMeters,
+                SafetyMarginMeters = SafetyMarginMeters,
+                MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
            };
-            settings.Validate();
+            settings = settings.CreateValidatedSnapshot();
            TimeSpan observerPeriod = TimeSpan.FromSeconds(settings.ObserverPeriodSeconds);
            IReadOnlyList<TrajectoryObservationObstacle> obstacles = ReadManualObstacles();
            long obstacleSnapshotVersion = obstacles.Count == 0
@@ -157,9 +165,6 @@ internal static class TrajectoryObservationMovementTestRunner
 {
    private const string StatusChannel = "TrajectoryObserver";
    private const string ObserveOnlyNotice = "OBSERVE_ONLY: no chassis command is sent.";
-    private const string GearSwitchWaitingNotice =
-        "等待真实档位/方向确认；观察模式不会推进下一方向段";
-    private static readonly TimeSpan StatusPeriod = TimeSpan.FromSeconds(1d);
    private static readonly object SessionSync = new object();
    private static readonly TrajectoryObservationPresentation Presentation =
        new TrajectoryObservationPresentation();
@@ -212,7 +217,9 @@ internal static class TrajectoryObservationMovementTestRunner
            activeCancellation = null;
            activeTask = null;
            activeSessionId = 0L;
-            Presentation.ClearAll();
+            if (TrajectoryObservationSessionLifecycle.ShouldClearLayers(
+                TrajectoryObservationSessionEndReason.Cancellation))
+                Presentation.ClearAll();
            PrintStatus("Observation stop requested; all observer layers were cleared.");
        }

@@ -241,7 +248,7 @@ internal static class TrajectoryObservationMovementTestRunner
        bootstrapTimer.Stop();
        if (!bootstrap.Succeeded)
        {
-            DrawIfCurrent(sessionId, bootstrap, null, null);
+            DrawIfCurrent(sessionId, bootstrap, null, null, null);
            LogIfCurrent(sessionId, "Planning bootstrap failed after " +
                bootstrapTimer.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
                " ms: " + bootstrap.FailureReason);
@@ -253,44 +260,33 @@ internal static class TrajectoryObservationMovementTestRunner
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new EmPlanningService(new OsqpNativeSolver()),
            "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
+        var observationLoop = new TrajectoryObservationLoop(controller);
        DirectionSegmentView segment = bootstrap.Segments[0];
-        DateTimeOffset nextStatusAt = DateTimeOffset.MinValue;
-        PlanningCycleResult latestCycle = null;
-        TimeSpan latestPlanningElapsed = TimeSpan.Zero;
        bool gearSwitchWaitingPrinted = false;

        while (true)
        {
            await Task.Delay(observerPeriod, token).ConfigureAwait(false);
-            DateTimeOffset now = DateTimeOffset.UtcNow;
            VehicleMotionState state = ReadVehicleState();
-            if (controller.ShouldStartCycle(now))
-            {
-                var planningTimer = Stopwatch.StartNew();
-                latestCycle = await controller.StartCycle(now, state, token).ConfigureAwait(false);
-                planningTimer.Stop();
-                latestPlanningElapsed = planningTimer.Elapsed;
-            }
-
-            TrajectoryObservationObservation observation = controller.Observe(now, state);
+            DateTimeOffset now = state.CapturedAtUtc;
+            TrajectoryObservationLoopTick tick = observationLoop.Tick(now, state, token);
+            TrajectoryObservationObservation observation = tick.Observation;
            TrajectoryObservationCharts charts = observation.PublishedTrajectory == null
                ? null
                : TrajectoryObservationCharts.Build(observation.PublishedTrajectory, segment,
                    settings.MapPaddingMeters);
-            DrawIfCurrent(sessionId, bootstrap, observation, charts);
-            bool waitingAtGearSwitch = HasReachedGearSwitchFinal(now, observation.PublishedTrajectory);
-            if (waitingAtGearSwitch && !gearSwitchWaitingPrinted)
+            TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(
+                now, observation.PublishedTrajectory);
+            DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState);
+            if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted)
            {
-                LogIfCurrent(sessionId, GearSwitchWaitingNotice);
+                LogIfCurrent(sessionId, runtimeState.WorldNotice);
                gearSwitchWaitingPrinted = true;
            }

-            if (now >= nextStatusAt)
-            {
+            if (tick.ShouldLog)
                LogIfCurrent(sessionId, CreateTickStatus(sessionId, observation, charts,
-                    latestCycle, latestPlanningElapsed, waitingAtGearSwitch));
-                nextStatusAt = now + StatusPeriod;
-            }
+                    tick.LatestCycle, tick.LatestPlanningElapsed, runtimeState.WaitingAtGearSwitch));
        }
    }

@@ -305,14 +301,6 @@ internal static class TrajectoryObservationMovementTestRunner
            speed.Vx, null, DateTimeOffset.UtcNow, Interlocked.Increment(ref stateSequence));
    }

-    private static bool HasReachedGearSwitchFinal(DateTimeOffset now, EmTrajectory trajectory)
-    {
-        if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
-            return false;
-        EmTrajectoryPoint finalPoint = trajectory.Points[trajectory.Points.Count - 1];
-        return (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds >= finalPoint.TimeFromStart;
-    }
-
    private static string CreateBootstrapStatus(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationSettings settings, int obstacleCount, TimeSpan elapsed)
    {
@@ -356,7 +344,9 @@ internal static class TrajectoryObservationMovementTestRunner
        string projectionFailures = charts == null
            ? "unavailable"
            : charts.FailedProjectionCount.ToString(CultureInfo.InvariantCulture);
-        string waiting = waitingAtGearSwitch ? "\n" + GearSwitchWaitingNotice : string.Empty;
+        string waiting = waitingAtGearSwitch
+            ? "\n" + TrajectoryObservationRuntimeState.GearSwitchWaitingNotice
+            : string.Empty;

        return "Session " + sessionId.ToString(CultureInfo.InvariantCulture) + ".\n" +
            "pose=(" + Format(observation.VehicleState.Pose.X) + ", " +
@@ -370,12 +360,13 @@ internal static class TrajectoryObservationMovementTestRunner
    }

    private static void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
-        TrajectoryObservationObservation observation, TrajectoryObservationCharts charts)
+        TrajectoryObservationObservation observation, TrajectoryObservationCharts charts,
+        TrajectoryObservationRuntimeState runtimeState)
    {
        lock (SessionSync)
        {
            if (activeSessionId != sessionId) return;
-            Presentation.DrawWorld(bootstrap, observation);
+            Presentation.DrawWorld(bootstrap, observation, runtimeState);
            Presentation.DrawLs(charts);
            Presentation.DrawSt(charts);
        }
@@ -406,8 +397,7 @@ internal static class TrajectoryObservationMovementTestRunner
        }
        catch (Exception exception)
        {
-            LogIfCurrent(sessionId, "Observation session failed: " +
-                exception.GetType().Name + ": " + exception.Message);
+            ClearAndLogRuntimeFaultIfCurrent(sessionId, exception);
        }
        finally
        {
@@ -423,6 +413,19 @@ internal static class TrajectoryObservationMovementTestRunner
        }
    }

+    private static void ClearAndLogRuntimeFaultIfCurrent(long sessionId, Exception exception)
+    {
+        lock (SessionSync)
+        {
+            if (activeSessionId != sessionId) return;
+            if (TrajectoryObservationSessionLifecycle.ShouldClearLayers(
+                TrajectoryObservationSessionEndReason.RuntimeFault))
+                Presentation.ClearAll();
+            PrintStatus("Observation session failed; all observer layers were cleared: " +
+                exception.GetType().Name + ": " + exception.Message);
+        }
+    }
+
    private static void CancelWithoutWaiting(CancellationTokenSource cancellation)
    {
        if (cancellation == null) return;
@@ -21,10 +21,17 @@ not create or dispatch a direction-change action.
 | `MapResolutionMm` | mm | `50` | Local occupancy-grid resolution. |
 | `ReplanPeriodSeconds` | s | `0.20` | Minimum interval between EM planning cycles. |
 | `ObserverPeriodSeconds` | s | `0.05` | Live-state sampling and redraw interval. |
+| `VehicleLengthMeters` | m | `0.80` | Vehicle envelope length supplied to coarse, smoothing, and EM planning. |
+| `VehicleWidthMeters` | m | `0.60` | Vehicle envelope width supplied to planning. |
+| `SafetyMarginMeters` | m | `0.05` | Additional planning clearance outside the vehicle envelope. |
+| `MaximumCurvaturePerMeter` | 1/m | `1 / 1.20` | Maximum allowed vehicle curvature (about `0.8333 1/m`). |

-The planning snapshot also uses the setup defaults: vehicle length `0.80 m`, vehicle width `0.60 m`, safety margin
-`0.05 m`, and maximum curvature `1 / 1.20 m` (about `0.8333 1/m`). All fields and manual obstacles are read and frozen
-before the background session starts. The live pose and actual longitudinal speed are then read once per observer tick.
+The four vehicle fields are public editable MovementTest inputs. They, the map/timing fields, goal, and manual obstacles
+are validated and copied into a frozen input snapshot before the background session starts. Later edits cannot change an
+active session. The live pose and actual longitudinal speed are then read once per observer tick.
+EM planning runs asynchronously with at most one planning cycle in flight. Every observer tick still captures fresh
+state, samples the currently published trajectory, redraws all three layers, and emits a session-guarded status; a slow
+planner therefore does not reduce the configured observation cadence.

 ## Manual obstacles

@@ -70,4 +77,6 @@ command is diagnostic information only and must not be copied into a vehicle-con

 To stop, use the vehicle UI's normal MovementTest stop action. Confirm the status says
 `Observation stop requested; all observer layers were cleared.` The cancellation request stops observer work and clears
-all three layers. Stopping, running, or starting this test must not issue chassis, motor, steering, brake, or gear output.
+all three layers. A current-session runtime fault also clears all three layers so stale active diagnostics are not left
+on screen; an intentional bootstrap failure preserves its diagnostic world view. Stopping, running, or starting this
+test must not issue chassis, motor, steering, brake, or gear output.
@@ -17,6 +17,23 @@ public sealed class TrajectoryObservationSettings
    public double SafetyMarginMeters { get; set; } = 0.05d;
    public double MaximumCurvaturePerMeter { get; set; } = 1d / 1.20d;

+    public TrajectoryObservationSettings CreateValidatedSnapshot()
+    {
+        var snapshot = new TrajectoryObservationSettings
+        {
+            MapPaddingMeters = MapPaddingMeters,
+            MapResolutionMillimeters = MapResolutionMillimeters,
+            ReplanPeriodSeconds = ReplanPeriodSeconds,
+            ObserverPeriodSeconds = ObserverPeriodSeconds,
+            VehicleLengthMeters = VehicleLengthMeters,
+            VehicleWidthMeters = VehicleWidthMeters,
+            SafetyMarginMeters = SafetyMarginMeters,
+            MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
+        };
+        snapshot.Validate();
+        return snapshot;
+    }
+
    public void Validate()
    {
        EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters));
@@ -252,6 +252,122 @@ public sealed class TrajectoryObservationController
    }
 }

+public sealed class TrajectoryObservationLoopTick
+{
+    internal TrajectoryObservationLoopTick(TrajectoryObservationObservation observation,
+        PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed, bool planningInFlight)
+    {
+        Observation = observation ?? throw new ArgumentNullException(nameof(observation));
+        LatestCycle = latestCycle;
+        LatestPlanningElapsed = latestPlanningElapsed;
+        PlanningInFlight = planningInFlight;
+    }
+
+    public TrajectoryObservationObservation Observation { get; }
+
+    public PlanningCycleResult LatestCycle { get; }
+
+    public TimeSpan LatestPlanningElapsed { get; }
+
+    public bool PlanningInFlight { get; }
+
+    public bool ShouldLog => true;
+}
+
+public sealed class TrajectoryObservationLoop
+{
+    private readonly TrajectoryObservationController controller;
+    private Task<PlanningCycleResult> planningTask;
+    private DateTimeOffset planningStartedAtUtc;
+    private PlanningCycleResult latestCycle;
+    private TimeSpan latestPlanningElapsed;
+
+    public TrajectoryObservationLoop(TrajectoryObservationController controller)
+    {
+        this.controller = controller ?? throw new ArgumentNullException(nameof(controller));
+    }
+
+    public TrajectoryObservationLoopTick Tick(DateTimeOffset now, VehicleMotionState state,
+        CancellationToken cancellationToken)
+    {
+        if (state == null) throw new ArgumentNullException(nameof(state));
+        cancellationToken.ThrowIfCancellationRequested();
+
+        ConsumeCompletedPlanning(now);
+        if (planningTask == null && controller.ShouldStartCycle(now))
+        {
+            planningStartedAtUtc = now;
+            planningTask = controller.StartCycle(now, state, cancellationToken);
+            ConsumeCompletedPlanning(now);
+        }
+
+        TrajectoryObservationObservation observation = controller.Observe(now, state);
+        return new TrajectoryObservationLoopTick(observation, latestCycle, latestPlanningElapsed,
+            planningTask != null);
+    }
+
+    private void ConsumeCompletedPlanning(DateTimeOffset observedAtUtc)
+    {
+        Task<PlanningCycleResult> completed = planningTask;
+        if (completed == null || !completed.IsCompleted) return;
+
+        latestCycle = completed.GetAwaiter().GetResult();
+        TimeSpan elapsed = observedAtUtc - planningStartedAtUtc;
+        latestPlanningElapsed = elapsed < TimeSpan.Zero ? TimeSpan.Zero : elapsed;
+        planningTask = null;
+    }
+}
+
+public enum TrajectoryObservationSessionEndReason
+{
+    BootstrapFailure,
+    RuntimeFault,
+    Cancellation,
+}
+
+public static class TrajectoryObservationSessionLifecycle
+{
+    public static bool ShouldClearLayers(TrajectoryObservationSessionEndReason reason)
+    {
+        switch (reason)
+        {
+            case TrajectoryObservationSessionEndReason.BootstrapFailure:
+                return false;
+            case TrajectoryObservationSessionEndReason.RuntimeFault:
+            case TrajectoryObservationSessionEndReason.Cancellation:
+                return true;
+            default:
+                throw new ArgumentOutOfRangeException(nameof(reason));
+        }
+    }
+}
+
+public sealed class TrajectoryObservationRuntimeState
+{
+    public const string GearSwitchWaitingNotice =
+        "等待真实档位/方向确认；观察模式不会推进下一方向段";
+
+    private TrajectoryObservationRuntimeState(bool waitingAtGearSwitch)
+    {
+        WaitingAtGearSwitch = waitingAtGearSwitch;
+        WorldNotice = waitingAtGearSwitch ? GearSwitchWaitingNotice : string.Empty;
+    }
+
+    public bool WaitingAtGearSwitch { get; }
+
+    public string WorldNotice { get; }
+
+    public static TrajectoryObservationRuntimeState Create(DateTimeOffset now, EmTrajectory trajectory)
+    {
+        if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
+            return new TrajectoryObservationRuntimeState(false);
+
+        EmTrajectoryPoint finalPoint = trajectory.Points[trajectory.Points.Count - 1];
+        bool waiting = (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds >= finalPoint.TimeFromStart;
+        return new TrajectoryObservationRuntimeState(waiting);
+    }
+}
+
 public sealed class TrajectoryObservationLsSample
 {
    public TrajectoryObservationLsSample(double pathS, double lateralOffset)
@@ -88,7 +88,7 @@ public sealed class TrajectoryObservationPresentation
    private readonly Painter stPainter = UI.GetPainter("TrajectoryObserver.ST", true);

    public void DrawWorld(TrajectoryObservationBootstrapResult bootstrap,
-        TrajectoryObservationObservation observation)
+        TrajectoryObservationObservation observation, TrajectoryObservationRuntimeState runtimeState)
    {
        worldPainter.Clear();
        if (bootstrap == null)
@@ -104,6 +104,12 @@ public sealed class TrajectoryObservationPresentation
        DrawLocalG2Path(bootstrap.SmoothedPath == null ? null : bootstrap.SmoothedPath.Path);
        DrawPose(observation == null || observation.VehicleState == null ? null : observation.VehicleState.Pose,
            Color.DeepSkyBlue, "real pose");
+        if (runtimeState != null && runtimeState.WorldNotice.Length > 0)
+        {
+            float noticeX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
+            float noticeY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMax - 200f;
+            worldPainter.DrawText(Color.OrangeRed, runtimeState.WorldNotice, noticeX, noticeY);
+        }

        EmTrajectory trajectory = observation == null ? null : observation.PublishedTrajectory;
        if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
@@ -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; }