From c87f8fd53824dfe624e930b13c5d9c2d4c74b212 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Thu, 26 Oct 2023 22:16:33 -0400 Subject: [PATCH] [commands] Add DeferredCommand (#5566) Allows commands to be constructed at runtime without proxying. --- .../wpi/first/wpilibj2/command/Commands.java | 13 +++ .../wpilibj2/command/DeferredCommand.java | 78 +++++++++++++++++ .../wpi/first/wpilibj2/command/Subsystem.java | 15 ++++ .../main/native/cpp/frc2/command/Commands.cpp | 6 ++ .../cpp/frc2/command/DeferredCommand.cpp | 46 ++++++++++ .../native/cpp/frc2/command/Subsystem.cpp | 4 + .../native/include/frc2/command/Commands.h | 10 +++ .../include/frc2/command/DeferredCommand.h | 61 +++++++++++++ .../native/include/frc2/command/Subsystem.h | 12 +++ .../wpilibj2/command/DeferredCommandTest.java | 85 +++++++++++++++++++ .../cpp/frc2/command/DeferredCommandTest.cpp | 75 ++++++++++++++++ 11 files changed, 405 insertions(+) create mode 100644 wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java create mode 100644 wpilibNewCommands/src/main/native/cpp/frc2/command/DeferredCommand.cpp create mode 100644 wpilibNewCommands/src/main/native/include/frc2/command/DeferredCommand.h create mode 100644 wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DeferredCommandTest.java create mode 100644 wpilibNewCommands/src/test/native/cpp/frc2/command/DeferredCommandTest.cpp diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java index 7571532de8..9cd71c5240 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java +++ b/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 supplier, Set 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 diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java new file mode 100644 index 0000000000..76a5276c75 --- /dev/null +++ b/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. + * + *

Note that the supplier must create a new Command each call. For selecting one of a + * preallocated set of commands, use {@link SelectCommand}. + * + *

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 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 must 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 supplier, Set 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); + } +} diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java index ef7c4c99d9..ac89dbdc21 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java +++ b/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 supplier) { + return Commands.defer(supplier, Set.of(this)); + } } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp index a020dc55fb..31a4b1fe1c 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Commands.cpp +++ b/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 supplier, + Requirements requirements) { + return DeferredCommand(std::move(supplier), requirements).ToPtr(); +} + CommandPtr cmd::Sequence(std::vector&& commands) { return SequentialCommandGroup(CommandPtr::UnwrapVector(std::move(commands))) .ToPtr(); diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/DeferredCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/DeferredCommand.cpp new file mode 100644 index 0000000000..2a7f16280a --- /dev/null +++ b/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 + +#include "frc2/command/Commands.h" + +using namespace frc2; + +DeferredCommand::DeferredCommand(wpi::unique_function 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); +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp index d1d50e1de0..4c06f1f4ea 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp @@ -54,3 +54,7 @@ CommandPtr Subsystem::RunEnd(std::function run, std::function end) { return cmd::RunEnd(std::move(run), std::move(end), {this}); } + +CommandPtr Subsystem::Defer(wpi::unique_function supplier) { + return cmd::Defer(std::move(supplier), {this}); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h index 6699bc7899..5c1d49a257 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h @@ -142,6 +142,16 @@ CommandPtr Select(std::function 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 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 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/DeferredCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/DeferredCommand.h new file mode 100644 index 0000000000..442076d8f2 --- /dev/null +++ b/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 +#include + +#include + +#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 must create a new Command each call. For + * selecting one of a preallocated set of commands, use SelectCommand. + * + *

This class is provided by the NewCommands VendorDep + */ +class DeferredCommand : public CommandHelper { + 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 must create a new Command each + * call. + * + * @param supplier The command supplier + * @param requirements The command requirements. + * + */ + DeferredCommand(wpi::unique_function 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 m_supplier; + std::unique_ptr m_command; +}; +} // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h index dbad8c32f2..cdac0c0466 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h @@ -8,6 +8,8 @@ #include #include +#include + #include "frc2/command/CommandScheduler.h" namespace frc2 { @@ -148,5 +150,15 @@ class Subsystem { */ [[nodiscard]] CommandPtr RunEnd(std::function run, std::function 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 supplier); }; } // namespace frc2 diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DeferredCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DeferredCommandTest.java new file mode 100644 index 0000000000..259ba0fdce --- /dev/null +++ b/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 supplier = (Supplier) 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); + }); + } +} diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/DeferredCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/DeferredCommandTest.cpp new file mode 100644 index 0000000000..1af8ae4e50 --- /dev/null +++ b/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 {}; + +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)); +}