[commands] Add C++ versions of Java-only decorators (#4457)

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

@@ -80,36 +80,44 @@ CommandPtr CommandPtr::WithInterruptBehavior(
 
 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);
+  return std::move(*this).AndThen(CommandPtr(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements)));
 }
 
 CommandPtr CommandPtr::AndThen(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
-  return std::move(*this).AndThen(std::move(toRun),
-                                  {requirements.begin(), requirements.end()});
+  return std::move(*this).AndThen(CommandPtr(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements)));
 }
 
-CommandPtr CommandPtr::BeforeStarting(
-    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
+CommandPtr CommandPtr::AndThen(CommandPtr&& next) && {
   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));
+  temp.emplace_back(std::move(next).Unwrap());
   m_ptr = std::make_unique<SequentialCommandGroup>(std::move(temp));
   return std::move(*this);
 }
 
+CommandPtr CommandPtr::BeforeStarting(
+    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
+  return std::move(*this).BeforeStarting(CommandPtr(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements)));
+}
+
 CommandPtr CommandPtr::BeforeStarting(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
-  return std::move(*this).BeforeStarting(
-      std::move(toRun), {requirements.begin(), requirements.end()});
+  return std::move(*this).BeforeStarting(CommandPtr(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements)));
+}
+
+CommandPtr CommandPtr::BeforeStarting(CommandPtr&& before) && {
+  std::vector<std::unique_ptr<Command>> temp;
+  temp.emplace_back(std::move(before).Unwrap());
+  temp.emplace_back(std::move(m_ptr));
+  m_ptr = std::make_unique<SequentialCommandGroup>(std::move(temp));
+  return std::move(*this);
 }
 
 CommandPtr CommandPtr::WithTimeout(units::second_t duration) && {
@@ -135,10 +143,38 @@ CommandPtr CommandPtr::Unless(std::function<bool()> condition) && {
   return std::move(*this);
 }
 
+CommandPtr CommandPtr::DeadlineWith(CommandPtr&& parallel) && {
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr =
+      std::make_unique<ParallelDeadlineGroup>(std::move(m_ptr), std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::AlongWith(CommandPtr&& parallel) && {
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr = std::make_unique<ParallelCommandGroup>(std::move(vec));
+  return std::move(*this);
+}
+
+CommandPtr CommandPtr::RaceWith(CommandPtr&& parallel) && {
+  std::vector<std::unique_ptr<Command>> vec;
+  vec.emplace_back(std::move(m_ptr));
+  vec.emplace_back(std::move(parallel).Unwrap());
+  m_ptr = std::make_unique<ParallelRaceGroup>(std::move(vec));
+  return std::move(*this);
+}
+
 Command* CommandPtr::get() const {
   return m_ptr.get();
 }
 
+std::unique_ptr<Command> CommandPtr::Unwrap() && {
+  return std::move(m_ptr);
+}
+
 void CommandPtr::Schedule() const {
   CommandScheduler::GetInstance().Schedule(*this);
 }
@@ -154,3 +190,12 @@ bool CommandPtr::IsScheduled() const {
 bool CommandPtr::HasRequirement(Subsystem* requirement) const {
   return m_ptr->HasRequirement(requirement);
 }
+
+std::vector<std::unique_ptr<Command>> CommandPtr::UnwrapVector(
+    std::vector<CommandPtr>&& vec) {
+  std::vector<std::unique_ptr<Command>> ptrs;
+  for (auto&& ptr : vec) {
+    ptrs.emplace_back(std::move(ptr).Unwrap());
+  }
+  return ptrs;
+}

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

@@ -7,12 +7,22 @@
 #include <functional>
 #include <initializer_list>
 #include <memory>
-#include <string>
 #include <utility>
+#include <vector>
 
 #include "frc2/command/Command.h"
 
 namespace frc2 {
+/**
+ * A wrapper around std::unique_ptr<Command> so commands have move-only
+ * semantics. Commands should only be stored and passed around when held in a
+ * CommandPtr instance. For more info, see
+ * https://github.com/wpilibsuite/allwpilib/issues/4303.
+ *
+ * Various classes in the command-based library accept a
+ * std::unique_ptr<Command>, use CommandPtr::Unwrap to convert.
+ * CommandPtr::UnwrapVector does the same for vectors.
+ */
 class CommandPtr final {
  public:
   explicit CommandPtr(std::unique_ptr<Command>&& command)
@@ -92,6 +102,16 @@ class CommandPtr final {
       std::function<void()> toRun,
       std::initializer_list<Subsystem*> requirements) &&;
 
+  /**
+   * Decorates this command with a set of commands to run after it in sequence.
+   * Often more convenient/less-verbose than constructing a new {@link
+   * SequentialCommandGroup} explicitly.
+   *
+   * @param next the commands to run next
+   * @return the decorated command
+   */
+  [[nodiscard]] CommandPtr AndThen(CommandPtr&& next) &&;
+
   /**
    * Decorates this command with a runnable to run before this command starts.
    *
@@ -114,6 +134,15 @@ class CommandPtr final {
       std::function<void()> toRun,
       wpi::span<Subsystem* const> requirements = {}) &&;
 
+  /**
+   * Decorates this command with another command to run before this command
+   * starts.
+   *
+   * @param before the command to run before this one
+   * @return the decorated command
+   */
+  [[nodiscard]] CommandPtr BeforeStarting(CommandPtr&& before) &&;
+
   /**
    * Decorates this command with a timeout.  If the specified timeout is
    * exceeded before the command finishes normally, the command will be
@@ -147,11 +176,47 @@ class CommandPtr final {
    */
   [[nodiscard]] CommandPtr Unless(std::function<bool()> condition) &&;
 
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending
+   * when the calling command ends and interrupting all the others. Often more
+   * convenient/less-verbose than constructing a new {@link
+   * ParallelDeadlineGroup} explicitly.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  [[nodiscard]] CommandPtr DeadlineWith(CommandPtr&& parallel) &&;
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending
+   * when the last command ends. Often more convenient/less-verbose than
+   * constructing a new {@link ParallelCommandGroup} explicitly.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  [[nodiscard]] CommandPtr AlongWith(CommandPtr&& parallel) &&;
+
+  /**
+   * Decorates this command with a set of commands to run parallel to it, ending
+   * when the first command ends. Often more convenient/less-verbose than
+   * constructing a new {@link ParallelRaceGroup} explicitly.
+   *
+   * @param parallel the commands to run in parallel
+   * @return the decorated command
+   */
+  [[nodiscard]] CommandPtr RaceWith(CommandPtr&& parallel) &&;
+
   /**
    * Get a raw pointer to the held command.
    */
   Command* get() const;
 
+  /**
+   * Convert to the underlying unique_ptr.
+   */
+  std::unique_ptr<Command> Unwrap() &&;
+
   /**
    * Schedules this command.
    */
@@ -182,6 +247,12 @@ class CommandPtr final {
    */
   bool HasRequirement(Subsystem* requirement) const;
 
+  /**
+   * Convert a vector of CommandPtr objects to their underlying unique_ptrs.
+   */
+  static std::vector<std::unique_ptr<Command>> UnwrapVector(
+      std::vector<CommandPtr>&& vec);
+
  private:
   std::unique_ptr<Command> m_ptr;
 };