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;