132 lines
7.8 KiB
C#
132 lines
7.8 KiB
C#
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),
|
|
});
|
|
}
|
|
}
|