Add an event in CommandScheduler before each command is executed

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<Consumer<Command>> m_initActions = new ArrayList<>();
+  private final List<Consumer<Command>> m_beforeExecuteActions = new ArrayList<>();
   private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
   private final List<BiConsumer<Command, Optional<Command>>> m_interruptActions = new ArrayList<>();
   private final List<Consumer<Command>> m_finishActions = new ArrayList<>();
@@ -285,6 +286,9 @@ public void run() {
         continue;
       }
 
+      for (Consumer<Command> action : m_beforeExecuteActions) {
+        action.accept(command);
+      }
       command.execute();
       for (Consumer<Command> action : m_executeActions) {
         action.accept(command);
@@ -562,7 +566,16 @@ public void onCommandInitialize(Consumer<Command> 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<Command> 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
    */

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<Action, 4> initActions;
+  wpi::SmallVector<Action, 4> beforeExecuteActions;
   wpi::SmallVector<Action, 4> executeActions;
   wpi::SmallVector<InterruptAction, 4> interruptActions;
   wpi::SmallVector<Action, 4> 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));
 }

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
    */

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

@@ -7,26 +7,59 @@
 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;
 
 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<String> 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);
     }
   }
 

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<void(const Command&)> onInitialize;
+  ::testing::MockFunction<void(const Command&)> onBeforeExecute;
+  ::testing::MockFunction<void(const Command&)> onAfterExecute;
+  ::testing::MockFunction<void(const Command&)> 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) {