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,42 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class VehicleMotionState
+{
+    public VehicleMotionState(
+        Pose2D pose,
+        double signedLongitudinalSpeedMetersPerSecond,
+        double? longitudinalAccelerationMetersPerSecondSquared,
+        DateTimeOffset capturedAtUtc,
+        long sequenceId)
+    {
+        if (pose == null)
+            throw new ArgumentNullException(nameof(pose));
+        ContractNumeric.RequireFinite(pose.X, nameof(pose));
+        ContractNumeric.RequireFinite(pose.Y, nameof(pose));
+        ContractNumeric.RequireFinite(pose.Heading, nameof(pose));
+        ContractNumeric.RequireFinite(signedLongitudinalSpeedMetersPerSecond, nameof(signedLongitudinalSpeedMetersPerSecond));
+        if (longitudinalAccelerationMetersPerSecondSquared.HasValue)
+            ContractNumeric.RequireFinite(longitudinalAccelerationMetersPerSecondSquared.Value, nameof(longitudinalAccelerationMetersPerSecondSquared));
+        if (sequenceId < 0)
+            throw new ArgumentOutOfRangeException(nameof(sequenceId));
+
+        Pose = pose;
+        SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond;
+        LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared;
+        CapturedAtUtc = capturedAtUtc;
+        SequenceId = sequenceId;
+    }
+
+    public Pose2D Pose { get; }
+
+    public double SignedLongitudinalSpeedMetersPerSecond { get; }
+
+    public double? LongitudinalAccelerationMetersPerSecondSquared { get; }
+
+    public DateTimeOffset CapturedAtUtc { get; }
+
+    public long SequenceId { get; }
+}