[commands] C++ unique_ptr migration (#4319)

Add a CommandPtr with an internal unique_ptr to enable not needing to move the underlying classes, which is error-prone due to the potential for lambda captures.

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

@@ -35,96 +35,54 @@ void Command::Initialize() {}
 void Command::Execute() {}
 void Command::End(bool interrupted) {}
 
-ParallelRaceGroup Command::WithTimeout(units::second_t duration) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::make_unique<WaitCommand>(duration));
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  return ParallelRaceGroup(std::move(temp));
+CommandPtr Command::WithTimeout(units::second_t duration) && {
+  return CommandPtr(std::move(*this).TransferOwnership()).WithTimeout(duration);
 }
 
-ParallelRaceGroup Command::Until(std::function<bool()> condition) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  return ParallelRaceGroup(std::move(temp));
+CommandPtr Command::Until(std::function<bool()> condition) && {
+  return CommandPtr(std::move(*this).TransferOwnership())
+      .Until(std::move(condition));
 }
 
-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);
+CommandPtr Command::IgnoringDisable(bool doesRunWhenDisabled) && {
+  return CommandPtr(std::move(*this).TransferOwnership())
+      .IgnoringDisable(doesRunWhenDisabled);
 }
 
-std::unique_ptr<Command> Command::WithInterruptBehavior(
+CommandPtr Command::WithInterruptBehavior(
     InterruptionBehavior interruptBehavior) && {
-  class InterruptBehaviorCommand
-      : public CommandHelper<WrapperCommand, InterruptBehaviorCommand> {
-   public:
-    InterruptBehaviorCommand(std::unique_ptr<Command>&& command,
-                             InterruptionBehavior interruptBehavior)
-        : CommandHelper(std::move(command)),
-          m_interruptBehavior(interruptBehavior) {}
-    InterruptionBehavior GetInterruptionBehavior() const override {
-      return m_interruptBehavior;
-    }
-
-   private:
-    InterruptionBehavior m_interruptBehavior;
-  };
-
-  return std::make_unique<InterruptBehaviorCommand>(
-      std::move(*this).TransferOwnership(), interruptBehavior);
+  return CommandPtr(std::move(*this).TransferOwnership())
+      .WithInterruptBehavior(interruptBehavior);
 }
 
-ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  return ParallelRaceGroup(std::move(temp));
+CommandPtr Command::WithInterrupt(std::function<bool()> condition) && {
+  return CommandPtr(std::move(*this).TransferOwnership())
+      .Until(std::move(condition));
 }
 
-SequentialCommandGroup Command::BeforeStarting(
+CommandPtr Command::BeforeStarting(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
   return std::move(*this).BeforeStarting(
       std::move(toRun), {requirements.begin(), requirements.end()});
 }
 
-SequentialCommandGroup Command::BeforeStarting(
+CommandPtr Command::BeforeStarting(
     std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(
-      std::make_unique<InstantCommand>(std::move(toRun), requirements));
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  return SequentialCommandGroup(std::move(temp));
+  return CommandPtr(std::move(*this).TransferOwnership())
+      .BeforeStarting(std::move(toRun), requirements);
 }
 
-SequentialCommandGroup Command::AndThen(
-    std::function<void()> toRun,
-    std::initializer_list<Subsystem*> requirements) && {
+CommandPtr Command::AndThen(std::function<void()> toRun,
+                            std::initializer_list<Subsystem*> requirements) && {
   return std::move(*this).AndThen(std::move(toRun),
                                   {requirements.begin(), requirements.end()});
 }
 
-SequentialCommandGroup Command::AndThen(
-    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  temp.emplace_back(
-      std::make_unique<InstantCommand>(std::move(toRun), requirements));
-  return SequentialCommandGroup(std::move(temp));
+CommandPtr Command::AndThen(std::function<void()> toRun,
+                            wpi::span<Subsystem* const> requirements) && {
+  return CommandPtr(std::move(*this).TransferOwnership())
+      .AndThen(std::move(toRun), requirements);
 }
 
 PerpetualCommand Command::Perpetually() && {
@@ -133,22 +91,21 @@ PerpetualCommand Command::Perpetually() && {
   WPI_UNIGNORE_DEPRECATED
 }
 
-EndlessCommand Command::Endlessly() && {
-  return EndlessCommand(std::move(*this).TransferOwnership());
+CommandPtr Command::Endlessly() && {
+  return CommandPtr(std::move(*this).TransferOwnership()).Endlessly();
 }
 
-RepeatCommand Command::Repeatedly() && {
-  return RepeatCommand(std::move(*this).TransferOwnership());
+CommandPtr Command::Repeatedly() && {
+  return CommandPtr(std::move(*this).TransferOwnership()).Repeatedly();
 }
 
-ProxyScheduleCommand Command::AsProxy() {
-  return ProxyScheduleCommand(this);
+CommandPtr Command::AsProxy() && {
+  return CommandPtr(std::move(*this).TransferOwnership()).AsProxy();
 }
 
-ConditionalCommand Command::Unless(std::function<bool()> condition) && {
-  return ConditionalCommand(std::make_unique<InstantCommand>(),
-                            std::move(*this).TransferOwnership(),
-                            std::move(condition));
+CommandPtr Command::Unless(std::function<bool()> condition) && {
+  return CommandPtr(std::move(*this).TransferOwnership())
+      .Unless(std::move(condition));
 }
 
 void Command::Schedule() {

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

@@ -0,0 +1,156 @@
+// 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/CommandPtr.h"
+
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/EndlessCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/RepeatCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/WrapperCommand.h"
+
+using namespace frc2;
+
+CommandPtr CommandPtr::Repeatedly() && {
+  m_ptr = std::make_unique<RepeatCommand>(std::move(m_ptr));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::Endlessly() && {
+  m_ptr = std::make_unique<EndlessCommand>(std::move(m_ptr));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AsProxy() && {
+  m_ptr = std::make_unique<ProxyScheduleCommand>(std::move(m_ptr));
+  return std::move(*this);
+}
+
+class RunsWhenDisabledCommand : public WrapperCommand {
+ public:
+  RunsWhenDisabledCommand(std::unique_ptr<Command>&& command,
+                          bool doesRunWhenDisabled)
+      : WrapperCommand(std::move(command)),
+        m_runsWhenDisabled(doesRunWhenDisabled) {}
+
+  bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+ private:
+  bool m_runsWhenDisabled;
+};
+
+CommandPtr CommandPtr::IgnoringDisable(bool doesRunWhenDisabled) && {
+  m_ptr = std::make_unique<RunsWhenDisabledCommand>(std::move(m_ptr),
+                                                    doesRunWhenDisabled);
+  return std::move(*this);
+}
+
+using InterruptionBehavior = Command::InterruptionBehavior;
+class InterruptBehaviorCommand : public WrapperCommand {
+ public:
+  InterruptBehaviorCommand(std::unique_ptr<Command>&& command,
+                           InterruptionBehavior interruptBehavior)
+      : WrapperCommand(std::move(command)),
+        m_interruptBehavior(interruptBehavior) {}
+
+  InterruptionBehavior GetInterruptionBehavior() const override {
+    return m_interruptBehavior;
+  }
+
+ private:
+  InterruptionBehavior m_interruptBehavior;
+};
+
+CommandPtr CommandPtr::WithInterruptBehavior(
+    InterruptionBehavior interruptBehavior) && {
+  m_ptr = std::make_unique<InterruptBehaviorCommand>(std::move(m_ptr),
+                                                     interruptBehavior);
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AndThen(std::function<void()> toRun,
+                               wpi::span<Subsystem* const> requirements) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(m_ptr));
+  temp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  m_ptr = std::make_unique<SequentialCommandGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AndThen(
+    std::function<void()> toRun,
+    std::initializer_list<Subsystem*> requirements) && {
+  return std::move(*this).AndThen(std::move(toRun),
+                                  {requirements.begin(), requirements.end()});
+}
+
+CommandPtr CommandPtr::BeforeStarting(
+    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  temp.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<SequentialCommandGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::BeforeStarting(
+    std::function<void()> toRun,
+    std::initializer_list<Subsystem*> requirements) && {
+  return std::move(*this).BeforeStarting(
+      std::move(toRun), {requirements.begin(), requirements.end()});
+}
+
+CommandPtr CommandPtr::WithTimeout(units::second_t duration) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::make_unique<WaitCommand>(duration));
+  temp.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::Until(std::function<bool()> condition) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
+  temp.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(temp));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::Unless(std::function<bool()> condition) && {
+  m_ptr = std::make_unique<ConditionalCommand>(
+      std::make_unique<InstantCommand>(), std::move(m_ptr),
+      std::move(condition));
+  return std::move(*this);
+}
+
+Command* CommandPtr::get() const {
+  return m_ptr.get();
+}
+
+void CommandPtr::Schedule() const {
+  CommandScheduler::GetInstance().Schedule(*this);
+}
+
+void CommandPtr::Cancel() const {
+  CommandScheduler::GetInstance().Cancel(*this);
+}
+
+bool CommandPtr::IsScheduled() const {
+  return CommandScheduler::GetInstance().IsScheduled(*this);
+}
+
+bool CommandPtr::HasRequirement(Subsystem* requirement) const {
+  return m_ptr->HasRequirement(requirement);
+}

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

@@ -19,6 +19,7 @@
 #include <wpi/sendable/SendableRegistry.h>
 
 #include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandPtr.h"
 #include "frc2/command/Subsystem.h"
 
 using namespace frc2;
@@ -173,6 +174,10 @@ void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
   }
 }
 
+void CommandScheduler::Schedule(const CommandPtr& command) {
+  Schedule(command.get());
+}
+
 void CommandScheduler::Run() {
   if (m_impl->disabled) {
     return;
@@ -326,6 +331,10 @@ void CommandScheduler::Cancel(Command* command) {
   m_watchdog.AddEpoch(command->GetName() + ".End(true)");
 }
 
+void CommandScheduler::Cancel(const CommandPtr& command) {
+  Cancel(command.get());
+}
+
 void CommandScheduler::Cancel(wpi::span<Command* const> commands) {
   for (auto command : commands) {
     Cancel(command);
@@ -370,6 +379,10 @@ bool CommandScheduler::IsScheduled(const Command* command) const {
   return m_impl->scheduledCommands.contains(command);
 }
 
+bool CommandScheduler::IsScheduled(const CommandPtr& command) const {
+  return m_impl->scheduledCommands.contains(command.get());
+}
+
 Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
   auto find = m_impl->requirements.find(subsystem);
   if (find != m_impl->requirements.end()) {

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp

@@ -13,6 +13,11 @@ Button Button::WhenPressed(Command* command) {
   return *this;
 }
 
+Button Button::WhenPressed(CommandPtr&& command) {
+  WhenActive(std::move(command));
+  return *this;
+}
+
 Button Button::WhenPressed(std::function<void()> toRun,
                            std::initializer_list<Subsystem*> requirements) {
   WhenActive(std::move(toRun), requirements);
@@ -30,6 +35,11 @@ Button Button::WhileHeld(Command* command) {
   return *this;
 }
 
+Button Button::WhileHeld(CommandPtr&& command) {
+  WhileActiveContinous(std::move(command));
+  return *this;
+}
+
 Button Button::WhileHeld(std::function<void()> toRun,
                          std::initializer_list<Subsystem*> requirements) {
   WhileActiveContinous(std::move(toRun), requirements);
@@ -47,11 +57,21 @@ Button Button::WhenHeld(Command* command) {
   return *this;
 }
 
+Button Button::WhenHeld(CommandPtr&& command) {
+  WhileActiveOnce(std::move(command));
+  return *this;
+}
+
 Button Button::WhenReleased(Command* command) {
   WhenInactive(command);
   return *this;
 }
 
+Button Button::WhenReleased(CommandPtr&& command) {
+  WhenInactive(std::move(command));
+  return *this;
+}
+
 Button Button::WhenReleased(std::function<void()> toRun,
                             std::initializer_list<Subsystem*> requirements) {
   WhenInactive(std::move(toRun), requirements);
@@ -69,6 +89,11 @@ Button Button::ToggleWhenPressed(Command* command) {
   return *this;
 }
 
+Button Button::ToggleWhenPressed(CommandPtr&& command) {
+  ToggleWhenActive(std::move(command));
+  return *this;
+}
+
 Button Button::CancelWhenPressed(Command* command) {
   CancelWhenActive(command);
   return *this;

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -18,6 +18,11 @@ Trigger Trigger::WhenActive(Command* command) {
   return *this;
 }
 
+Trigger Trigger::WhenActive(CommandPtr&& command) {
+  this->Rising().IfHigh([command = std::move(command)] { command.Schedule(); });
+  return *this;
+}
+
 Trigger Trigger::WhenActive(std::function<void()> toRun,
                             std::initializer_list<Subsystem*> requirements) {
   return WhenActive(std::move(toRun),
@@ -35,6 +40,13 @@ Trigger Trigger::WhileActiveContinous(Command* command) {
   return *this;
 }
 
+Trigger Trigger::WhileActiveContinous(CommandPtr&& command) {
+  auto ptr = std::make_shared<CommandPtr>(std::move(command));
+  this->IfHigh([ptr] { ptr->Schedule(); });
+  this->Falling().IfHigh([ptr] { ptr->Cancel(); });
+  return *this;
+}
+
 Trigger Trigger::WhileActiveContinous(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) {
@@ -53,11 +65,24 @@ Trigger Trigger::WhileActiveOnce(Command* command) {
   return *this;
 }
 
+Trigger Trigger::WhileActiveOnce(CommandPtr&& command) {
+  auto ptr = std::make_shared<CommandPtr>(std::move(command));
+  this->Rising().IfHigh([ptr] { ptr->Schedule(); });
+  this->Falling().IfHigh([ptr] { ptr->Cancel(); });
+  return *this;
+}
+
 Trigger Trigger::WhenInactive(Command* command) {
   this->Falling().IfHigh([command] { command->Schedule(); });
   return *this;
 }
 
+Trigger Trigger::WhenInactive(CommandPtr&& command) {
+  this->Falling().IfHigh(
+      [command = std::move(command)] { command.Schedule(); });
+  return *this;
+}
+
 Trigger Trigger::WhenInactive(std::function<void()> toRun,
                               std::initializer_list<Subsystem*> requirements) {
   return WhenInactive(std::move(toRun),
@@ -80,6 +105,17 @@ Trigger Trigger::ToggleWhenActive(Command* command) {
   return *this;
 }
 
+Trigger Trigger::ToggleWhenActive(CommandPtr&& command) {
+  this->Rising().IfHigh([command = std::move(command)] {
+    if (command.IsScheduled()) {
+      command.Cancel();
+    } else {
+      command.Schedule();
+    }
+  });
+  return *this;
+}
+
 Trigger Trigger::CancelWhenActive(Command* command) {
   this->Rising().IfHigh([command] { command->Cancel(); });
   return *this;