[commands] Add DeferredCommand (#5566)

Allows commands to be constructed at runtime without proxying.

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

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj2.command;
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import java.util.Map;
+import java.util.Set;
 import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
 
@@ -152,6 +153,18 @@ public final class Commands {
     return new SelectCommand(commands, selector);
   }
 
+  /**
+   * Runs the command supplied by the supplier.
+   *
+   * @param supplier the command supplier
+   * @param requirements the set of requirements for this command
+   * @return the command
+   * @see DeferredCommand
+   */
+  public static Command defer(Supplier<Command> supplier, Set<Subsystem> requirements) {
+    return new DeferredCommand(supplier, requirements);
+  }
+
   /**
    * Constructs a command that schedules the command returned from the supplier when initialized,
    * and ends when it is no longer scheduled. The supplier is called when the command is

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.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 edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.Set;
+import java.util.function.Supplier;
+
+/**
+ * Defers Command construction to runtime. Runs the command returned by the supplier when this
+ * command is initialized, and ends when it ends. Useful for performing runtime tasks before
+ * creating a new command. If this command is interrupted, it will cancel the command.
+ *
+ * <p>Note that the supplier <i>must</i> create a new Command each call. For selecting one of a
+ * preallocated set of commands, use {@link SelectCommand}.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class DeferredCommand extends Command {
+  private final Command m_nullCommand =
+      new PrintCommand("[DeferredCommand] Supplied command was null!");
+
+  private final Supplier<Command> m_supplier;
+  private Command m_command = m_nullCommand;
+
+  /**
+   * Creates a new DeferredCommand that runs the supplied command when initialized, and ends when it
+   * ends. Useful for lazily creating commands at runtime. The {@link Supplier} will be called each
+   * time this command is initialized. The Supplier <i>must</i> create a new Command each call.
+   *
+   * @param supplier The command supplier
+   * @param requirements The command requirements. This is a {@link Set} to prevent accidental
+   *     omission of command requirements. Use {@link Set#of()} to easily construct a requirement
+   *     set.
+   */
+  public DeferredCommand(Supplier<Command> supplier, Set<Subsystem> requirements) {
+    m_supplier = requireNonNullParam(supplier, "supplier", "DeferredCommand");
+    addRequirements(requirements.toArray(new Subsystem[0]));
+  }
+
+  @Override
+  public void initialize() {
+    var cmd = m_supplier.get();
+    if (cmd != null) {
+      m_command = cmd;
+      CommandScheduler.getInstance().registerComposedCommands(m_command);
+    }
+    m_command.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_command.execute();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_command.isFinished();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_command.end(interrupted);
+    m_command = m_nullCommand;
+  }
+
+  @Override
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty(
+        "deferred", () -> m_command == m_nullCommand ? "null" : m_command.getName(), null);
+  }
+}

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

@@ -4,6 +4,9 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import java.util.Set;
+import java.util.function.Supplier;
+
 /**
  * A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
  * framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and
@@ -133,4 +136,16 @@ public interface Subsystem {
   default Command runEnd(Runnable run, Runnable end) {
     return Commands.runEnd(run, end, this);
   }
+
+  /**
+   * Constructs a {@link DeferredCommand} with the provided supplier. This subsystem is added as a
+   * requirement.
+   *
+   * @param supplier the command supplier.
+   * @return the command.
+   * @see DeferredCommand
+   */
+  default Command defer(Supplier<Command> supplier) {
+    return Commands.defer(supplier, Set.of(this));
+  }
 }