[cmd3] Enforce command lifetimes across all opmode and command scopes (#8705)

Commands are no longer able to outlive their schedule-site's scope,
regardless of how they were scheduled (set as a default command, bound
to a trigger, or manually scheduled)

As a consequence, default commands need better tracking so the default
command setting can be released when their scope exits and the next-most
appropriate default command can be rescheduled (eg, an opmode sets a
default command, then the globally-scoped default is restored when the
opmode exits). Some complexity is required here to make it work well for
edge cases.

Like `schedule()`, `setDefaultCommand()` will immediately start the new
default command if called inside of another command to avoid 1-loop
delays. However, this does not apply when called by the _current_
default command, as it would result in attempting to cancel the default
command while it's mounted (which is impossible and would throw an
exception)

```java
class Robot extends OpModeRobot {
  final Drive drive = new Drive();
  final CommandXboxController controller = new CommandXboxController(1);

  public Robot() {
    // global default command, active unless overridden in an opmode or command
    drive.setDefaultCommand(drive.stop());

    // global trigger binding, always active
    controller.rightBumper().onTrue(drive.setX());
  }
}

@Teleop
class ExampleOpMode extends PeriodicOpMode {
  public ExampleOpMode(Robot robot) {
    // opmode-specific default command
    robot.drive.setDefaultCommand(robot.drive.operatorControl(robot.controller));

    // opmode-specific binding
    robot.controller.leftBumper().whileTrue(robot.drive.stop());

    // opmode-specific binding that takes precedence over the global binding
    // because it happens last; it "wins out" over the `setX()` binding
    robot.controller.rightBumper().onTrue(robot.drive.selfTest());
  }

  @Override
  public void periodic() {
    Scheduler.getDefault().run();
  }
}
```

commandsv3/src/test/java/org/wpilib/command3/BindingScopeTest.java

@@ -0,0 +1,78 @@
+// 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.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertInstanceOf;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertSame;
+
+import org.junit.jupiter.api.Test;
+
+class BindingScopeTest extends SchedulerTest {
+  @Test
+  void narrowestScopeInOpmode() {
+    m_opModeId = 1;
+    m_opModeName = "Opmode 1";
+
+    var scope = BindingScope.createNarrowestScope(m_scheduler);
+    assertInstanceOf(BindingScope.ForOpmode.class, scope);
+    assertEquals(m_opModeId, ((BindingScope.ForOpmode) scope).opmodeId());
+  }
+
+  @Test
+  void narrowestScopeInCommand() {
+    BindingScope[] scopeRef = new BindingScope[1];
+
+    var command =
+        Command.noRequirements()
+            .executing(
+                coroutine -> {
+                  BindingScope scope = BindingScope.createNarrowestScope(m_scheduler);
+                  scopeRef[0] = scope;
+                })
+            .named("Command 1");
+
+    m_scheduler.schedule(command);
+    m_scheduler.run();
+    assertNotNull(scopeRef[0], "Command did not execute");
+    assertInstanceOf(BindingScope.ForCommand.class, scopeRef[0]);
+    assertSame(command, ((BindingScope.ForCommand) scopeRef[0]).command());
+  }
+
+  @Test
+  void narrowestScopeInOpmodeAndCommand() {
+    m_opModeId = 314;
+    m_opModeName = "Opmode pi";
+
+    BindingScope[] scopeRef = new BindingScope[1];
+    var command =
+        Command.noRequirements()
+            .executing(
+                coroutine -> {
+                  BindingScope scope = BindingScope.createNarrowestScope(m_scheduler);
+                  scopeRef[0] = scope;
+                })
+            .named("Command pi");
+
+    m_scheduler.schedule(command);
+    m_scheduler.run();
+
+    // Scope should be for the command, not the opmode
+    assertNotNull(scopeRef[0], "Command did not execute");
+    assertInstanceOf(BindingScope.ForCommand.class, scopeRef[0]);
+    assertSame(command, ((BindingScope.ForCommand) scopeRef[0]).command());
+  }
+
+  @Test
+  void narrowestScopeWithoutOpmodeOrCommand() {
+    // Just to be explicit: an opmode ID of zero means no opmode is selected in the DS
+    m_opModeId = 0;
+    m_opModeName = null;
+
+    var scope = BindingScope.createNarrowestScope(m_scheduler);
+    assertSame(BindingScope.Global.INSTANCE, scope);
+  }
+}

commandsv3/src/test/java/org/wpilib/command3/CommandTestBase.java

@@ -6,12 +6,15 @@ package org.wpilib.command3;
 
 import java.util.ArrayList;
 import java.util.List;
