diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs new file mode 100644 index 0000000..d94f77c --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs @@ -0,0 +1,8 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum EmLongitudinalMode +{ + RollingContinuation, + ApproachStopBoundary, + ExactStopAtBoundary, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs index 410faf7..2cb113a 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs @@ -58,11 +58,11 @@ public sealed class EmPlanningService : IEmPlanningService initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason); if (horizonStatus != EmPlanningStatus.Success) return Failure(horizonStatus, request, horizonReason); - ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.TerminalReferenceS); + ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS); EmitDebug(request, "exact horizon and terminal selection succeeded"); IReadOnlyList previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment, - startProjection.ReferenceS, horizon.TerminalReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters); + startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters); var corridorSeed = new List(previousSeed.Count + 1) { startProjection }; for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]); EmitDebug(request, "previous-trajectory seed projection completed"); @@ -83,7 +83,8 @@ public sealed class EmPlanningService : IEmPlanningService EmitDebug(request, "LS optimization and validation succeeded"); var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed, - initialAcceleration, horizon.TerminalType, configuration, Array.Empty(), Array.Empty()); + initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration, + Array.Empty(), Array.Empty()); EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason); if (envelopeStatus != EmPlanningStatus.Success) return Failure(envelopeStatus, request, envelopeReason); diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs index 08ca926..e00179b 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs @@ -11,7 +11,8 @@ public sealed class LongitudinalPlanningInput private const double PathSTolerance = 1e-12d; public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond, - double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration, + double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode, + EmPlannerConfiguration configuration, IReadOnlyList previousPathS, IReadOnlyList previousProgressSpeedMetersPerSecond) { if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2) @@ -25,6 +26,12 @@ public sealed class LongitudinalPlanningInput throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared)); if (!Enum.IsDefined(typeof(EmTerminalType), terminalType)) throw new ArgumentOutOfRangeException(nameof(terminalType)); + if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode)) + throw new ArgumentOutOfRangeException(nameof(mode)); + if (mode == EmLongitudinalMode.RollingContinuation && terminalType != EmTerminalType.RollingSafetyStop) + throw new ArgumentException("Rolling continuation requires a rolling window boundary."); + if (mode != EmLongitudinalMode.RollingContinuation && terminalType == EmTerminalType.RollingSafetyStop) + throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch."); if (configuration == null) throw new ArgumentNullException(nameof(configuration)); @@ -33,6 +40,7 @@ public sealed class LongitudinalPlanningInput InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond; InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared; TerminalType = terminalType; + Mode = mode; Configuration = configuration.Copy(); PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS)); PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond, @@ -42,6 +50,17 @@ public sealed class LongitudinalPlanningInput nameof(previousProgressSpeedMetersPerSecond)); } + public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond, + double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration, + IReadOnlyList previousPathS, IReadOnlyList previousProgressSpeedMetersPerSecond) + : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, terminalType, + terminalType == EmTerminalType.RollingSafetyStop + ? EmLongitudinalMode.RollingContinuation + : EmLongitudinalMode.ExactStopAtBoundary, + configuration, previousPathS, previousProgressSpeedMetersPerSecond) + { + } + public LateralPath Path { get; } public TravelDirection Direction { get; } @@ -52,13 +71,33 @@ public sealed class LongitudinalPlanningInput public EmTerminalType TerminalType { get; } + public EmLongitudinalMode Mode { get; } + public EmPlannerConfiguration Configuration { get; } public IReadOnlyList PreviousPathS { get; } public IReadOnlyList PreviousProgressSpeedMetersPerSecond { get; } - public double TerminalPathS { get { return Path.Points[Path.Points.Count - 1].PathS; } } + public double PathUpperBoundS { get { return Path.Points[Path.Points.Count - 1].PathS; } } + + public bool HasStopBoundary { get { return TerminalType != EmTerminalType.RollingSafetyStop; } } + + public double StopBoundaryPathS { get { return HasStopBoundary ? PathUpperBoundS : double.NaN; } } + + public EmBoundaryType StopBoundaryType + { + get + { + return TerminalType == EmTerminalType.Goal + ? EmBoundaryType.Goal + : TerminalType == EmTerminalType.GearSwitch + ? EmBoundaryType.GearSwitchApproach + : EmBoundaryType.None; + } + } + + public double TerminalPathS { get { return PathUpperBoundS; } } public double DirectionMaximumSpeedMetersPerSecond { diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs new file mode 100644 index 0000000..98afda6 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs @@ -0,0 +1,39 @@ +using System; +using System.Collections.Generic; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public static class LongitudinalTerminalSchedule +{ + public static int GetStabilizationStartIndex(IReadOnlyList knotTimes, + double minimumStabilizationDurationSeconds) + { + if (knotTimes == null || knotTimes.Count < 3 || + double.IsNaN(minimumStabilizationDurationSeconds) || + double.IsInfinity(minimumStabilizationDurationSeconds) || + minimumStabilizationDurationSeconds <= 0d) + { + throw new ArgumentException("A positive stabilization tail and at least three knots are required."); + } + + double previous = double.NegativeInfinity; + for (int index = 0; index < knotTimes.Count; index++) + { + if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) || + knotTimes[index] <= previous) + { + throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing."); + } + previous = knotTimes[index]; + } + + double finalTime = knotTimes[knotTimes.Count - 1]; + for (int index = knotTimes.Count - 2; index >= 1; index--) + { + if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d) + return index; + } + + throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval."); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs index 55d419a..ea127cc 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs @@ -1,4 +1,5 @@ using System; +using System.Collections.Generic; using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; @@ -6,15 +7,31 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner; /// Reference-distance terminal chosen before LS without crossing the current direction segment. public sealed class PlanningHorizonSelection { - internal PlanningHorizonSelection(double terminalReferenceS, EmTerminalType terminalType) + internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType, + EmTerminalType terminalType, EmLongitudinalMode longitudinalMode, + double stopBoundaryReferenceS, bool hasStopBoundary) { - TerminalReferenceS = terminalReferenceS; + WindowEndReferenceS = windowEndReferenceS; + WindowEndBoundaryType = windowEndBoundaryType; TerminalType = terminalType; + LongitudinalMode = longitudinalMode; + StopBoundaryReferenceS = stopBoundaryReferenceS; + HasStopBoundary = hasStopBoundary; } - public double TerminalReferenceS { get; } + public double WindowEndReferenceS { get; } + + public double TerminalReferenceS => WindowEndReferenceS; + + public EmBoundaryType WindowEndBoundaryType { get; } public EmTerminalType TerminalType { get; } + + public EmLongitudinalMode LongitudinalMode { get; } + + public double StopBoundaryReferenceS { get; } + + public bool HasStopBoundary { get; } } public sealed class PlanningHorizonSelector @@ -73,75 +90,41 @@ public sealed class PlanningHorizonSelector return EmPlanningStatus.StoppingDistanceInsufficient; } - double timeReachable = CalculateReachableDistance(initialProgressSpeedMetersPerSecond, - initialAccelerationMetersPerSecondSquared, directionMaximum, longitudinal, scheduling.TimeHorizonSeconds); - double terminalReferenceS = currentSegmentReferenceS + Math.Min(remainingSegment, - Math.Min(scheduling.DistanceHorizonMeters, timeReachable)); - if (terminalReferenceS >= segment.LengthMeters - BoundaryTolerance) + double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters); + bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance; + bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType); + EmBoundaryType windowEndBoundaryType = windowReachesSegmentEnd + ? segment.EndBoundary.BoundaryType + : EmBoundaryType.RollingSafetyStop; + if (!hasStopBoundary) { - terminalReferenceS = segment.LengthMeters; - selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType)); - } - else - { - selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop); + selection = new PlanningHorizonSelection(windowEnd, windowEndBoundaryType, + EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, + segment.LengthMeters, false); + return EmPlanningStatus.Success; } + + IReadOnlyList knotTimes = LongitudinalCandidate.CreateKnotTimes( + scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds); + int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex( + knotTimes, scheduling.OutputTimeStepSeconds); + double availableMotionTime = knotTimes[stabilizationStart]; + double maximumStoppedDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance( + initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, directionMaximum, + longitudinal.MaximumAccelerationMetersPerSecondSquared, + longitudinal.MaximumDecelerationMetersPerSecondSquared, + longitudinal.MaximumJerkMetersPerSecondCubed, availableMotionTime); + EmLongitudinalMode mode = remainingSegment <= maximumStoppedDistance + BoundaryTolerance + ? EmLongitudinalMode.ExactStopAtBoundary + : EmLongitudinalMode.ApproachStopBoundary; + selection = new PlanningHorizonSelection(segment.LengthMeters, windowEndBoundaryType, + ToTerminalType(segment.EndBoundary.BoundaryType), mode, segment.LengthMeters, true); return EmPlanningStatus.Success; } - private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed, - LongitudinalConfiguration configuration, double timeHorizonSeconds) + private static bool IsStopBoundary(EmBoundaryType boundaryType) { - if (!JerkLimitedStoppingMath.TryCalculate(maximumSpeed, 0d, - configuration.MaximumDecelerationMetersPerSecondSquared, - configuration.MaximumJerkMetersPerSecondCubed, - out JerkLimitedStoppingProfile stopAtMaximumSpeed, out _)) - { - throw new ArgumentOutOfRangeException(nameof(configuration)); - } - double drivingDuration = timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds - stopAtMaximumSpeed.DurationSeconds; - if (drivingDuration <= 0d) - return Math.Min(initialSpeed, maximumSpeed) * Math.Max(0d, timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds); - - double speed = initialSpeed; - double acceleration = initialAcceleration; - double distance = 0d; - const double simulationStepSeconds = 0.001d; - while (drivingDuration > 0d) - { - double step = Math.Min(simulationStepSeconds, drivingDuration); - double jerk = ChooseAccelerationJerk(speed, acceleration, maximumSpeed, - configuration.MaximumAccelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed); - double nextSpeed = speed + acceleration * step + 0.5d * jerk * step * step; - if (nextSpeed > maximumSpeed) - { - nextSpeed = maximumSpeed; - acceleration = 0d; - jerk = 0d; - } - distance += speed * step + 0.5d * acceleration * step * step + jerk * step * step * step / 6d; - acceleration += jerk * step; - speed = Math.Max(0d, nextSpeed); - drivingDuration -= step; - } - return Math.Max(0d, distance + stopAtMaximumSpeed.DistanceMeters); - } - - private static double ChooseAccelerationJerk(double speed, double acceleration, double maximumSpeed, - double maximumAcceleration, double maximumJerk) - { - if (speed >= maximumSpeed - BoundaryTolerance) - { - if (acceleration > 0d) - return -maximumJerk; - return acceleration < 0d ? maximumJerk : 0d; - } - double speedToReduceAccelerationToZero = acceleration > 0d - ? acceleration * acceleration / (2d * maximumJerk) - : 0d; - if (speed + speedToReduceAccelerationToZero >= maximumSpeed - BoundaryTolerance) - return acceleration > 0d ? -maximumJerk : 0d; - return acceleration < maximumAcceleration - BoundaryTolerance ? maximumJerk : 0d; + return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach; } private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs index 29daec9..f03f68b 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs @@ -1,4 +1,5 @@ using System; +using System.Collections.Generic; using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle; using MultiWheelC.TrajectoryPlanning.PathSmoothing; using MultiWheelC.TrajectoryPlanning.Utils; @@ -142,6 +143,45 @@ public static class EmPlanningRequestValidator !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights)) return false; + 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 _)) + { + failureReason = "Configuration cannot construct the jerk-limited stopping model."; + return false; + } + + double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond, + longitudinal.MaximumReverseSpeedMetersPerSecond); + double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) + + maximumSpeed * scheduling.ReplanPeriodSeconds; + if (scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon) + { + failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon + + ";configured=" + scheduling.DistanceHorizonMeters; + return false; + } + + try + { + IReadOnlyList knotTimes = LongitudinalCandidate.CreateKnotTimes( + scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds); + LongitudinalTerminalSchedule.GetStabilizationStartIndex(knotTimes, + scheduling.OutputTimeStepSeconds); + } + catch (ArgumentException exception) + { + failureReason = exception.Message; + return false; + } + return true; } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs index dc7a352..049dfe6 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs @@ -106,6 +106,17 @@ internal static class FoundationChecks VerifyLateralWeights(configuration.Lateral.Weights); VerifyLongitudinalWeights(configuration.Longitudinal.Weights); + EmPlannerConfiguration insufficientPreview = EmPlannerConfiguration.CreateDefault(); + insufficientPreview.Scheduling.DistanceHorizonMeters = 0.01d; + EmPlanningRequestValidationResult insufficientPreviewValidation = EmPlanningRequestValidator.Validate( + CreateValidRequest(insufficientPreview)); + AssertStatus(EmPlanningStatus.InvalidInput, insufficientPreviewValidation, "insufficient stopping preview"); + EMPlannerVerificationHost.Verification.True( + insufficientPreviewValidation.FailureReason.Contains("DistanceHorizonMeters") && + insufficientPreviewValidation.FailureReason.Contains("required") && + insufficientPreviewValidation.FailureReason.Contains("configured"), + "insufficient stopping preview describes configured and required distance"); + EmPlanningRequest valid = CreateValidRequest(configuration); AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate( CreateRequest(null!, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null reference path"); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs index 05a540f..c9203c7 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs @@ -170,7 +170,7 @@ internal static class LongitudinalIntegrationChecks }, true); var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward, baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared, - baseline.TerminalType, baseline.Configuration, Array.Empty(), Array.Empty()); + baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty(), Array.Empty()); EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, out string speedFailure); Verification.Equal(EmPlanningStatus.Success, speedStatus, "curved-envelope setup: " + speedFailure); @@ -212,7 +212,7 @@ internal static class LongitudinalIntegrationChecks }, true); var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward, baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared, - baseline.TerminalType, baseline.Configuration, Array.Empty(), Array.Empty()); + baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty(), Array.Empty()); var solver = new FakeQpSolver(new[] { Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d), @@ -335,7 +335,8 @@ internal static class LongitudinalIntegrationChecks Point(2d, terminalPathS, 0d), }; return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction, - initialSpeed, initialAcceleration, EmTerminalType.Goal, configuration, Array.Empty(), Array.Empty())); + initialSpeed, initialAcceleration, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, + configuration, Array.Empty(), Array.Empty())); } private static void VerifyRealScenario(LongitudinalScenario scenario, LongitudinalPlanningResult result) @@ -392,7 +393,7 @@ internal static class LongitudinalIntegrationChecks Point(2d, terminalPathS, 0d), }, true); return new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d, EmTerminalType.Goal, - configuration, Array.Empty(), Array.Empty()); + EmLongitudinalMode.ExactStopAtBoundary, configuration, Array.Empty(), Array.Empty()); } private static LateralPathPoint Point(double referenceS, double pathS, double curvature) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index 6557fd3..71a53ef 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -16,7 +16,7 @@ internal static class LongitudinalModelChecks VerifiesStoppingEnvelopeIsRefinedOnActualPathS(); VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations(); VerifiesStoppingPrecheckBeforeQpAssembly(); - VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary(); + VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime(); VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints(); } @@ -64,7 +64,8 @@ internal static class LongitudinalModelChecks new PathFixture(1d, 1d, 0d, 0d), }); var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d, - EmTerminalType.Goal, EmPlannerConfiguration.CreateDefault(), Array.Empty(), Array.Empty()); + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, EmPlannerConfiguration.CreateDefault(), + Array.Empty(), Array.Empty()); EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput, out PathSpeedLimit directionEnvelope, out string directionFailureReason); Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " + @@ -83,7 +84,8 @@ internal static class LongitudinalModelChecks new PathFixture(21d, 5d, 0d, 0d), }); var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d, - EmTerminalType.Goal, configuration, Array.Empty(), Array.Empty()); + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + Array.Empty(), Array.Empty()); EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, out string failureReason); @@ -124,7 +126,8 @@ internal static class LongitudinalModelChecks new PathFixture(100d, 2d, 0d, 0d), }); var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d, - EmTerminalType.Goal, configuration, Array.Empty(), Array.Empty()); + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + Array.Empty(), Array.Empty()); EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, out string failureReason); @@ -145,7 +148,8 @@ internal static class LongitudinalModelChecks new PathFixture(100d, 2d, 0d, 0d), }); var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d, - EmTerminalType.Goal, configuration, Array.Empty(), Array.Empty()); + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + Array.Empty(), Array.Empty()); EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, out string failureReason); @@ -173,7 +177,8 @@ internal static class LongitudinalModelChecks new PathFixture(100d, 0.01d, 0d, 0d), }); var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d, - EmTerminalType.RollingSafetyStop, configuration, Array.Empty(), Array.Empty()); + EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration, + Array.Empty(), Array.Empty()); EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, out string failureReason); @@ -183,24 +188,46 @@ internal static class LongitudinalModelChecks Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection"); } - private static void VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary() + private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime() { EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); - DirectionSegmentView shortGoal = CreateSegment(0.25d, EmBoundaryType.Goal); - EmPlanningStatus status = new PlanningHorizonSelector().Select(shortGoal, 0d, 0d, 0d, configuration, - out PlanningHorizonSelection goalSelection, out string goalFailure); + configuration.Scheduling.DistanceHorizonMeters = 1d; + configuration.Scheduling.TimeHorizonSeconds = 2d; - Verification.Equal(EmPlanningStatus.Success, status, "goal horizon status: " + goalFailure); - Verification.Equal(EmTerminalType.Goal, goalSelection.TerminalType, "goal terminal type"); - Verification.NearlyEqual(0.25d, goalSelection.TerminalReferenceS, "goal terminal reference S"); + DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal); + EmPlanningStatus status = new PlanningHorizonSelector().Select( + longSegment, 0d, 0d, 0d, configuration, + out PlanningHorizonSelection rolling, out string failure); + Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure); + Verification.NearlyEqual(1d, rolling.WindowEndReferenceS, + "distance horizon defines the LS window"); + Verification.Equal(EmLongitudinalMode.RollingContinuation, + rolling.LongitudinalMode, "far boundary rolls"); - DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.GearSwitchApproach); - status = new PlanningHorizonSelector().Select(longSegment, 0d, 0d, 0d, configuration, - out PlanningHorizonSelection rollingSelection, out string rollingFailure); - Verification.Equal(EmPlanningStatus.Success, status, "rolling horizon status: " + rollingFailure); - Verification.Equal(EmTerminalType.RollingSafetyStop, rollingSelection.TerminalType, "rolling terminal type"); - Verification.True(rollingSelection.TerminalReferenceS >= 0d && rollingSelection.TerminalReferenceS < 4d, - "rolling horizon remains within the current segment"); + DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal); + status = new PlanningHorizonSelector().Select( + visibleButFar, 0d, 0d, 0d, configuration, + out PlanningHorizonSelection approach, out failure); + Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure); + Verification.Equal(EmLongitudinalMode.ApproachStopBoundary, + approach.LongitudinalMode, "visible unreachable boundary approaches"); + + DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal); + status = new PlanningHorizonSelector().Select( + reachableGoal, 0d, 0d, 0d, configuration, + out PlanningHorizonSelection exact, out failure); + Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure); + Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary, + exact.LongitudinalMode, "reachable goal stops exactly"); + Verification.Equal(EmTerminalType.Goal, exact.TerminalType, + "exact stop preserves Goal identity"); + + IReadOnlyList regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d); + Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex( + regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0"); + Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex( + new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d), + "short final interval moves the stop anchor earlier"); } private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints() @@ -240,7 +267,8 @@ internal static class LongitudinalModelChecks new PathFixture(2d, 2d, 0d, 0d), }); var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d, - EmTerminalType.Goal, configuration, new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d }, + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d }, new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d }); EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, out string speedFailure);