[command] Add ignoringDisable decorator (#4305)

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

@@ -315,6 +315,21 @@ public interface Command {
     return new ConditionalCommand(new InstantCommand(), this, condition);
   }
 
+  /**
+   * Decorates this command to run or stop when disabled.
+   *
+   * @param doesRunWhenDisabled true to run when disabled.
+   * @return the decorated command
+   */
+  default WrapperCommand ignoringDisable(boolean doesRunWhenDisabled) {
+    return new WrapperCommand(this) {
+      @Override
+      public boolean runsWhenDisabled() {
+        return doesRunWhenDisabled;
+      }
+    };
+  }
+
   /**
    * Schedules this command.
    *

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

@@ -0,0 +1,107 @@
+// 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.wpilibj2.command.CommandGroupBase.registerGroupedCommands;
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
+
+import java.util.Set;
+
+/**
+ * A class used internally to wrap commands while overriding a specific method; all other methods
+ * will call through to the wrapped command.
+ *
+ * <p>Wrapped commands may only be used through the wrapper, trying to directly schedule them or add
+ * them to a group will throw an exception.
+ */
+public abstract class WrapperCommand implements Command {
+  protected final Command m_command;
+
+  /**
+   * Wrap a command.
+   *
+   * @param command the command being wrapped. Trying to directly schedule this command or add it to
+   *     a group will throw an exception.
+   */
+  protected WrapperCommand(Command command) {
+    requireUngrouped(command);
+    registerGroupedCommands(command);
+    m_command = command;
+  }
+
+  /** The initial subroutine of a command. Called once when the command is initially scheduled. */
+  @Override
+  public void initialize() {
+    m_command.initialize();
+  }
+
+  /** The main body of a command. Called repeatedly while the command is scheduled. */
+  @Override
+  public void execute() {
+    m_command.execute();
+  }
+
+  /**
+   * The action to take when the command ends. Called when either the command finishes normally, or
+   * when it interrupted/canceled.
+   *
+   * <p>Do not schedule commands here that share requirements with this command. Use {@link
+   * #andThen(Command...)} instead.
+   *
+   * @param interrupted whether the command was interrupted/canceled
+   */
+  @Override
+  public void end(boolean interrupted) {
+    m_command.end(interrupted);
+  }
+
+  /**
+   * Whether the command has finished. Once a command finishes, the scheduler will call its end()
+   * method and un-schedule it.
+   *
+   * @return whether the command has finished.
+   */
+  @Override
+  public boolean isFinished() {
+    return m_command.isFinished();
+  }
+
+  /**
+   * Specifies the set of subsystems used by this command. Two commands cannot use the same
+   * subsystem at the same time. If the command is scheduled as interruptible and another command is
+   * scheduled that shares a requirement, the command will be interrupted. Else, the command will
+   * not be scheduled. If no subsystems are required, return an empty set.
+   *
+   * <p>Note: it is recommended that user implementations contain the requirements as a field, and
+   * return that field here, rather than allocating a new set every time this is called.
+   *
+   * @return the set of subsystems that are required
+   */
+  @Override
+  public Set<Subsystem> getRequirements() {
+    return m_command.getRequirements();
+  }
+
+  /**
+   * Whether the given command should run when the robot is disabled. Override to return true if the
+   * command should run when disabled.
+   *
+   * @return whether the command should run when the robot is disabled
+   */
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_command.runsWhenDisabled();
+  }
+
+  /**
+   * Gets the name of this Command.
+   *
+   * @return Name
+   */
+  @Override
+  public String getName() {
+    return m_command.getName();
+  }
+}

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

@@ -4,6 +4,7 @@
 
 #include "frc2/command/Command.h"
 
+#include "frc2/command/CommandHelper.h"
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/ConditionalCommand.h"
 #include "frc2/command/EndlessCommand.h"
@@ -17,6 +18,7 @@
 #include "frc2/command/SequentialCommandGroup.h"
 #include "frc2/command/WaitCommand.h"
 #include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/WrapperCommand.h"
 
 using namespace frc2;
 
@@ -47,6 +49,24 @@ ParallelRaceGroup Command::Until(std::function<bool()> condition) && {
   return ParallelRaceGroup(std::move(temp));
 }
 
