From 9154940f952c2775207747f6e28ddde641753709 Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Fri, 11 Oct 2024 17:34:15 +0300 Subject: [PATCH] Optimize copying `m_scheduledCommands` --- .../first/wpilibj2/command/CommandScheduler.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 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 88d22f8698d..98d11be9214 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 @@ -67,6 +67,9 @@ public static synchronized CommandScheduler getInstance() { // A set of the currently-running commands. private final Set m_scheduledCommands = new LinkedHashSet<>(); + // A copy of the currently-running commands, used for iteration stored on class for caching + // purposes. + private Command[] m_scheduledCommandsCopy = new Command[12]; // 12 is arbitrary, it auto-resizes // A map from required subsystems to their requiring commands. Also used as a set of the // currently-required subsystems. @@ -263,9 +266,16 @@ public void run() { boolean isDisabled = RobotState.isDisabled(); // Run scheduled commands, remove finished commands. - for (Command command : Set.copyOf(m_scheduledCommands)) { + m_scheduledCommandsCopy = m_scheduledCommands.toArray(m_scheduledCommandsCopy); + for (Command command : m_scheduledCommandsCopy) { + if (command == null) { + // at least 1 Command was removed before `run` was called (diff between copy and original). + // No more elements to iterate over (see toArray documentation) + break; + } + if (!isScheduled(command)) { - continue; // skip as the normal scheduledCommands was modified and that command was canceled + continue; // Command was canceled in the previous iterations. } if (isDisabled && !command.runsWhenDisabled()) {