feat: adapt EM trajectories to control commands

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;