[commands] Change grouping decorator impl to flatten nested group structures (#3335)

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

@@ -72,7 +72,7 @@ public interface Command {
    * @return the command with the timeout added
    */
   default ParallelRaceGroup withTimeout(double seconds) {
-    return new ParallelRaceGroup(this, new WaitCommand(seconds));
+    return raceWith(new WaitCommand(seconds));
   }
 
   /**
@@ -91,7 +91,7 @@ public interface Command {
    * @return the command with the interrupt condition added
    */
   default ParallelRaceGroup withInterrupt(BooleanSupplier condition) {
-    return new ParallelRaceGroup(this, new WaitUntilCommand(condition));
+    return raceWith(new WaitUntilCommand(condition));
   }
 
   /**
@@ -108,7 +108,23 @@ public interface Command {
    * @return the decorated command
    */
   default SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
-    return new SequentialCommandGroup(new InstantCommand(toRun, requirements), this);
+    return beforeStarting(new InstantCommand(toRun, requirements));
+  }
+
+  /**
+   * Decorates this command with another command to run before this command starts.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
+   *
+   * @param before the command to run before this one
+   * @return the decorated command
+   */
+  default SequentialCommandGroup beforeStarting(Command before) {
+    return new SequentialCommandGroup(before, this);
   }
 
   /**
@@ -125,7 +141,7 @@ public interface Command {
    * @return the decorated command
    */
   default SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
-    return new SequentialCommandGroup(this, new InstantCommand(toRun, requirements));
+    return andThen(new InstantCommand(toRun, requirements));
   }
 
   /**
@@ -266,10 +282,7 @@ public interface Command {
   }
 
   /**
-   * Whether the command requires a given subsystem. Named "hasRequirement" rather than "requires"
-   * to avoid confusion with {@link
-   * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)} - this
-   * may be able to be changed in a few years.
+   * Whether the command requires a given subsystem.
    *
    * @param requirement the subsystem to inquire about
    * @return whether the subsystem is required

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

@@ -86,11 +86,17 @@ public class ParallelCommandGroup extends CommandGroupBase {
 
   @Override
   public boolean isFinished() {
-    return !m_commands.values().contains(true);
+    return !m_commands.containsValue(true);
   }
 
   @Override
   public boolean runsWhenDisabled() {
     return m_runWhenDisabled;
   }
+
+  @Override
+  public ParallelCommandGroup alongWith(Command... parallel) {
+    addCommands(parallel);
+    return this;
+  }
 }

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

@@ -117,4 +117,10 @@ public class ParallelDeadlineGroup extends CommandGroupBase {
   public boolean runsWhenDisabled() {
     return m_runWhenDisabled;
   }
+
+  @Override
+  public ParallelDeadlineGroup deadlineWith(Command... parallel) {
+    addCommands(parallel);
+    return this;
+  }
 }

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

@@ -86,4 +86,10 @@ public class ParallelRaceGroup extends CommandGroupBase {
   public boolean runsWhenDisabled() {
     return m_runWhenDisabled;
   }
+
+  @Override
+  public ParallelRaceGroup raceWith(Command... parallel) {
+    addCommands(parallel);
+    return this;
+  }
 }

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

@@ -50,4 +50,9 @@ public class PerpetualCommand extends CommandBase {
   public boolean runsWhenDisabled() {
     return m_command.runsWhenDisabled();
   }
+
+  @Override
+  public PerpetualCommand perpetually() {
+    return this;
+  }
 }

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

@@ -92,4 +92,28 @@ public class SequentialCommandGroup extends CommandGroupBase {
   public boolean runsWhenDisabled() {
     return m_runWhenDisabled;
   }
+
+  @Override
+  public SequentialCommandGroup beforeStarting(Command before) {
+    // store all the commands
+    var commands = new ArrayList<Command>();
+    commands.add(before);
+    commands.addAll(m_commands);
+
+    // reset current state
+    commands.forEach(CommandGroupBase::clearGroupedCommand);
+    m_commands.clear();
+    m_requirements.clear();
+    m_runWhenDisabled = true;
+
+    // add them back
+    addCommands(commands.toArray(Command[]::new));
+    return this;
+  }
+
+  @Override
+  public SequentialCommandGroup andThen(Command... next) {
+    addCommands(next);
+    return this;
+  }
 }