feat: host EM observation dashboard

2026-08-06 11:48:46 +08:00
parent 819ff7324b
commit fbd3ba618a
4 changed files with 361 additions and 13 deletions
@@ -15,6 +15,7 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
 using MultiWheelC.TrajectoryPlanning.EMPlanner;
 using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+using TrajectoryPlanningVisualization;

 namespace MultiWheelC;

@@ -196,11 +197,11 @@ internal static class TrajectoryObservationMovementTestRunner
    private const string StatusChannel = "TrajectoryObserver";
    private const string ObserveOnlyNotice = "OBSERVE_ONLY: no chassis command is sent.";
    private static readonly object SessionSync = new object();
-    private static readonly TrajectoryObservationPresentation Presentation =
-        new TrajectoryObservationPresentation();

    private static CancellationTokenSource activeCancellation;
    private static Task activeTask;
+    private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
+    private static TrajectoryObservationPresentation activePresentation;
    private static long nextSessionId;
    private static long activeSessionId;
    private static long stateSequence;
@@ -222,7 +223,8 @@ internal static class TrajectoryObservationMovementTestRunner
            activeTask = null;
            sessionId = ++nextSessionId;
            activeSessionId = sessionId;
-            Presentation.ClearAll();
+            StopActiveVisualizationNoLock();
+            activePresentation = settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null;
            PrintStatus("Starting frozen observation session " +
                sessionId.ToString(CultureInfo.InvariantCulture) + ".");
        }
@@ -247,9 +249,7 @@ internal static class TrajectoryObservationMovementTestRunner
            activeCancellation = null;
            activeTask = null;
            activeSessionId = 0L;
-            if (TrajectoryObservationSessionLifecycle.ShouldClearLayers(
-                TrajectoryObservationSessionEndReason.Cancellation))
-                Presentation.ClearAll();
+            StopActiveVisualizationNoLock();
            PrintStatus("Observation stop requested; all observer layers were cleared.");
        }

@@ -293,7 +293,10 @@ internal static class TrajectoryObservationMovementTestRunner
            "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
        LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
        var observationLoop = new TrajectoryObservationLoop(controller);
-        DirectionSegmentView segment = bootstrap.Segments[0];
+        TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualizationIfEnabled(
+            sessionId, bootstrap, controller, settings, obstacleSnapshotVersion);
+        var dynamicSnapshotBuilder = new TrajectoryObservationDynamicSnapshotBuilder();
+        long visualizationSequence = 0L;
        bool gearSwitchWaitingPrinted = false;

        while (true)
@@ -306,6 +309,7 @@ internal static class TrajectoryObservationMovementTestRunner
            TrajectoryObservationDiagnostic diagnostic = TrajectoryObservationDiagnostics.Create(
                tick.LatestCycle, tick.LatestPlanningElapsed, tick.PlanningInFlight,
                observation.PublishedTrajectory);
+            DirectionSegmentView segment = controller.ActiveSegment;
            TrajectoryObservationCharts charts = observation.PublishedTrajectory == null
                ? null
                : TrajectoryObservationCharts.Build(observation.PublishedTrajectory, segment,
@@ -313,6 +317,9 @@ internal static class TrajectoryObservationMovementTestRunner
            TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(
                now, observation.PublishedTrajectory);
            DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic);
+            webPublisher?.TryPublish(now, () => dynamicSnapshotBuilder.Build(
+                Interlocked.Increment(ref visualizationSequence), tick, controller.ActiveSegment,
+                controller.PreviousTrajectoryForVisualization, controller.CreateEffectiveConfigurationSnapshot()));
            if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted)
            {
                LogIfCurrent(sessionId, runtimeState.WorldNotice);
@@ -395,6 +402,66 @@ internal static class TrajectoryObservationMovementTestRunner
            "LS projection failures=" + projectionFailures + "." + waiting;
    }

