feat: build EM path speed limits

2026-08-04 09:16:17 +08:00
parent e91dbceb0b
commit 62ea9db8cd
6 changed files with 734 additions and 1 deletions
@@ -0,0 +1,111 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable longitudinal inputs derived only from an independently validated lateral PathS path.</summary>
+public sealed class LongitudinalPlanningInput
+{
+    private const double PathSTolerance = 1e-12d;
+
+    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
+    {
+        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
+            throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.",
+                nameof(path));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d)
+            throw new ArgumentOutOfRangeException(nameof(initialProgressSpeedMetersPerSecond));
+        if (!IsFinite(initialAccelerationMetersPerSecondSquared))
+            throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
+        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+            throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (configuration == null)
+            throw new ArgumentNullException(nameof(configuration));
+
+        Path = CopyAndValidatePath(path);
+        Direction = direction;
+        InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
+        InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
+        TerminalType = terminalType;
+        Configuration = configuration.Copy();
+        PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
+        PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
+            nameof(previousProgressSpeedMetersPerSecond));
+        if (PreviousPathS.Count != PreviousProgressSpeedMetersPerSecond.Count)
+            throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.",
+                nameof(previousProgressSpeedMetersPerSecond));
+    }
+
+    public LateralPath Path { get; }
+
+    public TravelDirection Direction { get; }
+
+    public double InitialProgressSpeedMetersPerSecond { get; }
+
+    public double InitialAccelerationMetersPerSecondSquared { get; }
+
+    public EmTerminalType TerminalType { get; }
+
+    public EmPlannerConfiguration Configuration { get; }
+
+    public IReadOnlyList<double> PreviousPathS { get; }
+
+    public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }
+
+    public double TerminalPathS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+
+    public double DirectionMaximumSpeedMetersPerSecond
+    {
+        get
+        {
+            return Direction == TravelDirection.Forward
+                ? Configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond
+                : Configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond;
+        }
+    }
+
+    private static LateralPath CopyAndValidatePath(LateralPath source)
+    {
+        var copy = new List<LateralPathPoint>(source.Points.Count);
+        double previousPathS = double.NegativeInfinity;
+        for (int index = 0; index < source.Points.Count; index++)
+        {
+            LateralPathPoint point = source.Points[index];
+            if (point == null || !IsFinite(point.PathS) || point.PathS <= previousPathS)
+                throw new ArgumentException("Lateral PathS must be finite and strictly increasing for ST planning.", nameof(source));
+            if (index == 0 && Math.Abs(point.PathS) > PathSTolerance)
+                throw new ArgumentException("The lateral PathS supplied to ST must begin at zero.", nameof(source));
+            copy.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL,
+                point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature,
+                point.VehicleCurvatureDerivative));
+            previousPathS = point.PathS;
+        }
+        return new LateralPath(copy, true);
+    }
+
+    private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, string parameterName)
+    {
+        var copy = new List<double>(source == null ? 0 : source.Count);
+        if (source != null)
+        {
+            for (int index = 0; index < source.Count; index++)
+            {
+                if (!IsFinite(source[index]) || source[index] < 0d)
+                    throw new ArgumentOutOfRangeException(parameterName);
+                copy.Add(source[index]);
+            }
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}