From 822db6934a7a297cdad0345cfc408b8bd1665229 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= <lby115@126.com>
Date: Wed, 5 Aug 2026 09:55:35 +0800
Subject: [PATCH] feat: format EM observation diagnostics

---
 .../TrajectoryObservationDiagnostics.cs       | 40 +++++++++++++++++++
 .../TrajectoryObservationChecks.cs            | 26 ++++++++++++
 2 files changed, 66 insertions(+)
 create mode 100644 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs

diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs
new file mode 100644
index 0000000..b3a85d2
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs
@@ -0,0 +1,40 @@
+using System;
+using System.Globalization;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+public sealed class TrajectoryObservationDiagnostic
+{
+    public TrajectoryObservationDiagnostic(string text)
+    {
+        Text = text ?? string.Empty;
+    }
+
+    public string Text { get; }
+}
+
+public static class TrajectoryObservationDiagnostics
+{
+    public static TrajectoryObservationDiagnostic Create(PlanningCycleResult latestCycle,
+        TimeSpan elapsed, bool planningInFlight, EmTrajectory publishedTrajectory)
+    {
+        if (latestCycle == null)
+            return new TrajectoryObservationDiagnostic(planningInFlight
+                ? "planning status=pending"
+                : "planning status=not-started");
+
+        string text = "planning cycle=" + latestCycle.Version.ToString(CultureInfo.InvariantCulture) +
+            " status=" + latestCycle.Result.Status +
+            " published=" + latestCycle.Published +
+            " elapsed=" + Math.Max(0d, elapsed.TotalMilliseconds).ToString("F0", CultureInfo.InvariantCulture) +
+            "ms";
+        if (planningInFlight)
+            text += "\nreplan=pending";
+        if (publishedTrajectory != null)
+            text += "\ntrajectory=" + publishedTrajectory.Metadata.TrajectoryId;
+        if (!string.IsNullOrWhiteSpace(latestCycle.Result.FailureReason))
+            text += "\nreason=" + latestCycle.Result.FailureReason;
+        return new TrajectoryObservationDiagnostic(text);
+    }
+}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
index 1947136..355ac4d 100644
--- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
@@ -30,6 +30,7 @@ internal static class TrajectoryObservationChecks
         VerifiesGearSwitchWaitStateForWorldPresentation();
         VerifiesLsAndStUsePublishedTrajectoryData();
         VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
+        VerifiesPlanningDiagnosticsKeepRawFailureReason();
         VerifiesLsPresentationUsesPathSOnHorizontalAxis();
         VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
         FreezesBootstrapVehicleForRollingRequests();
@@ -459,6 +460,31 @@ internal static class TrajectoryObservationChecks
             "observer presentation LS projection failures");
     }
 
+    private static void VerifiesPlanningDiagnosticsKeepRawFailureReason()
+    {
+        var failed = new PlanningCycleResult(
+            4L,
+            new PlanningCycleIdentity(3L, "diagnostic-reference", 7L, string.Empty, 0),
+            new EmPlanningResult(EmPlanningStatus.CorridorInfeasible, null,
+                "map=3;reference=diagnostic-reference;state=7;previous=;segment=0;reason=no connected corridor"),
+            false,
+            "map=3;reference=diagnostic-reference;state=7;previous=;segment=0;reason=no connected corridor");
+
+        TrajectoryObservationDiagnostic diagnostic = TrajectoryObservationDiagnostics.Create(
+            failed, TimeSpan.FromMilliseconds(18d), false, null);
+
+        Verification.True(diagnostic.Text.Contains("cycle=4"), "diagnostic has cycle version");
+        Verification.True(diagnostic.Text.Contains("status=CorridorInfeasible"), "diagnostic preserves raw status");
+        Verification.True(diagnostic.Text.Contains("published=False"), "diagnostic preserves publish state");
+        Verification.True(diagnostic.Text.Contains("elapsed=18ms"), "diagnostic preserves elapsed time");
+        Verification.True(diagnostic.Text.Contains("reason=map=3;reference=diagnostic-reference"),
+            "diagnostic preserves planner failure reason");
+
+        TrajectoryObservationDiagnostic pending = TrajectoryObservationDiagnostics.Create(
+            null, TimeSpan.Zero, true, null);
+        Verification.Equal("planning status=pending", pending.Text, "diagnostic reports pending before completion");
+    }
+
     private static void VerifiesLsPresentationUsesPathSOnHorizontalAxis()
     {
         TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(