+    private static TrajectoryObservationVisualizationPublisher StartWebVisualizationIfEnabled(long sessionId,
+        TrajectoryObservationBootstrapResult bootstrap, TrajectoryObservationController controller,
+        TrajectoryObservationSettings settings, long obstacleSnapshotVersion)
+    {
+        if (!settings.EnableWebVisualization) return null;
+
+        var publisher = new TrajectoryObservationVisualizationPublisher(settings,
+            new PlanningVisualizationSessionSink(), message => LogIfCurrent(sessionId, message));
+        PlanningVisualizationSessionInfo sessionInfo;
+        try
+        {
+            var staticSnapshot = new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
+                controller.CreateEffectiveConfigurationSnapshot(), settings, obstacleSnapshotVersion);
+            sessionInfo = publisher.Start(staticSnapshot);
+        }
+        catch (Exception exception)
+        {
+            publisher.Disable(exception);
+            return null;
+        }
+
+        if (sessionInfo == null)
+            return null;
+        if (!TryAttachWebPublisher(sessionId, publisher))
+        {
+            publisher.Stop();
+            return null;
+        }
+
+        LogIfCurrent(sessionId, "网页可视化地址（含会话令牌）：" + sessionInfo.Uri.AbsoluteUri);
+        if (settings.AutoOpenWebVisualization)
+        {
+            try
+            {
+                Process.Start(new ProcessStartInfo
+                {
+                    FileName = sessionInfo.Uri.AbsoluteUri,
+                    UseShellExecute = true,
+                });
+            }
+            catch (Exception exception)
+            {
+                LogIfCurrent(sessionId, "无法自动打开网页；服务继续运行，请手动访问：" +
+                    sessionInfo.Uri.AbsoluteUri + "；" + exception.GetType().Name + "：" + exception.Message);
+            }
+        }
+        return publisher;
+    }
+
+    private static bool TryAttachWebPublisher(long sessionId,
+        TrajectoryObservationVisualizationPublisher publisher)
+    {
+        lock (SessionSync)
+        {
+            if (activeSessionId != sessionId) return false;
+            activeWebPublisher = publisher;
+            return true;
+        }
+    }
+
    private static void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationObservation observation, TrajectoryObservationCharts charts,
        TrajectoryObservationRuntimeState runtimeState, TrajectoryObservationDiagnostic diagnostic)
@@ -402,10 +469,12 @@ internal static class TrajectoryObservationMovementTestRunner
        lock (SessionSync)
        {
            if (activeSessionId != sessionId) return;
+            TrajectoryObservationPresentation presentation = activePresentation;
+            if (presentation == null) return;
            string diagnosticText = diagnostic == null ? string.Empty : diagnostic.Text;
-            Presentation.DrawWorld(bootstrap, observation, runtimeState, diagnosticText);
-            Presentation.DrawLs(charts, diagnosticText);
-            Presentation.DrawSt(charts, diagnosticText);
+            presentation.DrawWorld(bootstrap, observation, runtimeState, diagnosticText);
+            presentation.DrawLs(charts, diagnosticText);
+            presentation.DrawSt(charts, diagnosticText);
        }
    }

@@ -457,14 +526,23 @@ internal static class TrajectoryObservationMovementTestRunner
        lock (SessionSync)
        {
            if (activeSessionId != sessionId) return;
-            if (TrajectoryObservationSessionLifecycle.ShouldClearLayers(
-                TrajectoryObservationSessionEndReason.RuntimeFault))
-                Presentation.ClearAll();
+            StopActiveVisualizationNoLock();
            PrintStatus("Observation session failed; all observer layers were cleared: " +
                exception.GetType().Name + ": " + exception.Message);
        }
    }

