mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
e35ca772fd
Since there is no longer a requirement for Subsystems/Mechanisms to be registered to the command scheduler via a register() call, Mechanism can be changed to an interface instead to allow for more flexible inheritance structures. Specifically, this would allow compatibility with CTRE swerve (which previously required implementing Subsystem so that it could extend CTRE's base class).
735 lines
29 KiB
Java
735 lines
29 KiB
Java
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
package org.wpilib.command3;
|
|
|
|
import static org.junit.jupiter.api.Assertions.assertAll;
|
|
import static org.junit.jupiter.api.Assertions.assertEquals;
|
|
import static org.junit.jupiter.api.Assertions.assertFalse;
|
|
import static org.junit.jupiter.api.Assertions.assertThrows;
|
|
import static org.junit.jupiter.api.Assertions.assertTrue;
|
|
import static org.wpilib.command3.SchedulerEvent.Canceled;
|
|
import static org.wpilib.command3.SchedulerEvent.Mounted;
|
|
import static org.wpilib.command3.SchedulerEvent.Scheduled;
|
|
import static org.wpilib.command3.SchedulerEvent.Yielded;
|
|
|
|
import java.util.ArrayList;
|
|
import java.util.List;
|
|
import java.util.concurrent.atomic.AtomicBoolean;
|
|
import java.util.concurrent.atomic.AtomicInteger;
|
|
import org.junit.jupiter.api.Test;
|
|
import org.wpilib.annotation.PostConstructionInitializer;
|
|
|
|
@SuppressWarnings("PMD.CompareObjectsWithEquals")
|
|
class StateMachineTest extends CommandTestBase {
|
|
@Test
|
|
@SuppressWarnings(PostConstructionInitializer.SUPPRESSION_KEY)
|
|
void errorsWithoutInitialState() {
|
|
Mechanism mech = new DummyMechanism("Mechanism", m_scheduler);
|
|
Command command1 = mech.run(Coroutine::park).named("Command 1");
|
|
Command command2 = mech.run(Coroutine::park).named("Command 2");
|
|
|
|
StateMachine stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
// stateMachine.setInitialState(state1); // Oops, someone forgot to set the initial state!
|
|
state1.switchTo(state2).whenComplete();
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
|
|
// Don't worry, it'll be caught at runtime.
|
|
// It would actually be caught at compile time, but we disabled the compiler check for this test
|
|
var exception = assertThrows(IllegalStateException.class, () -> m_scheduler.run());
|
|
assertEquals(
|
|
"State Machine does not have an initial state. Use .setInitialState() to provide one.",
|
|
exception.getMessage());
|
|
assertFalse(m_scheduler.isRunning(stateMachine), "State machine should not be running");
|
|
}
|
|
|
|
@Test
|
|
void initialStateCanBeOverridden() {
|
|
Mechanism mech = new DummyMechanism("Mechanism", m_scheduler);
|
|
Command command1 = mech.run(Coroutine::park).named("Command 1");
|
|
Command command2 = mech.run(Coroutine::park).named("Command 2");
|
|
|
|
StateMachine stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
stateMachine.setInitialState(state1);
|
|
stateMachine.setInitialState(state2);
|
|
state2.switchTo(state1).whenComplete();
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertTrue(m_scheduler.isRunning(command2), "Command 2 should be running as the initial state");
|
|
assertFalse(m_scheduler.isRunning(command1), "Command 1 should not be running");
|
|
}
|
|
|
|
@Test
|
|
void transitions() {
|
|
AtomicBoolean signalA = new AtomicBoolean(false);
|
|
AtomicBoolean signalB = new AtomicBoolean(false);
|
|
|
|
Mechanism mech = new DummyMechanism("Mechanism", m_scheduler);
|
|
var command1 = mech.run(Coroutine::park).named("Command 1");
|
|
var command2 = mech.run(Coroutine::park).named("Command 2");
|
|
var command3 = mech.run(Coroutine::park).named("Command 3");
|
|
|
|
StateMachine stateMachine = new StateMachine("State Machine");
|
|
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
var state3 = stateMachine.addState(command3);
|
|
|
|
stateMachine.setInitialState(state1);
|
|
|
|
state1.switchTo(state2).when(signalA::get);
|
|
state2.switchTo(state3).when(signalB::get);
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertAll(
|
|
() -> assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running"),
|
|
() -> assertTrue(m_scheduler.isRunning(command1), "Command 1 should be running"),
|
|
() -> assertFalse(m_scheduler.isRunning(command2), "Command 2 should not be running"),
|
|
() -> assertFalse(m_scheduler.isRunning(command3), "Command 3 should not be running"));
|
|
|
|
signalA.set(true);
|
|
m_scheduler.run();
|
|
assertAll(
|
|
() -> assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running"),
|
|
() -> assertFalse(m_scheduler.isRunning(command1), "Command 1 should not be running"),
|
|
() -> assertTrue(m_scheduler.isRunning(command2), "Command 2 should be running"),
|
|
() -> assertFalse(m_scheduler.isRunning(command3), "Command 3 should not be running"));
|
|
|
|
signalB.set(true);
|
|
m_scheduler.run();
|
|
assertAll(
|
|
() -> assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running"),
|
|
() -> assertFalse(m_scheduler.isRunning(command1), "Command 1 should not be running"),
|
|
() -> assertFalse(m_scheduler.isRunning(command2), "Command 2 should not be running"),
|
|
() -> assertTrue(m_scheduler.isRunning(command3), "Command 3 should be running"));
|
|
}
|
|
|
|
@Test
|
|
void transitionsIfConditionIsAlreadyTrueWhenEntered() {
|
|
var command1 = Command.noRequirements(Coroutine::park).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
|
|
var signal = new AtomicBoolean(false);
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
stateMachine.setInitialState(state1);
|
|
state1.switchTo(state2).when(signal::get);
|
|
|
|
signal.set(true);
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertFalse(m_scheduler.isRunning(command1), "Command 1 should not be running");
|
|
assertTrue(m_scheduler.isRunning(command2), "State 1 should have transitioned to State 2");
|
|
}
|
|
|
|
@Test
|
|
void commandExits() {
|
|
AtomicBoolean signal = new AtomicBoolean(false);
|
|
|
|
var command1 = Command.noRequirements(co -> co.waitUntil(signal::get)).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
|
|
stateMachine.setInitialState(state1);
|
|
|
|
state1.switchTo(state2).whenComplete();
|
|
state2.exitStateMachine().whenComplete();
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertTrue(m_scheduler.isRunning(command1), "Command 1 should be running");
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertFalse(m_scheduler.isRunning(command1), "Command 1 should have ended");
|
|
assertTrue(m_scheduler.isRunning(command2), "Command 2 should have started");
|
|
}
|
|
|
|
@Test
|
|
void stateTransitionsToSelf() {
|
|
AtomicBoolean signal = new AtomicBoolean(false);
|
|
AtomicInteger initCount = new AtomicInteger(0);
|
|
|
|
var command =
|
|
Command.noRequirements(
|
|
co -> {
|
|
initCount.incrementAndGet();
|
|
co.park();
|
|
})
|
|
.named("Command");
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state = stateMachine.addState(command);
|
|
stateMachine.setInitialState(state);
|
|
state.switchTo(state).when(signal::get);
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertEquals(1, initCount.get(), "Command should be initialized once");
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should still be running");
|
|
assertEquals(2, initCount.get(), "Command should have reinitialized");
|
|
|
|
assertEquals(14, m_events.size());
|
|
assertAll(
|
|
// First run
|
|
() -> assertTrue(m_events.get(0) instanceof Scheduled s && s.command() == stateMachine),
|
|
() -> assertTrue(m_events.get(1) instanceof Mounted m && m.command() == stateMachine),
|
|
() -> assertTrue(m_events.get(2) instanceof Scheduled s && s.command() == command),
|
|
() -> assertTrue(m_events.get(3) instanceof Mounted m && m.command() == command),
|
|
() -> assertTrue(m_events.get(4) instanceof Yielded y && y.command() == command),
|
|
() -> assertTrue(m_events.get(5) instanceof Yielded y && y.command() == stateMachine),
|
|
() -> assertTrue(m_events.get(6) instanceof Mounted m && m.command() == command),
|
|
() -> assertTrue(m_events.get(7) instanceof Yielded y && y.command() == command),
|
|
// Second run
|
|
() -> assertTrue(m_events.get(8) instanceof Mounted m && m.command() == stateMachine),
|
|
() -> assertTrue(m_events.get(9) instanceof Canceled c && c.command() == command),
|
|
() -> assertTrue(m_events.get(10) instanceof Scheduled s && s.command() == command),
|
|
() -> assertTrue(m_events.get(11) instanceof Mounted m && m.command() == command),
|
|
() -> assertTrue(m_events.get(12) instanceof Yielded y && y.command() == command),
|
|
() -> assertTrue(m_events.get(13) instanceof Yielded y && y.command() == stateMachine));
|
|
}
|
|
|
|
@Test
|
|
void oneshotCommandTransitionsToSelfOnComplete() {
|
|
AtomicInteger count = new AtomicInteger(0);
|
|
var command = Command.noRequirements(c -> count.incrementAndGet()).named("Command");
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state = stateMachine.addState(command);
|
|
stateMachine.setInitialState(state);
|
|
state.switchTo(state).whenComplete();
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertEquals(1, count.get(), "Command should have run once");
|
|
}
|
|
|
|
@Test
|
|
void onlyFirstExplicitTransitionFires() {
|
|
var signal = new AtomicBoolean(false);
|
|
|
|
var command1 = Command.noRequirements(Coroutine::park).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
var command3 = Command.noRequirements(Coroutine::park).named("Command 3");
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
var state3 = stateMachine.addState(command3);
|
|
|
|
stateMachine.setInitialState(state1);
|
|
state1.switchTo(state2).when(signal::get);
|
|
state1.switchTo(state3).when(signal::get);
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command1), m_scheduler.getRunningCommands());
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command2), m_scheduler.getRunningCommands());
|
|
}
|
|
|
|
@Test
|
|
void onlyLastWhenCompleteTransitionFires() {
|
|
var command1 = Command.noRequirements(Coroutine::yield).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::yield).named("Command 2");
|
|
var command3 = Command.noRequirements(Coroutine::yield).named("Command 3");
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
var state3 = stateMachine.addState(command3);
|
|
|
|
stateMachine.setInitialState(state1);
|
|
state1.switchTo(state2).whenComplete();
|
|
state1.switchTo(state3).whenComplete(); // overrides the previous transition
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command1), m_scheduler.getRunningCommands());
|
|
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command3), m_scheduler.getRunningCommands());
|
|
}
|
|
|
|
@Test
|
|
void whenCompleteAndTakesPriorityOverWhenCompleteIfCalledLast() {
|
|
var signal = new AtomicBoolean(false);
|
|
|
|
var command1 = Command.noRequirements(Coroutine::yield).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::yield).named("Command 2");
|
|
var command3 = Command.noRequirements(Coroutine::yield).named("Command 3");
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
var state3 = stateMachine.addState(command3);
|
|
|
|
stateMachine.setInitialState(state1);
|
|
state1.switchTo(state2).whenComplete();
|
|
state1.switchTo(state3).whenCompleteAnd(signal::get);
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command1), m_scheduler.getRunningCommands());
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertEquals(
|
|
List.of(stateMachine, command3), // would be command2 if `whenComplete` took precedence
|
|
m_scheduler.getRunningCommands());
|
|
}
|
|
|
|
@Test
|
|
void whenCompleteAndTakesPriorityOverWhenCompleteIfCalleFirst() {
|
|
var signal = new AtomicBoolean(false);
|
|
|
|
var command1 = Command.noRequirements(Coroutine::yield).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::yield).named("Command 2");
|
|
var command3 = Command.noRequirements(Coroutine::yield).named("Command 3");
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
var state3 = stateMachine.addState(command3);
|
|
|
|
stateMachine.setInitialState(state1);
|
|
state1.switchTo(state3).whenCompleteAnd(signal::get);
|
|
state1.switchTo(state2).whenComplete();
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command1), m_scheduler.getRunningCommands());
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertEquals(
|
|
List.of(stateMachine, command3), // would be command3 if `whenCompleteAnd` took precedence
|
|
m_scheduler.getRunningCommands());
|
|
}
|
|
|
|
@Test
|
|
void composingComplete() {
|
|
AtomicBoolean signal = new AtomicBoolean(false);
|
|
var command1 = Command.noRequirements(Coroutine::yield).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
|
|
stateMachine.setInitialState(state1);
|
|
state1.exitStateMachine().whenComplete();
|
|
state1.switchTo(state2).whenCompleteAnd(signal::get);
|
|
|
|
// First run, signal is low - state machine exits on state completion
|
|
{
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertTrue(m_scheduler.isRunning(command1), "Command should be running");
|
|
|
|
m_scheduler.run();
|
|
assertFalse(m_scheduler.isRunning(stateMachine), "State machine should have exited");
|
|
}
|
|
|
|
// Second run, signal goes high - state machine switches to state2 instead of exiting
|
|
{
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertTrue(m_scheduler.isRunning(command1), "Command should be running");
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertFalse(m_scheduler.isRunning(command1), "Command should have ended");
|
|
assertTrue(m_scheduler.isRunning(command2), "Command 2 should have started");
|
|
}
|
|
}
|
|
|
|
@Test
|
|
void switchFromAny() {
|
|
var command1 = Command.noRequirements(Coroutine::yield).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
var command3 = Command.noRequirements(Coroutine::park).named("Command 3");
|
|
|
|
AtomicBoolean signal = new AtomicBoolean(false);
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
var state3 = stateMachine.addState(command3);
|
|
|
|
stateMachine.setInitialState(state1);
|
|
stateMachine.switchFromAny(state1, state2).to(state3).when(signal::get);
|
|
state1.switchTo(state2).whenComplete();
|
|
|
|
// transition from 1 -> 3
|
|
{
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertTrue(m_scheduler.isRunning(command1), "Command 1 should be running");
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertFalse(m_scheduler.isRunning(command1), "Command 1 should have ended");
|
|
assertTrue(m_scheduler.isRunning(command3), "Command 3 should have started");
|
|
}
|
|
|
|
m_scheduler.cancel(stateMachine);
|
|
signal.set(false);
|
|
|
|
// transition from 2 -> 3
|
|
{
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run(); // yield 1
|
|
assertEquals(
|
|
List.of("State Machine", "Command 1"),
|
|
m_scheduler.getRunningCommands().stream().map(Command::name).toList());
|
|
|
|
m_scheduler.run(); // transition 1 -> 2
|
|
assertEquals(
|
|
List.of("State Machine", "Command 2"),
|
|
m_scheduler.getRunningCommands().stream().map(Command::name).toList());
|
|
|
|
signal.set(true);
|
|
m_scheduler.run(); // transition 2 -> 3
|
|
assertEquals(
|
|
List.of("State Machine", "Command 3"),
|
|
m_scheduler.getRunningCommands().stream().map(Command::name).toList());
|
|
}
|
|
}
|
|
|
|
@Test
|
|
void switchToSupplierWhenComplete() {
|
|
AtomicInteger count = new AtomicInteger(0);
|
|
|
|
var command1 =
|
|
Command.noRequirements(
|
|
co -> {
|
|
count.incrementAndGet();
|
|
co.yield();
|
|
})
|
|
.named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
var command3 = Command.noRequirements(Coroutine::park).named("Command 3");
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
var state3 = stateMachine.addState(command3);
|
|
stateMachine.setInitialState(state1);
|
|
state1
|
|
.switchTo(
|
|
() -> {
|
|
if (count.get() == 1) {
|
|
return state2;
|
|
} else {
|
|
return state3;
|
|
}
|
|
})
|
|
.whenComplete();
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run(); // command 1 increments the count and then yields
|
|
assertEquals(List.of(stateMachine, command1), m_scheduler.getRunningCommands());
|
|
|
|
// command 1 completes, state machine moves to the next state
|
|
// if the supplier is checked at configuration time, the count would be 0 and return state3
|
|
// if the supplier is checked at runtime, the count would be 1 and return state2
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command2), m_scheduler.getRunningCommands());
|
|
}
|
|
|
|
@Test
|
|
void switchToSupplierWithCondition() {
|
|
AtomicInteger count = new AtomicInteger(0);
|
|
var command1 =
|
|
Command.noRequirements(
|
|
co -> {
|
|
while (true) {
|
|
// Increment after yielding. Otherwise, the condition is checked and the state
|
|
// machine immediately switches to the next state all within the first cycle;
|
|
// the running command1 is never observed.
|
|
co.yield();
|
|
count.incrementAndGet();
|
|
}
|
|
})
|
|
.named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
var command3 = Command.noRequirements(Coroutine::park).named("Command 3");
|
|
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
var state3 = stateMachine.addState(command3);
|
|
stateMachine.setInitialState(state1);
|
|
state1
|
|
.switchTo(
|
|
() -> {
|
|
if (count.get() == 1) {
|
|
return state2;
|
|
} else {
|
|
return state3;
|
|
}
|
|
})
|
|
.when(() -> count.get() == 1);
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command1), m_scheduler.getRunningCommands());
|
|
|
|
m_scheduler.run();
|
|
assertEquals(List.of(stateMachine, command2), m_scheduler.getRunningCommands());
|
|
}
|
|
|
|
@Test
|
|
void runsOnEnterForInitialState() {
|
|
var command1 = Command.noRequirements(Coroutine::park).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
|
|
AtomicInteger enterCount = new AtomicInteger(0);
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
stateMachine.setInitialState(state1);
|
|
state1.onEnter(enterCount::incrementAndGet);
|
|
state1.switchTo(state2).whenComplete();
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(1, enterCount.get(), "onEnter should have been called once");
|
|
}
|
|
|
|
@Test
|
|
void runsOnExitOnTransition() {
|
|
var command1 = Command.noRequirements(Coroutine::park).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
|
|
AtomicInteger exitCount = new AtomicInteger(0);
|
|
AtomicBoolean signal = new AtomicBoolean(false);
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
stateMachine.setInitialState(state1);
|
|
state1.onExit(exitCount::incrementAndGet);
|
|
state1.switchTo(state2).when(signal::get);
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(0, exitCount.get(), "onExit should not have been called");
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertEquals(1, exitCount.get(), "onExit should have been called");
|
|
}
|
|
|
|
@Test
|
|
void runsOnExitWhenComplete() {
|
|
var command1 = Command.noRequirements(co -> {}).named("Command 1");
|
|
|
|
AtomicInteger exitCount = new AtomicInteger(0);
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
stateMachine.setInitialState(state1);
|
|
state1.onExit(exitCount::incrementAndGet);
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(1, exitCount.get(), "onExit should have been called");
|
|
assertFalse(m_scheduler.isRunning(command1), "State should have exited");
|
|
}
|
|
|
|
@Test
|
|
void onExitCanSchedule() {
|
|
var mech = new DummyMechanism("Mechanism", m_scheduler);
|
|
var mainMechCommand = mech.run(Coroutine::park).named("Main Mech Command");
|
|
var backgroundMechCommand = mech.run(Coroutine::park).named("Background Mech Command");
|
|
var nextStateCommand = Command.noRequirements(Coroutine::park).named("Next");
|
|
|
|
AtomicBoolean signal = new AtomicBoolean(false);
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(mainMechCommand);
|
|
var state2 = stateMachine.addState(nextStateCommand);
|
|
stateMachine.setInitialState(state1);
|
|
state1.switchTo(state2).when(signal::get);
|
|
state1.onExit(() -> m_scheduler.schedule(backgroundMechCommand));
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertTrue(m_scheduler.isRunning(mainMechCommand), "Main Mechanism should be running");
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertTrue(m_scheduler.isRunning(stateMachine), "State machine should be running");
|
|
assertFalse(m_scheduler.isRunning(mainMechCommand), "Main Mechanism should have ended");
|
|
assertTrue(
|
|
m_scheduler.isRunning(backgroundMechCommand), "Background Mechanism should have started");
|
|
assertTrue(m_scheduler.isRunning(nextStateCommand), "Next State should have started");
|
|
}
|
|
|
|
@Test
|
|
void runsOnEnterCallbacksInInsertionOrder() {
|
|
var command1 = Command.noRequirements(co -> {}).named("Command 1");
|
|
|
|
List<String> callbackInfo = new ArrayList<>();
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
stateMachine.setInitialState(state1);
|
|
state1.onEnter(() -> callbackInfo.add("onEnter 1"));
|
|
state1.onEnter(() -> callbackInfo.add("onEnter 2"));
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(
|
|
List.of("onEnter 1", "onEnter 2"), callbackInfo, "onEnter callbacks did not run correctly");
|
|
}
|
|
|
|
@Test
|
|
void runsOnExitCallbacksInInsertionOrder() {
|
|
// Make the command immediately exit
|
|
var command1 = Command.noRequirements(co -> {}).named("Command 1");
|
|
|
|
List<String> callbackInfo = new ArrayList<>();
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
stateMachine.setInitialState(state1);
|
|
state1.onExit(() -> callbackInfo.add("onExit 1"));
|
|
state1.onExit(() -> callbackInfo.add("onExit 2"));
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertEquals(
|
|
List.of("onExit 1", "onExit 2"), callbackInfo, "onExit callbacks did not run correctly");
|
|
}
|
|
|
|
@Test
|
|
void onEnterSeesNewCommand() {
|
|
var command1 = Command.noRequirements(Coroutine::park).named("Command 1");
|
|
|
|
AtomicBoolean sawCommand1OnEnter = new AtomicBoolean(false);
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
stateMachine.setInitialState(state1);
|
|
state1.onEnter(() -> sawCommand1OnEnter.set(m_scheduler.isRunning(command1)));
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
assertTrue(sawCommand1OnEnter.get(), "onEnter should have seen the command running");
|
|
}
|
|
|
|
@Test
|
|
void onExitWithTransitionSeesExitedCommand() {
|
|
var command1 = Command.noRequirements(Coroutine::park).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
|
|
AtomicBoolean sawCommand1OnExit = new AtomicBoolean(false);
|
|
AtomicBoolean signal = new AtomicBoolean(false);
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
stateMachine.setInitialState(state1);
|
|
state1.onExit(() -> sawCommand1OnExit.set(m_scheduler.isRunning(command1)));
|
|
state1.switchTo(state2).when(signal::get);
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run();
|
|
|
|
signal.set(true);
|
|
m_scheduler.run();
|
|
assertTrue(sawCommand1OnExit.get(), "onExit should have seen the exiting command");
|
|
}
|
|
|
|
// Because completion is defined as the command finishing on its own, callbacks will never
|
|
// be able to see the command running in the scheduler because they're invoked _after_ the
|
|
// command has finished.
|
|
@Test
|
|
void onExitWithCompleteCannotSeeExitedCommand() {
|
|
var command1 = Command.noRequirements(Coroutine::yield).named("Command 1");
|
|
var command2 = Command.noRequirements(Coroutine::park).named("Command 2");
|
|
|
|
AtomicBoolean onExitCalled = new AtomicBoolean(false);
|
|
AtomicBoolean sawCommand1OnExit = new AtomicBoolean(false);
|
|
var stateMachine = new StateMachine("State Machine");
|
|
var state1 = stateMachine.addState(command1);
|
|
var state2 = stateMachine.addState(command2);
|
|
stateMachine.setInitialState(state1);
|
|
state1.onExit(
|
|
() -> {
|
|
onExitCalled.set(true);
|
|
sawCommand1OnExit.set(m_scheduler.isRunning(command1));
|
|
});
|
|
state1.switchTo(state2).whenComplete();
|
|
|
|
m_scheduler.schedule(stateMachine);
|
|
m_scheduler.run(); // command yields...
|
|
assertFalse(onExitCalled.get(), "onExit should not have been called yet");
|
|
|
|
m_scheduler.run(); // ...then exits here
|
|
assertTrue(onExitCalled.get(), "onExit should have been called");
|
|
assertFalse(sawCommand1OnExit.get(), "exiting command should be invisible");
|
|
}
|
|
|
|
@Test
|
|
void ledStateMachine() {
|
|
var leds =
|
|
new DummyMechanism("LEDs", m_scheduler) {
|
|
Command idleAnimation() {
|
|
return run(Coroutine::park).withPriority(-1).named("Default Animation");
|
|
}
|
|
|
|
Command infoAnimation() {
|
|
return run(Coroutine::yield).withPriority(0).named("Info");
|
|
}
|
|
|
|
Command warningAnimation() {
|
|
return run(Coroutine::yield).withPriority(1).named("Warning");
|
|
}
|
|
};
|
|
|
|
Trigger normalPriorityEvent = new Trigger(() -> true);
|
|
Trigger highPriorityEvent = new Trigger(() -> true);
|
|
|
|
StateMachine stateMachine = new StateMachine("State Machine");
|
|
|
|
var idleState = stateMachine.addState(leds.idleAnimation());
|
|
var infoState = stateMachine.addState(leds.infoAnimation());
|
|
var warningState = stateMachine.addState(leds.warningAnimation());
|
|
|
|
stateMachine.setInitialState(idleState);
|
|
|
|
idleState.switchTo(infoState).when(normalPriorityEvent.and(highPriorityEvent.negate()));
|
|
idleState.switchTo(warningState).when(highPriorityEvent);
|
|
|
|
warningState.switchTo(infoState).whenCompleteAnd(normalPriorityEvent);
|
|
infoState.switchTo(warningState).whenCompleteAnd(highPriorityEvent);
|
|
|
|
stateMachine.switchFromAny().to(warningState).when(highPriorityEvent);
|
|
stateMachine.switchFromAny().to(idleState).whenComplete();
|
|
}
|
|
}
|