feat: execute EM gear-switch boundaries
This commit is contained in:
梁薄云
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public enum GearSwitchState
|
||||
{
|
||||
Following,
|
||||
ApproachingGearSwitch,
|
||||
HoldingZero,
|
||||
RequestingDirectionChange,
|
||||
AwaitingDirectionConfirmation,
|
||||
Completed,
|
||||
}
|
||||
@@ -0,0 +1,144 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Pure caller-clocked state machine for a zero-speed direction change.</summary>
|
||||
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; }
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable trajectory-execution outcome without controller or hardware coupling.</summary>
|
||||
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; }
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Samples immutable EM trajectories and applies the pure zero-speed gear-switch state machine.</summary>
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -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),
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user