docs: explain rolling longitudinal planning diagnostics

2026-08-05 21:21:58 +08:00
parent eb050b19e4
commit 302798b66d
4 changed files with 79 additions and 4 deletions
@@ -153,6 +153,14 @@ if (result.Status != EmPlanningStatus.Success && result.Status != EmPlanningStat
 EmTrajectory trajectory = result.Trajectory;
 ```

+## Rolling longitudinal planning semantics
+
+`DistanceHorizonMeters` controls the L-S reference window. `TimeHorizonSeconds` controls the S-T output duration.
+Only a real `Goal` or `GearSwitchApproach` boundary may require an exact zero-speed terminal. `RollingContinuation`
+and `ApproachStopBoundary` may publish nonzero terminal speed; they do not add a synthetic zero-speed hold tail.
+`ExactStopAtBoundary` instead includes a real boundary anchor and a stationary S/U/A stabilization interval inside the
+QP horizon. `ZeroSpeedHoldSeconds`, when configured, is appended only after that QP horizon.
+
 该服务不负责周期调用、版本淘汰、轨迹采样或控制命令。需要滚动运行时，将成功结果交给 [TrajectoryExecution](../TrajectoryExecution/README.md)，并由调用方管理状态捕获和周期。

 ## 详细使用指南（Detailed Usage Guide）
@@ -10,6 +10,12 @@ diagnostic prediction only. Goal and rolling-safety-stop commands are logged, ne
 gear-switch trajectory, the observer remains on direction segment `0` and waits for real direction confirmation; it does
 not create or dispatch a direction-change action.

+## Rolling trajectory observation
+
+Rolling trajectory observation remains `OBSERVE_ONLY` and never sends a chassis command. Rolling and approach trajectories
+may end with nonzero speed because `DistanceHorizonMeters` is an L-S reference window and `TimeHorizonSeconds` is the
+single S-T output duration. Only a real Goal or gear-switch boundary may publish an exact zero-speed terminal.
+
 ## Configuration

 | Field | Unit | Default | Meaning |
@@ -20,12 +20,33 @@ public static class TrajectoryObservationDiagnostics
 {
    public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
    {
-        if (configuration == null || configuration.Scheduling == null || configuration.Solver == null)
+        if (configuration == null || configuration.Scheduling == null || configuration.Solver == null ||
+            configuration.Longitudinal == 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);
+        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
+        if (!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumForwardSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile forwardStop, out _) ||
+            !JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumReverseSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile reverseStop, out _))
+        {
+            throw new ArgumentException("The configuration cannot construct the jerk-limited stopping diagnostic.",
+                nameof(configuration));
+        }
+        JerkLimitedStoppingProfile worstStop = forwardStop.DistanceMeters >= reverseStop.DistanceMeters
+            ? forwardStop
+            : reverseStop;
+        double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond,
+            longitudinal.MaximumReverseSpeedMetersPerSecond);
+        double requiredDistanceHorizon = worstStop.DistanceMeters +
+            maximumSpeed * configuration.Scheduling.ReplanPeriodSeconds;
        return new TrajectoryObservationDiagnostic(
            "planning configuration:\n" +
            "timeHorizon=" + Format(horizon, "F2") + "s\n" +
@@ -35,7 +56,10 @@ public static class TrajectoryObservationDiagnostics
            "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");
+            "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s\n" +
+            "maximumJerkLimitedStopDistance=" + Format(worstStop.DistanceMeters, "F3") + "m\n" +
+            "maximumJerkLimitedStopDuration=" + Format(worstStop.DurationSeconds, "F3") + "s\n" +
+            "requiredDistanceHorizon=" + Format(requiredDistanceHorizon, "F3") + "m");
    }

    public static TrajectoryObservationDiagnostic Create(PlanningCycleResult latestCycle,
@@ -57,6 +81,12 @@ public static class TrajectoryObservationDiagnostics
            text += "\n" + CreateTrajectorySummary(latestCycle.Result.Trajectory);
        if (!string.IsNullOrWhiteSpace(latestCycle.Result.FailureReason))
            text += "\nreason=" + latestCycle.Result.FailureReason;
+        if (!latestCycle.Published || latestCycle.Result.Trajectory == null)
+        {
+            string missingFailureSummary = CreateMissingFailureSummary(latestCycle.Result.FailureReason);
+            if (!string.IsNullOrEmpty(missingFailureSummary))
+                text += "\n" + missingFailureSummary;
+        }
        return new TrajectoryObservationDiagnostic(text);
    }

@@ -101,7 +131,28 @@ public static class TrajectoryObservationDiagnostics
            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";
+            Format(maximumJerk, "F3") + "m/s3\n" +
+            "longitudinalMode=" + trajectory.Metadata.LongitudinalMode + "\n" +
+            "terminalSpeed=" + Format(last.SignedLongitudinalVelocity, "F3") + "m/s\n" +
+            "terminalAcceleration=" + Format(last.LongitudinalAcceleration, "F3") + "m/s2";
+    }
+
+    private static string CreateMissingFailureSummary(string failureReason)
+    {
+        string reason = failureReason ?? string.Empty;
+        var missing = new List<string>();
+        AddMissingFailureField(missing, reason, "longitudinalMode=");
+        AddMissingFailureField(missing, reason, "remainingToBoundary=");
+        AddMissingFailureField(missing, reason, "minimumStoppingDistance=");
+        AddMissingFailureField(missing, reason, "minimumStoppingDuration=");
+        AddMissingFailureField(missing, reason, "maximumStoppedReachableDistance=");
+        return string.Join("\n", missing);
+    }
+
+    private static void AddMissingFailureField(ICollection<string> missing, string failureReason, string field)
+    {
+        if (failureReason.IndexOf(field, StringComparison.Ordinal) < 0)
+            missing.Add(field + "unavailable");
    }

    private static bool IsPositiveFinite(double value)
@@ -217,7 +217,8 @@ internal static class TrajectoryObservationChecks
        {
            "planning configuration:", "timeHorizon=3.50s", "distanceHorizon=5.00m", "outputTimeStep=0.10s",
            "outputFrequency=10.00Hz", "trajectoryKnots=36", "maximumOsqpIterations=54321",
-            "solverTimeout=1.25s", "replanPeriod=0.25s",
+            "solverTimeout=1.25s", "replanPeriod=0.25s", "maximumJerkLimitedStopDistance=",
+            "maximumJerkLimitedStopDuration=", "requiredDistanceHorizon=",
        })
        {
            Verification.True(text.Contains(expected), "configuration diagnostic includes " + expected);
@@ -596,6 +597,14 @@ 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");
+        foreach (string expected in new[]
+        {
+            "longitudinalMode=", "remainingToBoundary=", "minimumStoppingDistance=",
+            "minimumStoppingDuration=", "maximumStoppedReachableDistance=",
+        })
+        {
+            Verification.True(diagnostic.Text.Contains(expected), "failure diagnostic includes " + expected);
+        }
        Verification.True(!diagnostic.Text.Contains("trajectory summary:"),
            "failed diagnostic has no stale trajectory summary");

@@ -618,6 +627,7 @@ internal static class TrajectoryObservationChecks
        {
            "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",
+            "longitudinalMode=RollingContinuation", "terminalSpeed=0.400m/s", "terminalAcceleration=0.000m/s2",
        })
        {
            Verification.True(text.Contains(expected), "published diagnostic includes " + expected);