diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md index ce6f62e..1691cc1 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md @@ -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） diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md index 6512904..0bda336 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md @@ -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 | diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs index 1c50d91..649dce2 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs @@ -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(); + 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 missing, string failureReason, string field) + { + if (failureReason.IndexOf(field, StringComparison.Ordinal) < 0) + missing.Add(field + "unavailable"); } private static bool IsPositiveFinite(double value) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 6577a2d..a47fde0 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -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);