fix: stabilize stopped-start EM planning and add txt diagnostics

2026-08-07 15:51:31 +08:00
parent f2970d0aff
commit d378ad6257
13 changed files with 373 additions and 53 deletions
@@ -204,6 +204,7 @@ internal static class TrajectoryObservationMovementTestRunner
    private static Task activeTask;
    private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
    private static TrajectoryObservationPresentation activePresentation;
+    private static TrajectoryObservationReportWriter activeReportWriter;
    private static long nextSessionId;
    private static long activeSessionId;
    private static long stateSequence;
@@ -218,6 +219,7 @@ internal static class TrajectoryObservationMovementTestRunner
        var cancellation = new CancellationTokenSource();
        CancellationTokenSource previousCancellation;
        long sessionId;
+        TrajectoryObservationReportWriter reportWriter = null;
        lock (SessionSync)
        {
            previousCancellation = activeCancellation;
@@ -227,6 +229,8 @@ internal static class TrajectoryObservationMovementTestRunner
            activeSessionId = sessionId;
            StopActiveVisualizationNoLock();
            activePresentation = settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null;
+            reportWriter = new TrajectoryObservationReportWriter(sessionId);
+            activeReportWriter = reportWriter;
            PrintStatus("Starting frozen observation session " +
                sessionId.ToString(CultureInfo.InvariantCulture) + ".");
        }
@@ -239,7 +243,7 @@ internal static class TrajectoryObservationMovementTestRunner
        {
            if (activeSessionId == sessionId) activeTask = task;
        }
-        _ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed), TaskScheduler.Default);
+        _ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed, reportWriter), TaskScheduler.Default);
    }

    internal static void Stop()
@@ -271,6 +275,13 @@ internal static class TrajectoryObservationMovementTestRunner
    {
        token.ThrowIfCancellationRequested();
        VehicleMotionState initialState = ReadVehicleState();
+        LogIfCurrent(sessionId, "initial state: pose=(" + Format(initialState.Pose.X) + ", " +
+            Format(initialState.Pose.Y) + ", " + Format(initialState.Pose.Heading) + ") m/rad, signed speed=" +
+            initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) + " m/s, acceleration=" +
+            (initialState.LongitudinalAccelerationMetersPerSecondSquared.HasValue
+                ? Format(initialState.LongitudinalAccelerationMetersPerSecondSquared.Value)
+                : "unavailable") + " m/s2, stateSequence=" +
+            initialState.SequenceId.ToString(CultureInfo.InvariantCulture));
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);

@@ -294,6 +305,15 @@ internal static class TrajectoryObservationMovementTestRunner
            new EmPlanningService(new OsqpNativeSolver()),
            "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
        LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
+        LogIfCurrent(sessionId, CreateSettingsDiagnostic(settings));
+        EmPlannerConfiguration effectiveConfiguration = controller.CreateEffectiveConfigurationSnapshot();
+        double stopSpeedTolerance = effectiveConfiguration.Longitudinal.StopSpeedToleranceMetersPerSecond;
+        LogIfCurrent(sessionId, "static start check: initialSignedSpeed=" +
+            initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) +
+            ", stopSpeedTolerance=" + stopSpeedTolerance.ToString("R", CultureInfo.InvariantCulture) +
+            ", treatedAsStopped=" +
+            (Math.Abs(initialState.SignedLongitudinalSpeedMetersPerSecond) <= stopSpeedTolerance)
+                .ToString(CultureInfo.InvariantCulture));
        var observationLoop = new TrajectoryObservationLoop(controller);
        TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualizationIfEnabled(
            sessionId, bootstrap, controller, settings, obstacleSnapshotVersion);
@@ -332,7 +352,14 @@ internal static class TrajectoryObservationMovementTestRunner
            if (tick.PlanningStarted)
                LogIfCurrent(sessionId, "planning status=pending");
            if (tick.PlanningCompleted)
+            {
                LogIfCurrent(sessionId, diagnostic.Text);
+                if (observation.PublishedTrajectory != null && charts != null)
+                {
+                    AppendReportIfCurrent(sessionId,
+                        TrajectoryObservationDiagnostics.CreateCurveReport(charts, observation.PublishedTrajectory).Text);
+                }
+            }
        }
    }

@@ -490,14 +517,25 @@ internal static class TrajectoryObservationMovementTestRunner
        }
    }

+    private static void AppendReportIfCurrent(long sessionId, string text)
+    {
+        lock (SessionSync)
+        {
+            if (activeSessionId != sessionId) return;
+            activeReportWriter?.Append(text);
+        }
+    }
+
    private static void PrintStatus(string message)
    {
        string text = ObserveOnlyNotice + "\n" + message;
        Hedingben.ToastText(text, StatusChannel);
        Console.WriteLine("[TrajectoryObserver] " + text);
+        activeReportWriter?.Append(message);
    }

