[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;
+  }
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp

@@ -26,3 +26,7 @@ void PerpetualCommand::Execute() {
 void PerpetualCommand::End(bool interrupted) {
   m_command->End(interrupted);
 }
+
+PerpetualCommand PerpetualCommand::Perpetually() && {
+  return std::move(*this);
+}

wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp

@@ -4,6 +4,8 @@
 
 #include "frc2/command/SequentialCommandGroup.h"
 
+#include "frc2/command/InstantCommand.h"
+
 using namespace frc2;
 
 SequentialCommandGroup::SequentialCommandGroup(
@@ -72,3 +74,33 @@ void SequentialCommandGroup::AddCommands(
     m_commands.emplace_back(std::move(command));
   }
 }
+
+SequentialCommandGroup SequentialCommandGroup::BeforeStarting(
+    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
+  // store all the commands
+  std::vector<std::unique_ptr<Command>> tmp;
+  tmp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  for (auto&& command : m_commands) {
+    command->SetGrouped(false);
+    tmp.emplace_back(std::move(command));
+  }
+
+  // reset current state
+  m_commands.clear();
+  m_requirements.clear();
+  m_runWhenDisabled = true;
+
+  // add the commands back
+  AddCommands(std::move(tmp));
+  return std::move(*this);
+}
+
+SequentialCommandGroup SequentialCommandGroup::AndThen(
+    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
+  std::vector<std::unique_ptr<Command>> tmp;
+  tmp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  AddCommands(std::move(tmp));
+  return std::move(*this);
+}

wpilibNewCommands/src/main/native/include/frc2/command/Command.h

@@ -107,7 +107,7 @@ class Command {
    * @param duration the timeout duration
    * @return the command with the timeout added
    */
-  ParallelRaceGroup WithTimeout(units::second_t duration) &&;
+  virtual ParallelRaceGroup WithTimeout(units::second_t duration) &&;
 
   /**
    * Decorates this command with an interrupt condition.  If the specified
@@ -118,7 +118,7 @@ class Command {
    * @param condition the interrupt condition
    * @return the command with the interrupt condition added
    */
-  ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
+  virtual ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
 
   /**
    * Decorates this command with a runnable to run before this command starts.
@@ -127,7 +127,7 @@ class Command {
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  SequentialCommandGroup BeforeStarting(
+  virtual SequentialCommandGroup BeforeStarting(
       std::function<void()> toRun,
       std::initializer_list<Subsystem*> requirements) &&;
 
@@ -138,7 +138,7 @@ class Command {
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  SequentialCommandGroup BeforeStarting(
+  virtual SequentialCommandGroup BeforeStarting(
       std::function<void()> toRun,
       wpi::span<Subsystem* const> requirements = {}) &&;
 
@@ -149,7 +149,7 @@ class Command {
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  SequentialCommandGroup AndThen(
+  virtual SequentialCommandGroup AndThen(
       std::function<void()> toRun,
       std::initializer_list<Subsystem*> requirements) &&;
 
@@ -160,7 +160,7 @@ class Command {
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  SequentialCommandGroup AndThen(
+  virtual SequentialCommandGroup AndThen(
       std::function<void()> toRun,
       wpi::span<Subsystem* const> requirements = {}) &&;
 
@@ -170,7 +170,7 @@ class Command {
    *
    * @return the decorated command
    */
-  PerpetualCommand Perpetually() &&;
+  virtual PerpetualCommand Perpetually() &&;
 
   /**
    * Decorates this command to run "by proxy" by wrapping it in a
@@ -180,7 +180,7 @@ class Command {
    *
    * @return the decorated command
    */
-  ProxyScheduleCommand AsProxy();
+  virtual ProxyScheduleCommand AsProxy();
 
   /**
    * Schedules this command.

wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h

@@ -65,6 +65,8 @@ class PerpetualCommand : public CommandHelper<CommandBase, PerpetualCommand> {
 
   void End(bool interrupted) override;
 
+  PerpetualCommand Perpetually() && override;
+
  private:
   std::unique_ptr<Command> m_command;
 };

wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h

@@ -84,6 +84,15 @@ class SequentialCommandGroup
 
   bool RunsWhenDisabled() const override;
 
+  SequentialCommandGroup BeforeStarting(
+      std::function<void()> toRun,
+      wpi::ArrayRef<Subsystem*> requirements = {}) &&
+      override;
+
+  SequentialCommandGroup AndThen(std::function<void()> toRun,
+                                 wpi::ArrayRef<Subsystem*> requirements = {}) &&
+      override;
+
  private:
   void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
 

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj2.command;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.ArgumentMatchers.anyDouble;
+import static org.mockito.ArgumentMatchers.notNull;
 import static org.mockito.Mockito.never;
 import static org.mockito.Mockito.verify;
 import static org.mockito.Mockito.when;
@@ -58,6 +59,7 @@ class WaitCommandTest extends CommandTestBase {
       MockCommandHolder command1Holder = new MockCommandHolder(true);
       Command command1 = command1Holder.getMock();
       when(command1.withTimeout(anyDouble())).thenCallRealMethod();
+      when(command1.raceWith(notNull())).thenCallRealMethod();
 
       Command timeout = command1.withTimeout(2);