[commands] Add Commands v3 framework (#6518)

The framework fundamentally relies on the continuation API added in Java 21 (which is currently internal to the JDK). Continuations allow for call stacks to be saved to the heap and resumed later.

The async framework allows command bodies to be written in an imperative style. However, an async command will need to be actively cooperative and periodically call coroutine.yield() in loops to yield control back to the command scheduler to let it process other commands.

There are also some other additions like priority levels (as opposed to a blanket yes/no for ignoring incoming commands), factories requiring names be provided for commands, and the scheduler tracking all running commands and not just the highest-level groups. However, those changes aren't unique to an async framework, and could just as easily be used in a traditional command framework.

commandsv3/src/test/java/org/wpilib/commands3/SchedulerSideloadFunctionTests.java

@@ -0,0 +1,100 @@
+// 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.commands3;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrowsExactly;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+
+class SchedulerSideloadFunctionTests extends CommandTestBase {
+  @Test
+  void sideloadThrowingException() {
+    m_scheduler.sideload(
+        co -> {
+          throw new RuntimeException("Bang!");
+        });
+
+    // An exception raised in a sideload function should bubble up
+    assertEquals(
+        "Bang!", assertThrowsExactly(RuntimeException.class, m_scheduler::run).getMessage());
+  }
+
+  @Test
+  void periodicSideload() {
+    AtomicInteger count = new AtomicInteger(0);
+    m_scheduler.addPeriodic(count::incrementAndGet);
+    assertEquals(0, count.get());
+
+    m_scheduler.run();
+    assertEquals(1, count.get());
+
+    m_scheduler.run();
+    assertEquals(2, count.get());
+  }
+
+  @Test
+  void sideloadSchedulingCommand() {
+    var command = Command.noRequirements().executing(Coroutine::park).named("Command");
+    // one-shot sideload forks a command and immediately exits
+    m_scheduler.sideload(co -> co.fork(command));
+    m_scheduler.run();
+    assertTrue(
+        m_scheduler.isRunning(command), "command should have started and outlasted the sideload");
+  }
+
+  @Test
+  void childCommandEscapesViaSideload() {
+    var child = Command.noRequirements().executing(Coroutine::park).named("Child");
+    var parent =
+        Command.noRequirements()
+            .executing(
+                parentCoroutine -> {
+                  m_scheduler.sideload(sidelodCoroutine -> sidelodCoroutine.fork(child));
+                })
+            .named("Parent");
+
+    m_scheduler.schedule(parent);
+    m_scheduler.run();
+    assertFalse(m_scheduler.isScheduledOrRunning(parent), "parent should have exited");
+    assertFalse(
+        m_scheduler.isScheduledOrRunning(child),
+        "the sideload to schedule the child should not have run yet");
+
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(child), "child should have started running");
+  }
+
+  @Test
+  void sideloadCancelingCommand() {
+    var command = Command.noRequirements().executing(Coroutine::park).named("Command");
+    m_scheduler.schedule(command);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command), "command should have started");
+
+    m_scheduler.sideload(co -> m_scheduler.cancel(command));
+    assertTrue(m_scheduler.isRunning(command), "sideload should not have run yet");
+
+    m_scheduler.run();
+    assertFalse(m_scheduler.isRunning(command), "sideload should have canceled the command");
+  }
+
+  @Test
+  void sideloadAffectsStateForTriggerInSameCycle() {
+    AtomicBoolean signal = new AtomicBoolean(false);
+    var trigger = new Trigger(m_scheduler, signal::get);
+    var command = Command.noRequirements().executing(Coroutine::park).named("Command");
+    trigger.onTrue(command);
+    m_scheduler.sideload(co -> signal.set(true));
+
+    m_scheduler.run();
+    assertTrue(signal.get(), "Sideload should have run and set the signal");
+    assertTrue(m_scheduler.isRunning(command), "Command should have started");
+  }
+}