+    private static void StopActiveVisualizationNoLock()
+    {
+        TrajectoryObservationVisualizationPublisher publisher = activeWebPublisher;
+        activeWebPublisher = null;
+        publisher?.Stop();
+
+        TrajectoryObservationPresentation presentation = activePresentation;
+        activePresentation = null;
+        presentation?.ClearAll();
+    }
+
    private static void CancelWithoutWaiting(CancellationTokenSource cancellation)
    {
        if (cancellation == null) return;
@@ -0,0 +1,145 @@
+using System;
+using System.Runtime.CompilerServices;
+using TrajectoryPlanningVisualization;
+
+[assembly: InternalsVisibleTo("EMPlannerVerificationHost")]
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+internal interface ITrajectoryObservationVisualizationSink
+{
+    PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options,
+        PlanningVisualizationStaticSnapshot snapshot);
+    void Publish(PlanningVisualizationDynamicSnapshot snapshot);
+    void Stop();
+}
+
+internal sealed class TrajectoryObservationVisualizationPublisher
+{
+    private readonly TrajectoryObservationSettings settings;
+    private readonly ITrajectoryObservationVisualizationSink sink;
+    private readonly Action<string> log;
+    private DateTimeOffset nextPublishAtUtc;
+    private bool started;
+    private bool stopped;
+    private bool sinkStopped;
+    private string faultReason = string.Empty;
+
+    internal TrajectoryObservationVisualizationPublisher(TrajectoryObservationSettings settings,
+        ITrajectoryObservationVisualizationSink sink, Action<string> log)
+    {
+        this.settings = settings ?? throw new ArgumentNullException(nameof(settings));
+        this.sink = sink ?? throw new ArgumentNullException(nameof(sink));
+        this.log = log ?? throw new ArgumentNullException(nameof(log));
+    }
+
+    internal string FaultReason => faultReason;
+
+    internal PlanningVisualizationSessionInfo Start(PlanningVisualizationStaticSnapshot snapshot)
+    {
+        if (!settings.EnableWebVisualization || stopped || faultReason.Length != 0)
+            return null;
+        if (started) return null;
+
+        try
+        {
+            if (snapshot == null) throw new ArgumentNullException(nameof(snapshot));
+            PlanningVisualizationSessionInfo session = sink.Start(new PlanningVisualizationOptions
+            {
+                Port = settings.WebVisualizationPort,
+                RefreshRateHz = Math.Max(1, (int)Math.Round(settings.WebRefreshRateHz,
+                    MidpointRounding.AwayFromZero)),
+                HistoryCycleLimit = settings.VisualizationHistoryCycleLimit,
+            }, snapshot);
+            if (session == null) throw new InvalidOperationException("网页会话未返回访问地址。");
+            started = true;
+            return session;
+        }
+        catch (Exception exception)
+        {
+            Fuse(exception);
+            return null;
+        }
+    }
+
+    internal bool TryPublish(DateTimeOffset now, Func<PlanningVisualizationDynamicSnapshot> snapshotFactory)
+    {
+        if (snapshotFactory == null) throw new ArgumentNullException(nameof(snapshotFactory));
+        if (!settings.EnableWebVisualization || !started || stopped || faultReason.Length != 0)
+            return false;
+        if (now < nextPublishAtUtc) return false;
+
+        try
+        {
+            PlanningVisualizationDynamicSnapshot snapshot = snapshotFactory();
+            if (snapshot == null) throw new InvalidOperationException("网页动态快照为空。");
+            nextPublishAtUtc = now + TimeSpan.FromSeconds(1d / settings.WebRefreshRateHz);
+            sink.Publish(snapshot);
+            return true;
+        }
+        catch (Exception exception)
+        {
+            Fuse(exception);
+            return false;
+        }
+    }
+
+    internal void Disable(Exception exception)
+    {
+        if (exception == null) throw new ArgumentNullException(nameof(exception));
+        Fuse(exception);
+    }
+
+    internal void Stop()
+    {
+        if (stopped) return;
+        stopped = true;
+        StopSink();
+    }
+
+    private void Fuse(Exception exception)
+    {
+        if (faultReason.Length != 0) return;
+        faultReason = "网页可视化已关闭：" + exception.GetType().Name + "：" + exception.Message;
+        StopSink();
+        log(faultReason);
+    }
+
+    private void StopSink()
+    {
+        if (sinkStopped) return;
+        sinkStopped = true;
+        try
+        {
+            sink.Stop();
+        }
+        catch
+        {
+        }
+    }
+}
+
+internal sealed class PlanningVisualizationSessionSink : ITrajectoryObservationVisualizationSink
+{
+    private PlanningVisualizationSession session;
+
+    public PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options,
+        PlanningVisualizationStaticSnapshot snapshot)
+    {
+        if (session != null) return session.Start(snapshot);
+        session = new PlanningVisualizationSession(options);
+        return session.Start(snapshot);
+    }
+
+    public void Publish(PlanningVisualizationDynamicSnapshot snapshot)
+    {
+        session?.Publish(snapshot);
+    }
+
+    public void Stop()
+    {
+        PlanningVisualizationSession current = session;
+        session = null;
+        current?.Dispose();
+    }
+}
@@ -24,6 +24,7 @@ internal static class TrajectoryObservationChecks
        VerifiesObservationSourceUsesRequiredOperatorText();
        VerifiesOperatorDocumentationUsesExactUiEntry();
        VerifiesMovementTestVehicleInputsAndSettingsSnapshot();
