diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
index 1f09ef53fc..9c72c734fb 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -47,14 +47,15 @@ public interface Command {
/**
* Specifies the set of subsystems used by this command. Two commands cannot use the same
- * subsystem at the same time. If the command is scheduled as interruptible and another command is
- * scheduled that shares a requirement, the command will be interrupted. Else, the command will
- * not be scheduled. If no subsystems are required, return an empty set.
+ * subsystem at the same time. If another command is scheduled that shares a requirement, {@link
+ * #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return
+ * an empty set.
*
*
Note: it is recommended that user implementations contain the requirements as a field, and
* return that field here, rather than allocating a new set every time this is called.
*
* @return the set of subsystems that are required
+ * @see InterruptionBehavior
*/
Set getRequirements();
@@ -331,23 +332,30 @@ public interface Command {
}
/**
- * Schedules this command.
+ * Decorates this command to have a different {@link InterruptionBehavior interruption behavior}.
*
- * @param interruptible whether this command can be interrupted by another command that shares one
- * of its requirements
+ * @param interruptBehavior the desired interrupt behavior
+ * @return the decorated command
*/
- default void schedule(boolean interruptible) {
- CommandScheduler.getInstance().schedule(interruptible, this);
+ default WrapperCommand withInterruptBehavior(InterruptionBehavior interruptBehavior) {
+ return new WrapperCommand(this) {
+ @Override
+ public InterruptionBehavior getInterruptionBehavior() {
+ return interruptBehavior;
+ }
+ };
}
- /** Schedules this command, defaulting to interruptible. */
+ /** Schedules this command. */
default void schedule() {
- schedule(true);
+ CommandScheduler.getInstance().schedule(this);
}
/**
- * Cancels this command. Will call the command's end() method with interrupted=true. Commands will
- * be canceled even if they are not marked as interruptible.
+ * Cancels this command. Will call {@link #end(boolean) end(true)}. Commands will be canceled
+ * regardless of {@link InterruptionBehavior interruption behavior}.
+ *
+ * @see CommandScheduler#cancel(Command...)
*/
default void cancel() {
CommandScheduler.getInstance().cancel(this);
@@ -373,6 +381,16 @@ public interface Command {
return getRequirements().contains(requirement);
}
+ /**
+ * How the command behaves when another command with a shared requirement is scheduled.
+ *
+ * @return a variant of {@link InterruptionBehavior}, defaulting to {@link
+ * InterruptionBehavior#kCancelSelf kCancelSelf}.
+ */
+ default InterruptionBehavior getInterruptionBehavior() {
+ return InterruptionBehavior.kCancelSelf;
+ }
+
/**
* Whether the given command should run when the robot is disabled. Override to return true if the
* command should run when disabled.
@@ -391,4 +409,20 @@ public interface Command {
default String getName() {
return this.getClass().getSimpleName();
}
+
+ /**
+ * An enum describing the command's behavior when another command with a shared requirement is
+ * scheduled.
+ */
+ enum InterruptionBehavior {
+ /**
+ * This command ends, {@link #end(boolean) end(true)} is called, and the incoming command is
+ * scheduled normally.
+ *
+ * This is the default behavior.
+ */
+ kCancelSelf,
+ /** This command continues, and the incoming command is not scheduled. */
+ kCancelIncoming
+ }
}
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 f68f0ce49b..5026385d92 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
@@ -20,14 +20,14 @@ import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.event.EventLoop;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;
import java.util.LinkedHashMap;
+import java.util.LinkedHashSet;
import java.util.List;
import java.util.Map;
-import java.util.Optional;
import java.util.Set;
import java.util.function.Consumer;
@@ -56,11 +56,10 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
return instance;
}
- // A map from commands to their scheduling state. Also used as a set of the currently-running
- // commands.
- private final Map m_scheduledCommands = new LinkedHashMap<>();
+ // A set of the currently-running commands.
+ private final Set m_scheduledCommands = new LinkedHashSet<>();
- // A map from required subsystems to their requiring commands. Also used as a set of the
+ // A map from required subsystems to their requiring commands. Also used as a set of the
// currently-required subsystems.
private final Map m_requirements = new LinkedHashMap<>();
@@ -83,7 +82,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
// Flag and queues for avoiding ConcurrentModificationException if commands are
// scheduled/canceled during run
private boolean m_inRunLoop;
- private final Map m_toSchedule = new LinkedHashMap<>();
+ private final Set m_toSchedule = new LinkedHashSet<>();
private final List m_toCancel = new ArrayList<>();
private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
@@ -103,7 +102,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
}
/**
- * Changes the period of the loop overrun watchdog. This should be be kept in sync with the
+ * Changes the period of the loop overrun watchdog. This should be kept in sync with the
* TimedRobot period.
*
* @param period Period in seconds.
@@ -151,7 +150,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* Adds a button binding to the scheduler, which will be polled to schedule commands.
*
* @param button The button to add
- * @deprecated Use {@link Trigger}
+ * @deprecated Use {@link edu.wpi.first.wpilibj2.command.button.Trigger}
*/
@Deprecated(since = "2023")
public void addButton(Runnable button) {
@@ -172,12 +171,10 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* Initializes a given command, adds its requirements to the list, and performs the init actions.
*
* @param command The command to initialize
- * @param interruptible Whether the command is interruptible
* @param requirements The command requirements
*/
- private void initCommand(Command command, boolean interruptible, Set requirements) {
- CommandState scheduledCommand = new CommandState(interruptible);
- m_scheduledCommands.put(command, scheduledCommand);
+ private void initCommand(Command command, Set requirements) {
+ m_scheduledCommands.add(command);
for (Subsystem requirement : requirements) {
m_requirements.put(requirement, command);
}
@@ -195,17 +192,15 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* using those requirements have been scheduled as interruptible. If this is the case, they will
* be interrupted and the command will be scheduled.
*
- * @param interruptible whether this command can be interrupted.
* @param command the command to schedule. If null, no-op.
*/
- private void schedule(boolean interruptible, Command command) {
+ private void schedule(Command command) {
if (command == null) {
DriverStation.reportWarning("Tried to schedule a null command", true);
return;
}
-
if (m_inRunLoop) {
- m_toSchedule.put(command, interruptible);
+ m_toSchedule.add(command);
return;
}
@@ -226,16 +221,14 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
// Schedule the command if the requirements are not currently in-use.
if (Collections.disjoint(m_requirements.keySet(), requirements)) {
- initCommand(command, interruptible, requirements);
+ initCommand(command, requirements);
} else {
// Else check if the requirements that are in use have all have interruptible commands,
// and if so, interrupt those commands and schedule the new command.
for (Subsystem requirement : requirements) {
Command requiring = requiring(requirement);
if (requiring != null
- && !Optional.ofNullable(m_scheduledCommands.get(requiring))
- .map(CommandState::isInterruptible)
- .orElse(true)) {
+ && requiring.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
return;
}
}
@@ -245,33 +238,19 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
cancel(requiring);
}
}
- initCommand(command, interruptible, requirements);
+ initCommand(command, requirements);
}
}
/**
- * Schedules multiple commands for execution. Does nothing if the command is already scheduled. If
- * a command's requirements are not available, it will only be started if all the commands
- * currently using those requirements have been scheduled as interruptible. If this is the case,
- * they will be interrupted and the command will be scheduled.
- *
- * @param interruptible whether the commands should be interruptible
- * @param commands the commands to schedule. No-op if null.
- */
- public void schedule(boolean interruptible, Command... commands) {
- for (Command command : commands) {
- schedule(interruptible, command);
- }
- }
-
- /**
- * Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
- * if the command is already scheduled.
+ * Schedules multiple commands for execution. Does nothing for commands already scheduled.
*
* @param commands the commands to schedule. No-op on null.
*/
public void schedule(Command... commands) {
- schedule(true, commands);
+ for (Command command : commands) {
+ schedule(command);
+ }
}
/**
@@ -312,8 +291,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
m_inRunLoop = true;
// Run scheduled commands, remove finished commands.
- for (Iterator iterator = m_scheduledCommands.keySet().iterator();
- iterator.hasNext(); ) {
+ for (Iterator iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) {
Command command = iterator.next();
if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
@@ -346,8 +324,8 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
m_inRunLoop = false;
// Schedule/cancel commands from queues populated during loop
- for (Map.Entry commandInterruptible : m_toSchedule.entrySet()) {
- schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
+ for (Command command : m_toSchedule) {
+ schedule(command);
}
for (Command command : m_toCancel) {
@@ -427,6 +405,14 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
throw new IllegalArgumentException("Default commands should not end!");
}
+ if (defaultCommand.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
+ DriverStation.reportWarning(
+ "Registering a non-interruptible default command!\n"
+ + "This will likely prevent any other commands from requiring this subsystem.",
+ true);
+ // Warn, but allow -- there might be a use case for this.
+ }
+
m_subsystems.put(subsystem, defaultCommand);
}
@@ -446,7 +432,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* canceled command with {@code true}, indicating they were canceled (as opposed to finishing
* normally).
*
- * Commands will be canceled even if they are not scheduled as interruptible.
+ *
Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}.
*
* @param commands the commands to cancel
*/
@@ -477,26 +463,8 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
/** Cancels all commands that are currently scheduled. */
public void cancelAll() {
- for (Command command : m_scheduledCommands.keySet().toArray(new Command[0])) {
- cancel(command);
- }
- }
-
- /**
- * Returns the time since a given command was scheduled. Note that this only works on commands
- * that are directly scheduled by the scheduler; it will not work on commands inside of
- * commandgroups, as the scheduler does not see them.
- *
- * @param command the command to query
- * @return the time since the command was scheduled, in seconds
- */
- public double timeSinceScheduled(Command command) {
- CommandState commandState = m_scheduledCommands.get(command);
- if (commandState != null) {
- return commandState.timeSinceInitialized();
- } else {
- return -1;
- }
+ // Copy to array to avoid concurrent modification.
+ cancel(m_scheduledCommands.toArray(new Command[0]));
}
/**
@@ -508,7 +476,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
* @return whether the command is currently scheduled
*/
public boolean isScheduled(Command... commands) {
- return m_scheduledCommands.keySet().containsAll(Set.of(commands));
+ return m_scheduledCommands.containsAll(Set.of(commands));
}
/**
@@ -583,7 +551,7 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
Map ids = new LinkedHashMap<>();
- for (Command command : m_scheduledCommands.keySet()) {
+ for (Command command : m_scheduledCommands) {
ids.put((double) command.hashCode(), command);
}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
deleted file mode 100644
index 173bc54b59..0000000000
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
+++ /dev/null
@@ -1,42 +0,0 @@
-// 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 edu.wpi.first.wpilibj.Timer;
-
-/**
- * Class that holds scheduling state for a command. Used internally by the {@link CommandScheduler}.
- *
- * This class is provided by the NewCommands VendorDep
- */
-class CommandState {
- // The time since this command was initialized.
- private double m_startTime = -1;
-
- // Whether or not it is interruptible.
- private final boolean m_interruptible;
-
- CommandState(boolean interruptible) {
- m_interruptible = interruptible;
- startTiming();
- startRunning();
- }
-
- private void startTiming() {
- m_startTime = Timer.getFPGATimestamp();
- }
-
- synchronized void startRunning() {
- m_startTime = -1;
- }
-
- boolean isInterruptible() {
- return m_interruptible;
- }
-
- double timeSinceInitialized() {
- return m_startTime != -1 ? Timer.getFPGATimestamp() - m_startTime : -1;
- }
-}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
index de1530c170..59b2205de9 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
@@ -95,6 +95,11 @@ public abstract class WrapperCommand implements Command {
return m_command.runsWhenDisabled();
}
+ @Override
+ public InterruptionBehavior getInterruptionBehavior() {
+ return m_command.getInterruptionBehavior();
+ }
+
/**
* Gets the name of this Command.
*
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
index 0ac8df15e8..4d9d1dac1e 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
@@ -40,19 +40,6 @@ public class Button extends Trigger {
* Starts the given command whenever the button is newly pressed.
*
* @param command the command to start
- * @param interruptible whether the command is interruptible
- * @return this button, so calls can be chained
- */
- public Button whenPressed(final Command command, boolean interruptible) {
- whenActive(command, interruptible);
- return this;
- }
-
- /**
- * Starts the given command whenever the button is newly pressed. The command is set to be
- * interruptible.
- *
- * @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenPressed(final Command command) {
@@ -75,23 +62,8 @@ public class Button extends Trigger {
/**
* Constantly starts the given command while the button is held.
*
- *
{@link Command#schedule(boolean)} will be called repeatedly while the button is held, and
- * will be canceled when the button is released.
- *
- * @param command the command to start
- * @param interruptible whether the command is interruptible
- * @return this button, so calls can be chained
- */
- public Button whileHeld(final Command command, boolean interruptible) {
- whileActiveContinuous(command, interruptible);
- return this;
- }
-
- /**
- * Constantly starts the given command while the button is held.
- *
- *
{@link Command#schedule(boolean)} will be called repeatedly while the button is held, and
- * will be canceled when the button is released. The command is set to be interruptible.
+ *
{@link Command#schedule()} will be called repeatedly while the button is held, and will be
+ * canceled when the button is released.
*
* @param command the command to start
* @return this button, so calls can be chained
@@ -118,36 +90,10 @@ public class Button extends Trigger {
* but does not start it again if it ends or is otherwise interrupted.
*
* @param command the command to start
- * @param interruptible whether the command is interruptible
- * @return this button, so calls can be chained
- */
- public Button whenHeld(final Command command, boolean interruptible) {
- whileActiveOnce(command, interruptible);
- return this;
- }
-
- /**
- * Starts the given command when the button is first pressed, and cancels it when it is released,
- * but does not start it again if it ends or is otherwise interrupted. The command is set to be
- * interruptible.
- *
- * @param command the command to start
* @return this button, so calls can be chained
*/
public Button whenHeld(final Command command) {
- whileActiveOnce(command, true);
- return this;
- }
-
- /**
- * Starts the command when the button is released.
- *
- * @param command the command to start
- * @param interruptible whether the command is interruptible
- * @return this button, so calls can be chained
- */
- public Button whenReleased(final Command command, boolean interruptible) {
- whenInactive(command, interruptible);
+ whileActiveOnce(command);
return this;
}
@@ -174,18 +120,6 @@ public class Button extends Trigger {
return this;
}
- /**
- * Toggles the command whenever the button is pressed (on then off then on).
- *
- * @param command the command to start
- * @param interruptible whether the command is interruptible
- * @return this button, so calls can be chained
- */
- public Button toggleWhenPressed(final Command command, boolean interruptible) {
- toggleWhenActive(command, interruptible);
- return this;
- }
-
/**
* Toggles the command whenever the button is pressed (on then off then on). The command is set to
* be interruptible.
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
index e1136f96f5..f69fc96c23 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
@@ -80,25 +80,13 @@ public class Trigger extends BooleanEvent {
* Starts the given command whenever the trigger just becomes active.
*
* @param command the command to start
- * @param interruptible whether the command is interruptible
- * @return this trigger, so calls can be chained
- */
- public Trigger whenActive(final Command command, boolean interruptible) {
- requireNonNullParam(command, "command", "whenActive");
-
- this.rising().ifHigh(() -> command.schedule(interruptible));
- return this;
- }
-
- /**
- * Starts the given command whenever the trigger just becomes active. The command is set to be
- * interruptible.
- *
- * @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whenActive(final Command command) {
- return whenActive(command, true);
+ requireNonNullParam(command, "command", "whenActive");
+
+ this.rising().ifHigh(command::schedule);
+ return this;
}
/**
@@ -115,33 +103,19 @@ public class Trigger extends BooleanEvent {
/**
* Constantly starts the given command while the button is held.
*
- *
{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
- * will be canceled when the trigger becomes inactive.
- *
- * @param command the command to start
- * @param interruptible whether the command is interruptible
- * @return this trigger, so calls can be chained
- */
- public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
- requireNonNullParam(command, "command", "whileActiveContinuous");
-
- this.ifHigh(() -> command.schedule(interruptible));
- this.falling().ifHigh(command::cancel);
-
- return this;
- }
-
- /**
- * Constantly starts the given command while the button is held.
- *
- *
{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
- * will be canceled when the trigger becomes inactive.
+ *
{@link Command#schedule()} will be called repeatedly while the trigger is active, and will
+ * be canceled when the trigger becomes inactive.
*
* @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whileActiveContinuous(final Command command) {
- return whileActiveContinuous(command, true);
+ requireNonNullParam(command, "command", "whileActiveContinuous");
+
+ this.ifHigh(command::schedule);
+ this.falling().ifHigh(command::cancel);
+
+ return this;
}
/**
@@ -160,52 +134,29 @@ public class Trigger extends BooleanEvent {
* inactive, but does not re-start it in-between.
*
* @param command the command to start
- * @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
- public Trigger whileActiveOnce(final Command command, boolean interruptible) {
+ public Trigger whileActiveOnce(final Command command) {
requireNonNullParam(command, "command", "whileActiveOnce");
- this.rising().ifHigh(() -> command.schedule(interruptible));
+ this.rising().ifHigh(command::schedule);
this.falling().ifHigh(command::cancel);
return this;
}
- /**
- * Starts the given command when the trigger initially becomes active, and ends it when it becomes
- * inactive, but does not re-start it in-between. The command is set to be interruptible.
- *
- * @param command the command to start
- * @return this trigger, so calls can be chained
- */
- public Trigger whileActiveOnce(final Command command) {
- return whileActiveOnce(command, true);
- }
-
/**
* Starts the command when the trigger becomes inactive.
*
* @param command the command to start
- * @param interruptible whether the command is interruptible
- * @return this trigger, so calls can be chained
- */
- public Trigger whenInactive(final Command command, boolean interruptible) {
- requireNonNullParam(command, "command", "whenInactive");
-
- this.falling().ifHigh(() -> command.schedule(interruptible));
-
- return this;
- }
-
- /**
- * Starts the command when the trigger becomes inactive. The command is set to be interruptible.
- *
- * @param command the command to start
* @return this trigger, so calls can be chained
*/
public Trigger whenInactive(final Command command) {
- return whenInactive(command, true);
+ requireNonNullParam(command, "command", "whenInactive");
+
+ this.falling().ifHigh(command::schedule);
+
+ return this;
}
/**
@@ -223,10 +174,9 @@ public class Trigger extends BooleanEvent {
* Toggles a command when the trigger becomes active.
*
* @param command the command to toggle
- * @param interruptible whether the command is interruptible
* @return this trigger, so calls can be chained
*/
- public Trigger toggleWhenActive(final Command command, boolean interruptible) {
+ public Trigger toggleWhenActive(final Command command) {
requireNonNullParam(command, "command", "toggleWhenActive");
this.rising()
@@ -235,23 +185,13 @@ public class Trigger extends BooleanEvent {
if (command.isScheduled()) {
command.cancel();
} else {
- command.schedule(interruptible);
+ command.schedule();
}
});
return this;
}
- /**
- * Toggles a command when the trigger becomes active. The command is set to be interruptible.
- *
- * @param command the command to toggle
- * @return this trigger, so calls can be chained
- */
- public Trigger toggleWhenActive(final Command command) {
- return toggleWhenActive(command, true);
- }
-
/**
* Cancels a command when the trigger becomes active.
*
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
index 4104ef8ae6..91a565eb52 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -67,6 +67,27 @@ std::unique_ptr Command::IgnoringDisable(bool doesRunWhenDisabled) && {
std::move(*this).TransferOwnership(), doesRunWhenDisabled);
}
+std::unique_ptr Command::WithInterruptBehavior(
+ InterruptionBehavior interruptBehavior) && {
+ class InterruptBehaviorCommand
+ : public CommandHelper {
+ public:
+ InterruptBehaviorCommand(std::unique_ptr&& command,
+ InterruptionBehavior interruptBehavior)
+ : CommandHelper(std::move(command)),
+ m_interruptBehavior(interruptBehavior) {}
+ InterruptionBehavior GetInterruptionBehavior() const override {
+ return m_interruptBehavior;
+ }
+
+ private:
+ InterruptionBehavior m_interruptBehavior;
+ };
+
+ return std::make_unique(
+ std::move(*this).TransferOwnership(), interruptBehavior);
+}
+
ParallelRaceGroup Command::WithInterrupt(std::function condition) && {
std::vector> temp;
temp.emplace_back(std::make_unique(std::move(condition)));
@@ -130,8 +151,8 @@ ConditionalCommand Command::Unless(std::function condition) && {
std::move(condition));
}
-void Command::Schedule(bool interruptible) {
- CommandScheduler::GetInstance().Schedule(interruptible, this);
+void Command::Schedule() {
+ CommandScheduler::GetInstance().Schedule(this);
}
void Command::Cancel() {
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
index 3f538792d7..63fa6546bd 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -19,16 +19,14 @@
#include
#include "frc2/command/CommandGroupBase.h"
-#include "frc2/command/CommandState.h"
#include "frc2/command/Subsystem.h"
using namespace frc2;
class CommandScheduler::Impl {
public:
- // A map from commands to their scheduling state. Also used as a set of the
- // currently-running commands.
- wpi::DenseMap scheduledCommands;
+ // A set of the currently-running commands.
+ wpi::SmallSet scheduledCommands;
// A map from required subsystems to their requiring commands. Also used as a
// set of the currently-required subsystems.
@@ -56,7 +54,7 @@ class CommandScheduler::Impl {
// scheduled/canceled during run
bool inRunLoop = false;
- wpi::DenseMap toSchedule;
+ wpi::SmallVector toSchedule;
wpi::SmallVector toCancel;
};
@@ -112,9 +110,9 @@ void CommandScheduler::ClearButtons() {
m_impl->activeButtonLoop->Clear();
}
-void CommandScheduler::Schedule(bool interruptible, Command* command) {
+void CommandScheduler::Schedule(Command* command) {
if (m_impl->inRunLoop) {
- m_impl->toSchedule.try_emplace(command, interruptible);
+ m_impl->toSchedule.emplace_back(command);
return;
}
@@ -126,7 +124,7 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
}
if (m_impl->disabled ||
(frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
- ContainsKey(m_impl->scheduledCommands, command)) {
+ m_impl->scheduledCommands.contains(command)) {
return;
}
@@ -139,8 +137,8 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
for (auto&& i1 : m_impl->requirements) {
if (requirements.find(i1.first) != requirements.end()) {
isDisjoint = false;
- allInterruptible &=
- m_impl->scheduledCommands[i1.second].IsInterruptible();
+ allInterruptible &= (i1.second->GetInterruptionBehavior() ==
+ Command::InterruptionBehavior::kCancelSelf);
intersection.emplace_back(i1.second);
}
}
@@ -151,7 +149,7 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
Cancel(cmdToCancel);
}
}
- m_impl->scheduledCommands[command] = CommandState{interruptible};
+ m_impl->scheduledCommands.insert(command);
for (auto&& requirement : requirements) {
m_impl->requirements[requirement] = command;
}
@@ -163,33 +161,15 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
}
}
-void CommandScheduler::Schedule(Command* command) {
- Schedule(true, command);
-}
-
-void CommandScheduler::Schedule(bool interruptible,
- wpi::span commands) {
- for (auto command : commands) {
- Schedule(interruptible, command);
- }
-}
-
-void CommandScheduler::Schedule(bool interruptible,
- std::initializer_list commands) {
- for (auto command : commands) {
- Schedule(interruptible, command);
- }
-}
-
void CommandScheduler::Schedule(wpi::span commands) {
for (auto command : commands) {
- Schedule(true, command);
+ Schedule(command);
}
}
void CommandScheduler::Schedule(std::initializer_list commands) {
for (auto command : commands) {
- Schedule(true, command);
+ Schedule(command);
}
}
@@ -218,10 +198,7 @@ void CommandScheduler::Run() {
m_impl->inRunLoop = true;
// Run scheduled commands, remove finished commands.
- for (auto iterator = m_impl->scheduledCommands.begin();
- iterator != m_impl->scheduledCommands.end(); iterator++) {
- Command* command = iterator->getFirst();
-
+ for (Command* command : m_impl->scheduledCommands) {
if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
Cancel(command);
continue;
@@ -243,14 +220,14 @@ void CommandScheduler::Run() {
m_impl->requirements.erase(requirement);
}
- m_impl->scheduledCommands.erase(iterator);
+ m_impl->scheduledCommands.erase(command);
m_watchdog.AddEpoch(command->GetName() + ".End(false)");
}
}
m_impl->inRunLoop = false;
- for (auto&& commandInterruptible : m_impl->toSchedule) {
- Schedule(commandInterruptible.second, commandInterruptible.first);
+ for (Command* command : m_impl->toSchedule) {
+ Schedule(command);
}
for (auto&& command : m_impl->toCancel) {
@@ -336,7 +313,7 @@ void CommandScheduler::Cancel(Command* command) {
if (find == m_impl->scheduledCommands.end()) {
return;
}
- m_impl->scheduledCommands.erase(find);
+ m_impl->scheduledCommands.erase(*find);
for (auto&& requirement : m_impl->requirements) {
if (requirement.second == command) {
m_impl->requirements.erase(requirement.first);
@@ -364,20 +341,11 @@ void CommandScheduler::Cancel(std::initializer_list commands) {
void CommandScheduler::CancelAll() {
wpi::SmallVector commands;
for (auto&& command : m_impl->scheduledCommands) {
- commands.emplace_back(command.first);
+ commands.emplace_back(command);
}
Cancel(commands);
}
-units::second_t CommandScheduler::TimeSinceScheduled(
- const Command* command) const {
- auto find = m_impl->scheduledCommands.find(command);
- if (find != m_impl->scheduledCommands.end()) {
- return find->second.TimeSinceInitialized();
- } else {
- return -1_s;
- }
-}
bool CommandScheduler::IsScheduled(
wpi::span commands) const {
for (auto command : commands) {
@@ -399,8 +367,7 @@ bool CommandScheduler::IsScheduled(
}
bool CommandScheduler::IsScheduled(const Command* command) const {
- return m_impl->scheduledCommands.find(command) !=
- m_impl->scheduledCommands.end();
+ return m_impl->scheduledCommands.contains(command);
}
Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
@@ -458,9 +425,9 @@ void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) {
wpi::SmallVector names;
wpi::SmallVector ids;
- for (auto&& command : m_impl->scheduledCommands) {
- names.emplace_back(command.first->GetName());
- uintptr_t ptrTmp = reinterpret_cast(command.first);
+ for (Command* command : m_impl->scheduledCommands) {
+ names.emplace_back(command->GetName());
+ uintptr_t ptrTmp = reinterpret_cast(command);
ids.emplace_back(static_cast(ptrTmp));
}
nt::NetworkTableEntry(namesEntry).SetStringArray(names);
@@ -470,5 +437,13 @@ void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) {
void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
std::unique_ptr command) {
+ if (command->GetInterruptionBehavior() ==
+ Command::InterruptionBehavior::kCancelIncoming) {
+ std::puts(
+ "Registering a non-interruptible default command!\n"
+ "This will likely prevent any other commands from "
+ "requiring this subsystem.");
+ // Warn, but allow -- there might be a use case for this.
+ }
m_impl->subsystems[subsystem] = std::move(command);
}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
deleted file mode 100644
index f53a694778..0000000000
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-// 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/CommandState.h"
-
-#include
-
-using namespace frc2;
-CommandState::CommandState(bool interruptible)
- : m_interruptible{interruptible} {
- StartTiming();
- StartRunning();
-}
-
-void CommandState::StartTiming() {
- m_startTime = frc::Timer::GetFPGATimestamp();
-}
-
-void CommandState::StartRunning() {
- m_startTime = -1_s;
-}
-
-units::second_t CommandState::TimeSinceInitialized() const {
- return m_startTime != -1_s ? frc::Timer::GetFPGATimestamp() - m_startTime
- : -1_s;
-}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
index 0021d3208e..a6e7468f7f 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
@@ -4,6 +4,8 @@
#include "frc2/command/WrapperCommand.h"
+#include "frc2/command/Command.h"
+
using namespace frc2;
WrapperCommand::WrapperCommand(std::unique_ptr&& command) {
@@ -33,3 +35,11 @@ void WrapperCommand::End(bool interrupted) {
bool WrapperCommand::RunsWhenDisabled() const {
return m_command->RunsWhenDisabled();
}
+
+Command::InterruptionBehavior WrapperCommand::GetInterruptionBehavior() const {
+ return m_command->GetInterruptionBehavior();
+}
+
+wpi::SmallSet WrapperCommand::GetRequirements() const {
+ return m_command->GetRequirements();
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
index 955830a672..373f0c6a7d 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
@@ -8,8 +8,8 @@ using namespace frc2;
Button::Button(std::function isPressed) : Trigger(isPressed) {}
-Button Button::WhenPressed(Command* command, bool interruptible) {
- WhenActive(command, interruptible);
+Button Button::WhenPressed(Command* command) {
+ WhenActive(command);
return *this;
}
@@ -25,8 +25,8 @@ Button Button::WhenPressed(std::function toRun,
return *this;
}
-Button Button::WhileHeld(Command* command, bool interruptible) {
- WhileActiveContinous(command, interruptible);
+Button Button::WhileHeld(Command* command) {
+ WhileActiveContinous(command);
return *this;
}
@@ -42,13 +42,13 @@ Button Button::WhileHeld(std::function toRun,
return *this;
}
-Button Button::WhenHeld(Command* command, bool interruptible) {
- WhileActiveOnce(command, interruptible);
+Button Button::WhenHeld(Command* command) {
+ WhileActiveOnce(command);
return *this;
}
-Button Button::WhenReleased(Command* command, bool interruptible) {
- WhenInactive(command, interruptible);
+Button Button::WhenReleased(Command* command) {
+ WhenInactive(command);
return *this;
}
@@ -64,8 +64,8 @@ Button Button::WhenReleased(std::function toRun,
return *this;
}
-Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
- ToggleWhenActive(command, interruptible);
+Button Button::ToggleWhenPressed(Command* command) {
+ ToggleWhenActive(command);
return *this;
}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
index 8db84fe5bf..40383a3c6b 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -13,9 +13,8 @@ using namespace frc2;
Trigger::Trigger(const Trigger& other) = default;
-Trigger Trigger::WhenActive(Command* command, bool interruptible) {
- this->Rising().IfHigh(
- [command, interruptible] { command->Schedule(interruptible); });
+Trigger Trigger::WhenActive(Command* command) {
+ this->Rising().IfHigh([command] { command->Schedule(); });
return *this;
}
@@ -30,8 +29,8 @@ Trigger Trigger::WhenActive(std::function toRun,
return WhenActive(InstantCommand(std::move(toRun), requirements));
}
-Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
- this->IfHigh([command, interruptible] { command->Schedule(interruptible); });
+Trigger Trigger::WhileActiveContinous(Command* command) {
+ this->IfHigh([command] { command->Schedule(); });
this->Falling().IfHigh([command] { command->Cancel(); });
return *this;
}
@@ -48,16 +47,14 @@ Trigger Trigger::WhileActiveContinous(
return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
}
-Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
- this->Rising().IfHigh(
- [command, interruptible] { command->Schedule(interruptible); });
+Trigger Trigger::WhileActiveOnce(Command* command) {
+ this->Rising().IfHigh([command] { command->Schedule(); });
this->Falling().IfHigh([command] { command->Cancel(); });
return *this;
}
-Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
- this->Falling().IfHigh(
- [command, interruptible] { command->Schedule(interruptible); });
+Trigger Trigger::WhenInactive(Command* command) {
+ this->Falling().IfHigh([command] { command->Schedule(); });
return *this;
}
@@ -72,12 +69,12 @@ Trigger Trigger::WhenInactive(std::function toRun,
return WhenInactive(InstantCommand(std::move(toRun), requirements));
}
-Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
- this->Rising().IfHigh([command, interruptible] {
+Trigger Trigger::ToggleWhenActive(Command* command) {
+ this->Rising().IfHigh([command] {
if (command->IsScheduled()) {
command->Cancel();
} else {
- command->Schedule(interruptible);
+ command->Schedule();
}
});
return *this;
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index 9256f03e49..0d82652d18 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -91,20 +91,36 @@ class Command {
virtual bool IsFinished() { return false; }
/**
- * Specifies the set of subsystems used by this command. Two commands cannot
- * use the same subsystem at the same time. If the command is scheduled as
- * interruptible and another command is scheduled that shares a requirement,
- * the command will be interrupted. Else, the command will not be scheduled.
- * If no subsystems are required, return an empty set.
+ * Specifies the set of subsystems used by this command. Two commands cannot
+ * use the same subsystem at the same time. If another command is scheduled
+ * that shares a requirement, GetInterruptionBehavior() will be checked and
+ * followed. If no subsystems are required, return an empty set.
*
* Note: it is recommended that user implementations contain the
* requirements as a field, and return that field here, rather than allocating
* a new set every time this is called.
*
* @return the set of subsystems that are required
+ * @see InterruptionBehavior
*/
virtual wpi::SmallSet GetRequirements() const = 0;
+ /**
+ * An enum describing the command's behavior when another command with a
+ * shared requirement is scheduled.
+ */
+ enum class InterruptionBehavior {
+ /**
+ * This command ends, End(true) is called, and the incoming command is
+ * scheduled normally.
+ *
+ * This is the default behavior.
+ */
+ kCancelSelf,
+ /** This command continues, and the incoming command is not scheduled. */
+ kCancelIncoming
+ };
+
/**
* Decorates this command with a timeout. If the specified timeout is
* exceeded before the command finishes normally, the command will be
@@ -238,21 +254,22 @@ class Command {
virtual std::unique_ptr IgnoringDisable(bool doesRunWhenDisabled) &&;
/**
- * Schedules this command.
+ * Decorates this command to run or stop when disabled.
*
- * @param interruptible whether this command can be interrupted by another
- * command that shares one of its requirements
+ * @param interruptBehavior true to run when disabled.
+ * @return the decorated command
*/
- void Schedule(bool interruptible);
+ virtual std::unique_ptr WithInterruptBehavior(
+ InterruptionBehavior interruptBehavior) &&;
/**
- * Schedules this command, defaulting to interruptible.
+ * Schedules this command.
*/
- void Schedule() { Schedule(true); }
+ void Schedule();
/**
- * Cancels this command. Will call the command's interrupted() method.
- * Commands will be canceled even if they are not marked as interruptible.
+ * Cancels this command. Will call End(true). Commands will be canceled
+ * regardless of interruption behavior.
*/
void Cancel();
@@ -296,6 +313,16 @@ class Command {
*/
virtual bool RunsWhenDisabled() const { return false; }
+ /**
+ * How the command behaves when another command with a shared requirement is
+ * scheduled.
+ *
+ * @return a variant of InterruptionBehavior, defaulting to kCancelSelf.
+ */
+ virtual InterruptionBehavior GetInterruptionBehavior() const {
+ return InterruptionBehavior::kCancelSelf;
+ }
+
virtual std::string GetName() const;
protected:
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
index 63ddb81f8d..75fc3ef2cc 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -83,60 +83,27 @@ class CommandScheduler final : public nt::NTSendable,
void ClearButtons();
/**
- * Schedules a command for execution. Does nothing if the command is already
+ * Schedules a command for execution. Does nothing if the command is already
* scheduled. If a command's requirements are not available, it will only be
* started if all the commands currently using those requirements have been
- * scheduled as interruptible. If this is the case, they will be interrupted
+ * scheduled as interruptible. If this is the case, they will be interrupted
* and the command will be scheduled.
*
- * @param interruptible whether this command can be interrupted
- * @param command the command to schedule
- */
- void Schedule(bool interruptible, Command* command);
-
- /**
- * Schedules a command for execution, with interruptible defaulted to true.
- * Does nothing if the command is already scheduled.
- *
* @param command the command to schedule
*/
void Schedule(Command* command);
/**
- * Schedules multiple commands for execution. Does nothing if the command is
- * already scheduled. If a command's requirements are not available, it will
- * only be started if all the commands currently using those requirements have
- * been scheduled as interruptible. If this is the case, they will be
- * interrupted and the command will be scheduled.
- *
- * @param interruptible whether the commands should be interruptible
- * @param commands the commands to schedule
- */
- void Schedule(bool interruptible, wpi::span commands);
-
- /**
- * Schedules multiple commands for execution. Does nothing if the command is
- * already scheduled. If a command's requirements are not available, it will
- * only be started if all the commands currently using those requirements have
- * been scheduled as interruptible. If this is the case, they will be
- * interrupted and the command will be scheduled.
- *
- * @param interruptible whether the commands should be interruptible
- * @param commands the commands to schedule
- */
- void Schedule(bool interruptible, std::initializer_list commands);
-
- /**
- * Schedules multiple commands for execution, with interruptible defaulted to
- * true. Does nothing if the command is already scheduled.
+ * Schedules multiple commands for execution. Does nothing for commands
+ * already scheduled.
*
* @param commands the commands to schedule
*/
void Schedule(wpi::span commands);
/**
- * Schedules multiple commands for execution, with interruptible defaulted to
- * true. Does nothing if the command is already scheduled.
+ * Schedules multiple commands for execution. Does nothing for commands
+ * already scheduled.
*
* @param commands the commands to schedule
*/
@@ -262,17 +229,6 @@ class CommandScheduler final : public nt::NTSendable,
*/
void CancelAll();
- /**
- * Returns the time since a given command was scheduled. Note that this only
- * works on commands that are directly scheduled by the scheduler; it will not
- * work on commands inside of commandgroups, as the scheduler does not see
- * them.
- *
- * @param command the command to query
- * @return the time since the command was scheduled
- */
- units::second_t TimeSinceScheduled(const Command* command) const;
-
/**
* Whether the given commands are running. Note that this only works on
* commands that are directly scheduled by the scheduler; it will not work on
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
deleted file mode 100644
index e040e4b861..0000000000
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
+++ /dev/null
@@ -1,34 +0,0 @@
-// 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
-
-namespace frc2 {
-/**
- * Class that holds scheduling state for a command. Used internally by the
- * CommandScheduler
- *
- * This class is provided by the NewCommands VendorDep
- */
-class CommandState final {
- public:
- CommandState() = default;
-
- explicit CommandState(bool interruptible);
-
- bool IsInterruptible() const { return m_interruptible; }
-
- // The time since this command was initialized.
- units::second_t TimeSinceInitialized() const;
-
- private:
- units::second_t m_startTime = -1_s;
- bool m_interruptible;
-
- void StartTiming();
- void StartRunning();
-};
-} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
index 9516728ef3..139f2110d4 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
@@ -64,6 +64,10 @@ class WrapperCommand : public CommandHelper {
bool RunsWhenDisabled() const override;
+ InterruptionBehavior GetInterruptionBehavior() const override;
+
+ wpi::SmallSet GetRequirements() const override;
+
protected:
std::unique_ptr m_command;
};
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
index 12a5aa7ff5..bbd71966e4 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
@@ -43,10 +43,9 @@ class Button : public Trigger {
* of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
- Button WhenPressed(Command* command, bool interruptible = true);
+ Button WhenPressed(Command* command);
/**
* Binds a command to start when the button is pressed. Transfers
@@ -55,13 +54,12 @@ class Button : public Trigger {
* *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template >>>
- Button WhenPressed(T&& command, bool interruptible = true) {
- WhenActive(std::forward(command), interruptible);
+ Button WhenPressed(T&& command) {
+ WhenActive(std::forward(command));
return *this;
}
@@ -89,10 +87,9 @@ class Button : public Trigger {
* users are responsible for the lifespan of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
- Button WhileHeld(Command* command, bool interruptible = true);
+ Button WhileHeld(Command* command);
/**
* Binds a command to be started repeatedly while the button is pressed, and
@@ -101,13 +98,12 @@ class Button : public Trigger {
* will be *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
template >>>
- Button WhileHeld(T&& command, bool interruptible = true) {
- WhileActiveContinous(std::forward(command), interruptible);
+ Button WhileHeld(T&& command) {
+ WhileActiveContinous(std::forward(command));
return *this;
}
@@ -135,10 +131,9 @@ class Button : public Trigger {
* responsible for the lifespan of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
- Button WhenHeld(Command* command, bool interruptible = true);
+ Button WhenHeld(Command* command);
/**
* Binds a command to be started when the button is pressed, and canceled
@@ -147,13 +142,12 @@ class Button : public Trigger {
* *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
template >>>
- Button WhenHeld(T&& command, bool interruptible = true) {
- WhileActiveOnce(std::forward(command), interruptible);
+ Button WhenHeld(T&& command) {
+ WhileActiveOnce(std::forward(command));
return *this;
}
@@ -163,10 +157,9 @@ class Button : public Trigger {
* of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
- Button WhenReleased(Command* command, bool interruptible = true);
+ Button WhenReleased(Command* command);
/**
* Binds a command to start when the button is pressed. Transfers
@@ -175,13 +168,12 @@ class Button : public Trigger {
* *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
template >>>
- Button WhenReleased(T&& command, bool interruptible = true) {
- WhenInactive(std::forward(command), interruptible);
+ Button WhenReleased(T&& command) {
+ WhenInactive(std::forward(command));
return *this;
}
@@ -209,10 +201,9 @@ class Button : public Trigger {
* responsible for the lifespan of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
- Button ToggleWhenPressed(Command* command, bool interruptible = true);
+ Button ToggleWhenPressed(Command* command);
/**
* Binds a command to start when the button is pressed, and be canceled when
@@ -221,13 +212,12 @@ class Button : public Trigger {
* *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The button, for chained calls.
*/
template >>>
- Button ToggleWhenPressed(T&& command, bool interruptible = true) {
- ToggleWhenActive(std::forward(command), interruptible);
+ Button ToggleWhenPressed(T&& command) {
+ ToggleWhenActive(std::forward(command));
return *this;
}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
index 409e34eb72..13fb10bd02 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -60,33 +60,30 @@ class Trigger : public frc::BooleanEvent {
Trigger(const Trigger& other);
/**
- * Binds a command to start when the trigger becomes active. Takes a
+ * Binds a command to start when the trigger becomes active. Takes a
* raw pointer, and so is non-owning; users are responsible for the lifespan
* of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
- Trigger WhenActive(Command* command, bool interruptible = true);
+ Trigger WhenActive(Command* command);
/**
- * Binds a command to start when the trigger becomes active. Transfers
+ * Binds a command to start when the trigger becomes active. Transfers
* command ownership to the button scheduler, so the user does not have to
* worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
* *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template >>>
- Trigger WhenActive(T&& command, bool interruptible = true) {
+ Trigger WhenActive(T&& command) {
this->Rising().IfHigh(
[command = std::make_unique>(
- std::forward(command)),
- interruptible] { command->Schedule(interruptible); });
+ std::forward(command))] { command->Schedule(); });
return *this;
}
@@ -115,10 +112,9 @@ class Trigger : public frc::BooleanEvent {
* non-owning; users are responsible for the lifespan of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
- Trigger WhileActiveContinous(Command* command, bool interruptible = true);
+ Trigger WhileActiveContinous(Command* command);
/**
* Binds a command to be started repeatedly while the trigger is active, and
@@ -127,15 +123,14 @@ class Trigger : public frc::BooleanEvent {
* rvalue refs will be *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template >>>
- Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
+ Trigger WhileActiveContinous(T&& command) {
std::shared_ptr ptr =
std::make_shared>(std::forward(command));
- this->IfHigh([ptr, interruptible] { ptr->Schedule(interruptible); });
+ this->IfHigh([ptr] { ptr->Schedule(); });
this->Falling().IfHigh([ptr] { ptr->Cancel(); });
return *this;
@@ -165,29 +160,26 @@ class Trigger : public frc::BooleanEvent {
* non-owning; users are responsible for the lifespan of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
- Trigger WhileActiveOnce(Command* command, bool interruptible = true);
+ Trigger WhileActiveOnce(Command* command);
/**
* Binds a command to be started when the trigger becomes active, and
- * canceled when it becomes inactive. Transfers command ownership to the
+ * canceled when it becomes inactive. Transfers command ownership to the
* button scheduler, so the user does not have to worry about lifespan -
* rvalue refs will be *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template >>>
- Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
+ Trigger WhileActiveOnce(T&& command) {
std::shared_ptr ptr =
std::make_shared>(std::forward(command));
- this->Rising().IfHigh(
- [ptr, interruptible] { ptr->Schedule(interruptible); });
+ this->Rising().IfHigh([ptr] { ptr->Schedule(); });
this->Falling().IfHigh([ptr] { ptr->Cancel(); });
return *this;
@@ -199,10 +191,9 @@ class Trigger : public frc::BooleanEvent {
* of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
- Trigger WhenInactive(Command* command, bool interruptible = true);
+ Trigger WhenInactive(Command* command);
/**
* Binds a command to start when the trigger becomes inactive. Transfers
@@ -211,16 +202,14 @@ class Trigger : public frc::BooleanEvent {
* *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template >>>
- Trigger WhenInactive(T&& command, bool interruptible = true) {
+ Trigger WhenInactive(T&& command) {
this->Falling().IfHigh(
[command = std::make_unique>(
- std::forward(command)),
- interruptible] { command->Schedule(interruptible); });
+ std::forward(command))] { command->Schedule(); });
return *this;
}
@@ -245,14 +234,13 @@ class Trigger : public frc::BooleanEvent {
/**
* Binds a command to start when the trigger becomes active, and be canceled
- * when it again becomes active. Takes a raw pointer, and so is non-owning;
+ * when it again becomes active. Takes a raw pointer, and so is non-owning;
* users are responsible for the lifespan of the command.
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
- Trigger ToggleWhenActive(Command* command, bool interruptible = true);
+ Trigger ToggleWhenActive(Command* command);
/**
* Binds a command to start when the trigger becomes active, and be canceled
@@ -261,18 +249,16 @@ class Trigger : public frc::BooleanEvent {
* will be *moved*, lvalue refs will be *copied.*
*
* @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
* @return The trigger, for chained calls.
*/
template >>>
- Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
+ Trigger ToggleWhenActive(T&& command) {
this->Rising().IfHigh(
[command = std::make_unique>(
- std::forward(command)),
- interruptible] {
+ std::forward(command))] {
if (!command->IsScheduled()) {
- command->Schedule(interruptible);
+ command->Schedule();
} else {
command->Cancel();
}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
index 2f831f6423..2da078c38f 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
@@ -44,12 +44,13 @@ class CommandRequirementsTest extends CommandTestBase {
try (CommandScheduler scheduler = new CommandScheduler()) {
Subsystem requirement = new SubsystemBase() {};
- MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
- Command notInterrupted = interruptedHolder.getMock();
+ Command notInterrupted =
+ new RunCommand(() -> {}, requirement)
+ .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming);
MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
Command interrupter = interrupterHolder.getMock();
- scheduler.schedule(false, notInterrupted);
+ scheduler.schedule(notInterrupted);
scheduler.schedule(interrupter);
assertTrue(scheduler.isScheduled(notInterrupted));
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
index a30a6ca667..c0295f51f3 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
@@ -66,7 +66,7 @@ class CommandScheduleTest extends CommandTestBase {
MockCommandHolder command3Holder = new MockCommandHolder(true);
Command command3 = command3Holder.getMock();
- scheduler.schedule(true, command1, command2, command3);
+ scheduler.schedule(command1, command2, command3);
assertTrue(scheduler.isScheduled(command1, command2, command3));
scheduler.run();
assertTrue(scheduler.isScheduled(command1, command2, command3));
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
index 6e6e5c9887..28e9e6fbeb 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
@@ -9,19 +9,20 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.params.ParameterizedTest;
-import org.junit.jupiter.params.provider.ValueSource;
+import org.junit.jupiter.params.provider.EnumSource;
class SchedulingRecursionTest extends CommandTestBase {
/**
* wpilibsuite/allwpilib#4259.
*/
- @ValueSource(booleans = {true, false})
+ @EnumSource(InterruptionBehavior.class)
@ParameterizedTest
- void cancelFromInitialize(boolean interruptible) {
+ void cancelFromInitialize(InterruptionBehavior interruptionBehavior) {
try (CommandScheduler scheduler = new CommandScheduler()) {
AtomicBoolean hasOtherRun = new AtomicBoolean();
Subsystem requirement = new SubsystemBase() {};
@@ -35,14 +36,18 @@ class SchedulingRecursionTest extends CommandTestBase {
public void initialize() {
scheduler.cancel(this);
}
+
+ @Override
+ public InterruptionBehavior getInterruptionBehavior() {
+ return interruptionBehavior;
+ }
};
Command other = new RunCommand(() -> hasOtherRun.set(true), requirement);
assertDoesNotThrow(
() -> {
- scheduler.schedule(interruptible, selfCancels);
+ scheduler.schedule(selfCancels);
scheduler.run();
- // interruptibility of new arrival isn't checked
scheduler.schedule(other);
});
assertFalse(scheduler.isScheduled(selfCancels));
@@ -52,9 +57,9 @@ class SchedulingRecursionTest extends CommandTestBase {
}
}
- @ValueSource(booleans = {true, false})
+ @EnumSource(InterruptionBehavior.class)
@ParameterizedTest
- void defaultCommand(boolean interruptible) {
+ void defaultCommandGetsRescheduledAfterSelfCanceling(InterruptionBehavior interruptionBehavior) {
try (CommandScheduler scheduler = new CommandScheduler()) {
AtomicBoolean hasOtherRun = new AtomicBoolean();
Subsystem requirement = new SubsystemBase() {};
@@ -68,13 +73,18 @@ class SchedulingRecursionTest extends CommandTestBase {
public void initialize() {
scheduler.cancel(this);
}
+
+ @Override
+ public InterruptionBehavior getInterruptionBehavior() {
+ return interruptionBehavior;
+ }
};
Command other = new RunCommand(() -> hasOtherRun.set(true), requirement);
scheduler.setDefaultCommand(requirement, other);
assertDoesNotThrow(
() -> {
- scheduler.schedule(interruptible, selfCancels);
+ scheduler.schedule(selfCancels);
scheduler.run();
});
scheduler.run();
@@ -161,12 +171,13 @@ class SchedulingRecursionTest extends CommandTestBase {
}
@ParameterizedTest
- @ValueSource(booleans = {true, false})
- void scheduleInitializeFromDefaultCommand(boolean interruptible) {
+ @EnumSource(InterruptionBehavior.class)
+ void scheduleInitializeFromDefaultCommand(InterruptionBehavior interruptionBehavior) {
try (CommandScheduler scheduler = new CommandScheduler()) {
AtomicInteger counter = new AtomicInteger();
Subsystem requirement = new SubsystemBase() {};
- Command other = new InstantCommand(() -> {}, requirement);
+ Command other =
+ new InstantCommand(() -> {}, requirement).withInterruptBehavior(interruptionBehavior);
Command defaultCommand =
new CommandBase() {
{
@@ -176,7 +187,7 @@ class SchedulingRecursionTest extends CommandTestBase {
@Override
public void initialize() {
counter.incrementAndGet();
- scheduler.schedule(interruptible, other);
+ scheduler.schedule(other);
}
};
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
index 2b13910fed..b4a1859a7e 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
@@ -31,11 +31,11 @@ class ButtonTest extends CommandTestBase {
button.setPressed(false);
button.whenPressed(command1);
scheduler.run();
- verify(command1, never()).schedule(true);
+ verify(command1, never()).schedule();
button.setPressed(true);
scheduler.run();
scheduler.run();
- verify(command1).schedule(true);
+ verify(command1).schedule();
}
@Test
@@ -48,11 +48,11 @@ class ButtonTest extends CommandTestBase {
button.setPressed(true);
button.whenReleased(command1);
scheduler.run();
- verify(command1, never()).schedule(true);
+ verify(command1, never()).schedule();
button.setPressed(false);
scheduler.run();
scheduler.run();
- verify(command1).schedule(true);
+ verify(command1).schedule();
}
@Test
@@ -65,11 +65,11 @@ class ButtonTest extends CommandTestBase {
button.setPressed(false);
button.whileHeld(command1);
scheduler.run();
- verify(command1, never()).schedule(true);
+ verify(command1, never()).schedule();
button.setPressed(true);
scheduler.run();
scheduler.run();
- verify(command1, times(2)).schedule(true);
+ verify(command1, times(2)).schedule();
button.setPressed(false);
scheduler.run();
verify(command1).cancel();
@@ -85,11 +85,11 @@ class ButtonTest extends CommandTestBase {
button.setPressed(false);
button.whenHeld(command1);
scheduler.run();
- verify(command1, never()).schedule(true);
+ verify(command1, never()).schedule();
button.setPressed(true);
scheduler.run();
scheduler.run();
- verify(command1).schedule(true);
+ verify(command1).schedule();
button.setPressed(false);
scheduler.run();
verify(command1).cancel();
@@ -105,12 +105,12 @@ class ButtonTest extends CommandTestBase {
button.setPressed(false);
button.toggleWhenPressed(command1);
scheduler.run();
- verify(command1, never()).schedule(true);
+ verify(command1, never()).schedule();
button.setPressed(true);
scheduler.run();
when(command1.isScheduled()).thenReturn(true);
scheduler.run();
- verify(command1).schedule(true);
+ verify(command1).schedule();
button.setPressed(false);
scheduler.run();
verify(command1, never()).cancel();
@@ -200,13 +200,13 @@ class ButtonTest extends CommandTestBase {
button.setPressed(true);
scheduler.run();
- verify(command, never()).schedule(true);
+ verify(command, never()).schedule();
SimHooks.stepTiming(0.3);
button.setPressed(true);
scheduler.run();
- verify(command).schedule(true);
+ verify(command).schedule();
}
@Test
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
index 9502977360..14513d79b0 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
@@ -37,10 +37,10 @@ class NetworkButtonTest extends CommandTestBase {
entry.setBoolean(false);
button.whenPressed(command);
scheduler.run();
- verify(command, never()).schedule(true);
+ verify(command, never()).schedule();
entry.setBoolean(true);
scheduler.run();
scheduler.run();
- verify(command).schedule(true);
+ verify(command).schedule();
}
}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
index c81267081a..00958b32f0 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
@@ -7,6 +7,7 @@
#include "CommandTestBase.h"
#include "frc2/command/CommandScheduler.h"
#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/FunctionalCommand.h"
#include "frc2/command/InstantCommand.h"
#include "frc2/command/ParallelCommandGroup.h"
#include "frc2/command/ParallelDeadlineGroup.h"
@@ -49,26 +50,35 @@ TEST_F(CommandRequirementsTest, RequirementUninterruptible) {
TestSubsystem requirement;
- MockCommand command1({&requirement});
- MockCommand command2({&requirement});
+ int initCounter = 0;
+ int exeCounter = 0;
+ int endCounter = 0;
- EXPECT_CALL(command1, Initialize());
- EXPECT_CALL(command1, Execute()).Times(2);
- EXPECT_CALL(command1, End(true)).Times(0);
- EXPECT_CALL(command1, End(false)).Times(0);
+ std::unique_ptr command1 =
+ FunctionalCommand([&initCounter] { initCounter++; },
+ [&exeCounter] { exeCounter++; },
+ [&endCounter](bool interruptible) { endCounter++; },
+ [] { return false; }, {&requirement})
+ .WithInterruptBehavior(
+ Command::InterruptionBehavior::kCancelIncoming);
+ MockCommand command2({&requirement});
EXPECT_CALL(command2, Initialize()).Times(0);
EXPECT_CALL(command2, Execute()).Times(0);
EXPECT_CALL(command2, End(true)).Times(0);
EXPECT_CALL(command2, End(false)).Times(0);
- scheduler.Schedule(false, &command1);
+ scheduler.Schedule(command1.get());
+ EXPECT_EQ(1, initCounter);
scheduler.Run();
- EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ EXPECT_EQ(1, exeCounter);
+ EXPECT_TRUE(scheduler.IsScheduled(command1.get()));
scheduler.Schedule(&command2);
- EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ EXPECT_TRUE(scheduler.IsScheduled(command1.get()));
EXPECT_FALSE(scheduler.IsScheduled(&command2));
scheduler.Run();
+ EXPECT_EQ(2, exeCounter);
+ EXPECT_EQ(0, endCounter);
}
TEST_F(CommandRequirementsTest, DefaultCommandRequirementError) {
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
index fee626efed..d06423e69c 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
@@ -10,6 +10,7 @@
#include
#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
#include "frc2/command/CommandScheduler.h"
#include "frc2/command/SetUtilities.h"
#include "frc2/command/SubsystemBase.h"
@@ -26,7 +27,10 @@ class CommandTestBase : public ::testing::Test {
class TestSubsystem : public SubsystemBase {};
protected:
- class MockCommand : public Command {
+ /**
+ * NOTE: Moving mock objects causes EXPECT_CALL to not work correctly!
+ */
+ class MockCommand : public CommandHelper {
public:
MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet());
MOCK_METHOD0(IsFinished, bool());
@@ -65,7 +69,7 @@ class CommandTestBase : public ::testing::Test {
.WillRepeatedly(::testing::Return(m_requirements));
}
- MockCommand(const MockCommand& other) : Command{other} {}
+ MockCommand(const MockCommand& other) : CommandHelper{other} {}
void SetFinished(bool finished) {
EXPECT_CALL(*this, IsFinished())
@@ -77,11 +81,6 @@ class CommandTestBase : public ::testing::Test {
scheduler.Cancel(this);
}
- protected:
- std::unique_ptr TransferOwnership() && { // NOLINT
- return std::make_unique(std::move(*this));
- }
-
private:
wpi::SmallSet m_requirements;
};
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp
index 6a6eae2a48..733117c7bc 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp
@@ -3,43 +3,51 @@
// the WPILib BSD license file in the root directory of this project.
#include "CommandTestBase.h"
+#include "frc2/command/Command.h"
#include "frc2/command/CommandHelper.h"
#include "frc2/command/RunCommand.h"
#include "gtest/gtest.h"
using namespace frc2;
-class SchedulingRecursionTest : public CommandTestBaseWithParam {};
+class SchedulingRecursionTest
+ : public CommandTestBaseWithParam {};
class SelfCancellingCommand
: public CommandHelper {
public:
- SelfCancellingCommand(CommandScheduler* scheduler, Subsystem* requirement)
- : m_scheduler(scheduler) {
+ SelfCancellingCommand(CommandScheduler* scheduler, Subsystem* requirement,
+ Command::InterruptionBehavior interruptionBehavior =
+ Command::InterruptionBehavior::kCancelSelf)
+ : m_scheduler(scheduler), m_interrupt(interruptionBehavior) {
AddRequirements(requirement);
}
void Initialize() override { m_scheduler->Cancel(this); }
+ InterruptionBehavior GetInterruptionBehavior() const override {
+ return m_interrupt;
+ }
+
private:
CommandScheduler* m_scheduler;
+ InterruptionBehavior m_interrupt;
};
/**
* Checks wpilibsuite/allwpilib#4259.
*/
-TEST_P(SchedulingRecursionTest, CancelFromInitialize) {
+TEST_F(SchedulingRecursionTest, CancelFromInitialize) {
CommandScheduler scheduler = GetScheduler();
bool hasOtherRun = false;
TestSubsystem requirement;
- SelfCancellingCommand selfCancels{&scheduler, &requirement};
+ auto selfCancels = SelfCancellingCommand(&scheduler, &requirement);
RunCommand other =
RunCommand([&hasOtherRun] { hasOtherRun = true; }, {&requirement});
- scheduler.Schedule(GetParam(), &selfCancels);
+ scheduler.Schedule(&selfCancels);
scheduler.Run();
- // interruptibility of new arrival isn't checked
scheduler.Schedule(&other);
EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
@@ -48,16 +56,18 @@ TEST_P(SchedulingRecursionTest, CancelFromInitialize) {
EXPECT_TRUE(hasOtherRun);
}
-TEST_P(SchedulingRecursionTest, DefaultCommand) {
+TEST_P(SchedulingRecursionTest,
+ DefaultCommandGetsRescheduledAfterSelfCanceling) {
CommandScheduler scheduler = GetScheduler();
bool hasOtherRun = false;
TestSubsystem requirement;
- SelfCancellingCommand selfCancels{&scheduler, &requirement};
+ auto selfCancels =
+ SelfCancellingCommand(&scheduler, &requirement, GetParam());
RunCommand other =
RunCommand([&hasOtherRun] { hasOtherRun = true; }, {&requirement});
scheduler.SetDefaultCommand(&requirement, std::move(other));
- scheduler.Schedule(GetParam(), &selfCancels);
+ scheduler.Schedule(&selfCancels);
scheduler.Run();
scheduler.Run();
EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
@@ -93,5 +103,7 @@ TEST_F(SchedulingRecursionTest, CancelFromEnd) {
EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
}
-INSTANTIATE_TEST_SUITE_P(SchedulingRecursionTests, SchedulingRecursionTest,
- testing::Bool());
+INSTANTIATE_TEST_SUITE_P(
+ SchedulingRecursionTests, SchedulingRecursionTest,
+ testing::Values(Command::InterruptionBehavior::kCancelSelf,
+ Command::InterruptionBehavior::kCancelIncoming));