[commands] Add DeferredCommand (#5566)

Allows commands to be constructed at runtime without proxying.

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

@@ -5,6 +5,7 @@
 #include "frc2/command/Commands.h"
 
 #include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/DeferredCommand.h"
 #include "frc2/command/FunctionalCommand.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/ParallelCommandGroup.h"
@@ -82,6 +83,11 @@ CommandPtr cmd::Either(CommandPtr&& onTrue, CommandPtr&& onFalse,
       .ToPtr();
 }
 
+CommandPtr cmd::Defer(wpi::unique_function<CommandPtr()> supplier,
+                      Requirements requirements) {
+  return DeferredCommand(std::move(supplier), requirements).ToPtr();
+}
+
 CommandPtr cmd::Sequence(std::vector<CommandPtr>&& commands) {
   return SequentialCommandGroup(CommandPtr::UnwrapVector(std::move(commands)))
       .ToPtr();

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

@@ -0,0 +1,46 @@
+// 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/DeferredCommand.h"
+
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc2/command/Commands.h"
+
+using namespace frc2;
+
+DeferredCommand::DeferredCommand(wpi::unique_function<CommandPtr()> supplier,
+                                 Requirements requirements)
+    : m_supplier{std::move(supplier)} {
+  AddRequirements(requirements);
+}
+
+void DeferredCommand::Initialize() {
+  m_command = m_supplier().Unwrap();
+  CommandScheduler::GetInstance().RequireUngrouped(m_command.get());
+  m_command->SetComposed(true);
+  m_command->Initialize();
+}
+
+void DeferredCommand::Execute() {
+  m_command->Execute();
+}
+
+void DeferredCommand::End(bool interrupted) {
+  m_command->End(interrupted);
+  m_command =
+      cmd::Print("[DeferredCommand] Lifecycle function called out-of-order!")
+          .WithName("none")
+          .Unwrap();
+}
+
+bool DeferredCommand::IsFinished() {
+  return m_command->IsFinished();
+}
+
+void DeferredCommand::InitSendable(wpi::SendableBuilder& builder) {
+  Command::InitSendable(builder);
+  builder.AddStringProperty(
+      "deferred", [this] { return m_command->GetName(); }, nullptr);
+}

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

@@ -54,3 +54,7 @@ CommandPtr Subsystem::RunEnd(std::function<void()> run,
                              std::function<void()> end) {
   return cmd::RunEnd(std::move(run), std::move(end), {this});
 }
+
+CommandPtr Subsystem::Defer(wpi::unique_function<CommandPtr()> supplier) {
+  return cmd::Defer(std::move(supplier), {this});
+}

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

@@ -142,6 +142,16 @@ CommandPtr Select(std::function<Key()> selector,
   return SelectCommand(std::move(selector), std::move(vec)).ToPtr();
 }
 
+/**
+ * Runs the command supplied by the supplier.
+ *
+ * @param supplier the command supplier
+ * @param requirements the set of requirements for this command
+ */
+[[nodiscard]]
+CommandPtr Defer(wpi::unique_function<CommandPtr()> supplier,
+                 Requirements requirements);
+
 /**
  * Constructs a command that schedules the command returned from the supplier
  * when initialized, and ends when it is no longer scheduled. The supplier is

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

@@ -0,0 +1,61 @@
+// 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
+
+#include <memory>
+#include <span>
+
+#include <wpi/FunctionExtras.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/PrintCommand.h"
+#include "frc2/command/Requirements.h"
+
+namespace frc2 {
+/**
+ * Defers Command construction to runtime. Runs the command returned by the
+ * supplier when this command is initialized, and ends when it ends. Useful for
+ * performing runtime tasks before creating a new command. If this command is
+ * interrupted, it will cancel the command.
+ *
+ * Note that the supplier <i>must</i> create a new Command each call. For
+ * selecting one of a preallocated set of commands, use SelectCommand.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+class DeferredCommand : public CommandHelper<Command, DeferredCommand> {
+ public:
+  /**
+   * Creates a new DeferredCommand that runs the supplied command when
+   * initialized, and ends when it ends. Useful for lazily
+   * creating commands at runtime. The supplier will be called each time this
+   * command is initialized. The supplier <i>must</i> create a new Command each
+   * call.
+   *
+   * @param supplier The command supplier
+   * @param requirements The command requirements.
+   *
+   */
+  DeferredCommand(wpi::unique_function<CommandPtr()> supplier,
+                  Requirements requirements);
+
+  DeferredCommand(DeferredCommand&& other) = default;
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  wpi::unique_function<CommandPtr()> m_supplier;
+  std::unique_ptr<Command> m_command;
+};
+}  // namespace frc2

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

@@ -8,6 +8,8 @@
 #include <functional>
 #include <utility>
 
+#include <wpi/FunctionExtras.h>
+
 #include "frc2/command/CommandScheduler.h"
 
 namespace frc2 {
@@ -148,5 +150,15 @@ class Subsystem {
    */
   [[nodiscard]]
   CommandPtr RunEnd(std::function<void()> run, std::function<void()> end);
+
+  /**
+   * Constructs a DeferredCommand with the provided supplier. This subsystem is
+   * added as a requirement.
+   *
+   * @param supplier the command supplier.
+   * @return the command.
+   */
+  [[nodiscard]]
+  CommandPtr Defer(wpi::unique_function<CommandPtr()> supplier);
 };
 }  // namespace frc2