feat: define full-direction EM planning scope

2026-08-06 20:48:44 +08:00
parent fcf7df17d5
commit f4e89b4b4f
20 changed files with 183 additions and 33 deletions
@@ -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,
            },
        };
    }
@@ -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,
@@ -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,
        };
    }
 }
@@ -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,
        };
    }
 }
@@ -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.
@@ -0,0 +1,7 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmPlanningScope
+{
+    RollingHorizon,
+    FullDirectionSegment,
+}
@@ -19,5 +19,8 @@ public enum EmPlanningStatus
    Cancelled,
    ValidationFailed,
    Superseded,
+    NoProgress,
+    TerminalPoseMismatch,
+    FullSegmentResourceLimitExceeded,
    Failed,
 }
@@ -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; }
 }
@@ -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");

@@ -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;