diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/CorridorConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/CorridorConfiguration.cs new file mode 100644 index 0000000..0f0cbe2 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/CorridorConfiguration.cs @@ -0,0 +1,22 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class CorridorConfiguration +{ + public double LongitudinalSampleSpacingMeters { get; set; } + public double LateralSampleSpacingMeters { get; set; } + public double MaximumLateralOffsetMeters { get; set; } + public double AdditionalClearanceReserveMeters { get; set; } + public double MaximumCollisionCheckStepMeters { get; set; } + + internal CorridorConfiguration Copy() + { + return new CorridorConfiguration + { + LongitudinalSampleSpacingMeters = LongitudinalSampleSpacingMeters, + LateralSampleSpacingMeters = LateralSampleSpacingMeters, + MaximumLateralOffsetMeters = MaximumLateralOffsetMeters, + AdditionalClearanceReserveMeters = AdditionalClearanceReserveMeters, + MaximumCollisionCheckStepMeters = MaximumCollisionCheckStepMeters, + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs new file mode 100644 index 0000000..a62d7c1 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs @@ -0,0 +1,111 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed partial class EmPlannerConfiguration +{ + public SchedulingConfiguration Scheduling { get; set; } + public FrenetConfiguration Frenet { get; set; } + public CorridorConfiguration Corridor { get; set; } + public LateralConfiguration Lateral { get; set; } + public LongitudinalConfiguration Longitudinal { get; set; } + public SolverConfiguration Solver { get; set; } + public ValidationConfiguration Validation { get; set; } + + public static EmPlannerConfiguration CreateDefault() + { + return new EmPlannerConfiguration + { + Scheduling = new SchedulingConfiguration + { + ReplanPeriodSeconds = 0.20d, + TimeHorizonSeconds = 6d, + DistanceHorizonMeters = 5d, + OutputTimeStepSeconds = 0.05d, + SolverTimeoutSeconds = 0.10d, + HandoffLookaheadSeconds = 0.30d, + MaximumVehicleStateAgeSeconds = 0.20d, + }, + Corridor = new CorridorConfiguration + { + LongitudinalSampleSpacingMeters = 0.10d, + LateralSampleSpacingMeters = 0.025d, + MaximumLateralOffsetMeters = 0.30d, + AdditionalClearanceReserveMeters = 0.02d, + MaximumCollisionCheckStepMeters = 0.025d, + }, + Frenet = new FrenetConfiguration + { + MaximumProjectionDistanceMeters = 0.50d, + MinimumFrenetDenominator = 0.20d, + BoundaryAnchorToleranceMeters = 1e-8d, + }, + Lateral = new LateralConfiguration + { + MaximumLateralStepPerIterationMeters = 0.05d, + MaximumLateralSlope = 0.50d, + MaximumLateralSecondDerivativePerMeter = 1d, + MaximumLateralThirdDerivativePerSquareMeter = 2d, + Weights = new LateralWeights + { + ReferenceOffset = 10d, + HeadingDeviation = 1d, + SecondDerivative = 5d, + ThirdDerivative = 10d, + Curvature = 5d, + CurvatureVariation = 20d, + PreviousTrajectory = 5d, + RollingTerminal = 10d, + }, + }, + Longitudinal = new LongitudinalConfiguration + { + MaximumForwardSpeedMetersPerSecond = 0.20d, + MaximumReverseSpeedMetersPerSecond = 0.20d, + MaximumAccelerationMetersPerSecondSquared = 0.20d, + MaximumDecelerationMetersPerSecondSquared = 0.30d, + MaximumJerkMetersPerSecondCubed = 0.50d, + MaximumLateralAccelerationMetersPerSecondSquared = 0.20d, + MaximumCurvatureRatePerMeterPerSecond = 0.50d, + StopSpeedToleranceMetersPerSecond = 0.01d, + ZeroSpeedHoldSeconds = 0.20d, + Weights = new LongitudinalWeights + { + ReferenceSpeed = 10d, + Acceleration = 1d, + Jerk = 10d, + PreviousTrajectory = 5d, + TerminalAcceleration = 1d, + }, + }, + Solver = new SolverConfiguration + { + MaximumOuterIterations = 5, + MaximumOsqpIterations = 4000, + AbsoluteTolerance = 1e-5d, + RelativeTolerance = 1e-5d, + StrictResidualTolerance = 1e-5d, + WarmStart = true, + Polish = true, + NativeVerbose = false, + }, + Validation = new ValidationConfiguration + { + SpatialToleranceMeters = 1e-8d, + KinematicTolerance = 1e-8d, + }, + }; + } + + internal EmPlannerConfiguration Copy() + { + return new EmPlannerConfiguration + { + Scheduling = Scheduling == null ? null : Scheduling.Copy(), + Frenet = Frenet == null ? null : Frenet.Copy(), + Corridor = Corridor == null ? null : Corridor.Copy(), + Lateral = Lateral == null ? null : Lateral.Copy(), + Longitudinal = Longitudinal == null ? null : Longitudinal.Copy(), + Solver = Solver == null ? null : Solver.Copy(), + Validation = Validation == null ? null : Validation.Copy(), + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/FrenetConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/FrenetConfiguration.cs new file mode 100644 index 0000000..2aa3c0a --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/FrenetConfiguration.cs @@ -0,0 +1,18 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class FrenetConfiguration +{ + public double MaximumProjectionDistanceMeters { get; set; } + public double MinimumFrenetDenominator { get; set; } + public double BoundaryAnchorToleranceMeters { get; set; } + + internal FrenetConfiguration Copy() + { + return new FrenetConfiguration + { + MaximumProjectionDistanceMeters = MaximumProjectionDistanceMeters, + MinimumFrenetDenominator = MinimumFrenetDenominator, + BoundaryAnchorToleranceMeters = BoundaryAnchorToleranceMeters, + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralConfiguration.cs new file mode 100644 index 0000000..6d622c5 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralConfiguration.cs @@ -0,0 +1,22 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class LateralConfiguration +{ + public double MaximumLateralStepPerIterationMeters { get; set; } + public double MaximumLateralSlope { get; set; } + public double MaximumLateralSecondDerivativePerMeter { get; set; } + public double MaximumLateralThirdDerivativePerSquareMeter { get; set; } + public LateralWeights Weights { get; set; } + + internal LateralConfiguration Copy() + { + return new LateralConfiguration + { + MaximumLateralStepPerIterationMeters = MaximumLateralStepPerIterationMeters, + MaximumLateralSlope = MaximumLateralSlope, + MaximumLateralSecondDerivativePerMeter = MaximumLateralSecondDerivativePerMeter, + MaximumLateralThirdDerivativePerSquareMeter = MaximumLateralThirdDerivativePerSquareMeter, + Weights = Weights == null ? null : Weights.Copy(), + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralWeights.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralWeights.cs new file mode 100644 index 0000000..ac98cdb --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LateralWeights.cs @@ -0,0 +1,28 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class LateralWeights +{ + public double ReferenceOffset { get; set; } + public double HeadingDeviation { get; set; } + public double SecondDerivative { get; set; } + public double ThirdDerivative { get; set; } + public double Curvature { get; set; } + public double CurvatureVariation { get; set; } + public double PreviousTrajectory { get; set; } + public double RollingTerminal { get; set; } + + internal LateralWeights Copy() + { + return new LateralWeights + { + ReferenceOffset = ReferenceOffset, + HeadingDeviation = HeadingDeviation, + SecondDerivative = SecondDerivative, + ThirdDerivative = ThirdDerivative, + Curvature = Curvature, + CurvatureVariation = CurvatureVariation, + PreviousTrajectory = PreviousTrajectory, + RollingTerminal = RollingTerminal, + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs new file mode 100644 index 0000000..28cbeef --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalConfiguration.cs @@ -0,0 +1,32 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class LongitudinalConfiguration +{ + public double MaximumForwardSpeedMetersPerSecond { get; set; } + public double MaximumReverseSpeedMetersPerSecond { get; set; } + public double MaximumAccelerationMetersPerSecondSquared { get; set; } + public double MaximumDecelerationMetersPerSecondSquared { get; set; } + public double MaximumJerkMetersPerSecondCubed { get; set; } + public double MaximumLateralAccelerationMetersPerSecondSquared { get; set; } + public double MaximumCurvatureRatePerMeterPerSecond { get; set; } + public double StopSpeedToleranceMetersPerSecond { get; set; } + public double ZeroSpeedHoldSeconds { get; set; } + public LongitudinalWeights Weights { get; set; } + + internal LongitudinalConfiguration Copy() + { + return new LongitudinalConfiguration + { + MaximumForwardSpeedMetersPerSecond = MaximumForwardSpeedMetersPerSecond, + MaximumReverseSpeedMetersPerSecond = MaximumReverseSpeedMetersPerSecond, + MaximumAccelerationMetersPerSecondSquared = MaximumAccelerationMetersPerSecondSquared, + MaximumDecelerationMetersPerSecondSquared = MaximumDecelerationMetersPerSecondSquared, + MaximumJerkMetersPerSecondCubed = MaximumJerkMetersPerSecondCubed, + MaximumLateralAccelerationMetersPerSecondSquared = MaximumLateralAccelerationMetersPerSecondSquared, + MaximumCurvatureRatePerMeterPerSecond = MaximumCurvatureRatePerMeterPerSecond, + StopSpeedToleranceMetersPerSecond = StopSpeedToleranceMetersPerSecond, + ZeroSpeedHoldSeconds = ZeroSpeedHoldSeconds, + Weights = Weights == null ? null : Weights.Copy(), + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalWeights.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalWeights.cs new file mode 100644 index 0000000..6967f66 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/LongitudinalWeights.cs @@ -0,0 +1,22 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class LongitudinalWeights +{ + public double ReferenceSpeed { get; set; } + public double Acceleration { get; set; } + public double Jerk { get; set; } + public double PreviousTrajectory { get; set; } + public double TerminalAcceleration { get; set; } + + internal LongitudinalWeights Copy() + { + return new LongitudinalWeights + { + ReferenceSpeed = ReferenceSpeed, + Acceleration = Acceleration, + Jerk = Jerk, + PreviousTrajectory = PreviousTrajectory, + TerminalAcceleration = TerminalAcceleration, + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs new file mode 100644 index 0000000..4a2e792 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SchedulingConfiguration.cs @@ -0,0 +1,26 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class SchedulingConfiguration +{ + public double ReplanPeriodSeconds { get; set; } + public double TimeHorizonSeconds { get; set; } + public double DistanceHorizonMeters { get; set; } + public double OutputTimeStepSeconds { get; set; } + public double SolverTimeoutSeconds { get; set; } + public double HandoffLookaheadSeconds { get; set; } + public double MaximumVehicleStateAgeSeconds { get; set; } + + internal SchedulingConfiguration Copy() + { + return new SchedulingConfiguration + { + ReplanPeriodSeconds = ReplanPeriodSeconds, + TimeHorizonSeconds = TimeHorizonSeconds, + DistanceHorizonMeters = DistanceHorizonMeters, + OutputTimeStepSeconds = OutputTimeStepSeconds, + SolverTimeoutSeconds = SolverTimeoutSeconds, + HandoffLookaheadSeconds = HandoffLookaheadSeconds, + MaximumVehicleStateAgeSeconds = MaximumVehicleStateAgeSeconds, + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SolverConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SolverConfiguration.cs new file mode 100644 index 0000000..b648664 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/SolverConfiguration.cs @@ -0,0 +1,28 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class SolverConfiguration +{ + public int MaximumOuterIterations { get; set; } + public int MaximumOsqpIterations { get; set; } + public double AbsoluteTolerance { get; set; } + public double RelativeTolerance { get; set; } + public double StrictResidualTolerance { get; set; } + public bool WarmStart { get; set; } + public bool Polish { get; set; } + public bool NativeVerbose { get; set; } + + internal SolverConfiguration Copy() + { + return new SolverConfiguration + { + MaximumOuterIterations = MaximumOuterIterations, + MaximumOsqpIterations = MaximumOsqpIterations, + AbsoluteTolerance = AbsoluteTolerance, + RelativeTolerance = RelativeTolerance, + StrictResidualTolerance = StrictResidualTolerance, + WarmStart = WarmStart, + Polish = Polish, + NativeVerbose = NativeVerbose, + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs new file mode 100644 index 0000000..cac9d0e --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/ValidationConfiguration.cs @@ -0,0 +1,16 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class ValidationConfiguration +{ + public double SpatialToleranceMeters { get; set; } + public double KinematicTolerance { get; set; } + + internal ValidationConfiguration Copy() + { + return new ValidationConfiguration + { + SpatialToleranceMeters = SpatialToleranceMeters, + KinematicTolerance = KinematicTolerance, + }; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs index cb719f4..da743b0 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs @@ -12,17 +12,6 @@ public sealed class VehicleMotionState DateTimeOffset capturedAtUtc, long sequenceId) { - if (pose == null) - throw new ArgumentNullException(nameof(pose)); - ContractNumeric.RequireFinite(pose.X, nameof(pose)); - ContractNumeric.RequireFinite(pose.Y, nameof(pose)); - ContractNumeric.RequireFinite(pose.Heading, nameof(pose)); - ContractNumeric.RequireFinite(signedLongitudinalSpeedMetersPerSecond, nameof(signedLongitudinalSpeedMetersPerSecond)); - if (longitudinalAccelerationMetersPerSecondSquared.HasValue) - ContractNumeric.RequireFinite(longitudinalAccelerationMetersPerSecondSquared.Value, nameof(longitudinalAccelerationMetersPerSecondSquared)); - if (sequenceId < 0) - throw new ArgumentOutOfRangeException(nameof(sequenceId)); - Pose = pose; SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond; LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared; diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlannerDebugOptions.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlannerDebugOptions.cs new file mode 100644 index 0000000..af16a8e --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlannerDebugOptions.cs @@ -0,0 +1,13 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class EmPlannerDebugOptions +{ + public bool EnableSummary { get; set; } + public bool EnableProjectionTrace { get; set; } + public bool EnableCorridorTrace { get; set; } + public bool EnableLateralSolverTrace { get; set; } + public bool EnableLongitudinalSolverTrace { get; set; } + public bool EnableTrajectoryDump { get; set; } + public bool EnableVisualization { get; set; } + public IEmPlannerDebugSink Sink { get; set; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlanningDiagnostics.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlanningDiagnostics.cs new file mode 100644 index 0000000..7c6a762 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/EmPlanningDiagnostics.cs @@ -0,0 +1,30 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class EmPlanningDiagnostics +{ + private readonly List _debugSinkFailures = new List(); + + public IReadOnlyList DebugSinkFailures + { + get { return new ReadOnlyCollection(new List(_debugSinkFailures)); } + } + + public void WriteDebug(EmPlannerDebugOptions options, string message) + { + if (options == null || options.Sink == null) + return; + + try + { + options.Sink.Write(message ?? string.Empty); + } + catch (Exception exception) + { + _debugSinkFailures.Add(exception.GetType().FullName + ": " + exception.Message); + } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/IEmPlannerDebugSink.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/IEmPlannerDebugSink.cs new file mode 100644 index 0000000..fd933b4 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/IEmPlannerDebugSink.cs @@ -0,0 +1,6 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public interface IEmPlannerDebugSink +{ + void Write(string message); +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs new file mode 100644 index 0000000..29daec9 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs @@ -0,0 +1,165 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; +using MultiWheelC.TrajectoryPlanning.Utils; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class EmPlanningRequestValidationResult +{ + internal EmPlanningRequestValidationResult(EmPlanningStatus status, string failureReason, EmPlanningRequestSnapshot snapshot) + { + Status = status; + FailureReason = failureReason ?? string.Empty; + Snapshot = snapshot; + } + + public EmPlanningStatus Status { get; } + + public string FailureReason { get; } + + public bool IsValid { get { return Status == EmPlanningStatus.Success; } } + + internal EmPlanningRequestSnapshot Snapshot { get; } +} + +internal sealed class EmPlanningRequestSnapshot +{ + public EmPlanningRequestSnapshot(EmPlanningRequest request, EmPlannerConfiguration configuration) + { + Request = request; + Configuration = configuration; + } + + public EmPlanningRequest Request { get; } + + public EmPlannerConfiguration Configuration { get; } +} + +public static class EmPlanningRequestValidator +{ + public static EmPlanningRequestValidationResult Validate(EmPlanningRequest request) + { + if (request == null) + return Invalid("Request is required."); + if (request.ReferencePath == null || request.Map == null || request.Vehicle == null || request.VehicleState == null || request.Configuration == null) + return Invalid("Request members ReferencePath, Map, Vehicle, VehicleState, and Configuration are required."); + if (request.MotionModel == EmMotionModel.CrabTranslation || request.MotionModel == EmMotionModel.InPlaceRotation) + 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)) + return Invalid(configurationFailure); + if (!request.Map.PlanningReady) + return Invalid("Planning map is not ready."); + if (!IsConsumable(request.ReferencePath.Status)) + return Result(EmPlanningStatus.InvalidReferencePath, "Reference path status is not consumable."); + if (request.ReferencePath.Path == null || request.ReferencePath.Segments == null || + request.SegmentIndex < 0 || request.SegmentIndex >= request.ReferencePath.Segments.Count) + return Result(EmPlanningStatus.InvalidReferencePath, "Requested direction segment is out of range."); + if (!IsValidVehicleState(request.VehicleState)) + return Invalid("Vehicle state contains invalid values."); + if (!IsValidVehicle(request.Vehicle)) + return Invalid("Vehicle geometry or curvature limit is invalid."); + if (request.RequestedAtUtc - request.VehicleState.CapturedAtUtc > + TimeSpan.FromSeconds(request.Configuration.Scheduling.MaximumVehicleStateAgeSeconds)) + return Result(EmPlanningStatus.StaleVehicleState, "Vehicle state is stale."); + + return new EmPlanningRequestValidationResult( + EmPlanningStatus.Success, + string.Empty, + new EmPlanningRequestSnapshot(request, request.Configuration.Copy())); + } + + private static EmPlanningRequestValidationResult Invalid(string reason) + { + return Result(EmPlanningStatus.InvalidInput, reason); + } + + private static EmPlanningRequestValidationResult Result(EmPlanningStatus status, string reason) + { + return new EmPlanningRequestValidationResult(status, reason, null); + } + + private static bool IsConsumable(PathSmoothingStatus status) + { + return status == PathSmoothingStatus.Complete || + status == PathSmoothingStatus.PartialImprovement || + status == PathSmoothingStatus.NotNeeded || + status == PathSmoothingStatus.Unchanged; + } + + private static bool IsValidVehicleState(VehicleMotionState state) + { + return state.Pose != null && + NumericGuard.IsFinite(state.Pose.X) && + NumericGuard.IsFinite(state.Pose.Y) && + NumericGuard.IsFinite(state.Pose.Heading) && + NumericGuard.IsFinite(state.SignedLongitudinalSpeedMetersPerSecond) && + (!state.LongitudinalAccelerationMetersPerSecondSquared.HasValue || NumericGuard.IsFinite(state.LongitudinalAccelerationMetersPerSecondSquared.Value)) && + state.SequenceId >= 0; + } + + private static bool IsValidVehicle(MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters vehicle) + { + return NumericGuard.IsPositiveFinite(vehicle.LengthMeters) && + NumericGuard.IsPositiveFinite(vehicle.WidthMeters) && + NumericGuard.IsFinite(vehicle.SafetyMarginMeters) && vehicle.SafetyMarginMeters >= 0d && + VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _); + } + + private static bool TryValidateConfiguration(EmPlannerConfiguration configuration, out string failureReason) + { + failureReason = "Configuration is invalid."; + if (configuration.Scheduling == null || configuration.Corridor == null || configuration.Frenet == null || + configuration.Lateral == null || configuration.Longitudinal == null || configuration.Solver == null || + configuration.Validation == null || configuration.Lateral.Weights == null || configuration.Longitudinal.Weights == null) + return false; + + SchedulingConfiguration scheduling = configuration.Scheduling; + CorridorConfiguration corridor = configuration.Corridor; + FrenetConfiguration frenet = configuration.Frenet; + LateralConfiguration lateral = configuration.Lateral; + LongitudinalConfiguration longitudinal = configuration.Longitudinal; + SolverConfiguration solver = configuration.Solver; + ValidationConfiguration validation = configuration.Validation; + + if (!Positive(scheduling.ReplanPeriodSeconds) || !Positive(scheduling.TimeHorizonSeconds) || !Positive(scheduling.DistanceHorizonMeters) || + !Positive(scheduling.OutputTimeStepSeconds) || !Positive(scheduling.SolverTimeoutSeconds) || !Positive(scheduling.HandoffLookaheadSeconds) || !Positive(scheduling.MaximumVehicleStateAgeSeconds) || + !Positive(corridor.LongitudinalSampleSpacingMeters) || !Positive(corridor.LateralSampleSpacingMeters) || !Positive(corridor.MaximumLateralOffsetMeters) || + !NonNegative(corridor.AdditionalClearanceReserveMeters) || !Positive(corridor.MaximumCollisionCheckStepMeters) || + !Positive(frenet.MaximumProjectionDistanceMeters) || !NumericGuard.IsFinite(frenet.MinimumFrenetDenominator) || + frenet.MinimumFrenetDenominator <= 0d || frenet.MinimumFrenetDenominator >= 1d || !Positive(frenet.BoundaryAnchorToleranceMeters) || + !Positive(lateral.MaximumLateralStepPerIterationMeters) || !Positive(lateral.MaximumLateralSlope) || + !Positive(lateral.MaximumLateralSecondDerivativePerMeter) || !Positive(lateral.MaximumLateralThirdDerivativePerSquareMeter) || + !Positive(longitudinal.MaximumForwardSpeedMetersPerSecond) || !Positive(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) || + !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights)) + return false; + + return true; + } + + private static bool Positive(double value) { return NumericGuard.IsPositiveFinite(value); } + + private static bool NonNegative(double value) { return NumericGuard.IsFinite(value) && value >= 0d; } + + private static bool WeightsAreValid(LateralWeights weights) + { + return NonNegative(weights.ReferenceOffset) && NonNegative(weights.HeadingDeviation) && + NonNegative(weights.SecondDerivative) && NonNegative(weights.ThirdDerivative) && + NonNegative(weights.Curvature) && NonNegative(weights.CurvatureVariation) && + NonNegative(weights.PreviousTrajectory) && NonNegative(weights.RollingTerminal); + } + + private static bool WeightsAreValid(LongitudinalWeights weights) + { + return NonNegative(weights.ReferenceSpeed) && NonNegative(weights.Acceleration) && NonNegative(weights.Jerk) && + NonNegative(weights.PreviousTrajectory) && NonNegative(weights.TerminalAcceleration); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs index 433b15b..dc7a352 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs @@ -1,11 +1,20 @@ using System; +using System.Collections.Generic; using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Mapping; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; internal static class FoundationChecks { public static void Run() + { + VerifyContracts(); + VerifyConfigurationAndRequestValidation(); + } + + private static void VerifyContracts() { var reverseState = new VehicleMotionState( new Pose2D(1.5d, -2d, 0.25d), @@ -48,4 +57,190 @@ internal static class FoundationChecks EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y"); EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate"); } + + private static void VerifyConfigurationAndRequestValidation() + { + 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.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"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.MaximumVehicleStateAgeSeconds, "vehicle state age"); + + EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Corridor.LongitudinalSampleSpacingMeters, "longitudinal sample spacing"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.LateralSampleSpacingMeters, "lateral sample spacing"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Corridor.MaximumLateralOffsetMeters, "maximum lateral offset"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.02d, configuration.Corridor.AdditionalClearanceReserveMeters, "clearance reserve"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.MaximumCollisionCheckStepMeters, "collision check step"); + + EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Frenet.MaximumProjectionDistanceMeters, "maximum projection distance"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Frenet.MinimumFrenetDenominator, "minimum Frenet denominator"); + EMPlannerVerificationHost.Verification.NearlyEqual(1e-8d, configuration.Frenet.BoundaryAnchorToleranceMeters, "boundary anchor tolerance"); + + EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Lateral.MaximumLateralStepPerIterationMeters, "lateral trust region"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Lateral.MaximumLateralSlope, "maximum lateral slope"); + 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(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"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared, "maximum lateral acceleration"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond, "maximum curvature rate"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.01d, configuration.Longitudinal.StopSpeedToleranceMetersPerSecond, "stop speed tolerance"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.ZeroSpeedHoldSeconds, "zero speed hold"); + + EMPlannerVerificationHost.Verification.Equal(5, configuration.Solver.MaximumOuterIterations, "maximum outer iterations"); + EMPlannerVerificationHost.Verification.Equal(4000, configuration.Solver.MaximumOsqpIterations, "maximum OSQP iterations"); + EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.AbsoluteTolerance, "absolute tolerance"); + EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.RelativeTolerance, "relative tolerance"); + EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.StrictResidualTolerance, "strict residual tolerance"); + EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.WarmStart, "warm start"); + EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.Polish, "polish"); + EMPlannerVerificationHost.Verification.Equal(false, configuration.Solver.NativeVerbose, "native verbose"); + + VerifyLateralWeights(configuration.Lateral.Weights); + VerifyLongitudinalWeights(configuration.Longitudinal.Weights); + + EmPlanningRequest valid = CreateValidRequest(configuration); + AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate( + CreateRequest(null!, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null reference path"); + AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, null!, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null map"); + AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, null!, 0, EmMotionModel.NonholonomicForwardReverse)), "null configuration"); + AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, CreateMap(false), valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "map not ready"); + AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate( + CreateRequest(CreateFailedReferencePath(), valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "smoothing failure"); + AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse)), "segment index"); + AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(double.NaN, 9L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "non-finite speed"); + AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, -1L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "negative sequence ID"); + AssertStatus(EmPlanningStatus.StaleVehicleState, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, 9L, valid.RequestedAtUtc.AddSeconds(-1d)), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "stale state"); + AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d }, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "missing curvature limit"); + AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.CrabTranslation)), "crab motion"); + AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.InPlaceRotation)), "in-place rotation"); + + EmPlanningRequestValidationResult first = EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse)); + EmPlanningRequestValidationResult second = EmPlanningRequestValidator.Validate( + CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse)); + EMPlannerVerificationHost.Verification.Equal(first.FailureReason, second.FailureReason, "deterministic rejection message"); + } + + private static void VerifyLateralWeights(LateralWeights weights) + { + EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceOffset, "lateral reference weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.HeadingDeviation, "lateral heading weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.SecondDerivative, "lateral second derivative weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ThirdDerivative, "lateral third derivative weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.Curvature, "lateral curvature weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(20d, weights.CurvatureVariation, "lateral curvature variation weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "lateral previous weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.RollingTerminal, "lateral rolling terminal weight"); + } + + private static void VerifyLongitudinalWeights(LongitudinalWeights weights) + { + EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceSpeed, "longitudinal speed weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.Acceleration, "longitudinal acceleration weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.Jerk, "longitudinal jerk weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "longitudinal previous weight"); + EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.TerminalAcceleration, "longitudinal terminal acceleration weight"); + } + + private static void AssertStatus(EmPlanningStatus expected, EmPlanningRequestValidationResult actual, string name) + { + EMPlannerVerificationHost.Verification.Equal(expected, actual.Status, name); + } + + private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration) + { + DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero); + return CreateRequest( + CreateValidReferencePath(), + CreateMap(true), + new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d, SafetyMarginMeters = 0.05d, MaximumCurvaturePerMeter = 0.5d }, + CreateState(0d, 9L, requestedAtUtc), + configuration, + 0, + EmMotionModel.NonholonomicForwardReverse); + } + + private static EmPlanningRequest CreateRequest( + PathSmoothingResult referencePath, + PlanningGridMap map, + VehicleParameters vehicle, + VehicleMotionState vehicleState, + EmPlannerConfiguration configuration, + int segmentIndex, + EmMotionModel motionModel) + { + DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero); + return new EmPlanningRequest( + referencePath, + map, + vehicle, + vehicleState, + configuration, + segmentIndex, + null!, + requestedAtUtc, + requestedAtUtc.AddSeconds(0.1d), + "trajectory-1", + "reference-1", + string.Empty, + motionModel); + } + + private static VehicleMotionState CreateState(double speed, long sequenceId, DateTimeOffset capturedAtUtc) + { + return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, null, capturedAtUtc, sequenceId); + } + + private static PlanningGridMap CreateMap(bool planningReady) + { + var result = new PlanningMapFactory().Create(new PlanningMapRequest + { + Bounds = new MapBoundsMm(-2000f, 2000f, -2000f, 2000f), + ResolutionMm = 100f, + ObstacleSources = new List(), + AllowExplicitEmptyMap = planningReady, + }); + if (!result.Succeeded || result.Map == null) + throw new InvalidOperationException("Unable to create test planning map."); + return result.Map; + } + + private static PathSmoothingResult CreateValidReferencePath() + { + var points = new List + { + new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + new SmoothedPathPoint(1d, 0d, 0d, 0d, 1d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + }; + var segments = new List + { + new SmoothedPathSegment(0, TravelDirection.Forward, 0, 1, false, false), + }; + var metrics = new PathQualityMetrics(true, 1d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d); + return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments, + new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List()); + } + + private static PathSmoothingResult CreateFailedReferencePath() + { + return PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics()); + } }