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,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(),
+        };
+    }
+}