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

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;