feat: show EM observation failure diagnostics

2026-08-05 09:59:52 +08:00
parent 6262a4573a
commit 0be35d73d4
3 changed files with 44 additions and 12 deletions
@@ -248,7 +248,8 @@ internal static class TrajectoryObservationMovementTestRunner
        bootstrapTimer.Stop();
        if (!bootstrap.Succeeded)
        {
-            DrawIfCurrent(sessionId, bootstrap, null, null, null);
+            DrawIfCurrent(sessionId, bootstrap, null, null, null,
+                new TrajectoryObservationDiagnostic("bootstrap failed: " + bootstrap.FailureReason));
            LogIfCurrent(sessionId, "Planning bootstrap failed after " +
                bootstrapTimer.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
                " ms: " + bootstrap.FailureReason);
@@ -280,7 +281,7 @@ internal static class TrajectoryObservationMovementTestRunner
                    settings.MapPaddingMeters);
            TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(
                now, observation.PublishedTrajectory);
-            DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState);
+            DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic);
            if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted)
            {
                LogIfCurrent(sessionId, runtimeState.WorldNotice);
@@ -365,14 +366,15 @@ internal static class TrajectoryObservationMovementTestRunner

    private static void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
        TrajectoryObservationObservation observation, TrajectoryObservationCharts charts,
-        TrajectoryObservationRuntimeState runtimeState)
+        TrajectoryObservationRuntimeState runtimeState, TrajectoryObservationDiagnostic diagnostic)
    {
        lock (SessionSync)
        {
            if (activeSessionId != sessionId) return;
-            Presentation.DrawWorld(bootstrap, observation, runtimeState);
-            Presentation.DrawLs(charts);
-            Presentation.DrawSt(charts);
+            string diagnosticText = diagnostic == null ? string.Empty : diagnostic.Text;
+            Presentation.DrawWorld(bootstrap, observation, runtimeState, diagnosticText);
+            Presentation.DrawLs(charts, diagnosticText);
+            Presentation.DrawSt(charts, diagnosticText);
        }
    }

@@ -88,7 +88,8 @@ public sealed class TrajectoryObservationPresentation
    private readonly Painter stPainter = UI.GetPainter("TrajectoryObserver.ST", true);

    public void DrawWorld(TrajectoryObservationBootstrapResult bootstrap,
-        TrajectoryObservationObservation observation, TrajectoryObservationRuntimeState runtimeState)
+        TrajectoryObservationObservation observation, TrajectoryObservationRuntimeState runtimeState,
+        string diagnosticText)
    {
        worldPainter.Clear();
        if (bootstrap == null)
@@ -114,18 +115,22 @@ public sealed class TrajectoryObservationPresentation
        EmTrajectory trajectory = observation == null ? null : observation.PublishedTrajectory;
        if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
        {
-            worldPainter.DrawText(Color.LightYellow, "No published trajectory available.", 0f, 0f);
+            float diagnosticX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
+            float diagnosticY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMin + 300f;
+            worldPainter.DrawText(Color.LightYellow,
+                EmptyChartMessage("No published trajectory available.\n", diagnosticText), diagnosticX, diagnosticY);
            return;
        }
        DrawEmPath(trajectory.Points);
    }

-    public void DrawLs(TrajectoryObservationCharts charts)
+    public void DrawLs(TrajectoryObservationCharts charts, string diagnosticText)
    {
        lsPainter.Clear();
        if (charts == null || charts.LsSamples == null || charts.LsSamples.Count == 0)
        {
-            lsPainter.DrawText(Color.LightYellow, "No published trajectory available for L-S chart.", 0f, 0f);
+            lsPainter.DrawText(Color.LightYellow,
+                EmptyChartMessage("No published trajectory available for L-S chart.\n", diagnosticText), 0f, 0f);
            return;
        }

@@ -147,12 +152,13 @@ public sealed class TrajectoryObservationPresentation
        }
    }

-    public void DrawSt(TrajectoryObservationCharts charts)
+    public void DrawSt(TrajectoryObservationCharts charts, string diagnosticText)
    {
        stPainter.Clear();
        if (charts == null || charts.StSamples == null || charts.StSamples.Count == 0)
        {
-            stPainter.DrawText(Color.LightYellow, "No published trajectory available for T-S/T-V charts.", 0f, 0f);
+            stPainter.DrawText(Color.LightYellow,
+                EmptyChartMessage("No published trajectory available for T-S/T-V charts.\n", diagnosticText), 0f, 0f);
            return;
        }

@@ -179,6 +185,13 @@ public sealed class TrajectoryObservationPresentation
        stPainter.Clear();
    }

+    private static string EmptyChartMessage(string label, string diagnosticText)
+    {
+        return string.IsNullOrWhiteSpace(diagnosticText)
+            ? label
+            : label + diagnosticText;
+    }
+
    private void DrawMap(PlanningGridMap map)
    {
        if (map == null) return;
@@ -31,6 +31,7 @@ internal static class TrajectoryObservationChecks
        VerifiesLsAndStUsePublishedTrajectoryData();
        VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
        VerifiesPlanningDiagnosticsKeepRawFailureReason();
+        VerifiesEmptyChartsReceivePersistentPlanningDiagnostic();
        VerifiesLsPresentationUsesPathSOnHorizontalAxis();
        VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
        FreezesBootstrapVehicleForRollingRequests();
@@ -490,6 +491,22 @@ internal static class TrajectoryObservationChecks
        Verification.Equal("planning status=pending", pending.Text, "diagnostic reports pending before completion");
    }

+    private static void VerifiesEmptyChartsReceivePersistentPlanningDiagnostic()
+    {
+        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
+            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
+        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
+
+        Verification.True(source.Contains("DrawLs(TrajectoryObservationCharts charts, string diagnosticText)"),
+            "LS painter accepts planning diagnostic input");
+        Verification.True(source.Contains("DrawSt(TrajectoryObservationCharts charts, string diagnosticText)"),
+            "ST painter accepts planning diagnostic input");
+        Verification.True(source.Contains("No published trajectory available for L-S chart.\\n"),
+            "LS empty state includes diagnostic after chart label");
+        Verification.True(source.Contains("No published trajectory available for T-S/T-V charts.\\n"),
+            "ST empty state includes diagnostic after chart label");
+    }
+
    private static void VerifiesLsPresentationUsesPathSOnHorizontalAxis()
    {
        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(