[commands] Add DeferredCommand (#5566)

Allows commands to be constructed at runtime without proxying.

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

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj2.command;
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import java.util.Map;
+import java.util.Set;
 import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
 
@@ -152,6 +153,18 @@ public final class Commands {
     return new SelectCommand(commands, selector);
   }
 
+  /**
+   * Runs the command supplied by the supplier.
+   *
+   * @param supplier the command supplier
+   * @param requirements the set of requirements for this command
+   * @return the command
+   * @see DeferredCommand
+   */
+  public static Command defer(Supplier<Command> supplier, Set<Subsystem> requirements) {
+    return new DeferredCommand(supplier, 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 called when the command is

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

@@ -0,0 +1,78 @@
+// 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.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.Set;
+import java.util.function.Supplier;
+
+/**
+ * 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.
+ *
+ * <p>Note that the supplier <i>must</i> create a new Command each call. For selecting one of a
+ * preallocated set of commands, use {@link SelectCommand}.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class DeferredCommand extends Command {
+  private final Command m_nullCommand =
+      new PrintCommand("[DeferredCommand] Supplied command was null!");
+
+  private final Supplier<Command> m_supplier;
+  private Command m_command = m_nullCommand;
+
+  /**
+   * Creates a new DeferredCommand that runs the supplied command when initialized, and ends when it
+   * ends. Useful for lazily creating commands at runtime. The {@link 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. This is a {@link Set} to prevent accidental
+   *     omission of command requirements. Use {@link Set#of()} to easily construct a requirement
+   *     set.
+   */
+  public DeferredCommand(Supplier<Command> supplier, Set<Subsystem> requirements) {
+    m_supplier = requireNonNullParam(supplier, "supplier", "DeferredCommand");
+    addRequirements(requirements.toArray(new Subsystem[0]));
+  }
+
+  @Override
+  public void initialize() {
+    var cmd = m_supplier.get();
+    if (cmd != null) {
+      m_command = cmd;
+      CommandScheduler.getInstance().registerComposedCommands(m_command);
+    }
+    m_command.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_command.execute();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_command.isFinished();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_command.end(interrupted);
+    m_command = m_nullCommand;
+  }
+
+  @Override
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty(
+        "deferred", () -> m_command == m_nullCommand ? "null" : m_command.getName(), null);
+  }
+}

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

@@ -4,6 +4,9 @@
 
 package edu.wpi.first.wpilibj2.command;
 
+import java.util.Set;
+import java.util.function.Supplier;
+
 /**
  * A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
  * framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and
@@ -133,4 +136,16 @@ public interface Subsystem {
   default Command runEnd(Runnable run, Runnable end) {
     return Commands.runEnd(run, end, this);
   }
+
+  /**
+   * Constructs a {@link DeferredCommand} with the provided supplier. This subsystem is added as a
+   * requirement.
+   *
+   * @param supplier the command supplier.
+   * @return the command.
+   * @see DeferredCommand
+   */
+  default Command defer(Supplier<Command> supplier) {
+    return Commands.defer(supplier, Set.of(this));
+  }
 }

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

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

@@ -0,0 +1,85 @@
+// 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 org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.only;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import java.util.Set;
+import java.util.function.Supplier;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
+class DeferredCommandTest extends CommandTestBase {
+  @ParameterizedTest
+  @ValueSource(booleans = {true, false})
+  void deferredFunctionsTest(boolean interrupted) {
+    MockCommandHolder innerCommand = new MockCommandHolder(false);
+    DeferredCommand command = new DeferredCommand(innerCommand::getMock, Set.of());
+
+    command.initialize();
+    verify(innerCommand.getMock()).initialize();
+
+    command.execute();
+    verify(innerCommand.getMock()).execute();
+
+    assertFalse(command.isFinished());
+    verify(innerCommand.getMock()).isFinished();
+
+    innerCommand.setFinished(true);
+    assertTrue(command.isFinished());
+    verify(innerCommand.getMock(), times(2)).isFinished();
+
+    command.end(interrupted);
+    verify(innerCommand.getMock()).end(interrupted);
+  }
+
+  @SuppressWarnings("unchecked")
+  @Test
+  void deferredSupplierOnlyCalledDuringInit() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Supplier<Command> supplier = (Supplier<Command>) mock(Supplier.class);
+      when(supplier.get()).thenReturn(Commands.none(), Commands.none());
+
+      DeferredCommand command = new DeferredCommand(supplier, Set.of());
+      verify(supplier, never()).get();
+
+      scheduler.schedule(command);
+      verify(supplier, only()).get();
+      scheduler.run();
+
+      scheduler.schedule(command);
+      verify(supplier, times(2)).get();
+    }
+  }
+
+  @Test
+  void deferredRequirementsTest() {
+    Subsystem subsystem = new Subsystem() {};
+    DeferredCommand command = new DeferredCommand(Commands::none, Set.of(subsystem));
+
+    assertTrue(command.getRequirements().contains(subsystem));
+  }
+
+  @Test
+  void deferredNullCommandTest() {
+    DeferredCommand command = new DeferredCommand(() -> null, Set.of());
+    assertDoesNotThrow(
+        () -> {
+          command.initialize();
+          command.execute();
+          command.isFinished();
+          command.end(false);
+        });
+  }
+}

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

@@ -0,0 +1,75 @@
+// 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 "CommandTestBase.h"
+#include "frc2/command/Commands.h"
+#include "frc2/command/DeferredCommand.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+
+class DeferredFunctionsTest : public CommandTestBaseWithParam<bool> {};
+
+TEST_P(DeferredFunctionsTest, DeferredFunctions) {
+  int initializeCount = 0;
+  int executeCount = 0;
+  int isFinishedCount = 0;
+  int endCount = 0;
+  bool finished = false;
+
+  DeferredCommand deferred{[&] {
+                             return FunctionalCommand{
+                                 [&] { initializeCount++; },
+                                 [&] { executeCount++; },
+                                 [&](bool interrupted) {
+                                   EXPECT_EQ(interrupted, GetParam());
+                                   endCount++;
+                                 },
+                                 [&] {
+                                   isFinishedCount++;
+                                   return finished;
+                                 }}
+                                 .ToPtr();
+                           },
+                           {}};
+
+  deferred.Initialize();
+  EXPECT_EQ(1, initializeCount);
+  deferred.Execute();
+  EXPECT_EQ(1, executeCount);
+  EXPECT_FALSE(deferred.IsFinished());
+  EXPECT_EQ(1, isFinishedCount);
+  finished = true;
+  EXPECT_TRUE(deferred.IsFinished());
+  EXPECT_EQ(2, isFinishedCount);
+  deferred.End(GetParam());
+  EXPECT_EQ(1, endCount);
+}
+
+INSTANTIATE_TEST_SUITE_P(DeferredCommandTests, DeferredFunctionsTest,
+                         testing::Values(true, false));
+
+TEST(DeferredCommandTest, DeferredSupplierOnlyCalledDuringInit) {
+  int count = 0;
+  DeferredCommand command{[&count] {
+                            count++;
+                            return cmd::None();
+                          },
+                          {}};
+
+  EXPECT_EQ(0, count);
+  command.Initialize();
+  EXPECT_EQ(1, count);
+  command.Execute();
+  command.IsFinished();
+  command.End(false);
+  EXPECT_EQ(1, count);
+}
+
+TEST(DeferredCommandTest, DeferredRequirements) {
+  TestSubsystem subsystem;
+  DeferredCommand command{cmd::None, {&subsystem}};
+
+  EXPECT_TRUE(command.GetRequirements().contains(&subsystem));
+}