-    private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed)
+    private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed,
+        TrajectoryObservationReportWriter reportWriter)
    {
        try
        {
@@ -514,16 +552,38 @@ internal static class TrajectoryObservationMovementTestRunner
        {
            lock (SessionSync)
            {
+                if (ReferenceEquals(activeReportWriter, reportWriter))
+                    activeReportWriter = null;
                if (activeSessionId == sessionId)
                {
                    activeTask = null;
                    activeCancellation = null;
                }
            }
+            try
+            {
+                string reportPath = reportWriter.Save();
+                Console.WriteLine("[TrajectoryObserver] report=" + reportPath);
+            }
+            catch (Exception exception)
+            {
+                Console.WriteLine("[TrajectoryObserver] report write failed: " + exception.Message);
+            }
            cancellation.Dispose();
        }
    }

+    private static string CreateSettingsDiagnostic(TrajectoryObservationSettings settings)
+    {
+        return "observation settings: planningScope=" + settings.PlanningScope +
+            ", webVisualization=" + settings.EnableWebVisualization +
+            ", nativePainter=" + settings.EnableNativePainterVisualization +
+            ", outputTimeStep=" + Format(settings.OutputTimeStepSeconds) + "s" +
+            ", observerPeriod=" + Format(settings.ObserverPeriodSeconds) + "s" +
+            ", solverTimeout=" + Format(settings.SolverTimeoutSeconds) + "s" +
+            ", maximumOsqpIterations=" + settings.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture);
+    }
+
    private static void ClearAndLogRuntimeFaultIfCurrent(long sessionId, Exception exception)
    {
        lock (SessionSync)
@@ -10,6 +10,10 @@ diagnostic prediction only. Goal and rolling-safety-stop commands are logged, ne
 gear-switch trajectory, the observer remains on the current direction segment and waits for real direction confirmation;
 it does not create or dispatch a direction-change action.

+Every frozen observation session writes a text report to
+`TrajectoryObservationReports/trajectory-observation-session-<id>.txt` in the process working directory when the
+session stops; the report path is printed to the console.
+
 ## FullDirectionSegment observation (default)

 `UseFullDirectionSegmentPlanning=true` maps to `EmPlanningScope.FullDirectionSegment`. MovementTest performs one frozen
@@ -20,8 +24,7 @@ and zero-speed hold behavior instead of being forced by a legacy time horizon.

 `DistanceHorizonMeters` and `TimeHorizonSeconds` are rolling-compatible fields and do not truncate full mode. Forward
 desired/hard maximum speed is `1.0 m/s`; reverse desired/hard maximum speed is `0.5 m/s`. A successful real boundary
-requires a terminal stop plus world-position error no greater than `0.03 m` and normalized yaw error no greater than
-`5 deg` (`5 * PI / 180 rad`).
+requires a terminal stop plus exact world-position and normalized yaw matching (`0 m` / `0 rad`).

 ## Rolling trajectory observation

@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Globalization;
+using System.Text;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.EMPlanner;

@@ -18,10 +19,69 @@ public sealed class TrajectoryObservationDiagnostic

 public static class TrajectoryObservationDiagnostics
 {
+    public static TrajectoryObservationDiagnostic CreateCurveReport(TrajectoryObservationCharts charts,
+        EmTrajectory trajectory)
+    {
+        if (charts == null || trajectory == null)
+            return new TrajectoryObservationDiagnostic("curve report unavailable");
+
+        IReadOnlyList<EmTrajectoryPoint> points = trajectory.Points;
+        var text = new StringBuilder();
+        text.AppendLine("LS_CURVE");
+        text.AppendLine("index,referenceS(m),lateralOffset(m)");
+        for (int index = 0; index < charts.LsSamples.Count; index++)
+        {
+            TrajectoryObservationLsSample sample = charts.LsSamples[index];
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                sample.PathS.ToString("R", CultureInfo.InvariantCulture) + "," +
+                sample.LateralOffset.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        text.AppendLine("ST_CURVE");
+        text.AppendLine("index,time(s),pathS(m)");
+        for (int index = 0; index < charts.StSamples.Count; index++)
+        {
+            TrajectoryObservationStSample sample = charts.StSamples[index];
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
+                sample.PathS.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        text.AppendLine("SPEED_CURVE");
+        text.AppendLine("index,time(s),signedSpeed(m/s)");
+        for (int index = 0; index < charts.SpeedSamples.Count; index++)
+        {
+            TrajectoryObservationSpeedSample sample = charts.SpeedSamples[index];
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
+                sample.SignedLongitudinalVelocity.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        text.AppendLine("ACCELERATION_CURVE");
+        text.AppendLine("index,time(s),acceleration(m/s2)");
+        for (int index = 0; index < points.Count; index++)
+        {
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
+                points[index].LongitudinalAcceleration.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        text.AppendLine("JERK_CURVE");
+        text.AppendLine("index,time(s),jerk(m/s3)");
+        for (int index = 0; index + 1 < points.Count; index++)
+        {
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
+                points[index].LongitudinalJerk.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        return new TrajectoryObservationDiagnostic(text.ToString());
+    }
+
    public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
    {
        if (configuration == null || configuration.Scheduling == null || configuration.Solver == null ||
-            configuration.Longitudinal == null)
+            configuration.Longitudinal == null || configuration.Validation == null)
            throw new ArgumentNullException(nameof(configuration));

        double horizon = configuration.Scheduling.TimeHorizonSeconds;
@@ -57,6 +117,19 @@ public static class TrajectoryObservationDiagnostics
            "maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" +
            "solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" +
            "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s\n" +
+            "kinematicTolerance=" + Format(configuration.Validation.KinematicTolerance, "G17") + "\n" +
+            "stopSpeedToleranceMetersPerSecond=" + Format(longitudinal.StopSpeedToleranceMetersPerSecond, "G17") + "\n" +
+            "terminalPositionToleranceMeters=" + Format(configuration.Validation.TerminalPositionToleranceMeters, "G17") + "\n" +
+            "terminalYawToleranceRadians=" + Format(configuration.Validation.TerminalYawToleranceRadians, "G17") + "\n" +
+            "maximumOptimizationTimeStepSeconds=" + Format(configuration.Scheduling.MaximumOptimizationTimeStepSeconds, "F3") + "\n" +
+            "maximumOptimizationSpatialStepMeters=" + Format(configuration.Scheduling.MaximumOptimizationSpatialStepMeters, "F3") + "\n" +
+            "maximumOptimizationKnotCount=" + configuration.Scheduling.MaximumOptimizationKnotCount.ToString(CultureInfo.InvariantCulture) + "\n" +
+            "maximumPublishedSampleCount=" + configuration.Scheduling.MaximumPublishedSampleCount.ToString(CultureInfo.InvariantCulture) + "\n" +
+            "maximumForwardSpeedMetersPerSecond=" + Format(longitudinal.MaximumForwardSpeedMetersPerSecond, "F3") + "\n" +
+            "maximumReverseSpeedMetersPerSecond=" + Format(longitudinal.MaximumReverseSpeedMetersPerSecond, "F3") + "\n" +
+            "maximumAccelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumAccelerationMetersPerSecondSquared, "F3") + "\n" +
+            "maximumDecelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumDecelerationMetersPerSecondSquared, "F3") + "\n" +
+            "maximumJerkMetersPerSecondCubed=" + Format(longitudinal.MaximumJerkMetersPerSecondCubed, "F3") + "\n" +
            "maximumJerkLimitedStopDistance=" + Format(worstStop.DistanceMeters, "F3") + "m\n" +
            "maximumJerkLimitedStopDuration=" + Format(worstStop.DurationSeconds, "F3") + "s\n" +
            "requiredDistanceHorizon=" + Format(requiredDistanceHorizon, "F3") + "m");
@@ -0,0 +1,67 @@
+using System;
+using System.Globalization;
+using System.IO;
+using System.Text;
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+/// <summary>Buffers a readable text report for one trajectory observation session.</summary>
+public sealed class TrajectoryObservationReportWriter
+{
+    private readonly long _sessionId;
+    private readonly string _reportDirectory;
+    private readonly StringBuilder _content = new StringBuilder();
+    private readonly object _sync = new object();
+
+    public TrajectoryObservationReportWriter(long sessionId, string reportDirectory = null)
+    {
+        if (sessionId <= 0)
+            throw new ArgumentOutOfRangeException(nameof(sessionId));
+
+        _sessionId = sessionId;
+        _reportDirectory = reportDirectory ??
+            Path.Combine(Environment.CurrentDirectory, "TrajectoryObservationReports");
+        AppendHeader();
+    }
+
+    public string FilePath
+    {
+        get
+        {
+            return Path.Combine(_reportDirectory, "trajectory-observation-session-" +
+                _sessionId.ToString(CultureInfo.InvariantCulture) + ".txt");
+        }
+    }
+
+    public void Append(string text)
+    {
+        if (string.IsNullOrEmpty(text))
+            return;
+
+        lock (_sync)
+        {
+            _content.Append(DateTimeOffset.Now.ToString("yyyy-MM-dd HH:mm:ss.fff", CultureInfo.InvariantCulture));
+            _content.Append(" | ");
+            _content.AppendLine(text);
+        }
+    }
+
+    public string Save()
+    {
+        lock (_sync)
+        {
+            Directory.CreateDirectory(_reportDirectory);
+            File.WriteAllText(FilePath, _content.ToString(), new UTF8Encoding(false));
+            return FilePath;
+        }
+    }
+
+    private void AppendHeader()
+    {
+        _content.AppendLine("EM trajectory observation report");
+        _content.AppendLine("session=" + _sessionId.ToString(CultureInfo.InvariantCulture));
+        _content.AppendLine("OBSERVE_ONLY: no chassis command is sent.");
+        _content.AppendLine("reportGeneratedUtc=" + DateTimeOffset.UtcNow.ToString("O", CultureInfo.InvariantCulture));
+        _content.AppendLine();
+    }
+}