diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs index 68053c0..1c50d91 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs +++ b/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 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; + } } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 77034b2..0e7af44 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/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",