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,7 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Execution-layer source of an immutable caller-owned vehicle-state snapshot.</summary>
+public interface IVehicleStateProvider
+{
+    VehicleMotionState Capture();
+}
@@ -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);
+    }
+}
@@ -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; }
+}
@@ -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;
@@ -11,6 +11,8 @@ internal static class ExecutorChecks
        VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse, "forward-to-reverse");
        VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward, "reverse-to-forward");
        VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals();
+        VerifiesGenericControlCommandMapping();
+        VerifiesVehicleStateProviderRemainsSnapshotOnly();
    }

    private static void VerifiesGearSwitchSequence(TravelDirection currentDirection, TravelDirection desiredDirection,
@@ -98,6 +100,97 @@ internal static class ExecutorChecks
        }
    }

+    private static void VerifiesGenericControlCommandMapping()
+    {
+        DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(700d);
+        var adapter = new TrajectoryControlAdapter();
+
+        VerifiesFollowingCommand(adapter, effectiveAt, TravelDirection.Forward, 0.12d, 0.35d, 0.042d,
+            "forward command");
+        VerifiesFollowingCommand(adapter, effectiveAt, TravelDirection.Reverse, -0.12d, 0.35d, -0.042d,
+            "reverse command");
+
+        var executor = new TrajectoryExecutor();
+        EmTrajectory gearTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward,
+            EmBoundaryType.GearSwitchApproach, EmTerminalType.GearSwitch);
+        executor.Update(effectiveAt, CreateMeasuredState(0.10d, effectiveAt, 80L), gearTrajectory,
+            TravelDirection.Reverse, TravelDirection.Forward, false);
+        TrajectoryExecutionState holding = executor.Update(effectiveAt.AddSeconds(0.30d),
+            CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 81L), gearTrajectory,
+            TravelDirection.Reverse, TravelDirection.Forward, false);
+        TrajectoryControlCommand holdCommand = adapter.CreateCommand(holding.SelectedPoint, holding);
+        Verification.NearlyEqual(0d, holdCommand.SignedLongitudinalVelocity,
+            "holding command overrides signed longitudinal velocity");
+        Verification.NearlyEqual(0d, holdCommand.YawRate, "holding command overrides yaw rate");
+        Verification.True(holdCommand.HoldBrake && !holdCommand.IsTrajectoryComplete,
+            "gear holding command requests brake without completing trajectory");
+
+        TrajectoryExecutionState requesting = executor.Update(effectiveAt.AddSeconds(0.50d),
+            CreateMeasuredState(0d, effectiveAt.AddSeconds(0.50d), 82L), gearTrajectory,
+            TravelDirection.Reverse, TravelDirection.Forward, false);
+        TrajectoryControlCommand requestCommand = adapter.CreateCommand(requesting.SelectedPoint, requesting);
+        Verification.True(requestCommand.RequestDirectionChange && requestCommand.HoldBrake,
+            "direction request remains a generic zero-speed brake command");
+
+        EmTrajectory terminalTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward, EmBoundaryType.Goal,
+            EmTerminalType.Goal);
+        var terminalExecutor = new TrajectoryExecutor();
+        TrajectoryExecutionState completed = terminalExecutor.Update(effectiveAt.AddSeconds(0.30d),
+            CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 82L), terminalTrajectory,
+            TravelDirection.Forward, TravelDirection.Forward, false);
+        TrajectoryControlCommand completeCommand = adapter.CreateCommand(completed.SelectedPoint, completed);
+        Verification.NearlyEqual(0d, completeCommand.SignedLongitudinalVelocity,
+            "completed command keeps signed longitudinal velocity at zero");
+        Verification.NearlyEqual(0d, completeCommand.YawRate, "completed command keeps yaw rate at zero");
+        Verification.True(completeCommand.HoldBrake && completeCommand.IsTrajectoryComplete,
+            "completed command holds brake and reports completion");
+
+        Verification.True(typeof(TrajectoryControlCommand).GetProperty("BodyLateralVelocity") == null,
+            "generic command exposes no body lateral velocity");
+    }
+
+    private static void VerifiesFollowingCommand(TrajectoryControlAdapter adapter, DateTimeOffset effectiveAt,
+        TravelDirection direction, double signedVelocity, double curvature, double expectedYawRate, string name)
+    {
+        EmTrajectory trajectory = CreateMotionTrajectory(effectiveAt, direction, signedVelocity, curvature);
+        var executor = new TrajectoryExecutor();
+        VehicleMotionState measured = CreateMeasuredState(signedVelocity, effectiveAt, 83L);
+        TrajectoryExecutionState following = executor.Update(effectiveAt, measured, trajectory, direction, direction, false);
+        TrajectoryControlCommand command = adapter.CreateCommand(following.SelectedPoint, following);
+        TrajectoryControlCommand executorCommand = executor.UpdateCommand(effectiveAt, measured, trajectory, direction,
+            direction, false);
+
+        Verification.NearlyEqual(following.SelectedPoint.SignedLongitudinalVelocity, command.SignedLongitudinalVelocity,
+            name + " copies signed longitudinal velocity exactly");
+        Verification.NearlyEqual(following.SelectedPoint.YawRate, command.YawRate,
+            name + " copies yaw rate exactly");
+        Verification.NearlyEqual(expectedYawRate, command.YawRate, name + " preserves signed yaw rate");
+        Verification.Equal(direction, command.Direction, name + " preserves direction");
+        Verification.True(!command.HoldBrake && !command.RequestDirectionChange && !command.IsTrajectoryComplete,
+            name + " remains a normal following command");
+        Verification.NearlyEqual(command.SignedLongitudinalVelocity, executorCommand.SignedLongitudinalVelocity,
+            name + " executor command preserves signed longitudinal velocity");
+        Verification.NearlyEqual(command.YawRate, executorCommand.YawRate,
+            name + " executor command preserves yaw rate");
+        Verification.True(command.SignedLongitudinalVelocity != 0d || command.YawRate == 0d,
+            name + " never creates in-place rotation");
+        Verification.NearlyEqual(Math.Abs(signedVelocity), following.SelectedPoint.Speed,
+            name + " leaves speed available in execution telemetry");
+        Verification.NearlyEqual(signedVelocity * Math.Cos(following.SelectedPoint.Yaw), following.SelectedPoint.VelocityX,
+            name + " leaves world velocity X in execution telemetry");
+        Verification.NearlyEqual(signedVelocity * Math.Sin(following.SelectedPoint.Yaw), following.SelectedPoint.VelocityY,
+            name + " leaves world velocity Y in execution telemetry");
+        Verification.NearlyEqual(curvature, following.SelectedPoint.VehicleCurvature,
+            name + " leaves curvature available in execution telemetry");
+    }
+
+    private static void VerifiesVehicleStateProviderRemainsSnapshotOnly()
+    {
+        DateTimeOffset capturedAt = DateTimeOffset.UnixEpoch.AddSeconds(710d);
+        IVehicleStateProvider provider = new FixedVehicleStateProvider(CreateMeasuredState(0.02d, capturedAt, 84L));
+        Verification.Equal(84L, provider.Capture().SequenceId, "vehicle state provider returns caller-owned snapshot");
+    }
+
    private static void AssertHeld(GearSwitchStateUpdate update, GearSwitchState expectedState, string name)
    {
        Verification.Equal(expectedState, update.State, name + " state");
@@ -128,4 +221,33 @@ internal static class ExecutorChecks
                terminalBoundary, 0d, 0d),
        });
    }
+
+    private static EmTrajectory CreateMotionTrajectory(DateTimeOffset effectiveAt, TravelDirection direction,
+        double signedVelocity, double curvature)
+    {
+        var metadata = new EmTrajectoryMetadata("control-" + direction, effectiveAt, effectiveAt, 85L,
+            "control-reference", 84L, string.Empty, 5, direction, EmTerminalType.Goal);
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, signedVelocity, 0d, curvature, 5, 0d, 0d, direction,
+                EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, 0d, 0.30d, curvature, 5, 0.04d, 0.04d, direction,
+                EmBoundaryType.Goal, 0d, 0d),
+        });
+    }
+
+    private sealed class FixedVehicleStateProvider : IVehicleStateProvider
+    {
+        private readonly VehicleMotionState state;
+
+        public FixedVehicleStateProvider(VehicleMotionState state)
+        {
+            this.state = state;
+        }
+
+        public VehicleMotionState Capture()
+        {
+            return state;
+        }
+    }
 }