diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs new file mode 100644 index 0000000..6b9276b --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs @@ -0,0 +1,7 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Execution-layer source of an immutable caller-owned vehicle-state snapshot. +public interface IVehicleStateProvider +{ + VehicleMotionState Capture(); +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs new file mode 100644 index 0000000..117378b --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs @@ -0,0 +1,27 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Converts immutable execution state into a controller-neutral longitudinal and yaw command. +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); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs new file mode 100644 index 0000000..8b29132 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs @@ -0,0 +1,38 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable generic motion command derived from a validated trajectory execution state. +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; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs index 49d6a08..bfed9af 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs +++ b/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; } + /// Updates pure execution state and returns its controller-neutral motion command. + 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; diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs index 6a56ff0..1b47221 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs @@ -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; + } + } }