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,27 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Converts immutable execution state into a controller-neutral longitudinal and yaw command.</summary>
+public sealed class TrajectoryControlAdapter
+{
+    public TrajectoryControlCommand CreateCommand(EmTrajectoryPoint trajectoryPoint,
+        TrajectoryExecutionState executionState)
+    {
+        if (trajectoryPoint == null)
+            throw new ArgumentNullException(nameof(trajectoryPoint));
+        if (executionState == null)
+            throw new ArgumentNullException(nameof(executionState));
+
+        bool holdBrake = executionState.HoldZero || executionState.IsTrajectoryComplete ||
+            !executionState.AllowsTrajectoryMotion;
+        if (holdBrake)
+        {
+            return new TrajectoryControlCommand(0d, 0d, trajectoryPoint.Direction,
+                executionState.RequestDirectionChange, true, executionState.IsTrajectoryComplete);
+        }
+
+        return new TrajectoryControlCommand(trajectoryPoint.SignedLongitudinalVelocity, trajectoryPoint.YawRate,
+            trajectoryPoint.Direction, false, false, false);
+    }
+}