feat: log EM observation session planning configuration

2026-08-05 15:32:37 +08:00
parent 32083db4d0
commit 9b59ce4a71
4 changed files with 80 additions and 8 deletions
@@ -271,6 +271,7 @@ internal static class TrajectoryObservationMovementTestRunner
        var controller = new TrajectoryObservationController(bootstrap, settings,
            new EmPlanningService(new OsqpNativeSolver()),
            "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
+        LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
        var observationLoop = new TrajectoryObservationLoop(controller);
        DirectionSegmentView segment = bootstrap.Segments[0];
        bool gearSwitchWaitingPrinted = false;
@@ -16,6 +16,26 @@ public sealed class TrajectoryObservationDiagnostic

 public static class TrajectoryObservationDiagnostics
 {
+    public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
+    {
+        if (configuration == null || configuration.Scheduling == null || configuration.Solver == null)
+            throw new ArgumentNullException(nameof(configuration));
+
+        double horizon = configuration.Scheduling.TimeHorizonSeconds;
+        double outputTimeStep = configuration.Scheduling.OutputTimeStepSeconds;
+        int trajectoryKnots = checked((int)Math.Ceiling(horizon / outputTimeStep) + 1);
+        return new TrajectoryObservationDiagnostic(
+            "planning configuration:\n" +
+            "timeHorizon=" + Format(horizon, "F2") + "s\n" +
+            "distanceHorizon=" + Format(configuration.Scheduling.DistanceHorizonMeters, "F2") + "m\n" +
+            "outputTimeStep=" + Format(outputTimeStep, "F2") + "s\n" +
+            "outputFrequency=" + Format(1d / outputTimeStep, "F2") + "Hz\n" +
+            "trajectoryKnots=" + trajectoryKnots.ToString(CultureInfo.InvariantCulture) + "\n" +
+            "maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" +
+            "solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" +
+            "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s");
+    }
+
    public static TrajectoryObservationDiagnostic Create(PlanningCycleResult latestCycle,
        TimeSpan elapsed, bool planningInFlight, EmTrajectory publishedTrajectory)
    {
@@ -37,4 +57,9 @@ public static class TrajectoryObservationDiagnostics
            text += "\nreason=" + latestCycle.Result.FailureReason;
        return new TrajectoryObservationDiagnostic(text);
    }
+
+    private static string Format(double value, string format)
+    {
+        return value.ToString(format, CultureInfo.InvariantCulture);
+    }
 }
@@ -218,6 +218,11 @@ public sealed class TrajectoryObservationController

    public EmTrajectory PublishedTrajectory => coordinator.PublishedTrajectory;

+    public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
+    {
+        return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
+    }
+
    public bool ShouldStartCycle(DateTimeOffset now)
    {
        return coordinator.ShouldStartCycle(now);