feat: summarize published EM observation trajectories

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs

@@ -1,5 +1,7 @@
 using System;
+using System.Collections.Generic;
 using System.Globalization;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.EMPlanner;
 
 namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
@@ -51,8 +53,8 @@ public static class TrajectoryObservationDiagnostics
             "ms";
         if (planningInFlight)
             text += "\nreplan=pending";
-        if (publishedTrajectory != null)
-            text += "\ntrajectory=" + publishedTrajectory.Metadata.TrajectoryId;
+        if (latestCycle.Published && latestCycle.Result.Trajectory != null)
+            text += "\n" + CreateTrajectorySummary(latestCycle.Result.Trajectory);
         if (!string.IsNullOrWhiteSpace(latestCycle.Result.FailureReason))
             text += "\nreason=" + latestCycle.Result.FailureReason;
         return new TrajectoryObservationDiagnostic(text);
@@ -62,4 +64,48 @@ public static class TrajectoryObservationDiagnostics
     {
         return value.ToString(format, CultureInfo.InvariantCulture);
     }
+
+    private static string CreateTrajectorySummary(EmTrajectory trajectory)
+    {
+        IReadOnlyList<EmTrajectoryPoint> points = trajectory.Points;
+        double maximumSpeed = 0d;
+        double maximumAcceleration = 0d;
+        double maximumJerk = 0d;
+        double previousAcceleration = 0d;
+        bool hasPreviousAcceleration = false;
+        double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
+        for (int index = 0; index < points.Count; index++)
+        {
+            EmTrajectoryPoint point = points[index];
+            maximumSpeed = Math.Max(maximumSpeed, Math.Abs(point.SignedLongitudinalVelocity));
+            if (index == 0) continue;
+
+            EmTrajectoryPoint previous = points[index - 1];
+            double dt = point.TimeFromStart - previous.TimeFromStart;
+            if (!IsPositiveFinite(dt)) continue;
+            double acceleration = (directionSign * point.SignedLongitudinalVelocity -
+                directionSign * previous.SignedLongitudinalVelocity) / dt;
+            maximumAcceleration = Math.Max(maximumAcceleration, Math.Abs(acceleration));
+            if (hasPreviousAcceleration)
+                maximumJerk = Math.Max(maximumJerk, Math.Abs((acceleration - previousAcceleration) / dt));
+            previousAcceleration = acceleration;
+            hasPreviousAcceleration = true;
+        }
+
+        EmTrajectoryPoint first = points[0];
+        EmTrajectoryPoint last = points[points.Count - 1];
+        return "trajectory summary:\n" +
+            "trajectoryId=" + trajectory.Metadata.TrajectoryId + ", points=" +
+            points.Count.ToString(CultureInfo.InvariantCulture) + ", duration=" +
+            Format(last.TimeFromStart, "F3") + "s, pathLength=" +
+            Format(last.PathS - first.PathS, "F3") + "m\n" +
+            "maxSpeed=" + Format(maximumSpeed, "F3") + "m/s, maxAcceleration=" +
+            Format(maximumAcceleration, "F3") + "m/s2, maxJerk=" +
+            Format(maximumJerk, "F3") + "m/s3";
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
+    }
 }

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -32,6 +32,7 @@ internal static class TrajectoryObservationChecks
         VerifiesLsAndStUsePublishedTrajectoryData();
         VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
         VerifiesPlanningDiagnosticsKeepRawFailureReason();
+        VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary();
         VerifiesEmptyChartsReceivePersistentPlanningDiagnostic();
         VerifiesLsPresentationUsesPathSOnHorizontalAxis();
         VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
@@ -595,12 +596,34 @@ internal static class TrajectoryObservationChecks
         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");
+        Verification.True(!diagnostic.Text.Contains("trajectory summary:"),
+            "failed diagnostic has no stale trajectory summary");
 
         TrajectoryObservationDiagnostic pending = TrajectoryObservationDiagnostics.Create(
             null, TimeSpan.Zero, true, null);
         Verification.Equal("planning status=pending", pending.Text, "diagnostic reports pending before completion");
     }
 
+    private static void VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary()
+    {
+        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero);
+        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
+        var succeeded = new PlanningCycleResult(9L,
+            new PlanningCycleIdentity(1L, "summary-reference", 2L, string.Empty, 0),
+            new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty), true, string.Empty);
+
+        string text = TrajectoryObservationDiagnostics.Create(
+            succeeded, TimeSpan.FromMilliseconds(12d), false, trajectory).Text;
+        foreach (string expected in new[]
+        {
+            "trajectory summary:", "trajectoryId=observer-published", "points=2", "duration=1.000s",
+            "pathLength=1.000m", "maxSpeed=0.400m/s", "maxAcceleration=0.200m/s2", "maxJerk=0.000m/s3",
+        })
+        {
+            Verification.True(text.Contains(expected), "published diagnostic includes " + expected);
+        }
+    }
+
     private static void VerifiesEmptyChartsReceivePersistentPlanningDiagnostic()
     {
         string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",