From 20691123fd0b90cf69a2dacf2501838beaefee00 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Tue, 1 Oct 2024 04:54:00 -0700 Subject: [PATCH] Revisions to Status system --- .vscode/settings.json | 2 + .../com/team766/framework3/Conditions.java | 127 +++++++-- .../com/team766/framework3/Mechanism.java | 2 +- .../java/com/team766/framework3/Request.java | 7 +- .../com/team766/framework3/StatusBus.java | 120 +++++++-- .../team766/framework3/Superstructure.java | 2 +- .../java/com/team766/hal/mock/MockClock.java | 31 --- .../team766/hal/mock/TestRobotProvider.java | 9 +- src/test/java/com/team766/TestCase.java | 3 +- src/test/java/com/team766/TestCase3.java | 6 +- .../team766/framework/TimedPredicateTest.java | 6 +- .../team766/framework3/ConditionsTest.java | 241 +++++++++++++++++- .../com/team766/framework3/FakeMechanism.java | 8 +- .../com/team766/framework3/StatusBusTest.java | 104 ++++++++ src/test/java/com/team766/hal/TestClock.java | 58 +++++ 15 files changed, 625 insertions(+), 101 deletions(-) delete mode 100644 src/main/java/com/team766/hal/mock/MockClock.java create mode 100644 src/test/java/com/team766/framework3/StatusBusTest.java create mode 100644 src/test/java/com/team766/hal/TestClock.java diff --git a/.vscode/settings.json b/.vscode/settings.json index f25587d5b..c07977ca2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -35,7 +35,9 @@ "java.checkstyle.configuration": "${workspaceFolder}/.github/linters/checkstyle.xml", "java.checkstyle.version": "9.3", "java.completion.favoriteStaticMembers": [ + "com.team766.framework3.Conditions.*", "com.team766.framework3.RulePersistence.*", + "com.team766.framework3.StatusBus.*", "org.junit.Assert.*", "org.junit.Assume.*", "org.junit.jupiter.api.Assertions.*", diff --git a/src/main/java/com/team766/framework3/Conditions.java b/src/main/java/com/team766/framework3/Conditions.java index 923023d0b..6b4d96446 100644 --- a/src/main/java/com/team766/framework3/Conditions.java +++ b/src/main/java/com/team766/framework3/Conditions.java @@ -1,47 +1,126 @@ package com.team766.framework3; +import java.util.Optional; +import java.util.concurrent.atomic.AtomicReference; import java.util.function.BooleanSupplier; +import java.util.function.Predicate; +import java.util.function.Supplier; /** * Pre-canned Conditions used frqeuently in robot programming. */ public class Conditions { + /** + * Suspend the Procedure until the given Supplier returns a non-empty Optional. Then, unwrap + * the T value from the Optional and return it. + */ + public static T waitForValue(Context context, Supplier> supplier) { + final AtomicReference result = new AtomicReference<>(); + context.waitFor( + () -> { + final var t = supplier.get(); + t.ifPresent(result::set); + return t.isPresent(); + }); + return result.get(); + } /** - * Predicate that checks whether or not the latest {@link Status} passes the - * check provided by {@link Checker}. + * Suspend the Procedure until the given Supplier returns a non-empty Optional, or we've waited + * for at least {@code timeoutSeconds}. Returns the last value returned by the Supplier. */ - public static class StatusCheck implements BooleanSupplier { + public static Optional waitForValueOrTimeout( + Context context, Supplier> supplier, double timeoutSeconds) { + final AtomicReference> result = new AtomicReference<>(Optional.empty()); + context.waitForConditionOrTimeout( + () -> { + result.set(supplier.get()); + return result.get().isPresent(); + }, + timeoutSeconds); + return result.get(); + } - /** Functional interface for checking whether or not a {@link Status} passes desired criteria. */ - @FunctionalInterface - public interface Checker { - boolean check(S status); - } + /** + * Predicate that checks whether or not a {@link Status} with the given class has been published + */ + public static boolean checkForStatus(Class statusClass) { + return StatusBus.getStatusEntry(statusClass).isPresent(); + } - private final Class clazz; - private final Checker checker; + /** + * Predicate that checks whether or not the latest {@link Status} with the given class passes + * the check provided by {@code predicate}. + */ + public static boolean checkForStatusWith( + Class statusClass, Predicate predicate) { + return StatusBus.getStatusValue(statusClass, predicate::test).orElse(false); + } - public StatusCheck(Class clazz, Checker checker) { - this.clazz = clazz; - this.checker = checker; - } + /** + * Predicate that checks whether or not the latest {@link Status} with the given class passes + * the check provided by {@code predicate}, including additional metadata about how the Status + * was published. + */ + public static boolean checkForStatusEntryWith( + Class statusClass, Predicate> predicate) { + return StatusBus.getStatusEntry(statusClass).map(predicate::test).orElse(false); + } - public boolean getAsBoolean() { - S status = StatusBus.getInstance().getStatus(clazz); - return checker.check(status); - } + /** + * Suspend the Procedure until a {@link Status} with the given class has been published, then + * return that Status. + */ + public static T waitForStatus(Context context, Class statusClass) { + return waitForValue(context, () -> StatusBus.getStatus(statusClass)); } /** - * Predicate that checks if the provided {@link Request} has been fulfilled by checking - * the latest {@link Status}. + * Suspend the Procedure until a {@link Status} with the given class has been published, or + * we've waited for at least {@code timeoutSeconds}. Returns an Optional containing the Status + * if one was published, or return an empty Optional if the timeout was reached. */ - public static class AwaitRequest extends StatusCheck { + public static Optional waitForStatusOrTimeout( + Context context, Class statusClass, double timeoutSeconds) { + return waitForValueOrTimeout( + context, () -> StatusBus.getStatus(statusClass), timeoutSeconds); + } - public AwaitRequest(Class clazz, Request request) { - super(clazz, request::isDone); - } + /** + * Suspend the Procedure until a {@link Status} with the given class has been published that + * passes the check provided by {@code predicate}, then return that Status. + */ + public static T waitForStatusWith( + Context context, Class statusClass, Predicate predicate) { + return waitForValue(context, () -> StatusBus.getStatus(statusClass).filter(predicate)); + } + + /** + * Suspend the Procedure until a {@link Status} with the given class has been published that + * passes the check provided by {@code predicate}, or we've waited for at least + * {@code timeoutSeconds}. Returns an Optional containing the Status if one was published, or + * return an empty Optional if the timeout was reached. + */ + public static Optional waitForStatusWithOrTimeout( + Context context, Class statusClass, Predicate predicate, double timeoutSeconds) { + return waitForValueOrTimeout( + context, () -> StatusBus.getStatus(statusClass).filter(predicate), timeoutSeconds); + } + + /** + * Suspend the Procedure until {@link Request#isDone} returns true. + */ + public static void waitForRequest(Context context, Request request) { + context.waitFor(request::isDone); + } + + /** + * Suspend the Procedure until {@link Request#isDone} returns true, or we've waited for at least + * {@code timeoutSeconds}. Returns true if the Request is done; false otherwise. + */ + public static boolean waitForRequestOrTimeout( + Context context, Request request, double timeoutSeconds) { + return context.waitForConditionOrTimeout(request::isDone, timeoutSeconds); } /** diff --git a/src/main/java/com/team766/framework3/Mechanism.java b/src/main/java/com/team766/framework3/Mechanism.java index d35c9895b..380e87aca 100644 --- a/src/main/java/com/team766/framework3/Mechanism.java +++ b/src/main/java/com/team766/framework3/Mechanism.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.Objects; -public abstract class Mechanism> extends SubsystemBase implements LoggingBase { +public abstract class Mechanism extends SubsystemBase implements LoggingBase { private boolean isRunningPeriodic = false; private Superstructure superstructure = null; diff --git a/src/main/java/com/team766/framework3/Request.java b/src/main/java/com/team766/framework3/Request.java index 3ed39b085..1b2f777d7 100644 --- a/src/main/java/com/team766/framework3/Request.java +++ b/src/main/java/com/team766/framework3/Request.java @@ -9,13 +9,12 @@ * * Each Mechanism will have its own implementation of the {@link Request} marker interface. */ -public interface Request { +public interface Request { /** - * Checks whether or not this request has been fulfilled, via the supplied {@link Status}. This - * {@link Status} should be the latest one retrieved via {@link StatusBus#getStatus(Class)}. + * Checks whether or not this request has been fulfilled. */ - boolean isDone(S status); + boolean isDone(); // TODO(MF3): do we need any way of checking if the request has been bumped/canceled? } diff --git a/src/main/java/com/team766/framework3/StatusBus.java b/src/main/java/com/team766/framework3/StatusBus.java index 9bf9ae779..73e71a23c 100644 --- a/src/main/java/com/team766/framework3/StatusBus.java +++ b/src/main/java/com/team766/framework3/StatusBus.java @@ -1,61 +1,125 @@ package com.team766.framework3; +import com.team766.hal.RobotProvider; import com.team766.logging.Category; +import com.team766.logging.Logger; +import com.team766.logging.Severity; import java.util.LinkedHashMap; import java.util.Map; +import java.util.NoSuchElementException; +import java.util.Optional; +import java.util.function.Function; /** * Bus for broadcasting and querying {@link Status} of different parts of the robot (eg state of a Mechanism). - * This is a Singleton, accessed via {@link #getInstance()}. Producers can call {@link #publish} to publish - * their latest {@link Status}. Consumers can call {@link #getStatus(Class)} with the class Object - * (eg, {@code MyStatus.class}) for the {@link Status} they are interested in querying, to get the latest - * published {@link Status}. + * This is a Singleton. Producers can call {@link #publish} to publish their latest {@link Status}. + * Consumers can call {@link #getStatus(Class)} with the class Object (eg, {@code MyStatus.class}) + * for the {@link Status} they are interested in querying, to get the latest published {@link Status}. */ -public class StatusBus implements LoggingBase { - - private static StatusBus s_instance = new StatusBus(); - private final Map, Status> statuses = new LinkedHashMap<>(); +public class StatusBus { /** - * Get the Singleton instance of the {@link StatusBus}. + * @param status The {@link Status} that was published + * @param timestamp The time at which the Status was published */ - public static StatusBus getInstance() { - return s_instance; + public record Entry(T status, double timestamp) { + public double age() { + return RobotProvider.instance.getClock().getTime() - timestamp; + } } - // TODO: would this be helpful? - // private void clear() { - // statuses.clear(); - // } + private static final Map, Entry> statuses = new LinkedHashMap<>(); /** - * Publish a new {@link Status} for the given specific class of {@link Status}. Each producer will + * Remove all published @{link Status}es from the StatusBus. + */ + public static void clear() { + statuses.clear(); + } + + /** + * Publish a new {@link Status} for the given specific class of {@link Status}. Each producer will * create their own implementation of the {@link Status} interface to contain its state information. * * This method also logs the Status to diagnostic logs. */ - public void publish(Status status) { - statuses.put(status.getClass(), status); - // TODO: also publish to data logs - log("StatusBus received Status (" + status.getClass().getName() + "): " + status); + public static Entry publishStatus(S status) { + var entry = new Entry<>(status, RobotProvider.instance.getClock().getTime()); + statuses.put(status.getClass(), entry); + // TODO(MF3): also publish to data logs + Logger.get(Category.FRAMEWORK) + .logRaw( + Severity.INFO, + "StatusBus received Status (" + + status.getClass().getName() + + "): " + + status); + return entry; } /** - * Gets the latest published {@link Status} for the given class of {@link Status}. Each producer will - * create their own implementation of the {@link Status} interface to contain its state information. Each + * Gets the latest published {@link Status} for the given class of {@link Status}, along with + * additional metadata about how the Status was published. Each producer will create their own + * implementation of the {@link Status} interface to contain its state information. Each * consumer will need to know the {@link Status} class a priori, in order to query it. * * @param The specific {@link Status} class of interest. * @param statusClass The Class object for the Status, eg {@code MyStatus.class}. - * @return The latest published {@link Status} or null if the {@link Status} hasn't been published. + * @return + * A {@link StatusBus.Entry} containing the latest published {@link Status}, or an empty + * Optional if the {@link Status} hasn't been published. */ @SuppressWarnings("unchecked") - public S getStatus(Class statusClass) { - return (S) statuses.get(statusClass); + public static Optional> getStatusEntry(Class statusClass) { + return Optional.ofNullable((Entry) statuses.get(statusClass)); } - @Override - public Category getLoggerCategory() { - return Category.FRAMEWORK; + /** + * Gets the latest published {@link Status} for the given class of {@link Status}. Each producer + * will create their own implementation of the {@link Status} interface to contain its state + * information. Each consumer will need to know the {@link Status} class a priori, in order to + * query it. + * + * @param The specific {@link Status} class of interest. + * @param statusClass The Class object for the Status, eg {@code MyStatus.class}. + * @return + * The latest published {@link Status} or an empty Optional if the {@link Status} hasn't + * been published. + */ + public static Optional getStatus(Class statusClass) { + return getStatusEntry(statusClass).map(Entry::status); + } + + /** + * Gets the latest published {@link Status} for the given class of {@link Status}. Each producer + * will create their own implementation of the {@link Status} interface to contain its state + * information. Each consumer will need to know the {@link Status} class a priori, in order to + * query it. + * + * @param The specific {@link Status} class of interest. + * @param statusClass The Class object for the Status, eg {@code MyStatus.class}. + * @return The latest published {@link Status} + * @throws NoSuchElementException if the {@link Status} hasn't been published + */ + public static S getStatusOrThrow(Class statusClass) { + return getStatus(statusClass).orElseThrow(); + } + + /** + * Gets the latest published {@link Status} for the given class of {@link Status}, then applies + * the given function to the Status and returns the result. Each producer will create their own + * implementation of the {@link Status} interface to contain its state information. Each + * consumer will need to know the {@link Status} class a priori, in order to query it. + * + * @param The specific {@link Status} class of interest. + * @param The return type of the Function + * @param statusClass The Class object for the Status, eg {@code MyStatus.class}. + * @return + * The result of the Function applied to the latest published {@link Status} or an empty + * Optional if the {@link Status} hasn't been published. + */ + public static Optional getStatusValue( + Class statusClass, Function getter) { + return getStatusEntry(statusClass).map(s -> getter.apply(s.status())); } } diff --git a/src/main/java/com/team766/framework3/Superstructure.java b/src/main/java/com/team766/framework3/Superstructure.java index a2fddea33..770a4efac 100644 --- a/src/main/java/com/team766/framework3/Superstructure.java +++ b/src/main/java/com/team766/framework3/Superstructure.java @@ -3,7 +3,7 @@ import java.util.ArrayList; import java.util.Objects; -public abstract class Superstructure> extends Mechanism { +public abstract class Superstructure extends Mechanism { private ArrayList> submechanisms = new ArrayList<>(); @Override diff --git a/src/main/java/com/team766/hal/mock/MockClock.java b/src/main/java/com/team766/hal/mock/MockClock.java deleted file mode 100644 index e86e7fe7c..000000000 --- a/src/main/java/com/team766/hal/mock/MockClock.java +++ /dev/null @@ -1,31 +0,0 @@ -package com.team766.hal.mock; - -import com.team766.hal.Clock; - -/** - * Clock implementation for usage in unit tests. - */ -public class MockClock implements Clock { - private double time; - - /** - * Create a new clock for the specified time, in seconds. - * @param timeInSeconds time in seconds. - */ - public MockClock(double timeInSeconds) { - this.time = timeInSeconds; - } - - /** - * Advance the clock by the specified time, in seconds. - * @param seconds how many seconds to advance. - */ - public void tick(double seconds) { - time += seconds; - } - - @Override - public double getTime() { - return time; - } -} diff --git a/src/main/java/com/team766/hal/mock/TestRobotProvider.java b/src/main/java/com/team766/hal/mock/TestRobotProvider.java index 1bac8f38d..da221cb47 100755 --- a/src/main/java/com/team766/hal/mock/TestRobotProvider.java +++ b/src/main/java/com/team766/hal/mock/TestRobotProvider.java @@ -16,14 +16,18 @@ import com.team766.hal.RelayOutput; import com.team766.hal.RobotProvider; import com.team766.hal.SolenoidController; -import com.team766.hal.wpilib.SystemClock; public class TestRobotProvider extends RobotProvider { + private final Clock clock; private MotorController[] motors = new MotorController[64]; private boolean m_hasDriverStationUpdate = false; private double m_batteryVoltage = 12.0; + public TestRobotProvider(Clock clock) { + this.clock = clock; + } + @Override public MotorController getMotor( final int index, @@ -131,8 +135,7 @@ public BeaconReader getBeaconSensor() { @Override public Clock getClock() { - // TODO Replace this with a controlled clock - return SystemClock.instance; + return clock; } @Override diff --git a/src/test/java/com/team766/TestCase.java b/src/test/java/com/team766/TestCase.java index 535a31d27..438438bc5 100644 --- a/src/test/java/com/team766/TestCase.java +++ b/src/test/java/com/team766/TestCase.java @@ -4,6 +4,7 @@ import com.team766.config.ConfigFileTestUtils; import com.team766.framework.Scheduler; import com.team766.hal.RobotProvider; +import com.team766.hal.TestClock; import com.team766.hal.mock.TestRobotProvider; import java.io.IOException; import java.nio.file.Files; @@ -16,7 +17,7 @@ public void setUp() { ConfigFileTestUtils.resetStatics(); Scheduler.getInstance().reset(); - RobotProvider.instance = new TestRobotProvider(); + RobotProvider.instance = new TestRobotProvider(new TestClock()); } protected void loadConfig(String configJson) throws IOException { diff --git a/src/test/java/com/team766/TestCase3.java b/src/test/java/com/team766/TestCase3.java index d368b9cd4..486178538 100644 --- a/src/test/java/com/team766/TestCase3.java +++ b/src/test/java/com/team766/TestCase3.java @@ -3,8 +3,10 @@ import com.team766.config.ConfigFileReader; import com.team766.config.ConfigFileTestUtils; import com.team766.framework3.SchedulerUtils; +import com.team766.framework3.StatusBus; import com.team766.framework3.TestLoggerExtension; import com.team766.hal.RobotProvider; +import com.team766.hal.TestClock; import com.team766.hal.mock.TestRobotProvider; import edu.wpi.first.hal.HAL; import edu.wpi.first.wpilibj.simulation.DriverStationSim; @@ -19,6 +21,7 @@ @ExtendWith(TestLoggerExtension.class) public abstract class TestCase3 { + protected final TestClock testClock = new TestClock(); @BeforeEach public final void setUpFramework() { @@ -28,8 +31,9 @@ public final void setUpFramework() { ConfigFileTestUtils.resetStatics(); SchedulerUtils.reset(); + StatusBus.clear(); - RobotProvider.instance = new TestRobotProvider(); + RobotProvider.instance = new TestRobotProvider(testClock); } protected void setRobotEnabled(boolean enabled) { diff --git a/src/test/java/com/team766/framework/TimedPredicateTest.java b/src/test/java/com/team766/framework/TimedPredicateTest.java index 90b9a0de0..acc81cb6b 100644 --- a/src/test/java/com/team766/framework/TimedPredicateTest.java +++ b/src/test/java/com/team766/framework/TimedPredicateTest.java @@ -2,7 +2,7 @@ import static org.junit.jupiter.api.Assertions.*; -import com.team766.hal.mock.MockClock; +import com.team766.hal.TestClock; import java.util.function.BooleanSupplier; import org.junit.jupiter.api.Test; @@ -10,7 +10,7 @@ public class TimedPredicateTest { @Test public void testTimedPredicateTimedOut() { - MockClock testClock = new MockClock(1710411240.0); + TestClock testClock = new TestClock(1710411240.0); ContextImpl.TimedPredicate predicate = new ContextImpl.TimedPredicate(testClock, () -> false, 1.766); assertFalse(predicate.getAsBoolean()); @@ -23,7 +23,7 @@ public void testTimedPredicateTimedOut() { @Test public void testTimedPredicateCondition() { - MockClock testClock = new MockClock(1710411240.0); + TestClock testClock = new TestClock(1710411240.0); ContextImpl.TimedPredicate predicate = new ContextImpl.TimedPredicate( testClock, diff --git a/src/test/java/com/team766/framework3/ConditionsTest.java b/src/test/java/com/team766/framework3/ConditionsTest.java index 12a0b7627..db8f41fff 100644 --- a/src/test/java/com/team766/framework3/ConditionsTest.java +++ b/src/test/java/com/team766/framework3/ConditionsTest.java @@ -2,10 +2,17 @@ import static org.junit.jupiter.api.Assertions.*; +import com.team766.TestCase3; +import com.team766.framework3.FakeMechanism.FakeStatus; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.Optional; +import java.util.Set; +import java.util.concurrent.atomic.AtomicReference; import java.util.function.BooleanSupplier; +import java.util.function.Consumer; import org.junit.jupiter.api.Test; -public class ConditionsTest { +public class ConditionsTest extends TestCase3 { private static class ValueProxy implements BooleanSupplier { boolean value = false; @@ -14,6 +21,238 @@ public boolean getAsBoolean() { } } + private static class ProxyRequest implements Request { + boolean isDone = false; + + public boolean isDone() { + return isDone; + } + } + + public record OtherStatus(int currentState) implements Status {} + + private static Command startContext(Consumer runnable) { + var context = new ContextImpl(new FunctionalProcedure(Set.of(), runnable)); + context.initialize(); + return context; + } + + private static boolean step(int numSteps, Command command) { + for (int i = 0; i < 5; ++i) { + command.execute(); + } + return command.isFinished(); + } + + private static void finish(Command command) { + while (!command.isFinished()) { + command.execute(); + Thread.yield(); + } + } + + @Test + public void testWaitForValue() { + AtomicReference> container = new AtomicReference<>(Optional.empty()); + var c = + startContext( + context -> { + assertEquals( + "the value", Conditions.waitForValue(context, container::get)); + }); + assertFalse(step(5, c)); + container.set(Optional.of("the value")); + assertTrue(step(1, c)); + } + + @Test + public void testWaitForValueOrTimeout() { + AtomicReference> container = new AtomicReference<>(Optional.empty()); + finish( + startContext( + context -> { + assertEquals( + Optional.empty(), + Conditions.waitForValueOrTimeout(context, container::get, 0.1)); + })); + var c = + startContext( + context -> { + assertEquals( + Optional.of("the value"), + Conditions.waitForValueOrTimeout( + context, container::get, 1000.0)); + }); + assertFalse(step(5, c)); + container.set(Optional.of("the value")); + assertTrue(step(1, c)); + } + + @Test + public void testCheckForStatus() { + assertFalse(Conditions.checkForStatus(FakeStatus.class)); + StatusBus.publishStatus(new FakeStatus(0)); + assertTrue(Conditions.checkForStatus(FakeStatus.class)); + assertFalse(Conditions.checkForStatus(OtherStatus.class)); + } + + @Test + public void testCheckForStatusWith() { + assertFalse(Conditions.checkForStatusWith(FakeStatus.class, s -> s.currentState() == 1)); + StatusBus.publishStatus(new OtherStatus(1)); + assertFalse(Conditions.checkForStatusWith(FakeStatus.class, s -> s.currentState() == 1)); + StatusBus.publishStatus(new FakeStatus(0)); + assertFalse(Conditions.checkForStatusWith(FakeStatus.class, s -> s.currentState() == 1)); + StatusBus.publishStatus(new FakeStatus(1)); + assertTrue(Conditions.checkForStatusWith(FakeStatus.class, s -> s.currentState() == 1)); + } + + @Test + public void testCheckForStatusEntryWith() { + assertFalse( + Conditions.checkForStatusEntryWith( + FakeStatus.class, s -> s.status().currentState() == 1)); + StatusBus.publishStatus(new OtherStatus(1)); + assertFalse( + Conditions.checkForStatusEntryWith( + FakeStatus.class, s -> s.status().currentState() == 1)); + StatusBus.publishStatus(new FakeStatus(0)); + assertFalse( + Conditions.checkForStatusEntryWith( + FakeStatus.class, s -> s.status().currentState() == 1)); + StatusBus.publishStatus(new FakeStatus(1)); + assertTrue( + Conditions.checkForStatusEntryWith( + FakeStatus.class, s -> s.status().currentState() == 1)); + } + + @Test + public void testWaitForStatus() { + var c = + startContext( + context -> { + assertEquals( + new FakeStatus(42), + Conditions.waitForStatus(context, FakeStatus.class)); + }); + assertFalse(step(5, c)); + StatusBus.publishStatus(new OtherStatus(42)); + assertFalse(step(5, c)); + StatusBus.publishStatus(new FakeStatus(42)); + assertTrue(step(1, c)); + } + + @Test + public void testWaitForStatusOrTimeout() { + finish( + startContext( + context -> { + assertEquals( + Optional.empty(), + Conditions.waitForStatusOrTimeout( + context, FakeStatus.class, 0.1)); + })); + var c = + startContext( + context -> { + assertEquals( + Optional.of(new FakeStatus(42)), + Conditions.waitForStatusOrTimeout( + context, FakeStatus.class, 1000.0)); + }); + assertFalse(step(5, c)); + StatusBus.publishStatus(new OtherStatus(42)); + assertFalse(step(5, c)); + StatusBus.publishStatus(new FakeStatus(42)); + assertTrue(step(1, c)); + } + + @Test + public void testWaitForStatusWith() { + var c = + startContext( + context -> { + assertEquals( + new FakeStatus(42), + Conditions.waitForStatusWith( + context, + FakeStatus.class, + s -> s.currentState() == 42)); + }); + assertFalse(step(5, c)); + StatusBus.publishStatus(new OtherStatus(42)); + assertFalse(step(5, c)); + StatusBus.publishStatus(new FakeStatus(0)); + assertFalse(step(5, c)); + StatusBus.publishStatus(new FakeStatus(42)); + assertTrue(step(1, c)); + } + + @Test + public void testWaitForStatusWithOrTimeout() { + finish( + startContext( + context -> { + assertEquals( + Optional.empty(), + Conditions.waitForStatusWithOrTimeout( + context, + FakeStatus.class, + s -> s.currentState() == 42, + 0.1)); + })); + var c = + startContext( + context -> { + assertEquals( + Optional.of(new FakeStatus(42)), + Conditions.waitForStatusWithOrTimeout( + context, + FakeStatus.class, + s -> s.currentState() == 42, + 1000.0)); + }); + assertFalse(step(5, c)); + StatusBus.publishStatus(new OtherStatus(42)); + assertFalse(step(5, c)); + StatusBus.publishStatus(new FakeStatus(0)); + assertFalse(step(5, c)); + StatusBus.publishStatus(new FakeStatus(42)); + assertTrue(step(1, c)); + } + + @Test + public void testWaitForRequest() { + var request = new ProxyRequest(); + var c = + startContext( + context -> { + Conditions.waitForRequest(context, request); + }); + assertFalse(step(5, c)); + request.isDone = true; + assertTrue(step(1, c)); + } + + @Test + public void testWaitForRequestOrTimeout() { + var request = new ProxyRequest(); + finish( + startContext( + context -> { + assertFalse(Conditions.waitForRequestOrTimeout(context, request, 0.1)); + })); + var c = + startContext( + context -> { + assertTrue( + Conditions.waitForRequestOrTimeout(context, request, 1000.0)); + }); + assertFalse(step(5, c)); + request.isDone = true; + assertTrue(step(1, c)); + } + @Test public void testToggle() { var v = new ValueProxy(); diff --git a/src/test/java/com/team766/framework3/FakeMechanism.java b/src/test/java/com/team766/framework3/FakeMechanism.java index f53d06073..faf8ac3df 100644 --- a/src/test/java/com/team766/framework3/FakeMechanism.java +++ b/src/test/java/com/team766/framework3/FakeMechanism.java @@ -1,12 +1,14 @@ package com.team766.framework3; +import static com.team766.framework3.Conditions.checkForStatusWith; + class FakeMechanism extends Mechanism { record FakeStatus(int currentState) implements Status {} - public record FakeRequest(int targetState) implements Request { + public record FakeRequest(int targetState) implements Request { @Override - public boolean isDone(FakeStatus status) { - return status.currentState() == targetState; + public boolean isDone() { + return checkForStatusWith(FakeStatus.class, s -> s.currentState() == targetState); } } diff --git a/src/test/java/com/team766/framework3/StatusBusTest.java b/src/test/java/com/team766/framework3/StatusBusTest.java new file mode 100644 index 000000000..c41f8f966 --- /dev/null +++ b/src/test/java/com/team766/framework3/StatusBusTest.java @@ -0,0 +1,104 @@ +package com.team766.framework3; + +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 com.team766.TestCase3; +import java.util.NoSuchElementException; +import java.util.Optional; +import org.junit.jupiter.api.Test; + +public class StatusBusTest extends TestCase3 { + + public record MyStatus(int value) implements Status {} + + public record OtherStatus(int value) implements Status {} + + @Test + public void testGetStatusEntry() { + assertEquals(Optional.empty(), StatusBus.getStatusEntry(MyStatus.class)); + StatusBus.publishStatus(new OtherStatus(0)); + assertEquals(Optional.empty(), StatusBus.getStatusEntry(MyStatus.class)); + + testClock.setTime(1234500000); + StatusBus.publishStatus(new MyStatus(0)); + + testClock.setTime(1234500123); + var maybeEntry = StatusBus.getStatusEntry(MyStatus.class); + var entry = maybeEntry.orElseThrow(); + assertEquals(new MyStatus(0), entry.status()); + assertEquals(1234500000, entry.timestamp()); + assertEquals(123, entry.age()); + } + + @Test + public void testPublishStatus() { + testClock.setTime(1234500000); + var publishEntry = StatusBus.publishStatus(new MyStatus(42)); + assertEquals(new MyStatus(42), publishEntry.status()); + assertEquals(1234500000, publishEntry.timestamp()); + + testClock.setTime(1234500123); + assertEquals(123, publishEntry.age()); + + var maybeEntry = StatusBus.getStatusEntry(MyStatus.class); + var entry = maybeEntry.orElseThrow(); + assertEquals(new MyStatus(42), entry.status()); + assertEquals(1234500000, entry.timestamp()); + assertEquals(123, entry.age()); + + // Test that publishing another status overwrites the first status. + + testClock.setTime(1234501000); + publishEntry = StatusBus.publishStatus(new MyStatus(66)); + assertEquals(new MyStatus(66), publishEntry.status()); + assertEquals(1234501000, publishEntry.timestamp()); + + testClock.setTime(1234501012); + assertEquals(12, publishEntry.age()); + + entry = StatusBus.getStatusEntry(MyStatus.class).orElseThrow(); + assertEquals(new MyStatus(66), entry.status()); + assertEquals(1234501000, entry.timestamp()); + assertEquals(12, entry.age()); + } + + @Test + public void testClear() { + StatusBus.publishStatus(new MyStatus(0)); + assertTrue(StatusBus.getStatusEntry(MyStatus.class).isPresent()); + StatusBus.clear(); + assertFalse(StatusBus.getStatusEntry(MyStatus.class).isPresent()); + } + + @Test + public void testGetStatus() { + assertEquals(Optional.empty(), StatusBus.getStatus(MyStatus.class)); + StatusBus.publishStatus(new OtherStatus(0)); + assertEquals(Optional.empty(), StatusBus.getStatus(MyStatus.class)); + StatusBus.publishStatus(new MyStatus(0)); + assertEquals(Optional.of(new MyStatus(0)), StatusBus.getStatus(MyStatus.class)); + } + + @Test + public void testGetStatusOrThrow() { + assertThrows( + NoSuchElementException.class, () -> StatusBus.getStatusOrThrow(MyStatus.class)); + StatusBus.publishStatus(new OtherStatus(0)); + assertThrows( + NoSuchElementException.class, () -> StatusBus.getStatusOrThrow(MyStatus.class)); + StatusBus.publishStatus(new MyStatus(0)); + assertEquals(new MyStatus(0), StatusBus.getStatusOrThrow(MyStatus.class)); + } + + @Test + public void testGetStatusValue() { + assertEquals(Optional.empty(), StatusBus.getStatusValue(MyStatus.class, s -> s.value())); + StatusBus.publishStatus(new OtherStatus(0)); + assertEquals(Optional.empty(), StatusBus.getStatusValue(MyStatus.class, s -> s.value())); + StatusBus.publishStatus(new MyStatus(42)); + assertEquals(Optional.of(42), StatusBus.getStatusValue(MyStatus.class, s -> s.value())); + } +} diff --git a/src/test/java/com/team766/hal/TestClock.java b/src/test/java/com/team766/hal/TestClock.java new file mode 100644 index 000000000..c0e3cf2ad --- /dev/null +++ b/src/test/java/com/team766/hal/TestClock.java @@ -0,0 +1,58 @@ +package com.team766.hal; + +import com.team766.hal.wpilib.SystemClock; + +/** + * Clock implementation for usage in unit tests. + */ +public class TestClock implements Clock { + private boolean useSystemClock = true; + private double time; + + /** + * Create a new clock that returns the system time, until one of the methods to control the + * clock time are called. After that, the clock will return the same time until its time is + * changed again. + */ + public TestClock() { + useSystemClock = true; + } + + /** + * Create a new clock set to the specified time. The clock will return the same time until its + * time is changed again. + */ + public TestClock(double time) { + useSystemClock = false; + this.time = time; + } + + /** + * Set the clock's current time. + * After calling this, the clock will no longer advance automatically. + */ + public void setTime(double time) { + this.useSystemClock = false; + this.time = time; + } + + /** + * Advance the clock by the specified time, in seconds. + * @param seconds how many seconds to advance. + */ + public void tick(double seconds) { + if (useSystemClock) { + setTime(SystemClock.instance.getTime()); + } + time += seconds; + } + + @Override + public double getTime() { + if (useSystemClock) { + return SystemClock.instance.getTime(); + } else { + return time; + } + } +}