From de402e61ee4e89c7b2846ee87aea55f6999dc95e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 13:01:59 +0800 Subject: [PATCH] feat: execute EM gear-switch boundaries --- .../TrajectoryExecution/GearSwitchState.cs | 11 ++ .../GearSwitchStateMachine.cs | 144 ++++++++++++++++++ .../TrajectoryExecutionState.cs | 35 +++++ .../TrajectoryExecution/TrajectoryExecutor.cs | 66 ++++++++ .../ExecutorChecks.cs | 131 ++++++++++++++++ .../EMPlannerVerificationHost/Program.cs | 8 +- 6 files changed, 394 insertions(+), 1 deletion(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs new file mode 100644 index 0000000..caa87d7 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs @@ -0,0 +1,11 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum GearSwitchState +{ + Following, + ApproachingGearSwitch, + HoldingZero, + RequestingDirectionChange, + AwaitingDirectionConfirmation, + Completed, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs new file mode 100644 index 0000000..f8d5e53 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs @@ -0,0 +1,144 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Pure caller-clocked state machine for a zero-speed direction change. +public sealed class GearSwitchStateMachine +{ + private readonly double stopSpeedToleranceMetersPerSecond; + private readonly TimeSpan zeroSpeedHoldDuration; + private DateTimeOffset? zeroSpeedSince; + + public GearSwitchStateMachine(double stopSpeedToleranceMetersPerSecond, double zeroSpeedHoldSeconds) + { + if (!IsNonNegativeFinite(stopSpeedToleranceMetersPerSecond)) + throw new ArgumentOutOfRangeException(nameof(stopSpeedToleranceMetersPerSecond)); + if (!IsNonNegativeFinite(zeroSpeedHoldSeconds)) + throw new ArgumentOutOfRangeException(nameof(zeroSpeedHoldSeconds)); + + this.stopSpeedToleranceMetersPerSecond = stopSpeedToleranceMetersPerSecond; + zeroSpeedHoldDuration = TimeSpan.FromSeconds(zeroSpeedHoldSeconds); + } + + public GearSwitchState State { get; private set; } = GearSwitchState.Following; + + public GearSwitchStateUpdate Update(DateTimeOffset now, double measuredSignedSpeed, TravelDirection desiredDirection, + TravelDirection currentDirection, bool directionConfirmed, bool atGearSwitchBoundary, bool atTerminal) + { + if (double.IsNaN(measuredSignedSpeed) || double.IsInfinity(measuredSignedSpeed)) + throw new ArgumentOutOfRangeException(nameof(measuredSignedSpeed)); + if (!Enum.IsDefined(typeof(TravelDirection), desiredDirection) || + !Enum.IsDefined(typeof(TravelDirection), currentDirection)) + { + throw new ArgumentOutOfRangeException(nameof(desiredDirection)); + } + + if (atTerminal) + { + State = GearSwitchState.Completed; + zeroSpeedSince = null; + return Held("terminal boundary reached", false, true); + } + + switch (State) + { + case GearSwitchState.Following: + if (desiredDirection != currentDirection) + { + State = GearSwitchState.ApproachingGearSwitch; + return Moving("approaching gear-switch boundary"); + } + return Moving("following current direction segment"); + + case GearSwitchState.ApproachingGearSwitch: + if (!atGearSwitchBoundary) + return Moving("approaching gear-switch boundary"); + State = GearSwitchState.HoldingZero; + zeroSpeedSince = IsStopped(measuredSignedSpeed) ? now : null; + return Held("holding at gear-switch boundary", false, false); + + case GearSwitchState.HoldingZero: + if (!IsStopped(measuredSignedSpeed)) + { + zeroSpeedSince = null; + return Held("measured speed remains above stop tolerance", false, false); + } + if (!zeroSpeedSince.HasValue) + { + zeroSpeedSince = now; + return Held("zero-speed dwell started", false, false); + } + if (now - zeroSpeedSince.Value < zeroSpeedHoldDuration) + return Held("zero-speed dwell in progress", false, false); + + State = GearSwitchState.RequestingDirectionChange; + return Held("requesting direction change", true, false); + + case GearSwitchState.RequestingDirectionChange: + State = GearSwitchState.AwaitingDirectionConfirmation; + return Held("awaiting direction confirmation", false, false); + + case GearSwitchState.AwaitingDirectionConfirmation: + if (directionConfirmed && desiredDirection == currentDirection) + { + State = GearSwitchState.Following; + zeroSpeedSince = null; + return Moving("direction change confirmed"); + } + return Held("awaiting direction confirmation", false, false); + + case GearSwitchState.Completed: + return Held("trajectory already completed", false, true); + + default: + throw new InvalidOperationException("Unsupported gear-switch state."); + } + } + + private GearSwitchStateUpdate Moving(string reason) + { + return new GearSwitchStateUpdate(State, false, false, true, false, reason); + } + + private GearSwitchStateUpdate Held(string reason, bool requestDirectionChange, bool isTrajectoryComplete) + { + return new GearSwitchStateUpdate(State, true, requestDirectionChange, false, isTrajectoryComplete, reason); + } + + private bool IsStopped(double measuredSignedSpeed) + { + return Math.Abs(measuredSignedSpeed) < stopSpeedToleranceMetersPerSecond; + } + + private static bool IsNonNegativeFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d; + } +} + +public sealed class GearSwitchStateUpdate +{ + internal GearSwitchStateUpdate(GearSwitchState state, bool holdZero, bool requestDirectionChange, + bool allowsTrajectoryMotion, bool isTrajectoryComplete, string reason) + { + State = state; + HoldZero = holdZero; + RequestDirectionChange = requestDirectionChange; + AllowsTrajectoryMotion = allowsTrajectoryMotion; + IsTrajectoryComplete = isTrajectoryComplete; + Reason = reason ?? string.Empty; + } + + public GearSwitchState State { get; } + + public bool HoldZero { get; } + + public bool RequestDirectionChange { get; } + + public bool AllowsTrajectoryMotion { get; } + + public bool IsTrajectoryComplete { get; } + + public string Reason { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs new file mode 100644 index 0000000..d115fcb --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs @@ -0,0 +1,35 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable trajectory-execution outcome without controller or hardware coupling. +public sealed class TrajectoryExecutionState +{ + internal TrajectoryExecutionState(EmTrajectoryPoint selectedPoint, GearSwitchStateUpdate gearSwitchUpdate) + { + SelectedPoint = selectedPoint ?? throw new ArgumentNullException(nameof(selectedPoint)); + if (gearSwitchUpdate == null) + throw new ArgumentNullException(nameof(gearSwitchUpdate)); + + GearSwitchState = gearSwitchUpdate.State; + HoldZero = gearSwitchUpdate.HoldZero; + RequestDirectionChange = gearSwitchUpdate.RequestDirectionChange; + AllowsTrajectoryMotion = gearSwitchUpdate.AllowsTrajectoryMotion; + IsTrajectoryComplete = gearSwitchUpdate.IsTrajectoryComplete; + Reason = gearSwitchUpdate.Reason; + } + + public EmTrajectoryPoint SelectedPoint { get; } + + public GearSwitchState GearSwitchState { get; } + + public bool HoldZero { get; } + + public bool RequestDirectionChange { get; } + + public bool AllowsTrajectoryMotion { get; } + + public bool IsTrajectoryComplete { get; } + + public string Reason { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs new file mode 100644 index 0000000..49d6a08 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs @@ -0,0 +1,66 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Samples immutable EM trajectories and applies the pure zero-speed gear-switch state machine. +public sealed class TrajectoryExecutor +{ + private readonly TrajectorySampler sampler = new TrajectorySampler(); + private readonly GearSwitchStateMachine gearSwitchStateMachine; + + public TrajectoryExecutor() + : this(EmPlannerConfiguration.CreateDefault()) + { + } + + public TrajectoryExecutor(EmPlannerConfiguration configuration) + { + if (configuration?.Longitudinal == null) + throw new ArgumentNullException(nameof(configuration)); + gearSwitchStateMachine = new GearSwitchStateMachine( + configuration.Longitudinal.StopSpeedToleranceMetersPerSecond, + configuration.Longitudinal.ZeroSpeedHoldSeconds); + } + + public TrajectoryExecutionState State { get; private set; } + + public TrajectoryExecutionState Update(DateTimeOffset now, VehicleMotionState measuredState, EmTrajectory trajectory, + TravelDirection desiredDirection, TravelDirection currentDirection, bool directionConfirmed) + { + if (measuredState == null) + throw new ArgumentNullException(nameof(measuredState)); + if (trajectory == null) + throw new ArgumentNullException(nameof(trajectory)); + + EmTrajectoryPoint selectedPoint = SelectPoint(trajectory, now); + bool atGearSwitchBoundary = selectedPoint.BoundaryType == EmBoundaryType.GearSwitchApproach; + bool atTerminal = selectedPoint.BoundaryType == EmBoundaryType.Goal || + selectedPoint.BoundaryType == EmBoundaryType.RollingSafetyStop; + GearSwitchStateUpdate update = gearSwitchStateMachine.Update(now, + measuredState.SignedLongitudinalSpeedMetersPerSecond, desiredDirection, currentDirection, + directionConfirmed, atGearSwitchBoundary, atTerminal); + State = new TrajectoryExecutionState(selectedPoint, update); + return State; + } + + private EmTrajectoryPoint SelectPoint(EmTrajectory trajectory, DateTimeOffset now) + { + double timeFromStart = (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds; + EmTrajectoryPoint first = trajectory.Points[0]; + EmTrajectoryPoint last = trajectory.Points[trajectory.Points.Count - 1]; + if (timeFromStart <= first.TimeFromStart) + return first; + if (timeFromStart >= last.TimeFromStart) + return last; + if (sampler.TrySample(trajectory, timeFromStart, out EmTrajectoryPoint sampled)) + return sampled; + + for (int index = trajectory.Points.Count - 1; index >= 0; index--) + { + if (trajectory.Points[index].TimeFromStart <= timeFromStart) + return trajectory.Points[index]; + } + return first; + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs new file mode 100644 index 0000000..6a56ff0 --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs @@ -0,0 +1,131 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.EMPlanner; + +namespace EMPlannerVerificationHost; + +internal static class ExecutorChecks +{ + public static void Run() + { + VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse, "forward-to-reverse"); + VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward, "reverse-to-forward"); + VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals(); + } + + private static void VerifiesGearSwitchSequence(TravelDirection currentDirection, TravelDirection desiredDirection, + string name) + { + var machine = new GearSwitchStateMachine(0.01d, 0.20d); + DateTimeOffset start = DateTimeOffset.UnixEpoch.AddSeconds(500d); + + GearSwitchStateUpdate approaching = machine.Update(start, 0.10d, desiredDirection, currentDirection, false, false, + false); + Verification.Equal(GearSwitchState.ApproachingGearSwitch, approaching.State, name + " approaches switch"); + Verification.True(approaching.AllowsTrajectoryMotion, name + " approach permits trajectory motion"); + + GearSwitchStateUpdate holding = machine.Update(start.AddMilliseconds(100), 0.005d, desiredDirection, + currentDirection, false, true, false); + AssertHeld(holding, GearSwitchState.HoldingZero, name + " holds at boundary"); + + GearSwitchStateUpdate movingAgain = machine.Update(start.AddMilliseconds(110), 0.01d, desiredDirection, + currentDirection, false, true, false); + AssertHeld(movingAgain, GearSwitchState.HoldingZero, name + " holds while measured speed is at tolerance"); + + GearSwitchStateUpdate dwellRestart = machine.Update(start.AddMilliseconds(120), 0d, desiredDirection, + currentDirection, false, true, false); + AssertHeld(dwellRestart, GearSwitchState.HoldingZero, name + " starts zero-speed dwell"); + GearSwitchStateUpdate dwellShort = machine.Update(start.AddMilliseconds(319), 0d, desiredDirection, + currentDirection, false, true, false); + AssertHeld(dwellShort, GearSwitchState.HoldingZero, name + " does not request before full dwell"); + + GearSwitchStateUpdate requesting = machine.Update(start.AddMilliseconds(320), 0d, desiredDirection, + currentDirection, false, true, false); + AssertHeld(requesting, GearSwitchState.RequestingDirectionChange, name + " requests after continuous dwell"); + Verification.True(requesting.RequestDirectionChange, name + " emits exactly one direction request"); + + GearSwitchStateUpdate awaiting = machine.Update(start.AddMilliseconds(321), 0d, desiredDirection, + currentDirection, false, true, false); + AssertHeld(awaiting, GearSwitchState.AwaitingDirectionConfirmation, name + " awaits confirmation"); + Verification.True(!awaiting.RequestDirectionChange, name + " does not repeat direction request"); + + GearSwitchStateUpdate following = machine.Update(start.AddMilliseconds(322), 0d, desiredDirection, + desiredDirection, true, false, false); + Verification.Equal(GearSwitchState.Following, following.State, name + " follows confirmed next segment"); + Verification.True(!following.HoldZero && following.AllowsTrajectoryMotion, + name + " resumes only after confirmation"); + + GearSwitchStateUpdate completed = machine.Update(start.AddMilliseconds(323), 0d, desiredDirection, + desiredDirection, false, false, true); + AssertHeld(completed, GearSwitchState.Completed, name + " completes at terminal while holding zero"); + Verification.True(completed.IsTrajectoryComplete, name + " marks terminal completion"); + } + + private static void VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals() + { + DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(600d); + var executor = new TrajectoryExecutor(); + EmTrajectory gearTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward, + EmBoundaryType.GearSwitchApproach, EmTerminalType.GearSwitch); + + TrajectoryExecutionState approaching = executor.Update(effectiveAt.AddSeconds(0.10d), + CreateMeasuredState(0.10d, effectiveAt.AddSeconds(0.10d), 60L), gearTrajectory, TravelDirection.Reverse, + TravelDirection.Forward, false); + Verification.Equal(GearSwitchState.ApproachingGearSwitch, approaching.GearSwitchState, + "executor enters approach before exact gear boundary"); + Verification.NearlyEqual(0.10d, approaching.SelectedPoint.SignedLongitudinalVelocity, + "executor samples moving approach point"); + + TrajectoryExecutionState holding = executor.Update(effectiveAt.AddSeconds(0.30d), + CreateMeasuredState(0.005d, effectiveAt.AddSeconds(0.30d), 61L), gearTrajectory, TravelDirection.Reverse, + TravelDirection.Forward, false); + AssertHeld(holding, GearSwitchState.HoldingZero, "executor holds at exact gear boundary"); + Verification.Equal(EmBoundaryType.GearSwitchApproach, holding.SelectedPoint.BoundaryType, + "executor preserves exact gear boundary point"); + Verification.NearlyEqual(0d, holding.SelectedPoint.SignedLongitudinalVelocity, + "executor does not release nonzero speed while holding"); + + foreach (EmBoundaryType terminalBoundary in new[] { EmBoundaryType.Goal, EmBoundaryType.RollingSafetyStop }) + { + var terminalExecutor = new TrajectoryExecutor(); + EmTrajectory terminal = CreateTrajectory(effectiveAt, TravelDirection.Forward, terminalBoundary, + terminalBoundary == EmBoundaryType.Goal ? EmTerminalType.Goal : EmTerminalType.RollingSafetyStop); + TrajectoryExecutionState completed = terminalExecutor.Update(effectiveAt.AddSeconds(0.30d), + CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 62L), terminal, TravelDirection.Forward, + TravelDirection.Forward, false); + AssertHeld(completed, GearSwitchState.Completed, "executor completes " + terminalBoundary); + Verification.True(completed.IsTrajectoryComplete, "executor marks " + terminalBoundary + " completion"); + } + } + + private static void AssertHeld(GearSwitchStateUpdate update, GearSwitchState expectedState, string name) + { + Verification.Equal(expectedState, update.State, name + " state"); + Verification.True(update.HoldZero && !update.AllowsTrajectoryMotion, name + " forbids nonzero motion output"); + } + + private static void AssertHeld(TrajectoryExecutionState state, GearSwitchState expectedState, string name) + { + Verification.Equal(expectedState, state.GearSwitchState, name + " state"); + Verification.True(state.HoldZero && !state.AllowsTrajectoryMotion, name + " forbids nonzero motion output"); + } + + private static VehicleMotionState CreateMeasuredState(double speed, DateTimeOffset capturedAt, long sequenceId) + { + return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, 0d, capturedAt, sequenceId); + } + + private static EmTrajectory CreateTrajectory(DateTimeOffset effectiveAt, TravelDirection direction, + EmBoundaryType terminalBoundary, EmTerminalType terminalType) + { + double signedSpeed = direction == TravelDirection.Forward ? 0.10d : -0.10d; + var metadata = new EmTrajectoryMetadata("executor-" + terminalBoundary, effectiveAt, effectiveAt, 70L, + "executor-reference", 60L, string.Empty, 4, direction, terminalType); + return new EmTrajectory(metadata, new[] + { + new EmTrajectoryPoint(0d, 0d, 0d, signedSpeed, 0d, 0d, 4, 0d, 0d, direction, EmBoundaryType.None, 0d, 0d), + new EmTrajectoryPoint(signedSpeed * 3d, 0d, 0d, 0d, 0.30d, 0d, 4, 0.03d, 0.03d, direction, + terminalBoundary, 0d, 0d), + }); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 87e5061..9fc64cb 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -12,7 +12,8 @@ internal static class Program args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" && args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" && args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" && - args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator")) + args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator" && + args[0] != "executor")) { Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration"); return 2; @@ -113,6 +114,11 @@ internal static class Program CoordinatorChecks.Run(); Console.WriteLine("PASS coordinator"); } + if (args[0] == "executor") + { + ExecutorChecks.Run(); + Console.WriteLine("PASS executor"); + } return 0; } catch (Exception exception)