+        VerifiesMovementTestKeepsNativePainterLazyAndConditional();
        VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration();
        RejectsInvalidObservationSettings();
        VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
@@ -1025,6 +1026,17 @@ internal static class TrajectoryObservationChecks
        }
    }

+    private static void VerifiesMovementTestKeepsNativePainterLazyAndConditional()
+    {
+        string movementTestPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
+            "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
+        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestPath));
+        Verification.True(!source.Contains("static readonly TrajectoryObservationPresentation"),
+            "MovementTest has no static eager Painter presentation");
+        Verification.True(source.Contains("settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null"),
+            "MovementTest creates Painter only under the native visualization flag");
+    }
+
    private static bool Throws(Action action)
    {
        try { action(); return false; }
@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Globalization;
+using System.IO;
 using System.Linq;
 using System.Reflection;
 using System.Threading;
@@ -20,6 +21,8 @@ internal static class TrajectoryObservationVisualizationChecks
        VerifiesKinematicChartsKeepKnotAndIntervalSemantics();
        VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure();
        VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints();
+        VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks();
+        VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput();
    }

    private static void VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry()
@@ -215,6 +218,116 @@ internal static class TrajectoryObservationVisualizationChecks
            "projection failure is reported while dynamic snapshot construction remains available");
    }

