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)