Add sequence and parallel groups that run the default commands of idle subsystems

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NonBlockingCommands.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 edu.wpi.first.wpilibj2.command;
+
+import java.util.HashSet;
+import java.util.Set;
+import java.util.concurrent.atomic.AtomicInteger;
+
+/**
+ * A utility class that constructs commands that preserve the default command behavior of their
+ * requirements.
+ */
+public final class NonBlockingCommands {
+  /**
+   * Creates a new command that requires additional subsystems without interrupting the default
+   * commands of said subsystems.
+   *
+   * @param command The base command6
+   * @param nonBlockingRequirements The requirements to add.
+   * @return A new command
+   */
+  public static Command of(Command command, Subsystem... nonBlockingRequirements) {
+    var cmd = command.deadlineFor(runDefaultCommands(Set.of(nonBlockingRequirements)));
+    cmd.addRequirements(nonBlockingRequirements);
+    return cmd;
+  }
+
+  /**
+   * Runs a group of commands in series, one after the other. Any subsystem required by the group
+   * but not by the currently running command will run its default command.
+   *
+   * @param commands the commands to include
+   * @return the command group
+   */
+  public static Command sequence(Command... commands) {
+    var allReqs = new HashSet<Subsystem>();
+    for (var command : commands) {
+      allReqs.addAll(command.getRequirements());
+    }
+    var group = new SequentialCommandGroup();
+    for (var command : commands) {
+      var requirementsToIdle = new HashSet<>(allReqs);
+      requirementsToIdle.removeAll(command.getRequirements());
+      group.addCommands(command.deadlineFor(runDefaultCommands(requirementsToIdle)));
+    }
+    return group.withName("NonBlockingSequentialCommandGroup");
+  }
+
+  /**
+   * Runs a group of commands at the same time. Ends once all commands in the group finish. Any
+   * subsystem required by the group but not by the currently running command will run its default
+   * command.
+   *
+   * @param commands the commands to include
+   * @return the command group
+   */
+  public static Command parallel(Command... commands) {
+    var group = new ParallelCommandGroup();
+    var numEnded = new AtomicInteger();
+    for (var cmd : commands) {
+      group.addCommands(
+          cmd.finallyDo(numEnded::getAndIncrement)
+              .andThen(runDefaultCommands(cmd.getRequirements())));
+    }
+    return group
+        .until(() -> numEnded.get() == commands.length)
+        .withName("NonBlockingParallelCommandGroup");
+  }
+
+  /**
+   * Runs a group of commands at the same time. Ends once a specific command finishes, and cancels
+   * the others. Any subsystem required by the group but not by the currently running command will
+   * run its default command.
+   *
+   * @param deadline the deadline command
+   * @param otherCommands the other commands to include
+   * @return the command group
+   * @see ParallelDeadlineGroup
+   * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
+   */
+  public static Command deadline(Command deadline, Command... otherCommands) {
+    return parallel(otherCommands)
+        .withDeadline(deadline)
+        .withName("NonBlockingParallelDeadlineGroup");
+  }
+
+  private static Command runDefaultCommands(Set<Subsystem> subsystems) {
+    var group = new ParallelCommandGroup();
+    for (var subsystem : subsystems) {
+      group.addCommands(new WrapperCommand(subsystem.getDefaultCommand(), false) {});
+    }
+    return group;
+  }
+
+  private NonBlockingCommands() {
+    throw new UnsupportedOperationException("This is a utility class");
+  }
+}

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java

@@ -26,7 +26,20 @@ public abstract class WrapperCommand extends Command {
    */
   @SuppressWarnings("this-escape")
   protected WrapperCommand(Command command) {
-    CommandScheduler.getInstance().registerComposedCommands(command);
+    this(command, true);
+  }
+
+  /**
+   * Wrap a command.
+   *
+   * @param command the command being wrapped. Trying to directly schedule this command or add it to
+   *     a composition will throw an exception.
+   * @param registerComposed whether the command should be registered as a composed command.
+   */
+  WrapperCommand(Command command, boolean registerComposed) {
+    if (registerComposed) {
+      CommandScheduler.getInstance().registerComposedCommands(command);
+    }
     m_command = command;
     // copy the wrapped command's name
     setName(command.getName());