Revisions to Status system

.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.*",

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> T waitForValue(Context context, Supplier<Optional<T>> supplier) {
+        final AtomicReference<T> 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<S extends Status> implements BooleanSupplier {
+    public static <T> Optional<T> waitForValueOrTimeout(
+            Context context, Supplier<Optional<T>> supplier, double timeoutSeconds) {
+        final AtomicReference<Optional<T>> 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<S> {
-            boolean check(S status);
-        }
+    /**
+     * Predicate that checks whether or not a {@link Status} with the given class has been published
+     */
+    public static <S extends Status> boolean checkForStatus(Class<S> statusClass) {
+        return StatusBus.getStatusEntry(statusClass).isPresent();
+    }
 
-        private final Class<S> clazz;
-        private final Checker<S> checker;
+    /**
+     * Predicate that checks whether or not the latest {@link Status} with the given class passes
+     * the check provided by {@code predicate}.
+     */
+    public static <S extends Status> boolean checkForStatusWith(
+            Class<S> statusClass, Predicate<S> predicate) {
+        return StatusBus.getStatusValue(statusClass, predicate::test).orElse(false);
+    }
 
-        public StatusCheck(Class<S> clazz, Checker<S> 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 <S extends Status> boolean checkForStatusEntryWith(
+            Class<S> statusClass, Predicate<StatusBus.Entry<S>> 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 extends Status> T waitForStatus(Context context, Class<T> 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<S extends Status> extends StatusCheck<S> {
+    public static <T extends Status> Optional<T> waitForStatusOrTimeout(
+            Context context, Class<T> statusClass, double timeoutSeconds) {
+        return waitForValueOrTimeout(
+                context, () -> StatusBus.getStatus(statusClass), timeoutSeconds);
+    }
 
-        public AwaitRequest(Class<S> clazz, Request<S> 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 extends Status> T waitForStatusWith(
+            Context context, Class<T> statusClass, Predicate<T> 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 <T extends Status> Optional<T> waitForStatusWithOrTimeout(
+            Context context, Class<T> statusClass, Predicate<T> 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);
     }
 
     /**

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<R extends Request<?>> extends SubsystemBase implements LoggingBase {
+public abstract class Mechanism<R extends Request> extends SubsystemBase implements LoggingBase {
     private boolean isRunningPeriodic = false;
 
     private Superstructure<?> superstructure = null;

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<S extends Status> {
+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?
 }

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<Class<? extends Status>, 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 extends Status>(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<Class<?>, 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 <S extends Record & Status> Entry<S> 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 <S> 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 extends Status> S getStatus(Class<S> statusClass) {
-        return (S) statuses.get(statusClass);
+    public static <S extends Status> Optional<Entry<S>> getStatusEntry(Class<S> statusClass) {
+        return Optional.ofNullable((Entry<S>) 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 <S> 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 <S extends Status> Optional<S> getStatus(Class<S> statusClass) {
+        return getStatusEntry(statusClass).map(Entry<S>::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 <S> 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 extends Status> S getStatusOrThrow(Class<S> 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 <S> The specific {@link Status} class of interest.
+     * @param <V> 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 <S extends Status, V> Optional<V> getStatusValue(
+            Class<S> statusClass, Function<S, V> getter) {
+        return getStatusEntry(statusClass).map(s -> getter.apply(s.status()));
     }
 }

src/main/java/com/team766/framework3/Superstructure.java

@@ -3,7 +3,7 @@
 import java.util.ArrayList;
 import java.util.Objects;
 
-public abstract class Superstructure<R extends Request<?>> extends Mechanism<R> {
+public abstract class Superstructure<R extends Request> extends Mechanism<R> {
     private ArrayList<Mechanism<?>> submechanisms = new ArrayList<>();
 
     @Override