feat: adapt EM trajectories to control commands

2026-08-04 13:14:13 +08:00
parent 865bc2b428
commit aa51ae2d81
5 changed files with 205 additions and 0 deletions
@@ -0,0 +1,38 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable generic motion command derived from a validated trajectory execution state.</summary>
+public sealed class TrajectoryControlCommand
+{
+    public TrajectoryControlCommand(double signedLongitudinalVelocity, double yawRate, TravelDirection direction,
+        bool requestDirectionChange, bool holdBrake, bool isTrajectoryComplete)
+    {
+        if (double.IsNaN(signedLongitudinalVelocity) || double.IsInfinity(signedLongitudinalVelocity))
+            throw new ArgumentOutOfRangeException(nameof(signedLongitudinalVelocity));
+        if (double.IsNaN(yawRate) || double.IsInfinity(yawRate))
+            throw new ArgumentOutOfRangeException(nameof(yawRate));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+
+        SignedLongitudinalVelocity = signedLongitudinalVelocity;
+        YawRate = yawRate;
+        Direction = direction;
+        RequestDirectionChange = requestDirectionChange;
+        HoldBrake = holdBrake;
+        IsTrajectoryComplete = isTrajectoryComplete;
+    }
+
+    public double SignedLongitudinalVelocity { get; }
+
+    public double YawRate { get; }
+
+    public TravelDirection Direction { get; }
+
+    public bool RequestDirectionChange { get; }
+
+    public bool HoldBrake { get; }
+
+    public bool IsTrajectoryComplete { get; }
+}