feat: validate EM planner requests

2026-08-03 22:22:44 +08:00
parent e12eb31205
commit b326431d63
16 changed files with 734 additions and 11 deletions
@@ -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,
+        };
+    }
+}
@@ -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(),
+        };
+    }
+}
@@ -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,
+        };
+    }
+}
@@ -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(),
+        };
+    }
+}
@@ -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,
+        };
+    }
+}
@@ -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(),
+        };
+    }
+}
@@ -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,
+        };
+    }
+}
@@ -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,
+        };
+    }
+}
@@ -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,
+        };
+    }
+}
@@ -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,
+        };
+    }
+}