+import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.wpilib.system.RobotController;
 
 class CommandTestBase {
   protected Scheduler m_scheduler;
   protected List<SchedulerEvent> m_events;
+  protected long m_opModeId = 0;
+  protected String m_opModeName = "";
 
   @BeforeEach
   void initScheduler() {
@@ -20,4 +23,26 @@ class CommandTestBase {
     m_events = new ArrayList<>();
     m_scheduler.addEventListener(m_events::add);
   }
+
+  @BeforeEach
+  void initOpmodeFetcher() {
+    OpModeFetcher.setFetcher(
+        new OpModeFetcher() {
+          @Override
+          long getOpModeId() {
+            return m_opModeId;
+          }
+
+          @Override
+          String getOpModeName() {
+            return m_opModeName;
+          }
+        });
+  }
+
+  @AfterEach
+  void resetOpmodeFetcher() {
+    m_opModeId = 0;
+    m_opModeName = "";
+  }
 }

commandsv3/src/test/java/org/wpilib/command3/SchedulerCancellationTests.java

@@ -179,9 +179,14 @@ class SchedulerCancellationTests extends CommandTestBase {
 
     // Then running should get it into the set of running commands
     m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(command));
+    for (Mechanism mechanism : mechanisms) {
+      assertEquals(List.of(command), m_scheduler.getRunningCommandsFor(mechanism));
+    }
 
     // Canceling should clear out the set of running commands
     m_scheduler.cancelAll();
+    assertFalse(m_scheduler.isRunning(command), "Command was not canceled by cancelAll()");
 
     // Then ticking the scheduler once to fully remove the command and schedule the defaults
     m_scheduler.run();

commandsv3/src/test/java/org/wpilib/command3/SchedulerDefaultCommandTests.java

@@ -0,0 +1,218 @@
+// 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.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class SchedulerDefaultCommandTests extends CommandTestBase {
+  @Test
+  void globalDefaultCommandIsAlwaysUsed() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var defaultCommand = mech.run(Coroutine::park).named("Custom Default Command");
+
+    mech.setDefaultCommand(defaultCommand);
+    assertFalse(m_scheduler.isScheduledOrRunning(defaultCommand));
+
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(defaultCommand));
+
+    m_opModeId = 1;
+    m_opModeName = "Opmode 1";
+
+    m_scheduler.run();
+    assertTrue(
+        m_scheduler.isRunning(defaultCommand),
+        "Default command should still be running when the opmode changes");
+  }
+
+  @Test
+  void defaultCommandSetInOpmodeStops() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var initialDefaultCommand = mech.idle();
+    mech.setDefaultCommand(initialDefaultCommand);
+
+    m_opModeId = 1;
+    m_opModeName = "Opmode 1";
+
+    var opModeScopedCommand = mech.run(Coroutine::park).named("Custom Default Command");
+    mech.setDefaultCommand(opModeScopedCommand);
+
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(opModeScopedCommand));
+
+    // Change opmode. The globally scoped default command should run
+    m_opModeId = 2;
+    m_opModeName = "Opmode 2";
+    m_scheduler.run();
+    assertFalse(m_scheduler.isRunning(opModeScopedCommand));
+    assertTrue(m_scheduler.isRunning(initialDefaultCommand));
+  }
+
+  @Test
+  void defaultCommandSetInCommandStops() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var initialDefaultCommand = mech.idle();
+    mech.setDefaultCommand(initialDefaultCommand);
+
+    var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
+
+    var scopingCommand =
+        Command.noRequirements()
+            .executing(
+                co -> {
+                  mech.setDefaultCommand(commandScopedCommand);
+                  co.park();
+                })
+            .named("Scoping Command");
+
+    m_scheduler.schedule(scopingCommand);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(scopingCommand));
+    assertTrue(m_scheduler.isRunning(commandScopedCommand));
+    assertFalse(m_scheduler.isRunning(initialDefaultCommand));
+
+    m_scheduler.cancel(scopingCommand);
+    m_scheduler.run();
+    assertFalse(m_scheduler.isRunning(scopingCommand));
+    assertFalse(m_scheduler.isRunning(commandScopedCommand));
+    assertTrue(m_scheduler.isRunning(initialDefaultCommand));
+  }
+
+  @Test
+  void interruptingDefaultCommandInterruptsOwner() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var initialDefaultCommand = mech.idle();
+    mech.setDefaultCommand(initialDefaultCommand);
+
+    var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
+    var scopingCommand =
+        Command.noRequirements()
+            .executing(
+                co -> {
+                  mech.setDefaultCommand(commandScopedCommand);
+                  co.park();
+                })
+            .named("Scoping Command");
+
+    final Command interruptor = mech.run(Coroutine::park).named("Interruptor");
+    m_scheduler.schedule(scopingCommand);
+    m_scheduler.run();
+    assertTrue(m_scheduler.isRunning(scopingCommand));
+    assertTrue(m_scheduler.isRunning(commandScopedCommand));
+    assertFalse(m_scheduler.isRunning(initialDefaultCommand));
+
+    m_scheduler.schedule(interruptor);
+    m_scheduler.run();
+    assertFalse(m_scheduler.isRunning(scopingCommand));
+    assertFalse(m_scheduler.isRunning(commandScopedCommand));
+    assertFalse(m_scheduler.isRunning(initialDefaultCommand));
+    assertTrue(m_scheduler.isRunning(interruptor));
+  }
+
+  @Test
+  void defaultCommandStackup() {
+    var mech = new Mechanism("Mech", m_scheduler);
+    var initialDefaultCommand = mech.idle();
+    mech.setDefaultCommand(initialDefaultCommand);
+
+    var opModeScopedCommand = mech.run(Coroutine::park).named("Opmode Scoped Default Command");
+    var commandScopedCommand = mech.run(Coroutine::park).named("Command-Scoped Default Command");
+
+    final Command scopingCommand =
+        Command.noRequirements()
+            .executing(
+                co -> {
+                  mech.setDefaultCommand(commandScopedCommand);
+                  co.park();
+                })
+            .named("Scoping Command");
+
+    m_opModeId = 1;
+    m_opModeName = "Opmode 1";
+    mech.setDefaultCommand(opModeScopedCommand);
+
+    m_scheduler.run();
+    assertTrue(
+        m_scheduler.isRunning(opModeScopedCommand),
+        "Opmode-scoped default command should be active");
+
+    m_scheduler.schedule(scopingCommand);
+    m_scheduler.run();
+    assertFalse(
+        m_scheduler.isRunning(opModeScopedCommand),
+        "Opmode-scoped default command should have stopped");
+    assertTrue(m_scheduler.isRunning(commandScopedCommand));
+
+    m_events.clear();
+    m_opModeId = 2;
+    m_opModeName = "Opmode 2";
+    m_scheduler.run();
+
+    assertFalse(
+        m_scheduler.isRunning(opModeScopedCommand),
+        "Opmode-scoped default command should have stopped when the opmode changed");
+    assertFalse(
+        m_scheduler.isRunning(scopingCommand),
+        "Running opmode-scoped command should have stopped when the opmode changed");
+    assertFalse(
+        m_scheduler.isRunning(commandScopedCommand),
+        "Command-scoped default command should have stopped when its parent command stopped");
+    assertTrue(
+        m_scheduler.isRunning(initialDefaultCommand), "Global default command should be active");
+  }
+
+  @Test
+  void defaultCommandChangingDefaultCommand() {
+    var mech =
+        new Mechanism("Mech", m_scheduler) {
+          Command makeCommand1() {
+            return run(co -> {
+                  setDefaultCommand(makeCommand2());
+                  // One-shot default commands won't appear in the `getRunningCommandsFor` list,
+                  // We need to park the coroutine so we can observe the change
+                  co.park();
+                })
+                .named("Command 1");
+          }
+
+          Command makeCommand2() {
+            return run(co -> {
+                  setDefaultCommand(makeCommand1());
+                  co.park();
+                })
+                .named("Command 2");
+          }
+        };
+
+    mech.setDefaultCommand(mech.makeCommand1());
+
+    // Command 1 and Command 2 should alternate as default commands
+    for (int i = 0; i < 10; i++) {
+      m_scheduler.run();
+
+      String expectedDefaultCommand = i % 2 == 0 ? "Command 1" : "Command 2";
+      assertEquals(
+          List.of(expectedDefaultCommand),
+          m_scheduler.getRunningCommandsFor(mech).stream().map(Command::name).toList());
+    }
+
+    // Check for memory leaks. If every call to `setDefaultCommand` were to add a new binding,
+    // we'd have 1 binding object per loop and quickly use a ton of memory
+    var defaultCommandBindings = m_scheduler.getDefaultCommandBindingsFor(mech);
+    assertEquals(
+        List.of("Mech[IDLE]", "Command 1", "Command 2", "Command 1"),
+        defaultCommandBindings.stream().map(b -> b.command().name()).toList());
+
+    m_scheduler.run();
+    assertEquals(
+        List.of("Mech[IDLE]", "Command 1", "Command 2"),
+        defaultCommandBindings.stream().map(b -> b.command().name()).toList());
+  }
+}