Skip to content

Commit

Permalink
Use queues, defer canceling, add tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@Gold856
Gold856 committed Jul 28, 2023
1 parent dd9a73f commit 8e93516
Show file tree
Hide file tree
Showing 3 changed files with 140 additions and 31 deletions.
39 changes: 24 additions & 15 deletions wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,11 @@
import edu.wpi.first.wpilibj.event.EventLoop;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.Deque;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.LinkedHashSet;
Expand Down Expand Up @@ -86,9 +88,11 @@ public static synchronized CommandScheduler getInstance() {
// scheduled/canceled during run
private boolean m_inRunLoop;
private final Set<Command> m_toSchedule = new LinkedHashSet<>();
private final List<Command> m_toCancel = new ArrayList<>();
private final Deque<Command> m_toCancel = new ArrayDeque<>();

private boolean m_deferCancel;
private final Deque<Command> m_deferredCanceledCommands = new ArrayDeque<>();

private Command m_currentCancelingCommand;
private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});

CommandScheduler() {
Expand Down Expand Up @@ -272,7 +276,13 @@ public void run() {
Command command = iterator.next();

if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
cancel(command);
command.end(true);
for (Consumer<Command> action : m_interruptActions) {
action.accept(command);
}
m_scheduledCommands.remove(command);
m_requirements.keySet().removeAll(command.getRequirements());
m_watchdog.addEpoch(command.getName() + ".end(true)");
continue;
}

Expand All @@ -299,12 +309,11 @@ public void run() {
schedule(command);
}

for (Command command : m_toCancel) {
cancel(command);
while (!m_toCancel.isEmpty()) {
cancel(m_toCancel.poll());
}

m_toSchedule.clear();
m_toCancel.clear();

// Add default commands for un-required registered subsystems.
for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) {
Expand Down Expand Up @@ -434,13 +443,16 @@ public Command getDefaultCommand(Subsystem subsystem) {
*
* @param commands the commands to cancel
*/
@SuppressWarnings("PMD.CompareObjectsWithEquals")
public void cancel(Command... commands) {
if (m_inRunLoop) {
m_toCancel.addAll(List.of(commands));
return;
}

if (m_deferCancel) {
m_deferredCanceledCommands.addAll(List.of(commands));
return;
}
m_deferCancel = true;
for (Command command : commands) {
if (command == null) {
DriverStation.reportWarning("Tried to cancel a null command", true);
Expand All @@ -449,21 +461,18 @@ public void cancel(Command... commands) {
if (!isScheduled(command)) {
continue;
}
// Prevents infinite recursion if cancel() is called from end() by checking if the command is
// already being cancelled
if (command == m_currentCancelingCommand) {
continue;
}
m_currentCancelingCommand = command;
command.end(true);
for (Consumer<Command> action : m_interruptActions) {
action.accept(command);
}
m_scheduledCommands.remove(command);
m_requirements.keySet().removeAll(command.getRequirements());
m_currentCancelingCommand = null;
m_watchdog.addEpoch(command.getName() + ".end(true)");
}
m_deferCancel = false;
while (!m_deferredCanceledCommands.isEmpty()) {
cancel(m_deferredCanceledCommands.poll());
}
}

/** Cancels all commands that are currently scheduled. */
Expand Down
47 changes: 31 additions & 16 deletions wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
#include "frc2/command/CommandScheduler.h"

#include <cstdio>

#include <queue>
#include <frc/RobotBase.h>
#include <frc/RobotState.h>
#include <frc/TimedRobot.h>
Expand Down Expand Up @@ -56,9 +56,10 @@ class CommandScheduler::Impl {

bool inRunLoop = false;
wpi::SmallVector<Command*, 4> toSchedule;
wpi::SmallVector<Command*, 4> toCancel;
std::queue<Command*> toCancel;

Command* currentCancelingCommand = nullptr;
bool deferCancel = false;
std::queue<Command*> deferredCanceledCommands;
};

template <typename TMap, typename TKey>
Expand Down Expand Up @@ -198,7 +199,16 @@ void CommandScheduler::Run() {
// Run scheduled commands, remove finished commands.
for (Command* command : m_impl->scheduledCommands) {
if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
Cancel(command);
command->End(true);
for (auto&& action : m_impl->interruptActions) {
action(*command);
}

m_impl->scheduledCommands.erase(command);
for (auto&& requirement : command->GetRequirements()) {
m_impl->requirements.erase(requirement);
}
m_watchdog.AddEpoch(command->GetName() + ".End(true)");
continue;
}

Expand All @@ -214,11 +224,11 @@ void CommandScheduler::Run() {
action(*command);
}

m_impl->scheduledCommands.erase(command);
for (auto&& requirement : command->GetRequirements()) {
m_impl->requirements.erase(requirement);
}

m_impl->scheduledCommands.erase(command);
m_watchdog.AddEpoch(command->GetName() + ".End(false)");
}
}
Expand All @@ -228,12 +238,13 @@ void CommandScheduler::Run() {
Schedule(command);
}

for (auto&& command : m_impl->toCancel) {
Cancel(command);
while (!(m_impl->toCancel.empty())) {
auto nextCommand = m_impl->toCancel.front();
m_impl->toCancel.pop();
Cancel(nextCommand);
}

m_impl->toSchedule.clear();
m_impl->toCancel.clear();

// Add default commands for un-required registered subsystems.
for (auto&& subsystem : m_impl->subsystems) {
Expand Down Expand Up @@ -327,20 +338,19 @@ void CommandScheduler::Cancel(Command* command) {
}

if (m_impl->inRunLoop) {
m_impl->toCancel.emplace_back(command);
m_impl->toCancel.push(command);
return;
}
if (m_impl->deferCancel) {
m_impl->deferredCanceledCommands.push(command);
return;
}

m_impl->deferCancel = true;
auto find = m_impl->scheduledCommands.find(command);
if (find == m_impl->scheduledCommands.end()) {
return;
}
// Prevents infinite recursion if cancel() is called from end() by checking if
// the command is already being cancelled
if (command == m_impl->currentCancelingCommand) {
return;
}
m_impl->currentCancelingCommand = command;
command->End(true);
for (auto&& action : m_impl->interruptActions) {
action(*command);
Expand All @@ -351,8 +361,13 @@ void CommandScheduler::Cancel(Command* command) {
m_impl->requirements.erase(requirement.first);
}
}
m_impl->currentCancelingCommand = nullptr;
m_watchdog.AddEpoch(command->GetName() + ".End(true)");
m_impl->deferCancel = false;
while (!(m_impl->deferredCanceledCommands.empty())) {
auto nextCommand = m_impl->deferredCanceledCommands.front();
m_impl->deferredCanceledCommands.pop();
Cancel(nextCommand);
}
}

void CommandScheduler::Cancel(const CommandPtr& command) {
Expand Down
85 changes: 85 additions & 0 deletions wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,91 @@ public void end(boolean interrupted) {
}
}

@Test
void cancelFromEndLoop() {
try (CommandScheduler scheduler = new CommandScheduler()) {
AtomicInteger counter = new AtomicInteger();
Command dCancelsAll =
new Command() {
@Override
public void end(boolean interrupted) {
counter.incrementAndGet();
scheduler.cancelAll();
}
};
Command cCancelsD =
new Command() {
@Override
public void end(boolean interrupted) {
counter.incrementAndGet();
scheduler.cancel(dCancelsAll);
scheduler.cancel(this);
}
};
Command bCancelsC =
new Command() {
@Override
public void end(boolean interrupted) {
counter.incrementAndGet();
scheduler.cancel(cCancelsD);
scheduler.cancel(this);
}
};
Command aCancelsB =
new Command() {
@Override
public void end(boolean interrupted) {
counter.incrementAndGet();
scheduler.cancel(bCancelsC);
scheduler.cancel(this);
}
};

scheduler.schedule(aCancelsB);
scheduler.schedule(bCancelsC);
scheduler.schedule(cCancelsD);
scheduler.schedule(dCancelsAll);

assertDoesNotThrow(() -> scheduler.cancel(aCancelsB));
assertEquals(4, counter.get());
assertFalse(scheduler.isScheduled(aCancelsB));
assertFalse(scheduler.isScheduled(bCancelsC));
assertFalse(scheduler.isScheduled(cCancelsD));
assertFalse(scheduler.isScheduled(dCancelsAll));
}
}

@Test
void multiCancelFromEnd() {
try (CommandScheduler scheduler = new CommandScheduler()) {
AtomicInteger counter = new AtomicInteger();
Command bCancelsA =
new Command() {
@Override
public void end(boolean interrupted) {
counter.incrementAndGet();
}
};
Command aCancelsB =
new Command() {
@Override
public void end(boolean interrupted) {
counter.incrementAndGet();
scheduler.cancel(bCancelsA);
scheduler.cancel(this);
}
};

scheduler.schedule(aCancelsB);
scheduler.schedule(bCancelsA);

assertDoesNotThrow(() -> scheduler.cancel(aCancelsB));
assertEquals(2, counter.get());
assertFalse(scheduler.isScheduled(aCancelsB));
assertFalse(scheduler.isScheduled(bCancelsA));
}
}

@Test
void scheduleFromEndCancel() {
try (CommandScheduler scheduler = new CommandScheduler()) {
Expand Down

0 comments on commit 8e93516

Please sign in to comment.