[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/main/java/org/wpilib/command3/Scheduler.java

@@ -95,7 +95,18 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
  * protobuf serializer. However, it is up to the user to log those events themselves.
  */
 public final class Scheduler implements ProtobufSerializable {
-  private final Map<Mechanism, Command> m_defaultCommands = new LinkedHashMap<>();
+  /**
+   * The default command bindings for each mechanism. Binding lists are ordered by priority; the
+   * last element in the list is the highest priority default command to be used. Bindings need to
+   * be periodically checked and removed when they're inactive.
+   */
+  private final Map<Mechanism, List<Binding>> m_defaultCommandBindings = new LinkedHashMap<>();
+
+  /**
+   * All bindings attached to this scheduler. This lets us cancel commands tied to scopes that go
+   * inactive. Bindings need to be periodically checked and removed when they're inactive.
+   */
+  private final Collection<Binding> m_activeBindings = new ArrayList<>();
 
   /** The set of commands scheduled since the start of the previous run. */
   private final SequencedSet<CommandState> m_queuedToRun = new LinkedHashSet<>();
@@ -164,14 +175,24 @@ public final class Scheduler implements ProtobufSerializable {
   private Scheduler() {}
 
   /**
-   * Sets the default command for a mechanism. The command must require that mechanism, and cannot
-   * require any other mechanisms.
+   * Sets the default command for a mechanism. The command must require that mechanism and cannot
+   * require any other mechanisms. If another default command has already been set for this
+   * mechanism, the one provided will supersede it.
+   *
+   * <p>If this is called inside a running opmode or in a running command, the default command
+   * setting will only apply while that opmode or command is active. When the opmode or command
+   * exits, the previous default command setting will be restored.
+   *
+   * <p>Commands running as default commands may call this method to change their mechanism's
+   * default command on the fly. The new default command will take effect at the end of the
+   * scheduler's loop cycle.
    *
    * @param mechanism the mechanism for which to set the default command
    * @param defaultCommand the default command to execute on the mechanism
    * @throws IllegalArgumentException if the command does not meet the requirements for being a
    *     default command
    */
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
   public void setDefaultCommand(Mechanism mechanism, Command defaultCommand) {
     if (!defaultCommand.requires(mechanism)) {
       throw new IllegalArgumentException(
@@ -183,17 +204,51 @@ public final class Scheduler implements ProtobufSerializable {
           "A mechanism's default command cannot require other mechanisms");
     }
 
-    m_defaultCommands.put(mechanism, defaultCommand);
+    var currentCommand = currentCommand();
+    BindingScope scope = BindingScope.createNarrowestScope(this);
+
+    var binding =
+        new Binding(
+            scope,
+            BindingType.CONTINUOUSLY_SCHEDULE_WHILE_HIGH,
+            defaultCommand,
+            new Throwable().getStackTrace());
+
+    var currentDefaultCommand = getDefaultCommandFor(mechanism);
+    m_defaultCommandBindings.computeIfAbsent(mechanism, k -> new ArrayList<>()).add(binding);
+
+    if (currentCommand != null && currentCommand != currentDefaultCommand) {
+      // User called `setDefaultCommand` inside another command.
+      // Immediately reprocess the default commands for this mechanism to ensure it's in sync with
+      // the rest of the commands in the scheduler. This is required because we normally schedule
+      // the default commands at the start of the scheduler `run()` method, so the new default
+      // command wouldn't be handled until the next run (ie, the previous default command would
+      // still be active for the current iteration)
+      //
+      // Note that we cannot do this if the current default command is the caller because commands
+      // cannot be canceled while mounted.
+      processDefaultCommands(mechanism);
+    }
   }
 
   /**
-   * Gets the default command set for a mechanism.
+   * Gets the default command currently used for a mechanism.
    *
    * @param mechanism The mechanism
    * @return The default command, or null if no default command was ever set
    */
   public Command getDefaultCommandFor(Mechanism mechanism) {
-    return m_defaultCommands.get(mechanism);
+    var bindings = m_defaultCommandBindings.getOrDefault(mechanism, Collections.emptyList());
+    if (bindings.isEmpty()) {
+      return null;
+    }
+
+    return bindings.getLast().command();
+  }
+
+  // package-private helper for unit test access
+  List<Binding> getDefaultCommandBindingsFor(Mechanism mechanism) {
+    return m_defaultCommandBindings.getOrDefault(mechanism, Collections.emptyList());
   }
 
   /**
@@ -275,17 +330,7 @@ public final class Scheduler implements ProtobufSerializable {
     // This prevents commands from outliving the opmodes that scheduled them, or from outliving
     // their parents (eg if someone writes a command that manually calls schedule(Command) instead
     // of using triggers to do so).
-    Command currentCommand = currentCommand();
-    long currentOpmode = OpModeFetcher.getFetcher().getOpModeId();
-
-    BindingScope scope;
-    if (currentCommand != null) {
-      scope = BindingScope.forCommand(this, currentCommand);
-    } else if (currentOpmode != 0) {
-      scope = BindingScope.forOpmode(currentOpmode);
-    } else {
-      scope = BindingScope.global();
-    }
+    BindingScope scope = BindingScope.createNarrowestScope(this);
 
     // Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier
     //       stack frame filtering and modification.
@@ -319,6 +364,11 @@ public final class Scheduler implements ProtobufSerializable {
       }
     }
 
+    // Track this binding so we can disable it when it's out of scope.
+    // Note that, even though triggers can clean themselves up, commands that are manually scheduled
+    // cannot do the same, so we have to track them in the scheduler.
+    m_activeBindings.add(binding);
+
     // Evict conflicting on-deck commands
     // We check above if the input command is lower priority than any of these,
     // so at this point we're guaranteed to be >= priority than anything already on deck
@@ -491,6 +541,8 @@ public final class Scheduler implements ProtobufSerializable {
    * Updates the command scheduler. This will run operations in the following order:
    *
    * <ol>
+   *   <li>Cancel any commands bound to scopes that have gone inactive, such as having been
+   *       scheduled in an opmode that's no longer selected on the driverstation
    *   <li>Run sideloaded functions from {@link #sideload(Consumer)} and {@link
    *       #addPeriodic(Runnable)}
    *   <li>Update trigger bindings to queue and cancel bound commands
@@ -506,6 +558,9 @@ public final class Scheduler implements ProtobufSerializable {
   public void run() {
     final long startMicros = RobotController.getTime();
 
+    // Cancel any commands with stale binding scopes
+    cancelStaleBindings();
+
     // Sideloads may change some state that affects triggers. Run them first.
     runPeriodicSideloads();
 
@@ -526,6 +581,17 @@ public final class Scheduler implements ProtobufSerializable {
     m_lastRunTimeMs = Milliseconds.convertFrom(endMicros - startMicros, Microseconds);
   }
 
+  private void cancelStaleBindings() {
+    for (var iterator = m_activeBindings.iterator(); iterator.hasNext(); ) {
+      var binding = iterator.next();
+      if (binding.scope().active()) {
+        continue;
+      }
+      cancel(binding.command());
+      iterator.remove();
+    }
+  }
+
   private void promoteScheduledCommands() {
     // Clear any commands that conflict with the scheduled set
     for (var queuedState : m_queuedToRun) {
@@ -696,18 +762,49 @@ public final class Scheduler implements ProtobufSerializable {
   }
 
   private void scheduleDefaultCommands() {
-    // Schedule the default commands for every mechanism that doesn't currently have a running or
-    // scheduled command.
-    m_defaultCommands.forEach(
-        (mechanism, defaultCommand) -> {
-          if (m_runningCommands.keySet().stream().noneMatch(c -> c.requires(mechanism))
-              && m_queuedToRun.stream().noneMatch(c -> c.command().requires(mechanism))
-              && defaultCommand != null) {
-            // Nothing currently running or scheduled
-            // Schedule the mechanism's default command, if it has one
-            schedule(defaultCommand);
+    m_defaultCommandBindings.keySet().forEach(this::processDefaultCommands);
+  }
+
+  private void processDefaultCommands(Mechanism mechanism) {
+    var bindings = m_defaultCommandBindings.get(mechanism);
+
+    // Remove default command bindings that are no longer active.
+    // If a default command is running when its scope goes inactive, also be sure to cancel it.
+    bindings.removeIf(
+        b -> {
+          if (!b.scope().active()) {
+            cancel(b.command());
+            return true;
           }
+          return false;
         });
+
+    if (bindings.isEmpty()) {
+      // Nothing to do. No active bindings remain.
+      return;
+    }
+
+    // Cancel any default command except the narrowest-scoped one (the last binding in the list)
+    for (int i = 0; i < bindings.size() - 1; i++) {
+      Command widerScopeDefaultCommand = bindings.get(i).command();
+      cancel(widerScopeDefaultCommand);
+    }
+
+    // Check if the mechanism is currently in use. We can queue the default command if it's not.
+    for (Command runningCommand : m_runningCommands.keySet()) {
+      if (runningCommand.requires(mechanism)) {
+        return;
+      }
+    }
+    for (CommandState queuedState : m_queuedToRun) {
+      if (queuedState.command().requires(mechanism)) {
+        return;
+      }
+    }
+
+    // Nothing currently running or queued that needs this mechanism. Queue the default command.
+    var defaultCommand = bindings.getLast();
+    schedule(defaultCommand);
   }
 
   /**