+    private static void VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks()
+    {
+        var settings = new TrajectoryObservationSettings { EnableWebVisualization = true };
+        var sink = new RecordingVisualizationSink { ThrowOnPublish = true };
+        var logs = new List<string>();
+        var publisher = new TrajectoryObservationVisualizationPublisher(settings, sink, logs.Add);
+        DateTimeOffset now = DateTimeOffset.UnixEpoch;
+
+        publisher.Start(CreateStaticSnapshot(settings));
+        Verification.True(!publisher.TryPublish(now, () => CreateDynamicSnapshot(now)),
+            "publisher reports a failed web publish");
+        Verification.True(!publisher.TryPublish(now.AddSeconds(1d), () => throw new InvalidOperationException("must not build after fuse")),
+            "publisher permanently disables web output after the first fault");
+        Verification.Equal(1, sink.PublishCalls, "publisher invokes the throwing sink once");
+        Verification.Equal(1, sink.StopCalls, "publisher stops the sink once after its fault");
+        Verification.Equal(1, logs.Count, "publisher logs the web fault once");
+        Verification.True(publisher.FaultReason.Contains("网页"), "publisher records an immutable Chinese web fault reason");
+
+        TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
+        DateTimeOffset tickAt = now.AddSeconds(1d);
+        var controller = new TrajectoryObservationController(bootstrap, settings,
+            new FixedPlanningService(CreateTrajectory("publisher-loop", tickAt, 2, EmTerminalType.Goal,
+                EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "publisher-loop");
+        var loop = new TrajectoryObservationLoop(controller);
+        var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, null, tickAt, 1L);
+        TrajectoryObservationLoopTick firstTick = loop.Tick(tickAt, state, CancellationToken.None);
+        TrajectoryObservationLoopTick secondTick = loop.Tick(tickAt.AddSeconds(settings.ObserverPeriodSeconds), state,
+            CancellationToken.None);
+        Verification.True(firstTick.PlanningStarted || secondTick.PlanningStarted || secondTick.PlanningCompleted,
+            "observer loop continues producing ticks after a web publisher fault");
+    }
+
+    private static void VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput()
+    {
+        var settings = new TrajectoryObservationSettings
+        {
+            EnableWebVisualization = true,
+            WebRefreshRateHz = 10d,
+        };
+        var sink = new RecordingVisualizationSink();
+        var publisher = new TrajectoryObservationVisualizationPublisher(settings, sink, _ => { });
+        DateTimeOffset start = DateTimeOffset.UnixEpoch;
+        publisher.Start(CreateStaticSnapshot(settings));
+        for (int index = 0; index <= 20; index++)
+            publisher.TryPublish(start.AddMilliseconds(index * 50), () => CreateDynamicSnapshot(start));
+        Verification.True(sink.PublishCalls <= 11, "20 Hz observer ticks publish no more than 11 snapshots at 10 Hz over one second");
+        Verification.Equal(11, sink.PublishCalls, "publisher includes the initial 10 Hz snapshot");
+        publisher.Stop();
+        publisher.Stop();
+        Verification.Equal(1, sink.StopCalls, "publisher stop is idempotent");
+
+        var disabledSettings = new TrajectoryObservationSettings { EnableWebVisualization = false };
+        var disabledSink = new RecordingVisualizationSink();
+        var disabledPublisher = new TrajectoryObservationVisualizationPublisher(disabledSettings, disabledSink, _ => { });
+        int factoryCalls = 0;
+        disabledPublisher.Start(CreateStaticSnapshot(disabledSettings));
+        disabledPublisher.TryPublish(start, () =>
+        {
+            factoryCalls++;
+            return CreateDynamicSnapshot(start);
+        });
+        Verification.Equal(0, disabledSink.StartCalls, "disabled web visualization does not start a service");
+        Verification.Equal(0, factoryCalls, "disabled web visualization does not build dynamic snapshots");
+    }
+
+    private static PlanningVisualizationStaticSnapshot CreateStaticSnapshot(TrajectoryObservationSettings settings)
+    {
+        TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);
+        var controller = new TrajectoryObservationController(bootstrap, settings,
+            new FixedPlanningService(CreateTrajectory("publisher-static", DateTimeOffset.UnixEpoch, 2,
+                EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "publisher-static");
+        return new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
+            ReadEffectiveConfiguration(controller), settings.CreateValidatedSnapshot(), 0L);
+    }
+
+    private static PlanningVisualizationDynamicSnapshot CreateDynamicSnapshot(DateTimeOffset now)
+    {
+        return new PlanningVisualizationDynamicSnapshot(1L, now, "OBSERVE_ONLY", 0, "Forward",
+            new VisualizationPose(0d, 0d, 0d), Array.Empty<VisualizationPolyline>(),
+            Array.Empty<VisualizationMarker>(), Array.Empty<VisualizationChart>(), Array.Empty<VisualizationValue>(),
+            new VisualizationCycleSummary(1L, now, "Success", true, 0d, 0, "Forward", "RollingContinuation",
+                "Goal", 0d, 0d, string.Empty));
+    }
+
+    private sealed class RecordingVisualizationSink : ITrajectoryObservationVisualizationSink
+    {
+        public bool ThrowOnPublish { get; set; }
+        public int StartCalls { get; private set; }
+        public int PublishCalls { get; private set; }
+        public int StopCalls { get; private set; }
+
+        public PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options,
+            PlanningVisualizationStaticSnapshot snapshot)
+        {
+            StartCalls++;
+            return new PlanningVisualizationSessionInfo(new Uri("http://127.0.0.1:12345/?token=test"), "test");
+        }
+
+        public void Publish(PlanningVisualizationDynamicSnapshot snapshot)
+        {
+            PublishCalls++;
+            if (ThrowOnPublish) throw new InvalidOperationException("synthetic publish failure");
+        }
+
+        public void Stop()
+        {
+            StopCalls++;
+        }
+    }
+
    private static TrajectoryObservationBootstrapResult Bootstrap(TrajectoryObservationSettings settings)
    {
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(