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