From 18fbb63474faf16eb4651918fd788acea93edebe Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Tue, 24 Sep 2024 08:25:12 -0700 Subject: [PATCH] Add an event in CommandScheduler before each command is executed --- .../wpilibj2/command/CommandScheduler.java | 15 +++++- .../cpp/frc2/command/CommandScheduler.cpp | 8 +++ .../include/frc2/command/CommandScheduler.h | 11 ++++- .../first/wpilibj2/command/SchedulerTest.java | 49 ++++++++++++++++--- .../native/cpp/frc2/command/SchedulerTest.cpp | 44 ++++++++++++----- 5 files changed, 106 insertions(+), 21 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 0e80a74c613..262baa8eeda 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -85,6 +85,7 @@ public static synchronized CommandScheduler getInstance() { // Lists of user-supplied actions to be executed on scheduling events for every command. private final List> m_initActions = new ArrayList<>(); + private final List> m_beforeExecuteActions = new ArrayList<>(); private final List> m_executeActions = new ArrayList<>(); private final List>> m_interruptActions = new ArrayList<>(); private final List> m_finishActions = new ArrayList<>(); @@ -285,6 +286,9 @@ public void run() { continue; } + for (Consumer action : m_beforeExecuteActions) { + action.accept(command); + } command.execute(); for (Consumer action : m_executeActions) { action.accept(command); @@ -562,7 +566,16 @@ public void onCommandInitialize(Consumer action) { } /** - * Adds an action to perform on the execution of any command by the scheduler. + * Adds an action to perform before the execution of any command by the scheduler. + * + * @param action the action to perform + */ + public void onCommandBeforeExecute(Consumer action) { + m_beforeExecuteActions.add(requireNonNullParam(action, "action", "onCommandBeforeExecute")); + } + + /** + * Adds an action to perform after the execution of any command by the scheduler. * * @param action the action to perform */ diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index aa57bf00d08..a02a0b90502 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -51,6 +51,7 @@ class CommandScheduler::Impl { // Lists of user-supplied actions to be executed on scheduling events for // every command. wpi::SmallVector initActions; + wpi::SmallVector beforeExecuteActions; wpi::SmallVector executeActions; wpi::SmallVector interruptActions; wpi::SmallVector finishActions; @@ -206,6 +207,9 @@ void CommandScheduler::Run() { continue; } + for (auto&& action : m_impl->beforeExecuteActions) { + action(*command); + } command->Execute(); for (auto&& action : m_impl->executeActions) { action(*command); @@ -440,6 +444,10 @@ void CommandScheduler::OnCommandInitialize(Action action) { m_impl->initActions.emplace_back(std::move(action)); } +void CommandScheduler::OnCommandBeforeExecute(Action action) { + m_impl->beforeExecuteActions.emplace_back(std::move(action)); +} + void CommandScheduler::OnCommandExecute(Action action) { m_impl->executeActions.emplace_back(std::move(action)); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index fde2c6d3e3f..bddfae0017a 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h @@ -347,7 +347,16 @@ class CommandScheduler final : public wpi::Sendable, void OnCommandInitialize(Action action); /** - * Adds an action to perform on the execution of any command by the scheduler. + * Adds an action to perform before the execution of any command by the + * scheduler. + * + * @param action the action to perform + */ + void OnCommandBeforeExecute(Action action); + + /** + * Adds an action to perform after the execution of any command by the + * scheduler. * * @param action the action to perform */ diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java index 1e0b33332e8..16983f2d08c 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java @@ -7,9 +7,13 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertIterableEquals; import static org.junit.jupiter.api.Assertions.assertSame; import static org.junit.jupiter.api.Assertions.assertTrue; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; import java.util.concurrent.atomic.AtomicInteger; import org.junit.jupiter.api.Test; @@ -17,16 +21,45 @@ class SchedulerTest extends CommandTestBase { @Test void schedulerLambdaTestNoInterrupt() { try (CommandScheduler scheduler = new CommandScheduler()) { - AtomicInteger counter = new AtomicInteger(); - - scheduler.onCommandInitialize(command -> counter.incrementAndGet()); - scheduler.onCommandExecute(command -> counter.incrementAndGet()); - scheduler.onCommandFinish(command -> counter.incrementAndGet()); - - scheduler.schedule(new InstantCommand()); + List testEvents = Collections.synchronizedList(new ArrayList<>()); + + scheduler.onCommandInitialize(command -> testEvents.add("OnInitialize " + command.getName())); + scheduler.onCommandBeforeExecute( + command -> testEvents.add("OnBeforeExecute " + command.getName())); + scheduler.onCommandExecute(command -> testEvents.add("OnAfterExecute " + command.getName())); + scheduler.onCommandFinish(command -> testEvents.add("OnFinish " + command.getName())); + + scheduler.schedule( + new FunctionalCommand( + () -> testEvents.add("Initializing OnceCommand"), + () -> testEvents.add("Executing OnceCommand"), + (Boolean interrupted) -> testEvents.add("Ending OnceCommand"), + () -> true) + .withName("OnceCommand")); + scheduler.schedule( + new FunctionalCommand( + () -> testEvents.add("Initializing ContinuingCommand"), + () -> testEvents.add("Executing ContinuingCommand"), + (Boolean interrupted) -> testEvents.add("Ending ContinuingCommand"), + () -> false) + .withName("ContinuingCommand")); scheduler.run(); - assertEquals(counter.get(), 3); + assertIterableEquals( + List.of( + "Initializing OnceCommand", + "OnInitialize OnceCommand", + "Initializing ContinuingCommand", + "OnInitialize ContinuingCommand", + "OnBeforeExecute OnceCommand", + "Executing OnceCommand", + "OnAfterExecute OnceCommand", + "Ending OnceCommand", + "OnFinish OnceCommand", + "OnBeforeExecute ContinuingCommand", + "Executing ContinuingCommand", + "OnAfterExecute ContinuingCommand"), + testEvents); } } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp index ef97bb07692..42973e503ce 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include "CommandTestBase.h" +#include "frc2/command/FunctionalCommand.h" #include "frc2/command/InstantCommand.h" #include "frc2/command/RunCommand.h" #include "frc2/command/StartEndCommand.h" @@ -13,18 +14,39 @@ class SchedulerTest : public CommandTestBase {}; TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) { CommandScheduler scheduler = GetScheduler(); - InstantCommand command; - - int counter = 0; - - scheduler.OnCommandInitialize([&counter](const Command&) { counter++; }); - scheduler.OnCommandExecute([&counter](const Command&) { counter++; }); - scheduler.OnCommandFinish([&counter](const Command&) { counter++; }); - - scheduler.Schedule(&command); + ::testing::MockFunction onInitialize; + ::testing::MockFunction onBeforeExecute; + ::testing::MockFunction onAfterExecute; + ::testing::MockFunction onFinish; + + MockCommand onceCommand({}, true); + MockCommand continuingCommand({}, false); + + { + ::testing::InSequence s; + + EXPECT_CALL(onceCommand, Initialize()); + EXPECT_CALL(onInitialize, Call(testing::Ref(onceCommand))); + EXPECT_CALL(continuingCommand, Initialize()); + EXPECT_CALL(onInitialize, Call(testing::Ref(continuingCommand))); + EXPECT_CALL(onBeforeExecute, Call(testing::Ref(onceCommand))); + EXPECT_CALL(onceCommand, Execute()); + EXPECT_CALL(onAfterExecute, Call(testing::Ref(onceCommand))); + EXPECT_CALL(onceCommand, End(testing::_)); + EXPECT_CALL(onFinish, Call(testing::Ref(onceCommand))); + EXPECT_CALL(onBeforeExecute, Call(testing::Ref(continuingCommand))); + EXPECT_CALL(continuingCommand, Execute()); + EXPECT_CALL(onAfterExecute, Call(testing::Ref(continuingCommand))); + } + + scheduler.OnCommandInitialize(onInitialize.AsStdFunction()); + scheduler.OnCommandBeforeExecute(onBeforeExecute.AsStdFunction()); + scheduler.OnCommandExecute(onAfterExecute.AsStdFunction()); + scheduler.OnCommandFinish(onFinish.AsStdFunction()); + + scheduler.Schedule(&onceCommand); + scheduler.Schedule(&continuingCommand); scheduler.Run(); - - EXPECT_EQ(counter, 3); } TEST_F(SchedulerTest, SchedulerLambdaInterrupt) {