+std::unique_ptr<Command> Command::IgnoringDisable(bool doesRunWhenDisabled) && {
+  class RunsWhenDisabledCommand
+      : public CommandHelper<WrapperCommand, RunsWhenDisabledCommand> {
+   public:
+    RunsWhenDisabledCommand(std::unique_ptr<Command>&& command,
+                            bool doesRunWhenDisabled)
+        : CommandHelper(std::move(command)),
+          m_runsWhenDisabled(doesRunWhenDisabled) {}
+    bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+   private:
+    bool m_runsWhenDisabled;
+  };
+
+  return std::make_unique<RunsWhenDisabledCommand>(
+      std::move(*this).TransferOwnership(), doesRunWhenDisabled);
+}
+
 ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));

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

@@ -0,0 +1,35 @@
+// 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.
+
+#include "frc2/command/WrapperCommand.h"
+
+using namespace frc2;
+
+WrapperCommand::WrapperCommand(std::unique_ptr<Command>&& command) {
+  if (!CommandGroupBase::RequireUngrouped(*command)) {
+    return;
+  }
+  m_command = std::move(command);
+  m_command->SetGrouped(true);
+}
+
+void WrapperCommand::Initialize() {
+  m_command->Initialize();
+}
+
+void WrapperCommand::Execute() {
+  m_command->Execute();
+}
+
+bool WrapperCommand::IsFinished() {
+  return m_command->IsFinished();
+}
+
+void WrapperCommand::End(bool interrupted) {
+  m_command->End(interrupted);
+}
+
+bool WrapperCommand::RunsWhenDisabled() const {
+  return m_command->RunsWhenDisabled();
+}

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

@@ -229,6 +229,14 @@ class Command {
    */
   virtual ConditionalCommand Unless(std::function<bool()> condition) &&;
 
+  /**
+   * Decorates this command to run or stop when disabled.
+   *
+   * @param doesRunWhenDisabled true to run when disabled.
+   * @return the decorated command
+   */
+  virtual std::unique_ptr<Command> IgnoringDisable(bool doesRunWhenDisabled) &&;
+
   /**
    * Schedules this command.
    *

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

@@ -0,0 +1,74 @@
+// 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.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A class used internally to wrap commands while overriding a specific method;
+ * all other methods will call through to the wrapped command.
+ *
+ * <p>Wrapped commands may only be used through the wrapper, trying to directly
+ * schedule them or add them to a group will throw an exception.
+ */
+class WrapperCommand : public CommandHelper<CommandBase, WrapperCommand> {
+ public:
+  /**
+   * Wrap a command.
+   *
+   * @param command the command being wrapped. Trying to directly schedule this
+   * command or add it to a group will throw an exception.
+   */
+  explicit WrapperCommand(std::unique_ptr<Command>&& command);
+
+  /**
+   * Wrap a command.
+   *
+   * @param command the command being wrapped. Trying to directly schedule this
+   * command or add it to a group will throw an exception.
+   */
+  template <class T, typename = std::enable_if_t<std::is_base_of_v<
+                         Command, std::remove_reference_t<T>>>>
+  explicit WrapperCommand(T&& command)
+      : WrapperCommand(std::make_unique<std::remove_reference_t<T>>(
+            std::forward<T>(command))) {}
+
+  WrapperCommand(WrapperCommand&& other) = default;
+
+  // No copy constructors for command groups
+  WrapperCommand(const WrapperCommand& other) = delete;
+
+  // Prevent template expansion from emulating copy ctor
+  WrapperCommand(WrapperCommand&) = delete;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  bool IsFinished() override;
+
+  void End(bool interrupted) override;
+
+  bool RunsWhenDisabled() const override;
+
+ protected:
+  std::unique_ptr<Command> m_command;
+};
+}  // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif

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

@@ -55,6 +55,20 @@ class CommandDecoratorTest extends CommandTestBase {
     }
   }
 
+  @Test
+  void ignoringDisableTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      var command = new RunCommand(() -> {}).ignoringDisable(true);
+
+      setDSEnabled(false);
+
+      scheduler.schedule(command);
+
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command));
+    }
+  }
+
   @Test
   void beforeStartingTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp

@@ -54,6 +54,19 @@ TEST_F(CommandDecoratorTest, Until) {
   EXPECT_FALSE(scheduler.IsScheduled(&command));
 }
 
+TEST_F(CommandDecoratorTest, IgnoringDisable) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = RunCommand([] {}, {}).IgnoringDisable(true);
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(command.get());
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(command.get()));
+}
+
 TEST_F(CommandDecoratorTest, BeforeStarting) {
   CommandScheduler scheduler = GetScheduler();