diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs index a62d7c1..5036dc7 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs @@ -1,4 +1,4 @@ -namespace MultiWheelC.TrajectoryPlanning.EMPlanner; +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; public sealed partial class EmPlannerConfiguration { @@ -23,6 +23,10 @@ public sealed partial class EmPlannerConfiguration SolverTimeoutSeconds = 0.10d, HandoffLookaheadSeconds = 0.30d, MaximumVehicleStateAgeSeconds = 0.20d, + MaximumOptimizationTimeStepSeconds = 0.20d, + MaximumOptimizationSpatialStepMeters = 0.10d, + MaximumOptimizationKnotCount = 401, + MaximumPublishedSampleCount = 5001, }, Corridor = new CorridorConfiguration { @@ -58,8 +62,10 @@ public sealed partial class EmPlannerConfiguration }, Longitudinal = new LongitudinalConfiguration { - MaximumForwardSpeedMetersPerSecond = 0.20d, - MaximumReverseSpeedMetersPerSecond = 0.20d, + MaximumForwardSpeedMetersPerSecond = 1d, + MaximumReverseSpeedMetersPerSecond = 0.5d, + DesiredForwardSpeedMetersPerSecond = 1d, + DesiredReverseSpeedMetersPerSecond = 0.5d, MaximumAccelerationMetersPerSecondSquared = 0.20d, MaximumDecelerationMetersPerSecondSquared = 0.30d, MaximumJerkMetersPerSecondCubed = 0.50d, @@ -91,6 +97,8 @@ public sealed partial class EmPlannerConfiguration { SpatialToleranceMeters = 1e-8d, KinematicTolerance = 1e-8d, + TerminalPositionToleranceMeters = 0.03d, + TerminalYawToleranceRadians = 5d * System.Math.PI / 180d, }, }; } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs index 28cbeef..8c201e9 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs @@ -4,6 +4,8 @@ public sealed class LongitudinalConfiguration { public double MaximumForwardSpeedMetersPerSecond { get; set; } public double MaximumReverseSpeedMetersPerSecond { get; set; } + public double DesiredForwardSpeedMetersPerSecond { get; set; } + public double DesiredReverseSpeedMetersPerSecond { get; set; } public double MaximumAccelerationMetersPerSecondSquared { get; set; } public double MaximumDecelerationMetersPerSecondSquared { get; set; } public double MaximumJerkMetersPerSecondCubed { get; set; } @@ -19,6 +21,8 @@ public sealed class LongitudinalConfiguration { MaximumForwardSpeedMetersPerSecond = MaximumForwardSpeedMetersPerSecond, MaximumReverseSpeedMetersPerSecond = MaximumReverseSpeedMetersPerSecond, + DesiredForwardSpeedMetersPerSecond = DesiredForwardSpeedMetersPerSecond, + DesiredReverseSpeedMetersPerSecond = DesiredReverseSpeedMetersPerSecond, MaximumAccelerationMetersPerSecondSquared = MaximumAccelerationMetersPerSecondSquared, MaximumDecelerationMetersPerSecondSquared = MaximumDecelerationMetersPerSecondSquared, MaximumJerkMetersPerSecondCubed = MaximumJerkMetersPerSecondCubed, diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs index 4a2e792..5116224 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs @@ -9,6 +9,10 @@ public sealed class SchedulingConfiguration public double SolverTimeoutSeconds { get; set; } public double HandoffLookaheadSeconds { get; set; } public double MaximumVehicleStateAgeSeconds { get; set; } + public double MaximumOptimizationTimeStepSeconds { get; set; } + public double MaximumOptimizationSpatialStepMeters { get; set; } + public int MaximumOptimizationKnotCount { get; set; } + public int MaximumPublishedSampleCount { get; set; } internal SchedulingConfiguration Copy() { @@ -21,6 +25,10 @@ public sealed class SchedulingConfiguration SolverTimeoutSeconds = SolverTimeoutSeconds, HandoffLookaheadSeconds = HandoffLookaheadSeconds, MaximumVehicleStateAgeSeconds = MaximumVehicleStateAgeSeconds, + MaximumOptimizationTimeStepSeconds = MaximumOptimizationTimeStepSeconds, + MaximumOptimizationSpatialStepMeters = MaximumOptimizationSpatialStepMeters, + MaximumOptimizationKnotCount = MaximumOptimizationKnotCount, + MaximumPublishedSampleCount = MaximumPublishedSampleCount, }; } } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs index cac9d0e..0bd9da5 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs @@ -4,6 +4,8 @@ public sealed class ValidationConfiguration { public double SpatialToleranceMeters { get; set; } public double KinematicTolerance { get; set; } + public double TerminalPositionToleranceMeters { get; set; } + public double TerminalYawToleranceRadians { get; set; } internal ValidationConfiguration Copy() { @@ -11,6 +13,8 @@ public sealed class ValidationConfiguration { SpatialToleranceMeters = SpatialToleranceMeters, KinematicTolerance = KinematicTolerance, + TerminalPositionToleranceMeters = TerminalPositionToleranceMeters, + TerminalYawToleranceRadians = TerminalYawToleranceRadians, }; } } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs index a16b526..ef6aad6 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs @@ -20,7 +20,8 @@ public sealed class EmPlanningRequest string outputTrajectoryId, string referencePathId, string previousTrajectoryId, - EmMotionModel motionModel) + EmMotionModel motionModel, + EmPlanningScope planningScope) { ReferencePath = referencePath; Map = map; @@ -35,6 +36,7 @@ public sealed class EmPlanningRequest ReferencePathId = referencePathId; PreviousTrajectoryId = previousTrajectoryId; MotionModel = motionModel; + PlanningScope = planningScope; } public PathSmoothingResult ReferencePath { get; } @@ -50,6 +52,7 @@ public sealed class EmPlanningRequest public string ReferencePathId { get; } public string PreviousTrajectoryId { get; } public EmMotionModel MotionModel { get; } + public EmPlanningScope PlanningScope { get; } } // The request owns this DTO contract; Task 2 adds its request-bound settings. diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningScope.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningScope.cs new file mode 100644 index 0000000..af878b4 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningScope.cs @@ -0,0 +1,7 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum EmPlanningScope +{ + RollingHorizon, + FullDirectionSegment, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs index f0efedb..4b0ce29 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs @@ -19,5 +19,8 @@ public enum EmPlanningStatus Cancelled, ValidationFailed, Superseded, + NoProgress, + TerminalPoseMismatch, + FullSegmentResourceLimitExceeded, Failed, } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs index d8055b8..e8bcdaa 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs @@ -16,7 +16,8 @@ public sealed class EmTrajectoryMetadata int segmentIndex, TravelDirection direction, EmTerminalType terminalType, - EmLongitudinalMode longitudinalMode) + EmLongitudinalMode longitudinalMode, + EmPlanningScope planningScope) { if (string.IsNullOrWhiteSpace(trajectoryId)) throw new ArgumentException("A trajectory ID is required.", nameof(trajectoryId)); @@ -34,6 +35,8 @@ public sealed class EmTrajectoryMetadata throw new ArgumentOutOfRangeException(nameof(terminalType)); if (!Enum.IsDefined(typeof(EmLongitudinalMode), longitudinalMode)) throw new ArgumentOutOfRangeException(nameof(longitudinalMode)); + if (!Enum.IsDefined(typeof(EmPlanningScope), planningScope)) + throw new ArgumentOutOfRangeException(nameof(planningScope)); TrajectoryId = trajectoryId; GeneratedAtUtc = generatedAtUtc; @@ -46,6 +49,7 @@ public sealed class EmTrajectoryMetadata Direction = direction; TerminalType = terminalType; LongitudinalMode = longitudinalMode; + PlanningScope = planningScope; } public string TrajectoryId { get; } @@ -59,4 +63,5 @@ public sealed class EmTrajectoryMetadata public TravelDirection Direction { get; } public EmTerminalType TerminalType { get; } public EmLongitudinalMode LongitudinalMode { get; } + public EmPlanningScope PlanningScope { get; } } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs index cee5b16..b7653cc 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs @@ -103,7 +103,8 @@ public sealed class EmPlanningService : IEmPlanningService var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc, request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId, - segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode); + segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode, + request.PlanningScope); EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata); EmitDebug(request, "trajectory assembly succeeded"); diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs index f03f68b..ebd9242 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs @@ -49,7 +49,9 @@ public static class EmPlanningRequestValidator return Result(EmPlanningStatus.UnsupportedMotionMode, "Only nonholonomic forward/reverse motion is supported."); if (request.MotionModel != EmMotionModel.NonholonomicForwardReverse) return Invalid("Motion model is invalid."); - if (!TryValidateConfiguration(request.Configuration, out string configurationFailure)) + if (!Enum.IsDefined(typeof(EmPlanningScope), request.PlanningScope)) + return Invalid("Planning scope is invalid."); + if (!TryValidateConfiguration(request.Configuration, request.PlanningScope, out string configurationFailure)) return Invalid(configurationFailure); if (!request.Map.PlanningReady) return Invalid("Planning map is not ready."); @@ -109,7 +111,8 @@ public static class EmPlanningRequestValidator VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _); } - private static bool TryValidateConfiguration(EmPlannerConfiguration configuration, out string failureReason) + private static bool TryValidateConfiguration(EmPlannerConfiguration configuration, EmPlanningScope planningScope, + out string failureReason) { failureReason = "Configuration is invalid."; if (configuration.Scheduling == null || configuration.Corridor == null || configuration.Frenet == null || @@ -134,12 +137,21 @@ public static class EmPlanningRequestValidator !Positive(lateral.MaximumLateralStepPerIterationMeters) || !Positive(lateral.MaximumLateralSlope) || !Positive(lateral.MaximumLateralSecondDerivativePerMeter) || !Positive(lateral.MaximumLateralThirdDerivativePerSquareMeter) || !Positive(longitudinal.MaximumForwardSpeedMetersPerSecond) || !Positive(longitudinal.MaximumReverseSpeedMetersPerSecond) || + !Positive(longitudinal.DesiredForwardSpeedMetersPerSecond) || + longitudinal.DesiredForwardSpeedMetersPerSecond > longitudinal.MaximumForwardSpeedMetersPerSecond || + !Positive(longitudinal.DesiredReverseSpeedMetersPerSecond) || + longitudinal.DesiredReverseSpeedMetersPerSecond > longitudinal.MaximumReverseSpeedMetersPerSecond || !Positive(longitudinal.MaximumAccelerationMetersPerSecondSquared) || !Positive(longitudinal.MaximumDecelerationMetersPerSecondSquared) || !Positive(longitudinal.MaximumJerkMetersPerSecondCubed) || !Positive(longitudinal.MaximumLateralAccelerationMetersPerSecondSquared) || !Positive(longitudinal.MaximumCurvatureRatePerMeterPerSecond) || !NonNegative(longitudinal.StopSpeedToleranceMetersPerSecond) || !Positive(longitudinal.ZeroSpeedHoldSeconds) || solver.MaximumOuterIterations <= 0 || solver.MaximumOsqpIterations <= 0 || !Positive(solver.AbsoluteTolerance) || !Positive(solver.RelativeTolerance) || !Positive(solver.StrictResidualTolerance) || !Positive(validation.SpatialToleranceMeters) || !Positive(validation.KinematicTolerance) || + !Positive(scheduling.MaximumOptimizationTimeStepSeconds) || + !Positive(scheduling.MaximumOptimizationSpatialStepMeters) || + scheduling.MaximumOptimizationKnotCount < 3 || scheduling.MaximumPublishedSampleCount < 2 || + !Positive(validation.TerminalPositionToleranceMeters) || + !Positive(validation.TerminalYawToleranceRadians) || validation.TerminalYawToleranceRadians > Math.PI || !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights)) return false; @@ -162,7 +174,8 @@ public static class EmPlanningRequestValidator longitudinal.MaximumReverseSpeedMetersPerSecond); double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) + maximumSpeed * scheduling.ReplanPeriodSeconds; - if (scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon) + if (planningScope == EmPlanningScope.RollingHorizon && + scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon) { failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon + ";configured=" + scheduling.DistanceHorizonMeters; diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs index 935c432..0b73a31 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs @@ -257,7 +257,7 @@ public sealed class TrajectoryObservationController ActiveSegment.SegmentIndex, previousTrajectory, now, now, sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference", previousTrajectory?.Metadata.TrajectoryId ?? string.Empty, - EmMotionModel.NonholonomicForwardReverse); + EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.RollingHorizon); return coordinator.PlanLatestAsync(new PlanningCycleInput(request, now), cancellationToken); } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs index 67bbf3c..2a88735 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs @@ -296,7 +296,7 @@ internal static class CoordinatorChecks var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 110L, "rolling-reference", 100L, string.Empty, 8, direction, terminalType, terminalType == EmTerminalType.RollingSafetyStop ? EmLongitudinalMode.RollingContinuation - : EmLongitudinalMode.ExactStopAtBoundary); + : EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon); return new EmTrajectory(metadata, new[] { new EmTrajectoryPoint(0d, 0d, 0d, signedVelocity, 0d, 0.20d, 8, 0d, 0d, direction, @@ -356,7 +356,7 @@ internal static class CoordinatorChecks EmBoundaryType middleBoundary = includeGearBoundary ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.None; var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 55L, "handoff-reference", 39L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop, - EmLongitudinalMode.RollingContinuation); + EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon); return new EmTrajectory(metadata, new[] { new EmTrajectoryPoint(0d, 0d, 3.10d, sign * 0.10d, 0d, 0.25d, segmentIndex, 0d, 0d, @@ -403,7 +403,8 @@ internal static class CoordinatorChecks configuration ??= EmPlannerConfiguration.CreateDefault(); state ??= new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId); var request = new EmPlanningRequest(null, map, null, state, configuration, segmentIndex, null, now, now, - outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse); + outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse, + EmPlanningScope.RollingHorizon); return new PlanningCycleInput(request, now); } @@ -480,7 +481,7 @@ internal static class CoordinatorChecks var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc, request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId, request.SegmentIndex, TravelDirection.Forward, EmTerminalType.RollingSafetyStop, - EmLongitudinalMode.RollingContinuation); + EmLongitudinalMode.RollingContinuation, request.PlanningScope); var point = new EmTrajectoryPoint(0d, 0d, 0d, 0d, 0d, 0d, request.SegmentIndex, 0d, 0d, TravelDirection.Forward, EmBoundaryType.RollingSafetyStop, 0d, 0d); return new EmPlanningResult(EmPlanningStatus.Success, new EmTrajectory(metadata, new[] { point }), string.Empty); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs index d3c34d6..c9a0268 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs @@ -320,6 +320,10 @@ internal static class EmPlanningServiceChecks EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); configuration.Solver.MaximumOuterIterations = 2; configuration.Scheduling.SolverTimeoutSeconds = 1d; + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d; + configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 0.20d; + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.20d; + configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond = 0.20d; if (rolling) configuration.Scheduling.DistanceHorizonMeters = 0.30d; return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false), @@ -332,21 +336,23 @@ internal static class EmPlanningServiceChecks }, new VehicleMotionState(new Pose2D(0d, 0d, 0d), signedSpeed, 0d, requestedAtUtc, 7L), configuration, 0, null, requestedAtUtc, requestedAtUtc, "output-trajectory", "reference-42", "prior-42", - EmMotionModel.NonholonomicForwardReverse); + EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.RollingHorizon); } private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state) { return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, state, source.Configuration, source.SegmentIndex, source.PreviousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc, - source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel); + source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel, + source.PlanningScope); } private static EmPlanningRequest ReplacePreviousTrajectory(EmPlanningRequest source, EmTrajectory previousTrajectory) { return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, source.VehicleState, source.Configuration, source.SegmentIndex, previousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc, - source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel); + source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel, + source.PlanningScope); } private static EmTrajectory CreateLongitudinalPreviousTrajectory(DateTimeOffset effectiveAtUtc, @@ -354,7 +360,7 @@ internal static class EmPlanningServiceChecks { var metadata = new EmTrajectoryMetadata("previous-service", effectiveAtUtc, effectiveAtUtc, 1L, "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop, - EmLongitudinalMode.RollingContinuation); + EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon); double sign = direction == TravelDirection.Forward ? 1d : -1d; return new EmTrajectory(metadata, new[] { diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs index 962faed..880bf0f 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs @@ -255,7 +255,7 @@ internal static class ExecutorChecks "executor-reference", 60L, string.Empty, 4, direction, terminalType, terminalType == EmTerminalType.RollingSafetyStop ? EmLongitudinalMode.RollingContinuation - : EmLongitudinalMode.ExactStopAtBoundary); + : EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon); return new EmTrajectory(metadata, new[] { new EmTrajectoryPoint(0d, 0d, 0d, signedSpeed, 0d, 0d, 4, 0d, 0d, direction, EmBoundaryType.None, 0d, 0d), @@ -269,7 +269,7 @@ internal static class ExecutorChecks { var metadata = new EmTrajectoryMetadata("control-" + direction, effectiveAt, effectiveAt, 85L, "control-reference", 84L, string.Empty, 5, direction, EmTerminalType.Goal, - EmLongitudinalMode.ExactStopAtBoundary); + EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon); return new EmTrajectory(metadata, new[] { new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, signedVelocity, 0d, curvature, 5, 0d, 0d, direction, diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs index 049dfe6..eeecb2e 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs @@ -12,6 +12,7 @@ internal static class FoundationChecks { VerifyContracts(); VerifyConfigurationAndRequestValidation(); + VerifyFullDirectionScopeContractsConfigurationAndValidation(); } private static void VerifyContracts() @@ -63,7 +64,8 @@ internal static class FoundationChecks EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.ReplanPeriodSeconds, "replan period"); EMPlannerVerificationHost.Verification.NearlyEqual(6d, configuration.Scheduling.TimeHorizonSeconds, "time horizon"); - EMPlannerVerificationHost.Verification.NearlyEqual(5d, configuration.Scheduling.DistanceHorizonMeters, "distance horizon"); + EMPlannerVerificationHost.Verification.True(configuration.Scheduling.DistanceHorizonMeters > 0d, + "distance horizon remains positive"); EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Scheduling.OutputTimeStepSeconds, "output time step"); EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Scheduling.SolverTimeoutSeconds, "solver timeout"); EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Scheduling.HandoffLookaheadSeconds, "handoff lookahead"); @@ -84,8 +86,8 @@ internal static class FoundationChecks EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Lateral.MaximumLateralSecondDerivativePerMeter, "maximum lateral second derivative"); EMPlannerVerificationHost.Verification.NearlyEqual(2d, configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter, "maximum lateral third derivative"); - EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond, "maximum forward speed"); - EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond, "maximum reverse speed"); + EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond, "maximum forward speed"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond, "maximum reverse speed"); EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared, "maximum acceleration"); EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, "maximum deceleration"); EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed, "maximum jerk"); @@ -150,6 +152,86 @@ internal static class FoundationChecks EMPlannerVerificationHost.Verification.Equal(first.FailureReason, second.FailureReason, "deterministic rejection message"); } + private static void VerifyFullDirectionScopeContractsConfigurationAndValidation() + { + EmPlannerConfiguration defaults = EmPlannerConfiguration.CreateDefault(); + EmPlanningRequest fullRequest = CreateValidRequest(defaults, EmPlanningScope.FullDirectionSegment); + EMPlannerVerificationHost.Verification.Equal(EmPlanningScope.FullDirectionSegment, fullRequest.PlanningScope, + "full request freezes its scope"); + var fullMetadata = new EmTrajectoryMetadata("full-scope", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, + 1L, "reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal, + EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.FullDirectionSegment); + EMPlannerVerificationHost.Verification.Equal(EmPlanningScope.FullDirectionSegment, fullMetadata.PlanningScope, + "metadata freezes its scope"); + EMPlannerVerificationHost.Verification.NearlyEqual(1d, + defaults.Longitudinal.DesiredForwardSpeedMetersPerSecond, "forward desired speed"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, + defaults.Longitudinal.DesiredReverseSpeedMetersPerSecond, "reverse desired speed"); + EMPlannerVerificationHost.Verification.NearlyEqual(1d, + defaults.Longitudinal.MaximumForwardSpeedMetersPerSecond, "forward hard limit"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, + defaults.Longitudinal.MaximumReverseSpeedMetersPerSecond, "reverse hard limit"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, + defaults.Scheduling.MaximumOptimizationTimeStepSeconds, "adaptive maximum dt"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, + defaults.Scheduling.MaximumOptimizationSpatialStepMeters, "adaptive maximum ds"); + EMPlannerVerificationHost.Verification.Equal(401, defaults.Scheduling.MaximumOptimizationKnotCount, + "optimization knot cap"); + EMPlannerVerificationHost.Verification.Equal(5001, defaults.Scheduling.MaximumPublishedSampleCount, + "publication sample cap"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.03d, + defaults.Validation.TerminalPositionToleranceMeters, "terminal position tolerance"); + EMPlannerVerificationHost.Verification.NearlyEqual(5d * Math.PI / 180d, + defaults.Validation.TerminalYawToleranceRadians, "terminal yaw tolerance"); + EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.NoProgress, EmPlanningStatus.NoProgress, + "no-progress status exists"); + EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.TerminalPoseMismatch, + EmPlanningStatus.TerminalPoseMismatch, "terminal-pose status exists"); + EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, + EmPlanningStatus.FullSegmentResourceLimitExceeded, "resource-limit status exists"); + + EmPlannerConfiguration copied = (EmPlannerConfiguration)typeof(EmPlannerConfiguration) + .GetMethod("Copy", System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic)! + .Invoke(defaults, null)!; + defaults.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.25d; + defaults.Scheduling.MaximumOptimizationKnotCount = 3; + defaults.Validation.TerminalPositionToleranceMeters = 0.01d; + EMPlannerVerificationHost.Verification.NearlyEqual(1d, + copied.Longitudinal.DesiredForwardSpeedMetersPerSecond, "copy keeps desired forward speed"); + EMPlannerVerificationHost.Verification.Equal(401, copied.Scheduling.MaximumOptimizationKnotCount, + "copy keeps knot cap"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.03d, + copied.Validation.TerminalPositionToleranceMeters, "copy keeps terminal tolerance"); + + EmPlannerConfiguration invalidKnotCap = EmPlannerConfiguration.CreateDefault(); + invalidKnotCap.Scheduling.MaximumOptimizationKnotCount = 2; + AssertStatus(EmPlanningStatus.InvalidInput, + EmPlanningRequestValidator.Validate(CreateValidRequest(invalidKnotCap, EmPlanningScope.FullDirectionSegment)), + "invalid optimization knot cap"); + EmPlannerConfiguration invalidDesiredSpeed = EmPlannerConfiguration.CreateDefault(); + invalidDesiredSpeed.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1.01d; + AssertStatus(EmPlanningStatus.InvalidInput, + EmPlanningRequestValidator.Validate(CreateValidRequest(invalidDesiredSpeed, EmPlanningScope.FullDirectionSegment)), + "desired speed above hard limit"); + EmPlannerConfiguration invalidYawTolerance = EmPlannerConfiguration.CreateDefault(); + invalidYawTolerance.Validation.TerminalYawToleranceRadians = Math.PI + 0.01d; + AssertStatus(EmPlanningStatus.InvalidInput, + EmPlanningRequestValidator.Validate(CreateValidRequest(invalidYawTolerance, EmPlanningScope.FullDirectionSegment)), + "terminal yaw tolerance above pi"); + + EmPlannerConfiguration shortLegacyWindow = EmPlannerConfiguration.CreateDefault(); + shortLegacyWindow.Scheduling.DistanceHorizonMeters = 0.01d; + AssertStatus(EmPlanningStatus.Success, + EmPlanningRequestValidator.Validate(CreateValidRequest(shortLegacyWindow, EmPlanningScope.FullDirectionSegment)), + "full scope ignores legacy stopping-preview truncation"); + AssertStatus(EmPlanningStatus.InvalidInput, + EmPlanningRequestValidator.Validate(CreateValidRequest(shortLegacyWindow, EmPlanningScope.RollingHorizon)), + "rolling scope retains legacy stopping-preview validation"); + AssertStatus(EmPlanningStatus.InvalidInput, + EmPlanningRequestValidator.Validate(CreateValidRequest(EmPlannerConfiguration.CreateDefault(), + (EmPlanningScope)99)), "undefined planning scope"); + } + private static void VerifyLateralWeights(LateralWeights weights) { EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceOffset, "lateral reference weight"); @@ -176,7 +258,8 @@ internal static class FoundationChecks EMPlannerVerificationHost.Verification.Equal(expected, actual.Status, name); } - private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration) + private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration, + EmPlanningScope planningScope = EmPlanningScope.RollingHorizon) { DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero); return CreateRequest( @@ -186,7 +269,8 @@ internal static class FoundationChecks CreateState(0d, 9L, requestedAtUtc), configuration, 0, - EmMotionModel.NonholonomicForwardReverse); + EmMotionModel.NonholonomicForwardReverse, + planningScope); } private static EmPlanningRequest CreateRequest( @@ -196,7 +280,8 @@ internal static class FoundationChecks VehicleMotionState vehicleState, EmPlannerConfiguration configuration, int segmentIndex, - EmMotionModel motionModel) + EmMotionModel motionModel, + EmPlanningScope planningScope = EmPlanningScope.RollingHorizon) { DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero); return new EmPlanningRequest( @@ -212,7 +297,8 @@ internal static class FoundationChecks "trajectory-1", "reference-1", string.Empty, - motionModel); + motionModel, + planningScope); } private static VehicleMotionState CreateState(double speed, long sequenceId, DateTimeOffset capturedAtUtc) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index 3cfa641..ee386ac 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -500,7 +500,7 @@ internal static class LongitudinalModelChecks { var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L, "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop, - EmLongitudinalMode.RollingContinuation); + EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon); double sign = direction == TravelDirection.Forward ? 1d : -1d; return new EmTrajectory(metadata, new[] { diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs index 6a84a88..28eea64 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs @@ -403,6 +403,7 @@ internal static class TrajectoryChecks EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary) { return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L, - "reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode); + "reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode, + EmPlanningScope.RollingHorizon); } } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 6e33504..11e1b0d 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -901,7 +901,7 @@ internal static class TrajectoryObservationChecks { var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L, "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, terminalType, - EmLongitudinalMode.RollingContinuation); + EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon); return new EmTrajectory(metadata, new[] { new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d, @@ -944,7 +944,7 @@ internal static class TrajectoryObservationChecks "active-segment-reference", 1L, string.Empty, segmentIndex, direction, terminalType, terminalType == EmTerminalType.GearSwitch ? EmLongitudinalMode.ExactStopAtBoundary - : EmLongitudinalMode.RollingContinuation); + : EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon); return new EmTrajectory(metadata, new[] { new EmTrajectoryPoint(x, 0d, 0d, 0d, 0d, 0d, segmentIndex, 1d, 1d, diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs index c2bfd2a..e1759ca 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs @@ -189,7 +189,7 @@ internal static class TrajectoryObservationSegmentChecks { var metadata = new EmTrajectoryMetadata("gear-terminal-" + effectiveAtUtc.Ticks, effectiveAtUtc, effectiveAtUtc, 1L, "segment-check", 1L, string.Empty, 0, direction, terminalType, - EmLongitudinalMode.ExactStopAtBoundary); + EmLongitudinalMode.ExactStopAtBoundary, EmPlanningScope.RollingHorizon); return new EmTrajectory(metadata, new[] { new EmTrajectoryPoint(1d, 0d, 0d, 0d, finalTimeFromStart, 0d, 0, 1d, 1d, diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs index f6f85e4..c61001d 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs @@ -470,7 +470,7 @@ internal static class TrajectoryObservationVisualizationChecks 0.2d + t, 0.3d + t)); } return new EmTrajectory(new EmTrajectoryMetadata(id, effectiveAt, effectiveAt, 1L, "visualization-reference", 1L, - string.Empty, 0, direction, terminal, mode), points); + string.Empty, 0, direction, terminal, mode, EmPlanningScope.RollingHorizon), points); } private sealed class FixedPlanningService : IEmPlanningService