feat: adapt EM trajectories to control commands

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs

@@ -0,0 +1,7 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Execution-layer source of an immutable caller-owned vehicle-state snapshot.</summary>
+public interface IVehicleStateProvider
+{
+    VehicleMotionState Capture();
+}

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs

@@ -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);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs

@@ -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; }
+}

ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs

@@ -7,6 +7,7 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 public sealed class TrajectoryExecutor
 {
     private readonly TrajectorySampler sampler = new TrajectorySampler();
+    private readonly TrajectoryControlAdapter controlAdapter = new TrajectoryControlAdapter();
     private readonly GearSwitchStateMachine gearSwitchStateMachine;
 
     public TrajectoryExecutor()
@@ -44,6 +45,16 @@ public sealed class TrajectoryExecutor
         return State;
     }
 
+    /// <summary>Updates pure execution state and returns its controller-neutral motion command.</summary>
+    public TrajectoryControlCommand UpdateCommand(DateTimeOffset now, VehicleMotionState measuredState,
+        EmTrajectory trajectory, TravelDirection desiredDirection, TravelDirection currentDirection,
+        bool directionConfirmed)
+    {
+        TrajectoryExecutionState state = Update(now, measuredState, trajectory, desiredDirection, currentDirection,
+            directionConfirmed);
+        return controlAdapter.CreateCommand(state.SelectedPoint, state);
+    }
+
     private EmTrajectoryPoint SelectPoint(EmTrajectory trajectory, DateTimeOffset now)
     {
         double timeFromStart = (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds;