feat: add EM planner contracts

2026-08-03 22:14:37 +08:00
parent 882f80bca0
commit e12eb31205
16 changed files with 477 additions and 0 deletions
@@ -0,0 +1,84 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmTrajectoryPoint
+{
+    public EmTrajectoryPoint(
+        double x,
+        double y,
+        double yaw,
+        double signedLongitudinalVelocity,
+        double timeFromStart,
+        double vehicleCurvature,
+        int segmentIndex,
+        double segmentLocalS,
+        double pathS,
+        TravelDirection direction,
+        EmBoundaryType boundaryType,
+        double longitudinalAcceleration,
+        double longitudinalJerk)
+    {
+        ContractNumeric.RequireFinite(x, nameof(x));
+        ContractNumeric.RequireFinite(y, nameof(y));
+        ContractNumeric.RequireFinite(yaw, nameof(yaw));
+        ContractNumeric.RequireFinite(signedLongitudinalVelocity, nameof(signedLongitudinalVelocity));
+        ContractNumeric.RequireFinite(timeFromStart, nameof(timeFromStart));
+        ContractNumeric.RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        ContractNumeric.RequireFinite(segmentLocalS, nameof(segmentLocalS));
+        ContractNumeric.RequireFinite(pathS, nameof(pathS));
+        ContractNumeric.RequireFinite(longitudinalAcceleration, nameof(longitudinalAcceleration));
+        ContractNumeric.RequireFinite(longitudinalJerk, nameof(longitudinalJerk));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (timeFromStart < 0d)
+            throw new ArgumentOutOfRangeException(nameof(timeFromStart));
+        if (segmentLocalS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(segmentLocalS));
+        if (pathS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType))
+            throw new ArgumentOutOfRangeException(nameof(boundaryType));
+
+        X = x;
+        Y = y;
+        Yaw = yaw;
+        SignedLongitudinalVelocity = signedLongitudinalVelocity;
+        Speed = Math.Abs(signedLongitudinalVelocity);
+        VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
+        VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
+        YawRate = signedLongitudinalVelocity * vehicleCurvature;
+        TimeFromStart = timeFromStart;
+        VehicleCurvature = vehicleCurvature;
+        SegmentIndex = segmentIndex;
+        SegmentLocalS = segmentLocalS;
+        PathS = pathS;
+        Direction = direction;
+        BoundaryType = boundaryType;
+        LongitudinalAcceleration = longitudinalAcceleration;
+        LongitudinalJerk = longitudinalJerk;
+    }
+
+    public double X { get; }
+    public double Y { get; }
+    public double Yaw { get; }
+    public double SignedLongitudinalVelocity { get; }
+    public double Speed { get; }
+    public double VelocityX { get; }
+    public double VelocityY { get; }
+    public double YawRate { get; }
+    public double TimeFromStart { get; }
+    public double VehicleCurvature { get; }
+    public int SegmentIndex { get; }
+    public double SegmentLocalS { get; }
+    public double PathS { get; }
+    public TravelDirection Direction { get; }
+    public EmBoundaryType BoundaryType { get; }
+
+    internal double LongitudinalAcceleration { get; }
+
+    internal double LongitudinalJerk { get; }
+}