[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});
+}