diff --git a/.gittrack b/.gittrack new file mode 100644 index 00000000..889f2d8f --- /dev/null +++ b/.gittrack @@ -0,0 +1,5 @@ +[git-source-track] +upstream_root = ../allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command +upstream_branch = main +upstream_commit = 4595f84719759d71491e11e8df55d60ce2a6154c +validation_root = commands2 \ No newline at end of file diff --git a/commands2/__init__.py b/commands2/__init__.py index aecadeb6..d64e2a4f 100644 --- a/commands2/__init__.py +++ b/commands2/__init__.py @@ -1,40 +1,21 @@ -from .button import Trigger from .command import Command, InterruptionBehavior +from . import button from . import cmd -# from .cmd import ( -# deadline, -# either, -# none, -# parallel, -# print_, -# race, -# repeatingSequence, -# run, -# runEnd, -# runOnce, -# select, -# sequence, -# startEnd, -# waitSeconds, -# waitUntil, -# ) -from .commandgroup import CommandGroup, IllegalCommandUse from .commandscheduler import CommandScheduler from .conditionalcommand import ConditionalCommand +from .exceptions import IllegalCommandUse from .functionalcommand import FunctionalCommand from .instantcommand import InstantCommand from .notifiercommand import NotifierCommand from .parallelcommandgroup import ParallelCommandGroup from .paralleldeadlinegroup import ParallelDeadlineGroup from .parallelracegroup import ParallelRaceGroup -from .perpetualcommand import PerpetualCommand from .pidcommand import PIDCommand from .pidsubsystem import PIDSubsystem from .printcommand import PrintCommand from .proxycommand import ProxyCommand -from .proxyschedulecommand import ProxyScheduleCommand from .repeatcommand import RepeatCommand from .runcommand import RunCommand from .schedulecommand import ScheduleCommand @@ -52,9 +33,9 @@ from typing import TYPE_CHECKING __all__ = [ + "button", "cmd", "Command", - "CommandGroup", "CommandScheduler", "ConditionalCommand", "FunctionalCommand", @@ -65,12 +46,10 @@ "ParallelCommandGroup", "ParallelDeadlineGroup", "ParallelRaceGroup", - "PerpetualCommand", "PIDCommand", "PIDSubsystem", "PrintCommand", "ProxyCommand", - "ProxyScheduleCommand", "RepeatCommand", "RunCommand", "ScheduleCommand", @@ -84,22 +63,6 @@ "WaitCommand", "WaitUntilCommand", "WrapperCommand", - # "none", - # "runOnce", - # "run", - # "startEnd", - # "runEnd", - # "print_", - # "waitSeconds", - # "waitUntil", - # "either", - # "select", - # "sequence", - # "repeatingSequence", - # "parallel", - # "race", - # "deadline", - "Trigger", # was here in 2023 ] if not TYPE_CHECKING: diff --git a/commands2/button/commandgenerichid.py b/commands2/button/commandgenerichid.py index 1512fd15..3bf95e09 100644 --- a/commands2/button/commandgenerichid.py +++ b/commands2/button/commandgenerichid.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 92149efa11fa button/CommandGenericHID.java from typing import Optional from wpilib.event import EventLoop @@ -9,7 +10,7 @@ class CommandGenericHID: """ - A version of GenericHID with Trigger factories for command-based. + A version of :class:`wpilib.interfaces.GenericHID` with :class:`.Trigger` factories for command-based. """ def __init__(self, port: int): @@ -31,7 +32,10 @@ def button(self, button: int, loop: Optional[EventLoop] = None) -> Trigger: Constructs an event instance around this button's digital signal. :param button: The button index - :param loop: the event loop instance to attache the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + + :returns: A trigger instance attached to the event loop """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -48,8 +52,9 @@ def pov( :param angle: POV angle in degrees, or -1 for the center / not pressed. :param pov: index of the POV to read (starting at 0). Defaults to 0. - :param loop: the event loop instance to attach the event to. Defaults to {@link - CommandScheduler#getDefaultButtonLoop() the default command scheduler button loop}. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: a Trigger instance based around this angle of a POV on the HID. """ if loop is None: @@ -59,8 +64,7 @@ def pov( def povUp(self) -> Trigger: """ Constructs a Trigger instance based around the 0 degree angle (up) of the default (index 0) POV - on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command - scheduler button loop}. + on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop` :returns: a Trigger instance based around the 0 degree angle of a POV on the HID. """ @@ -69,8 +73,7 @@ def povUp(self) -> Trigger: def povUpRight(self) -> Trigger: """ Constructs a Trigger instance based around the 45 degree angle (right up) of the default (index - 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default - command scheduler button loop}. + 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. :returns: a Trigger instance based around the 45 degree angle of a POV on the HID. """ @@ -79,8 +82,7 @@ def povUpRight(self) -> Trigger: def povRight(self) -> Trigger: """ Constructs a Trigger instance based around the 90 degree angle (right) of the default (index 0) - POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command - scheduler button loop}. + POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. :returns: a Trigger instance based around the 90 degree angle of a POV on the HID. """ @@ -89,8 +91,7 @@ def povRight(self) -> Trigger: def povDownRight(self) -> Trigger: """ Constructs a Trigger instance based around the 135 degree angle (right down) of the default - (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the - default command scheduler button loop}. + (index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. :returns: a Trigger instance based around the 135 degree angle of a POV on the HID. """ @@ -99,8 +100,7 @@ def povDownRight(self) -> Trigger: def povDown(self) -> Trigger: """ Constructs a Trigger instance based around the 180 degree angle (down) of the default (index 0) - POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command - scheduler button loop}. + POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. :returns: a Trigger instance based around the 180 degree angle of a POV on the HID. """ @@ -109,8 +109,7 @@ def povDown(self) -> Trigger: def povDownLeft(self) -> Trigger: """ Constructs a Trigger instance based around the 225 degree angle (down left) of the default - (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the - default command scheduler button loop}. + (index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. :returns: a Trigger instance based around the 225 degree angle of a POV on the HID. """ @@ -119,8 +118,7 @@ def povDownLeft(self) -> Trigger: def povLeft(self) -> Trigger: """ Constructs a Trigger instance based around the 270 degree angle (left) of the default (index 0) - POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command - scheduler button loop}. + POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. :returns: a Trigger instance based around the 270 degree angle of a POV on the HID. """ @@ -129,8 +127,7 @@ def povLeft(self) -> Trigger: def povUpLeft(self) -> Trigger: """ Constructs a Trigger instance based around the 315 degree angle (left up) of the default (index - 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default - command scheduler button loop}. + 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. :returns: a Trigger instance based around the 315 degree angle of a POV on the HID. """ @@ -139,8 +136,7 @@ def povUpLeft(self) -> Trigger: def povCenter(self) -> Trigger: """ Constructs a Trigger instance based around the center (not pressed) position of the default - (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the - default command scheduler button loop}. + (index 0) POV on the HID, attached to :func:`commands2.CommandScheduler.getDefaultButtonLoop`. :returns: a Trigger instance based around the center position of a POV on the HID. """ @@ -150,14 +146,15 @@ def axisLessThan( self, axis: int, threshold: float, loop: Optional[EventLoop] = None ) -> Trigger: """ - Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, + Constructs a Trigger instance that is true when the axis value is less than ``threshold``, attached to the given loop. :param axis: The axis to read, starting at 0 :param threshold: The value below which this trigger should return true. :param loop: the event loop instance to attach the trigger to + :returns: a Trigger instance that is true when the axis value is less than the provided - threshold. + threshold. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -167,14 +164,15 @@ def axisGreaterThan( self, axis: int, threshold: float, loop: Optional[EventLoop] = None ) -> Trigger: """ - Constructs a Trigger instance that is true when the axis value is greater than {@code - threshold}, attached to the given loop. + Constructs a Trigger instance that is true when the axis value is greater than + ``threshold``, attached to the given loop. :param axis: The axis to read, starting at 0 :param threshold: The value above which this trigger should return true. :param loop: the event loop instance to attach the trigger to. + :returns: a Trigger instance that is true when the axis value is greater than the provided - threshold. + threshold. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() diff --git a/commands2/button/commandjoystick.py b/commands2/button/commandjoystick.py index f5f28c51..909fb00b 100644 --- a/commands2/button/commandjoystick.py +++ b/commands2/button/commandjoystick.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 92aecab2ef05 button/CommandJoystick.java from typing import Optional from wpilib import Joystick @@ -10,7 +11,7 @@ class CommandJoystick(CommandGenericHID): """ - A version of Joystick with Trigger factories for command-based. + A version of :class:`wpilib.Joystick` with :class:`.Trigger` factories for command-based. """ _hid: Joystick @@ -25,7 +26,7 @@ def __init__(self, port: int): super().__init__(port) self._hid = Joystick(port) - def getHID(self): + def getHID(self) -> Joystick: """ Get the underlying GenericHID object. @@ -37,7 +38,9 @@ def trigger(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the trigger button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the trigger button's digital signal attached to the given loop. """ @@ -49,9 +52,11 @@ def top(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the top button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the top button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -121,21 +126,21 @@ def getZChannel(self) -> int: """ return self._hid.getZChannel() - def getThrottleChannel(self) -> int: + def getTwistChannel(self) -> int: """ - Get the channel currently associated with the throttle axis. + Get the channel currently associated with the twist axis. :returns: The channel for the axis. """ - return self._hid.getThrottleChannel() + return self._hid.getTwistChannel() - def getTwistChannel(self) -> int: + def getThrottleChannel(self) -> int: """ - Get the channel currently associated with the twist axis. + Get the channel currently associated with the throttle axis. :returns: The channel for the axis. """ - return self._hid.getTwistChannel() + return self._hid.getThrottleChannel() def getX(self) -> float: """ diff --git a/commands2/button/commandps4controller.py b/commands2/button/commandps4controller.py index 9b2da01d..562839f9 100644 --- a/commands2/button/commandps4controller.py +++ b/commands2/button/commandps4controller.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS d426873ed15b button/CommandPS4Controller.java from typing import Optional from wpilib import PS4Controller @@ -24,7 +25,7 @@ def __init__(self, port: int): super().__init__(port) self._hid = PS4Controller(port) - def getHID(self): + def getHID(self) -> PS4Controller: """ Get the underlying GenericHID object. @@ -36,9 +37,11 @@ def L2(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the L2 button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the L2 button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -48,9 +51,11 @@ def R2(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the R2 button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the R2 button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -60,9 +65,11 @@ def L1(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the L1 button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the L1 button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -72,7 +79,9 @@ def R1(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the R1 button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the R1 button's digital signal attached to the given loop. """ @@ -84,9 +93,11 @@ def L3(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the L3 button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the L3 button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -96,9 +107,11 @@ def R3(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the R3 button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the R3 button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -108,9 +121,11 @@ def square(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the square button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the square button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -120,9 +135,11 @@ def cross(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the cross button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the cross button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -132,9 +149,11 @@ def triangle(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the triangle button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the triangle button's digital signal attached to the - given loop. + given loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -144,7 +163,9 @@ def circle(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the circle button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the circle button's digital signal attached to the given loop. """ @@ -156,9 +177,11 @@ def share(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the share button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the share button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -168,9 +191,11 @@ def PS(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the PS button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the PS button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -180,9 +205,11 @@ def options(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the options button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the options button's digital signal attached to the - given loop. + given loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -192,9 +219,11 @@ def touchpad(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the touchpad's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the touchpad's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() diff --git a/commands2/button/commandxboxcontroller.py b/commands2/button/commandxboxcontroller.py index 2c613f4b..54880810 100644 --- a/commands2/button/commandxboxcontroller.py +++ b/commands2/button/commandxboxcontroller.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 3ba501f9478a button/CommandXboxController.java from typing import Optional from wpilib import XboxController @@ -24,7 +25,7 @@ def __init__(self, port: int): super().__init__(port) self._hid = XboxController(port) - def getHID(self): + def getHID(self) -> XboxController: """ Get the underlying GenericHID object. @@ -36,9 +37,11 @@ def leftBumper(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the left bumper's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the right bumper's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -48,9 +51,11 @@ def rightBumper(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the right bumper's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the left bumper's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -60,9 +65,11 @@ def leftStick(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the left stick button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the left stick button's digital signal attached to the - given loop. + given loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -72,9 +79,11 @@ def rightStick(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the right stick button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the right stick button's digital signal attached to the - given loop. + given loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -84,9 +93,11 @@ def a(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the A button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the A button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -96,9 +107,11 @@ def b(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the B button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the B button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -108,9 +121,11 @@ def x(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the X button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the X button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -120,9 +135,11 @@ def y(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the Y button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the Y button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -132,9 +149,11 @@ def start(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the start button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the start button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -144,9 +163,11 @@ def back(self, loop: Optional[EventLoop] = None) -> Trigger: """ Constructs an event instance around the back button's digital signal. - :param loop: the event loop instance to attach the event to. + :param loop: the event loop instance to attach the event to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: an event instance representing the back button's digital signal attached to the given - loop. + loop. """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() @@ -160,8 +181,10 @@ def leftTrigger( will be true when the axis value is greater than {@code threshold}. :param threshold: the minimum axis value for the returned Trigger to be true. This value - should be in the range [0, 1] where 0 is the unpressed state of the axis. - :param loop: the event loop instance to attach the Trigger to. + should be in the range [0, 1] where 0 is the unpressed state of the axis. + :param loop: the event loop instance to attach the Trigger to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: a Trigger instance that is true when the left trigger's axis exceeds the provided threshold, attached to the given event loop """ @@ -177,10 +200,12 @@ def rightTrigger( will be true when the axis value is greater than {@code threshold}. :param threshold: the minimum axis value for the returned Trigger to be true. This value - should be in the range [0, 1] where 0 is the unpressed state of the axis. - :param loop: the event loop instance to attach the Trigger to. + should be in the range [0, 1] where 0 is the unpressed state of the axis. + :param loop: the event loop instance to attach the Trigger to, defaults + to :func:`commands2.CommandScheduler.getDefaultButtonLoop` + :returns: a Trigger instance that is true when the right trigger's axis exceeds the provided - threshold, attached to the given event loop + threshold, attached to the given event loop """ if loop is None: loop = CommandScheduler.getInstance().getDefaultButtonLoop() diff --git a/commands2/button/joystickbutton.py b/commands2/button/joystickbutton.py index 2e0e107c..0fbb2b4a 100644 --- a/commands2/button/joystickbutton.py +++ b/commands2/button/joystickbutton.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 7a099cb02a33 button/JoystickButton.java from wpilib.interfaces import GenericHID from .trigger import Trigger @@ -5,9 +6,7 @@ class JoystickButton(Trigger): """ - A Button that gets its state from a GenericHID. - - This class is provided by the NewCommands VendorDep + A Button that gets its state from a :class:`wpilib.interfaces.GenericHID`. """ def __init__(self, joystick: GenericHID, buttonNumber: int): @@ -15,6 +14,6 @@ def __init__(self, joystick: GenericHID, buttonNumber: int): Creates a joystick button for triggering commands. :param joystick: The GenericHID object that has the button (e.g. Joystick, KinectStick, etc) - :param buttonNumber: The button number (see GenericHID#getRawButton(int) + :param buttonNumber: The button number (see :func:`wpilib.interfaces.GenericHID.getRawButton` """ super().__init__(lambda: joystick.getRawButton(buttonNumber)) diff --git a/commands2/button/networkbutton.py b/commands2/button/networkbutton.py index c6c6602c..34fe28aa 100644 --- a/commands2/button/networkbutton.py +++ b/commands2/button/networkbutton.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 7a099cb02a33 button/NetworkButton.java from typing import overload from ntcore import BooleanSubscriber, BooleanTopic, NetworkTable, NetworkTableInstance @@ -8,9 +9,7 @@ class NetworkButton(Trigger): """ - A Button that uses a NetworkTable boolean field. - - This class is provided by the NewCommands VendorDep + A Button that uses a :class:`ntcore.NetworkTable` boolean field. """ @overload @@ -121,7 +120,7 @@ def init_str_table_field(table: str, field: str): 3. (self: NetworkButton, table: NetworkTable, field: str) 4. (self: NetworkButton, table: str, field: str) 5. (self: NetworkButton, inst: NetworkTableInstance, table: str, field: str) - + Invoked with: {format_args_kwargs(self, *args, **kwargs)} """ ) diff --git a/commands2/button/povbutton.py b/commands2/button/povbutton.py index 8e0c06cd..d7114dfd 100644 --- a/commands2/button/povbutton.py +++ b/commands2/button/povbutton.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 7a099cb02a33 button/POVButton.java from wpilib.interfaces import GenericHID from .trigger import Trigger @@ -5,9 +6,7 @@ class POVButton(Trigger): """ - A Button that gets its state from a POV on a GenericHID. - - This class is provided by the NewCommands VendorDep + A Button that gets its state from a POV on a :class:`wpilib.interfaces.GenericHID`. """ def __init__(self, joystick: GenericHID, angle: int, povNumber: int = 0): @@ -16,6 +15,6 @@ def __init__(self, joystick: GenericHID, angle: int, povNumber: int = 0): :param joystick: The GenericHID object that has the POV :param angle: The desired angle in degrees (e.g. 90, 270) - :param povNumber: The POV number (see GenericHID#getPOV(int)) + :param povNumber: The POV number (see :func:`wpilib.interfaces.GenericHID.getPOV`) """ super().__init__(lambda: joystick.getPOV(povNumber) == angle) diff --git a/commands2/button/trigger.py b/commands2/button/trigger.py index d9f88329..b21b93e7 100644 --- a/commands2/button/trigger.py +++ b/commands2/button/trigger.py @@ -1,3 +1,4 @@ +# validated: 2024-01-23 DS 70b60e3a7465 button/Trigger.java from types import SimpleNamespace from typing import Callable, overload @@ -44,6 +45,7 @@ def __init__(self, loop: EventLoop, condition: Callable[[], bool]): def __init__(self, *args, **kwargs): def init_loop_condition(loop: EventLoop, condition: Callable[[], bool]): + assert callable(condition) self._loop = loop self._condition = condition @@ -77,7 +79,7 @@ def init_condition(condition: Callable[[], bool]): 1. (self: Trigger) 2. (self: Trigger, condition: () -> bool) 3. (self: Trigger, loop: EventLoop, condition: () -> bool) - + Invoked with: {format_args_kwargs(self, *args, **kwargs)} """ ) @@ -90,8 +92,10 @@ def onTrue(self, command: Command) -> Self: :returns: this trigger, so calls can be chained """ + state = SimpleNamespace(pressed_last=self._condition()) + @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): + def _(): pressed = self._condition() if not state.pressed_last and pressed: command.schedule() @@ -107,8 +111,10 @@ def onFalse(self, command: Command) -> Self: :returns: this trigger, so calls can be chained """ + state = SimpleNamespace(pressed_last=self._condition()) + @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): + def _(): pressed = self._condition() if state.pressed_last and not pressed: command.schedule() @@ -122,14 +128,16 @@ def whileTrue(self, command: Command) -> Self: changes to `False`. Doesn't re-start the command if it ends while the condition is still `True`. If the command - should restart, see RepeatCommand. + should restart, see :class:`commands2.RepeatCommand`. :param command: the command to start :returns: this trigger, so calls can be chained """ + state = SimpleNamespace(pressed_last=self._condition()) + @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): + def _(): pressed = self._condition() if not state.pressed_last and pressed: command.schedule() @@ -145,14 +153,16 @@ def whileFalse(self, command: Command) -> Self: condition changes to `True`. Doesn't re-start the command if it ends while the condition is still `False`. If the command - should restart, see RepeatCommand. + should restart, see :class:`commands2.RepeatCommand`. :param command: the command to start :returns: this trigger, so calls can be chained """ + state = SimpleNamespace(pressed_last=self._condition()) + @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): + def _(): pressed = self._condition() if state.pressed_last and not pressed: command.schedule() @@ -170,8 +180,10 @@ def toggleOnTrue(self, command: Command) -> Self: :returns: this trigger, so calls can be chained """ + state = SimpleNamespace(pressed_last=self._condition()) + @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): + def _(): pressed = self._condition() if not state.pressed_last and pressed: if command.isScheduled(): @@ -190,8 +202,10 @@ def toggleOnFalse(self, command: Command) -> Self: :returns: this trigger, so calls can be chained """ + state = SimpleNamespace(pressed_last=self._condition()) + @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): + def _(): pressed = self._condition() if state.pressed_last and not pressed: if command.isScheduled(): @@ -212,6 +226,7 @@ def __bool__(self) -> bool: return self._condition() def __and__(self, other: Callable[[], bool]) -> "Trigger": + assert callable(other) return Trigger(lambda: self() and other()) def and_(self, other: Callable[[], bool]) -> "Trigger": @@ -224,6 +239,7 @@ def and_(self, other: Callable[[], bool]) -> "Trigger": return self & other def __or__(self, other: Callable[[], bool]) -> "Trigger": + assert callable(other) return Trigger(lambda: self() or other()) def or_(self, other: Callable[[], bool]) -> "Trigger": diff --git a/commands2/cmd.py b/commands2/cmd.py index 62a2e24c..7e9f535a 100644 --- a/commands2/cmd.py +++ b/commands2/cmd.py @@ -1,5 +1,8 @@ +# validated: 2024-01-20 DS 8aeee0362672 Commands.java from typing import Any, Callable, Dict, Hashable +from wpimath import units + from .command import Command from .conditionalcommand import ConditionalCommand from .functionalcommand import FunctionalCommand @@ -28,6 +31,16 @@ def none() -> Command: return InstantCommand() +def idle(*requirements: Subsystem) -> Command: + """ + Constructs a command that does nothing until interrupted. + + :param requirements: Subsystems to require + :return: the command + """ + return run(lambda: None, *requirements) + + def runOnce(action: Callable[[], Any], *requirements: Subsystem) -> Command: """ Constructs a command that runs an action once and finishes. @@ -62,7 +75,7 @@ def startEnd( :param requirements: subsystems the action requires :returns: the command """ - return StartEndCommand(run, lambda: end(), *requirements) + return StartEndCommand(run, end, *requirements) def runEnd( @@ -92,7 +105,7 @@ def print_(message: str) -> Command: return PrintCommand(message) -def waitSeconds(seconds: float) -> Command: +def waitSeconds(seconds: units.seconds) -> Command: """ Constructs a command that does nothing, finishing after a specified duration. @@ -189,3 +202,22 @@ def deadline(deadline: Command, *commands: Command) -> Command: :returns: the command group """ return ParallelDeadlineGroup(deadline, *commands) + + +__all__ = [ + "none", + "runOnce", + "run", + "startEnd", + "runEnd", + "print_", + "waitSeconds", + "waitUntil", + "either", + "select", + "sequence", + "repeatingSequence", + "parallel", + "race", + "deadline", +] diff --git a/commands2/command.py b/commands2/command.py index e8b0546f..5be9d733 100644 --- a/commands2/command.py +++ b/commands2/command.py @@ -1,10 +1,11 @@ +# validated: 2024-01-20 DS f29a7d2e501b Command.java # Don't import stuff from the package here, it'll cause a circular import from __future__ import annotations from enum import Enum -from typing import TYPE_CHECKING, Any, Callable, Set +from typing import TYPE_CHECKING, Any, Callable, Set, Union from typing_extensions import Self, TypeAlias @@ -15,7 +16,6 @@ from .sequentialcommandgroup import SequentialCommandGroup from .paralleldeadlinegroup import ParallelDeadlineGroup from .parallelcommandgroup import ParallelCommandGroup - from .perpetualcommand import PerpetualCommand from .repeatcommand import RepeatCommand from .proxycommand import ProxyCommand from .conditionalcommand import ConditionalCommand @@ -32,7 +32,7 @@ class InterruptionBehavior(Enum): kCancelIncoming = 0 """ - This command ends, #end(boolean) end(true) is called, and the incoming command is + This command ends, :func:`commands2.Command.end` is called, and the incoming command is scheduled normally. This is the default behavior. @@ -45,13 +45,11 @@ class InterruptionBehavior(Enum): class Command(Sendable): """ A state machine representing a complete action to be performed by the robot. Commands are run by - the CommandScheduler, and can be composed into CommandGroups to allow users to build + the :class:`commands2.CommandScheduler`, and can be composed into CommandGroups to allow users to build complicated multistep actions without the need to roll the state machine logic themselves. Commands are run synchronously from the main robot loop; no multithreading is used, unless specified explicitly from the command implementation. - - This class is provided by the NewCommands VendorDep """ InterruptionBehavior: TypeAlias = ( @@ -80,36 +78,34 @@ def execute(self): """The main body of a command. Called repeatedly while the command is scheduled.""" pass - def isFinished(self) -> bool: - """ - Whether the command has finished. Once a command finishes, the scheduler will call its end() - method and un-schedule it. - - :returns: whether the command has finished. - """ - return False - def end(self, interrupted: bool): """ The action to take when the command ends. Called when either the command finishes normally, or when it interrupted/canceled. - Do not schedule commands here that share requirements with this command. Use {@link - #andThen(Command...)} instead. + Do not schedule commands here that share requirements with this command. Use :meth:`.andThen` instead. :param interrupted: whether the command was interrupted/canceled """ pass + def isFinished(self) -> bool: + """ + Whether the command has finished. Once a command finishes, the scheduler will call its :meth:`commands2.Command.end` + method and un-schedule it. + + :returns: whether the command has finished. + """ + return False + def getRequirements(self) -> Set[Subsystem]: """ 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, {@link - #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return + subsystem at the same time. If another command is scheduled that shares a requirement, :meth:`.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. + .. 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. :returns: the set of subsystems that are required """ @@ -120,64 +116,44 @@ def addRequirements(self, *requirements: Subsystem): Adds the specified subsystems to the requirements of the command. The scheduler will prevent two commands that require the same subsystem from being scheduled simultaneously. - Note that the scheduler determines the requirements of a command when it is scheduled, so - this method should normally be called from the command's constructor. + .. note:: The scheduler determines the requirements of a command when it is scheduled, so + this method should normally be called from the command's constructor. :param requirements: the requirements to add """ self.requirements.update(requirements) - def runsWhenDisabled(self) -> bool: - """ - Whether the given command should run when the robot is disabled. Override to return true if the - command should run when disabled. - - :returns: whether the command should run when the robot is disabled + def getName(self) -> str: """ - return False - - def schedule(self, interruptible: bool = True) -> None: - """Schedules this command.""" - from .commandscheduler import CommandScheduler - - CommandScheduler.getInstance().schedule(self) + Gets the name of this Command. - def cancel(self) -> None: - """ - Cancels this command. Will call #end(boolean) end(true). Commands will be canceled - regardless of InterruptionBehavior interruption behavior. + :returns: Name """ - from .commandscheduler import CommandScheduler - - CommandScheduler.getInstance().cancel(self) + return SendableRegistry.getName(self) - def isScheduled(self) -> bool: + def setName(self, name: str): """ - Whether the command is currently scheduled. Note that this does not detect whether the command - is in a composition, only whether it is directly being run by the scheduler. + Sets the name of this Command. - :returns: Whether the command is scheduled. + :param name: Name """ - from .commandscheduler import CommandScheduler - - return CommandScheduler.getInstance().isScheduled(self) + SendableRegistry.setName(self, name) - def hasRequirement(self, requirement: Subsystem) -> bool: + def getSubsystem(self) -> str: """ - Whether the command requires a given subsystem. + Gets the subsystem name of this Subsystem. - :param requirement: the subsystem to inquire about - :returns: whether the subsystem is required + :returns: Subsystem name """ - return requirement in self.requirements + return SendableRegistry.getSubsystem(self) - def getInterruptionBehavior(self) -> InterruptionBehavior: + def setSubsystem(self, subsystem: str): """ - How the command behaves when another command with a shared requirement is scheduled. + Sets the subsystem name of this Subsystem. - :returns: a variant of InterruptionBehavior, defaulting to {@link InterruptionBehavior#kCancelSelf kCancelSelf}. + :param subsystem: subsystem name """ - return InterruptionBehavior.kCancelSelf + SendableRegistry.setSubsystem(self, subsystem) def withTimeout(self, seconds: float) -> ParallelRaceGroup: """ @@ -185,11 +161,13 @@ def withTimeout(self, seconds: float) -> ParallelRaceGroup: finishes normally, the command will be interrupted and un-scheduled. Note that the timeout only applies to the command returned by this method; the calling command is not itself changed. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param seconds: the timeout duration :returns: the command with the timeout added @@ -203,11 +181,13 @@ def until(self, condition: Callable[[], bool]) -> ParallelRaceGroup: Decorates this command with an interrupt condition. If the specified condition becomes true before the command finishes normally, the command will be interrupted and un-scheduled. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param condition: the interrupt condition :returns: the command with the interrupt condition added @@ -221,51 +201,43 @@ def onlyWhile(self, condition: Callable[[], bool]) -> ParallelRaceGroup: Decorates this command with a run condition. If the specified condition becomes false before the command finishes normally, the command will be interrupted and un-scheduled. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param condition: the interrupt condition :returns: the command with the interrupt condition added """ + assert callable(condition) return self.until(lambda: not condition()) - def withInterrupt(self, condition: Callable[[], bool]) -> ParallelRaceGroup: - """ - Decorates this command with an interrupt condition. If the specified condition becomes true - before the command finishes normally, the command will be interrupted and un-scheduled. Note - that this only applies to the command returned by this method; the calling command is not - itself changed. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param condition: the interrupt condition - :returns: the command with the interrupt condition added - @deprecated Replace with #until(BooleanSupplier) - """ - return self.until(condition) - - def beforeStarting(self, before: Command) -> SequentialCommandGroup: + def beforeStarting( + self, before: Union[Command, Callable[[], None]] + ) -> SequentialCommandGroup: """ - Decorates this command with another command to run before this command starts. + Decorates this command with a callable or command to run before this command starts. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param before: the command to run before this one :returns: the decorated command """ + from .instantcommand import InstantCommand from .sequentialcommandgroup import SequentialCommandGroup + if callable(before): + before = InstantCommand(before) + return SequentialCommandGroup(before, self) def andThen(self, *next: Command) -> SequentialCommandGroup: @@ -273,11 +245,13 @@ def andThen(self, *next: Command) -> SequentialCommandGroup: Decorates this command with a set of commands to run after it in sequence. Often more convenient/less-verbose than constructing a new SequentialCommandGroup explicitly. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param next: the commands to run next :returns: the decorated command @@ -292,11 +266,13 @@ def deadlineWith(self, *parallel: Command) -> ParallelDeadlineGroup: command ends and interrupting all the others. Often more convenient/less-verbose than constructing a new ParallelDeadlineGroup explicitly. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param parallel: the commands to run in parallel :returns: the decorated command @@ -311,11 +287,13 @@ def alongWith(self, *parallel: Command) -> ParallelCommandGroup: command ends. Often more convenient/less-verbose than constructing a new {@link ParallelCommandGroup} explicitly. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param parallel: the commands to run in parallel :returns: the decorated command @@ -330,11 +308,13 @@ def raceWith(self, *parallel: Command) -> ParallelRaceGroup: command ends. Often more convenient/less-verbose than constructing a new {@link ParallelRaceGroup} explicitly. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param parallel: the commands to run in parallel :returns: the decorated command @@ -343,37 +323,18 @@ def raceWith(self, *parallel: Command) -> ParallelRaceGroup: return ParallelRaceGroup(self, *parallel) - def perpetually(self) -> PerpetualCommand: - """ - Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated - command can still be interrupted or canceled. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :returns: the decorated command - @deprecated PerpetualCommand violates the assumption that execute() doesn't get called after - isFinished() returns true -- an assumption that should be valid. This was unsafe/undefined - behavior from the start, and RepeatCommand provides an easy way to achieve similar end - results with slightly different (and safe) semantics. - """ - from .perpetualcommand import PerpetualCommand - - return PerpetualCommand(self) - def repeatedly(self) -> RepeatCommand: """ Decorates this command to run repeatedly, restarting it when it ends, until this command is interrupted. The decorated command can still be canceled. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :returns: the decorated command """ @@ -399,11 +360,13 @@ def unless(self, condition: Callable[[], bool]) -> ConditionalCommand: running and the condition changes to true, the command will not stop running. The requirements of this command will be kept for the new conditional command. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param condition: the condition that will prevent the command from running :returns: the decorated command @@ -419,11 +382,13 @@ def onlyIf(self, condition: Callable[[], bool]) -> ConditionalCommand: and the condition changes to false, the command will not stop running. The requirements of this command will be kept for the new conditional command. - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. + .. note:: This decorator works by adding this command to a composition. + The command the decorator was called on cannot be scheduled + independently or be added to a different composition (namely, + decorators), unless it is manually cleared from the list of composed + commands with :func:`commands2.CommandScheduler.removeComposedCommand`. + The command composition returned from this method can be further + decorated without issue. :param condition: the condition that will allow the command to run :returns: the decorated command @@ -463,7 +428,7 @@ def getInterruptionBehavior(self) -> InterruptionBehavior: def finallyDo(self, end: Callable[[bool], Any]) -> WrapperCommand: """ Decorates this command with a lambda to call on interrupt or end, following the command's - inherent #end(boolean) method. + inherent :func:`commands2.Command.end` method. :param end: a lambda accepting a boolean parameter specifying whether the command was interrupted. @@ -481,28 +446,64 @@ def end(self, interrupted: bool) -> None: def handleInterrupt(self, handler: Callable[[], Any]) -> WrapperCommand: """ Decorates this command with a lambda to call on interrupt, following the command's inherent - #end(boolean) method. + :func:`commands2.Command.end` method. :param handler: a lambda to run when the command is interrupted :returns: the decorated command """ return self.finallyDo(lambda interrupted: handler() if interrupted else None) - def getName(self) -> str: + def schedule(self) -> None: + """Schedules this command.""" + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().schedule(self) + + def cancel(self) -> None: """ - Gets the name of this Command. + Cancels this command. Will call ``end(true)``. Commands will be canceled + regardless of InterruptionBehavior interruption behavior. + """ + from .commandscheduler import CommandScheduler - :returns: Name + CommandScheduler.getInstance().cancel(self) + + def isScheduled(self) -> bool: """ - return SendableRegistry.getName(self) + Whether the command is currently scheduled. Note that this does not detect whether the command + is in a composition, only whether it is directly being run by the scheduler. - def setName(self, name: str): + :returns: Whether the command is scheduled. """ - Sets the name of this Command. + from .commandscheduler import CommandScheduler - :param name: Name + return CommandScheduler.getInstance().isScheduled(self) + + def hasRequirement(self, requirement: Subsystem) -> bool: """ - SendableRegistry.setName(self, name) + Whether the command requires a given subsystem. + + :param requirement: the subsystem to inquire about + :returns: whether the subsystem is required + """ + return requirement in self.requirements + + def getInterruptionBehavior(self) -> InterruptionBehavior: + """ + How the command behaves when another command with a shared requirement is scheduled. + + :returns: a variant of InterruptionBehavior, defaulting to {@link InterruptionBehavior#kCancelSelf kCancelSelf}. + """ + return InterruptionBehavior.kCancelSelf + + def runsWhenDisabled(self) -> bool: + """ + Whether the given command should run when the robot is disabled. Override to return true if the + command should run when disabled. + + :returns: whether the command should run when the robot is disabled + """ + return False def withName(self, name: str) -> WrapperCommand: """ diff --git a/commands2/commandgroup.py b/commands2/commandgroup.py deleted file mode 100644 index 8a00f242..00000000 --- a/commands2/commandgroup.py +++ /dev/null @@ -1,21 +0,0 @@ -from __future__ import annotations - -from .command import Command - - -class IllegalCommandUse(Exception): - pass - - -class CommandGroup(Command): - """ - A base for CommandGroups. - """ - - def addCommands(self, *commands: Command): - """ - Adds the given commands to the command group. - - :param commands: The commands to add. - """ - raise NotImplementedError diff --git a/commands2/commandscheduler.py b/commands2/commandscheduler.py index ef51150c..b09e496e 100644 --- a/commands2/commandscheduler.py +++ b/commands2/commandscheduler.py @@ -1,23 +1,36 @@ +# validated: 2024-01-23 DS 8aeee0362672 CommandScheduler.java from __future__ import annotations +import inspect +import os.path +import traceback from typing import Any, Callable, Dict, Iterable, List, Optional, Set, Union import hal from typing_extensions import Self -from wpilib import RobotBase, RobotState, TimedRobot, Watchdog +from wpilib import ( + LiveWindow, + RobotBase, + RobotState, + TimedRobot, + Watchdog, + reportWarning, +) from wpilib.event import EventLoop +from wpiutil import Sendable, SendableBuilder, SendableRegistry from .command import Command, InterruptionBehavior -from .commandgroup import * +from .exceptions import IllegalCommandUse from .subsystem import Subsystem +_cmd_path = os.path.dirname(__file__) -class CommandScheduler: + +class CommandScheduler(Sendable): """ - The scheduler responsible for running Commands. A Command-based robot should call {@link - CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands - synchronously from the main loop. Subsystems should be registered with the scheduler using {@link - CommandScheduler#registerSubsystem(Subsystem...)} in order for their Subsystem#periodic() + The scheduler responsible for running Commands. A Command-based robot should call + :meth:`.run` on the singleton instance in its periodic block in order to run commands + synchronously from the main loop. Subsystems should be registered with the scheduler using :meth:`.registerSubsystem` in order for their :meth:`commands2.Subsystem.periodic` methods to be called and for their default commands to be scheduled. """ @@ -29,7 +42,7 @@ def __new__(cls) -> CommandScheduler: return cls._instance @staticmethod - def getInstance() -> "CommandScheduler": + def getInstance() -> CommandScheduler: """ Returns the Scheduler instance. @@ -46,33 +59,53 @@ def resetInstance() -> None: inst = CommandScheduler._instance if inst: inst._defaultButtonLoop.clear() + LiveWindow.setEnabledCallback(lambda: None) + LiveWindow.setDisabledCallback(lambda: None) + SendableRegistry.remove(inst) + CommandScheduler._instance = None def __init__(self) -> None: """ Gets the scheduler instance. """ + super().__init__() if CommandScheduler._instance is not None: return CommandScheduler._instance = self - self._composedCommands: Set[Command] = set() + self._composedCommands: Dict[Command, str] = {} + + # A set of the currently-running commands. self._scheduledCommands: Dict[Command, None] = {} + + # A map from required subsystems to their requiring commands. Also used as a set + # of the currently-required subsystems. self._requirements: Dict[Subsystem, Command] = {} + + # A map from subsystems registered with the scheduler to their default commands. + # Also used as a list of currently-registered subsystems. self._subsystems: Dict[Subsystem, Optional[Command]] = {} self._defaultButtonLoop = EventLoop() - self.setActiveButtonLoop(self._defaultButtonLoop) + + # The set of currently-registered buttons that will be polled every iteration. + self._activeButtonLoop = self._defaultButtonLoop self._disabled = False + # Lists of user-supplied actions to be executed on scheduling events for every + # command. self._initActions: List[Callable[[Command], None]] = [] self._executeActions: List[Callable[[Command], None]] = [] - self._interruptActions: List[Callable[[Command], None]] = [] + self._interruptActions: List[Callable[[Command, Optional[Command]], None]] = [] self._finishActions: List[Callable[[Command], None]] = [] self._inRunLoop = False self._toSchedule: Dict[Command, None] = {} - self._toCancel: Dict[Command, None] = {} + # python: toCancelInterruptors stored in _toCancel dict + self._toCancel: Dict[Command, Optional[Command]] = {} + # self._toCancelInterruptors: List[Optional[Command]] = [] + self._endingCommands: Set[Command] = set() self._watchdog = Watchdog(TimedRobot.kDefaultPeriod, lambda: None) @@ -80,6 +113,14 @@ def __init__(self) -> None: hal.tResourceType.kResourceType_Command.value, hal.tInstances.kCommand2_Scheduler.value, ) + SendableRegistry.addLW(self, "Scheduler") + + def _on_lw_enabled(): + self.disable() + self.cancelAll() + + LiveWindow.setEnabledCallback(_on_lw_enabled) + LiveWindow.setDisabledCallback(self.enable) def setPeriod(self, period: float) -> None: """ @@ -114,7 +155,7 @@ def setActiveButtonLoop(self, loop: EventLoop) -> None: """ self._activeButtonLoop = loop - def initCommand(self, command: Command, *requirements: Subsystem) -> None: + def _initCommand(self, command: Command, *requirements: Subsystem) -> None: """ Initializes a given command, adds its requirements to the list, and performs the init actions. @@ -127,9 +168,9 @@ def initCommand(self, command: Command, *requirements: Subsystem) -> None: command.initialize() for action in self._initActions: action(command) - # self._watchdog.addEpoch() + self._watchdog.addEpoch(f"{command.getName()}.initialize()") - def schedule(self, *commands) -> None: + def schedule(self, *commands: Command) -> None: """ 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 @@ -138,40 +179,41 @@ def schedule(self, *commands) -> None: :param commands: the commands to schedule. """ - if len(commands) > 1: - for command in commands: - self.schedule(command) - return - - command = commands[0] + for command in commands: + self._schedule(command) + def _schedule(self, command: Optional[Command]) -> None: if command is None: - # DriverStation.reportWarning("CommandScheduler tried to schedule a null command!", True) + reportWarning("Tried to schedule a null command!", True) return if self._inRunLoop: self._toSchedule[command] = None return - if command in self.getComposedCommands(): - raise IllegalCommandUse( - "A command that is part of a CommandGroup cannot be independently scheduled" - ) + self.requireNotComposed(command) + + # Do nothing if the scheduler is disabled, the robot is disabled and the command + # doesn't run when disabled, or the command is already scheduled. if self._disabled: return - if RobotState.isDisabled() and not command.runsWhenDisabled(): + if self.isScheduled(command): return - if self.isScheduled(command): + if RobotState.isDisabled() and not command.runsWhenDisabled(): return requirements = command.getRequirements() + # Schedule the command if the requirements are not currently in-use. if self._requirements.keys().isdisjoint(requirements): - self.initCommand(command, *requirements) + self._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 requirement in requirements: requiringCommand = self.requiring(requirement) if ( @@ -184,77 +226,79 @@ def schedule(self, *commands) -> None: for requirement in requirements: requiringCommand = self.requiring(requirement) if requiringCommand is not None: - self.cancel(requiringCommand) + self._cancel(requiringCommand, command) - self.initCommand(command, *requirements) + self._initCommand(command, *requirements) - def run(self): + def run(self) -> None: """ Runs a single iteration of the scheduler. The execution occurs in the following order: - Subsystem periodic methods are called. - - Button bindings are polled, and new commands are scheduled from them. - - Currently-scheduled commands are executed. - - End conditions are checked on currently-scheduled commands, and commands that are finished - have their end methods called and are removed. - - Any subsystems not being used as requirements have their default methods started. + * Subsystem periodic methods are called. + * Button bindings are polled, and new commands are scheduled from them. + * Currently-scheduled commands are executed. + * End conditions are checked on currently-scheduled commands, and commands that are finished + have their end methods called and are removed. + * Any subsystems not being used as requirements have their default methods started. """ if self._disabled: return self._watchdog.reset() + # Run the periodic method of all registered subsystems. for subsystem in self._subsystems: subsystem.periodic() if RobotBase.isSimulation(): subsystem.simulationPeriodic() - # self._watchdog.addEpoch() + self._watchdog.addEpoch(f"{subsystem.getName()}.periodic()") + # Cache the active instance to avoid concurrency problems if setActiveLoop() is + # called from inside the button bindings. loopCache = self._activeButtonLoop + # Poll buttons for new commands to add. loopCache.poll() self._watchdog.addEpoch("buttons.run()") self._inRunLoop = True + isDisabled = RobotState.isDisabled() + # Run scheduled commands, remove finished commands. for command in self._scheduledCommands.copy(): - if not command.runsWhenDisabled() and RobotState.isDisabled(): - command.end(True) - for action in self._interruptActions: - action(command) - for requirement in command.getRequirements(): - self._requirements.pop(requirement) - self._scheduledCommands.pop(command) + if isDisabled and not command.runsWhenDisabled(): + self._cancel(command, None) continue command.execute() for action in self._executeActions: action(command) - # self._watchdog.addEpoch() + self._watchdog.addEpoch(f"{command.getName()}.execute()") if command.isFinished(): + self._endingCommands.add(command) command.end(False) for action in self._finishActions: action(command) + self._endingCommands.remove(command) self._scheduledCommands.pop(command) for requirement in command.getRequirements(): self._requirements.pop(requirement) + self._watchdog.addEpoch(f"{command.getName()}.end(False)") self._inRunLoop = False + # Schedule/cancel commands from queues populated during loop for command in self._toSchedule: - self.schedule(command) + self._schedule(command) - for command in self._toCancel: - self.cancel(command) + for command, interruptor in self._toCancel.items(): + self._cancel(command, interruptor) self._toSchedule.clear() self._toCancel.clear() - for subsystem, command in self._subsystems.items(): - if subsystem not in self._requirements and command is not None: - self.schedule(command) + # Add default commands for un-required registered subsystems. + for subsystem, scommand in self._subsystems.items(): + if subsystem not in self._requirements and scommand is not None: + self._schedule(scommand) self._watchdog.disable() if self._watchdog.isExpired(): @@ -269,8 +313,11 @@ def registerSubsystem(self, *subsystems: Subsystem) -> None: :param subsystems: the subsystem to register """ for subsystem in subsystems: + if subsystem is None: + reportWarning("Tried to register a null subsystem", True) + continue if subsystem in self._subsystems: - # DriverStation.reportWarning("Tried to register an already-registered subsystem", True) + reportWarning("Tried to register an already-registered subsystem", True) continue self._subsystems[subsystem] = None @@ -282,28 +329,49 @@ def unregisterSubsystem(self, *subsystems: Subsystem) -> None: :param subsystems: the subsystem to un-register """ for subsystem in subsystems: - self._subsystems.pop(subsystem) + self._subsystems.pop(subsystem, None) + + def unregisterAllSubsystems(self): + """ + Un-registers all registered Subsystems with the scheduler. All currently registered subsystems + will no longer have their periodic block called, and will not have their default command + scheduled. + """ + self._subsystems.clear() def setDefaultCommand(self, subsystem: Subsystem, defaultCommand: Command) -> None: """ Sets the default command for a subsystem. Registers that subsystem if it is not already registered. Default commands will run whenever there is no other command currently scheduled - that requires the subsystem. Default commands should be written to never end (i.e. their {@link - Command#isFinished()} method should return false), as they would simply be re-scheduled if they + that requires the subsystem. Default commands should be written to never end (i.e. their + :func:`commands2.Command.isFinished` method should return False), as they would simply be re-scheduled if they do. Default commands must also require their subsystem. :param subsystem: the subsystem whose default command will be set :param defaultCommand: the default command to associate with the subsystem """ - self.requireNotComposed([defaultCommand]) + if subsystem is None: + reportWarning("Tried to set a default command for a null subsystem", True) + return + if defaultCommand is None: + reportWarning("Tried to set a null default command", True) + return + + self.requireNotComposed(defaultCommand) + if subsystem not in defaultCommand.getRequirements(): raise IllegalCommandUse("Default commands must require their subsystem!") + if ( defaultCommand.getInterruptionBehavior() - != InterruptionBehavior.kCancelIncoming + == InterruptionBehavior.kCancelIncoming ): - # DriverStation.reportWarning("Registering a non-interruptible default command\nThis will likely prevent any other commands from requiring this subsystem.", True) - pass + reportWarning( + "Registering a non-interruptible default command\nThis will likely prevent any other commands from requiring this subsystem.", + True, + ) + # Warn, but allow -- there might be a use case for this. + self._subsystems[subsystem] = defaultCommand def removeDefaultCommand(self, subsystem: Subsystem) -> None: @@ -314,6 +382,12 @@ def removeDefaultCommand(self, subsystem: Subsystem) -> None: :param subsystem: the subsystem whose default command will be removed """ + if subsystem is None: + reportWarning( + "Tried to remove a default command for a null subsystem", True + ) + return + self._subsystems[subsystem] = None def getDefaultCommand(self, subsystem: Subsystem) -> Optional[Command]: @@ -326,31 +400,47 @@ def getDefaultCommand(self, subsystem: Subsystem) -> Optional[Command]: """ return self._subsystems[subsystem] - def cancel(self, *commands: Command) -> None: + def cancel( + self, + *commands: Command, + ) -> None: """ - Cancels commands. The scheduler will only call Command#end(boolean) method of the - canceled command with {@code true}, indicating they were canceled (as opposed to finishing + Cancels commands. The scheduler will only call :meth:`commands2.Command.end` method of the + canceled command with ``True``, indicating they were canceled (as opposed to finishing normally). Commands will be canceled regardless of InterruptionBehavior interruption behavior. :param commands: the commands to cancel """ + for command in commands: + self._cancel(command, None) + + def _cancel(self, command: Command, interruptor: Optional[Command]): + if command is None: + reportWarning("Tried to cancel a null command", True) + return + + if command in self._endingCommands: + return + if self._inRunLoop: - for command in commands: - self._toCancel[command] = None + self._toCancel[command] = interruptor return - for command in commands: - if not self.isScheduled(command): - continue + if not self.isScheduled(command): + return - self._scheduledCommands.pop(command) - for requirement in command.getRequirements(): - del self._requirements[requirement] - command.end(True) - for action in self._interruptActions: - action(command) + self._endingCommands.add(command) + command.end(True) + for action in self._interruptActions: + action(command, interruptor) + + self._endingCommands.remove(command) + self._scheduledCommands.pop(command) + for requirement in command.getRequirements(): + del self._requirements[requirement] + self._watchdog.addEpoch(f"{command.getName()}.end(true)") def cancelAll(self) -> None: """Cancels all commands that are currently scheduled.""" @@ -392,6 +482,7 @@ def onCommandInitialize(self, action: Callable[[Command], Any]) -> None: :param action: the action to perform """ + assert callable(action) self._initActions.append(action) def onCommandExecute(self, action: Callable[[Command], Any]) -> None: @@ -400,6 +491,7 @@ def onCommandExecute(self, action: Callable[[Command], Any]) -> None: :param action: the action to perform """ + assert callable(action) self._executeActions.append(action) def onCommandInterrupt(self, action: Callable[[Command], Any]) -> None: @@ -408,6 +500,20 @@ def onCommandInterrupt(self, action: Callable[[Command], Any]) -> None: :param action: the action to perform """ + + assert callable(action) + self._interruptActions.append(lambda command, interruptor: action(command)) + + def onCommandInterruptWithCause( + self, action: Callable[[Command, Optional[Command]], Any] + ) -> None: + """ + Adds an action to perform on the interruption of any command by the scheduler. The action receives the interrupted command and the command that interrupted it + + :param action: the action to perform + """ + + assert callable(action) self._interruptActions.append(action) def onCommandFinish(self, action: Callable[[Command], Any]) -> None: @@ -416,6 +522,7 @@ def onCommandFinish(self, action: Callable[[Command], Any]) -> None: :param action: the action to perform """ + assert callable(action) self._finishActions.append(action) def registerComposedCommands(self, commands: Iterable[Command]) -> None: @@ -424,43 +531,82 @@ def registerComposedCommands(self, commands: Iterable[Command]) -> None: directly or added to a composition. :param commands: the commands to register - @throws IllegalArgumentException if the given commands have already been composed. + + :raises IllegalCommandUse: if the given commands have already been composed. """ - self.requireNotComposed(commands) - self._composedCommands.update(commands) + cmds = tuple(commands) + if len(set(cmds)) != len(cmds): + raise IllegalCommandUse( + "Cannot compose a command twice in the same composition!" + ) + + self.requireNotComposedOrScheduled(*cmds) + + # Find where the user called us from + # - it would be better to give a full traceback, but this is fine for now + location = "" + + for f, lineno in traceback.walk_stack(None): + info = inspect.getframeinfo(f) + if not info.filename.startswith(_cmd_path): + location = f"{info.filename}:{lineno}" + break + + for cmd in cmds: + self._composedCommands[cmd] = location def clearComposedCommands(self) -> None: """ Clears the list of composed commands, allowing all commands to be freely used again. - WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use - this unless you fully understand what you are doing. + .. warning:: Using this haphazardly can result in unexpected/undesirable behavior. Do not use + this unless you fully understand what you are doing. """ self._composedCommands.clear() - def removeComposedCommands(self, commands: Iterable[Command]) -> None: + def removeComposedCommand(self, command: Command) -> None: """ Removes a single command from the list of composed commands, allowing it to be freely used again. - WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use - this unless you fully understand what you are doing. + .. warning:: Using this haphazardly can result in unexpected/undesirable behavior. Do not use + this unless you fully understand what you are doing. :param command: the command to remove from the list of grouped commands """ - self._composedCommands.difference_update(commands) + self._composedCommands.pop(command, None) - def requireNotComposed(self, commands: Iterable[Command]) -> None: + def requireNotComposed(self, *commands: Command) -> None: """ Requires that the specified command hasn't been already added to a composition. :param commands: The commands to check - @throws IllegalArgumentException if the given commands have already been composed. + + :raises IllegalCommandUse: if the given commands have already been composed. """ - if not self._composedCommands.isdisjoint(commands): - raise IllegalCommandUse( - "Commands that have been composed may not be added to another composition or scheduled individually" - ) + for command in commands: + location = self._composedCommands.get(command) + if location is not None: + raise IllegalCommandUse( + "Commands that have been composed may not be added to another" + f"composition or scheduled individually (originally composed at {location})" + ) + + def requireNotComposedOrScheduled(self, *commands: Command) -> None: + """ + Requires that the specified command hasn't already been added to a composition, and is not + currently scheduled. + + :param command: The command to check + + :raises IllegalCommandUse: if the given command has already been composed or scheduled. + """ + for command in commands: + if self.isScheduled(command): + raise IllegalCommandUse( + "Commands that have been scheduled individually may not be added to a composition!" + ) + self.requireNotComposed(command) def isComposed(self, command: Command) -> bool: """ @@ -469,7 +615,28 @@ def isComposed(self, command: Command) -> bool: :param command: The command to check :returns: true if composed """ - return command in self.getComposedCommands() + return command in self._composedCommands - def getComposedCommands(self) -> Set[Command]: - return self._composedCommands + def initSendable(self, builder: SendableBuilder): + builder.setSmartDashboardType("Scheduler") + builder.addStringArrayProperty( + "Names", + lambda: [command.getName() for command in self._scheduledCommands], + lambda _: None, + ) + builder.addIntegerArrayProperty( + "Ids", + lambda: [id(command) for command in self._scheduledCommands], + lambda _: None, + ) + + def cancel_commands(to_cancel: List[int]): + ids = {id(command): command for command in self._scheduledCommands} + for hash_value in to_cancel: + cancelCmd = ids.get(hash_value) + if cancelCmd is not None: + self.cancel(cancelCmd) + + builder.addIntegerArrayProperty( + "Cancel", lambda: [], lambda to_cancel: cancel_commands(to_cancel) + ) diff --git a/commands2/conditionalcommand.py b/commands2/conditionalcommand.py index b26760de..8e9720f3 100644 --- a/commands2/conditionalcommand.py +++ b/commands2/conditionalcommand.py @@ -1,9 +1,11 @@ +# validated: 2024-01-19 DS aaea85ff1656 ConditionalCommand.java from __future__ import annotations -from typing import Callable +from typing import Callable, Optional -from .command import Command -from .commandgroup import * +from wpiutil import SendableBuilder + +from .command import Command, InterruptionBehavior from .commandscheduler import CommandScheduler @@ -15,10 +17,9 @@ class ConditionalCommand(Command): The rules for command compositions apply: command instances that are passed to it cannot be added to any other composition or scheduled individually, and the composition requires all subsystems its components require. + """ - This class is provided by the NewCommands VendorDep""" - - selectedCommand: Command + selectedCommand: Optional[Command] def __init__( self, onTrue: Command, onFalse: Command, condition: Callable[[], bool] @@ -28,7 +29,8 @@ def __init__( :param onTrue: the command to run if the condition is true :param onFalse: the command to run if the condition is false - :param condition: the condition to determine which command to run""" + :param condition: the condition to determine which command to run + """ super().__init__() CommandScheduler.getInstance().registerComposedCommands([onTrue, onFalse]) @@ -49,13 +51,43 @@ def initialize(self): self.selectedCommand.initialize() def execute(self): + assert self.selectedCommand is not None self.selectedCommand.execute() - def isFinished(self) -> bool: - return self.selectedCommand.isFinished() - def end(self, interrupted: bool): + assert self.selectedCommand is not None self.selectedCommand.end(interrupted) + def isFinished(self) -> bool: + assert self.selectedCommand is not None + return self.selectedCommand.isFinished() + def runsWhenDisabled(self) -> bool: return self.onTrue.runsWhenDisabled() and self.onFalse.runsWhenDisabled() + + def getInterruptionBehavior(self) -> InterruptionBehavior: + if ( + self.onTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf + or self.onFalse.getInterruptionBehavior() + == InterruptionBehavior.kCancelSelf + ): + return InterruptionBehavior.kCancelSelf + else: + return InterruptionBehavior.kCancelIncoming + + def initSendable(self, builder: SendableBuilder): + super().initSendable(builder) + builder.addStringProperty("onTrue", self.onTrue.getName, lambda _: None) + builder.addStringProperty("onFalse", self.onFalse.getName, lambda _: None) + + def _selected(): + if self.selectedCommand is None: + return "null" + else: + return self.selectedCommand.getName() + + builder.addStringProperty( + "selected", + _selected, + lambda _: None, + ) diff --git a/commands2/exceptions.py b/commands2/exceptions.py new file mode 100644 index 00000000..36d6c341 --- /dev/null +++ b/commands2/exceptions.py @@ -0,0 +1,8 @@ +# notrack +class IllegalCommandUse(Exception): + """ + This exception is raised when a command is used in a way that it shouldn't be. + + You shouldn't try to catch this exception, if it occurs it means your code is + doing something it probably shouldn't be doing + """ diff --git a/commands2/functionalcommand.py b/commands2/functionalcommand.py index 9498568e..0fd864f9 100644 --- a/commands2/functionalcommand.py +++ b/commands2/functionalcommand.py @@ -1,9 +1,9 @@ +# validated: 2024-01-19 DS 6e58db398d63 FunctionalCommand.java from __future__ import annotations from typing import Any, Callable from .command import Command -from .commandgroup import * from .subsystem import Subsystem @@ -12,7 +12,8 @@ class FunctionalCommand(Command): A command that allows the user to pass in functions for each of the basic command methods through the constructor. Useful for inline definitions of complex commands - note, however, that if a command is beyond a certain complexity it is usually better practice to write a proper class for - it than to inline it.""" + it than to inline it. + """ def __init__( self, @@ -29,9 +30,15 @@ def __init__( :param onExecute: the function to run on command execution :param onEnd: the function to run on command end :param isFinished: the function that determines whether the command has finished - :param requirements: the subsystems required by this command""" + :param requirements: the subsystems required by this command + """ super().__init__() + assert callable(onInit) + assert callable(onExecute) + assert callable(onEnd) + assert callable(isFinished) + self._onInit = onInit self._onExecute = onExecute self._onEnd = onEnd diff --git a/commands2/instantcommand.py b/commands2/instantcommand.py index 6d20a2e6..9a651ff9 100644 --- a/commands2/instantcommand.py +++ b/commands2/instantcommand.py @@ -1,3 +1,4 @@ +# validated: 2024-01-19 DS 5cf961edb973 InstantCommand.java from __future__ import annotations from typing import Callable, Optional @@ -9,17 +10,20 @@ class InstantCommand(FunctionalCommand): """ A Command that runs instantly; it will initialize, execute once, and end on the same iteration of - the scheduler. Users can either pass in a Runnable and a set of requirements, or else subclass - this command if desired.""" + the scheduler. Users can either pass in a Callable and a set of requirements, or else subclass + this command if desired. + """ def __init__( self, toRun: Optional[Callable[[], None]] = None, *requirements: Subsystem ): """ - Creates a new InstantCommand that runs the given Runnable with the given requirements. + Creates a new InstantCommand that runs the given Callable with the given requirements. :param toRun: the Runnable to run - :param requirements: the subsystems required by this command""" + :param requirements: the subsystems required by this command + """ + assert toRun is None or callable(toRun) super().__init__( toRun or (lambda: None), lambda: None, diff --git a/commands2/notifiercommand.py b/commands2/notifiercommand.py index 0a6e57e8..4bfedf1f 100644 --- a/commands2/notifiercommand.py +++ b/commands2/notifiercommand.py @@ -1,41 +1,46 @@ +# validated: 2024-01-19 DS 6e58db398d63 NotifierCommand.java from __future__ import annotations from typing import Any, Callable from wpilib import Notifier +from wpimath import units from .command import Command -from .commandgroup import * from .subsystem import Subsystem class NotifierCommand(Command): """ - A command that starts a notifier to run the given runnable periodically in a separate thread. Has - no end condition as-is; either subclass it or use Command#withTimeout(double) or {@link - Command#until(java.util.function.BooleanSupplier)} to give it one. + A command that starts a notifier to run the given Callable periodically in a separate thread. Has + no end condition as-is; either subclass it or use :func:`commands2.Command.withTimeout` or :func:`commands2.Command.until` to give it one. - WARNING: Do not use this class unless you are confident in your ability to make the executed - code thread-safe. If you do not know what "thread-safe" means, that is a good sign that you - should not use this class.""" + .. warning:: Do not use this class unless you are confident in your ability + to make the executed code thread-safe. If you do not know what + "thread-safe" means, that is a good sign that you should not use + this class. + """ def __init__( - self, toRun: Callable[[], Any], period: float, *requirements: Subsystem + self, toRun: Callable[[], Any], period: units.seconds, *requirements: Subsystem ): """ Creates a new NotifierCommand. - :param toRun: the runnable for the notifier to run + :param toRun: the Callable for the notifier to run :param period: the period at which the notifier should run, in seconds - :param requirements: the subsystems required by this command""" + :param requirements: the subsystems required by this command + """ super().__init__() - self.notifier = Notifier(toRun) - self.period = period + assert callable(toRun) + + self._notifier = Notifier(toRun) + self._period = period self.addRequirements(*requirements) def initialize(self): - self.notifier.startPeriodic(self.period) + self._notifier.startPeriodic(self._period) def end(self, interrupted: bool): - self.notifier.stop() + self._notifier.stop() diff --git a/commands2/parallelcommandgroup.py b/commands2/parallelcommandgroup.py index e3ad01dd..d391776d 100644 --- a/commands2/parallelcommandgroup.py +++ b/commands2/parallelcommandgroup.py @@ -1,22 +1,22 @@ +# validated: 2024-01-19 DS aaea85ff1656 ParallelCommandGroup.java from __future__ import annotations from typing import Dict -from commands2.command import Command, InterruptionBehavior - from .command import Command, InterruptionBehavior -from .commandgroup import * from .commandscheduler import CommandScheduler +from .exceptions import IllegalCommandUse from .util import flatten_args_commands -class ParallelCommandGroup(CommandGroup): +class ParallelCommandGroup(Command): """ A command composition that runs a set of commands in parallel, ending when the last command ends. The rules for command compositions apply: command instances that are passed to it cannot be added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" + subsystems its components require. + """ def __init__(self, *commands: Command): """ @@ -24,7 +24,8 @@ def __init__(self, *commands: Command): command composition will finish when the last command finishes. If the composition is interrupted, only the commands that are still running will be interrupted. - :param commands: the commands to include in this composition.""" + :param commands: the commands to include in this composition. + """ super().__init__() self._commands: Dict[Command, bool] = {} self._runsWhenDisabled = True @@ -32,6 +33,11 @@ def __init__(self, *commands: Command): self.addCommands(*commands) def addCommands(self, *commands: Command): + """ + Adds the given commands to the group. + + :param commands: Commands to add to the group + """ commands = flatten_args_commands(commands) if True in self._commands.values(): raise IllegalCommandUse( diff --git a/commands2/paralleldeadlinegroup.py b/commands2/paralleldeadlinegroup.py index eb064f49..e416cd4b 100644 --- a/commands2/paralleldeadlinegroup.py +++ b/commands2/paralleldeadlinegroup.py @@ -1,46 +1,78 @@ +# validated: 2024-01-19 DS e07de37e64f2 ParallelDeadlineGroup.java from __future__ import annotations from typing import Dict -from commands2.command import Command, InterruptionBehavior +from wpiutil import SendableBuilder from .command import Command, InterruptionBehavior -from .commandgroup import * from .commandscheduler import CommandScheduler +from .exceptions import IllegalCommandUse from .util import flatten_args_commands -class ParallelDeadlineGroup(CommandGroup): +class ParallelDeadlineGroup(Command): """ - A command composition that runs one of a selection of commands, either using a selector and a key - to command mapping, or a supplier that returns the command directly at runtime. + A command composition that runs a set of commands in parallel, ending only when a specific + command (the "deadline") ends, interrupting all other commands that are still running at that + point. The rules for command compositions apply: command instances that are passed to it cannot be added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" + subsystems its components require. + """ def __init__(self, deadline: Command, *commands: Command): """ - Creates a new SelectCommand. + Creates a new ParallelDeadlineGroup. The given commands (including the + deadline) will be executed simultaneously. The composition will finish when + the deadline finishes, interrupting all other still-running commands. If + the composition is interrupted, only the commands still running will be + interrupted. + + :param deadline: the command that determines when the composition ends + :param commands: the commands to be executed - :param commands: the map of commands to choose from - :param selector: the selector to determine which command to run""" + :raises IllegalCommandUse: if the deadline command is also in the otherCommands argument + """ super().__init__() self._commands: Dict[Command, bool] = {} self._runsWhenDisabled = True self._finished = True - self._deadline = deadline self._interruptBehavior = InterruptionBehavior.kCancelIncoming self.addCommands(*commands) - if deadline not in self._commands: - self.addCommands(deadline) + self.setDeadline(deadline) def setDeadline(self, deadline: Command): - if deadline not in self._commands: - self.addCommands(deadline) + """ + Sets the deadline to the given command. The deadline is added to the group if it is not already + contained. + + :param deadline: the command that determines when the group ends + + :raises IllegalCommandUse: if the deadline command is already in the composition + """ + + # use getattr here because deadline not set in constructor + isAlreadyDeadline = deadline == getattr(self, "_deadline", None) + if isAlreadyDeadline: + return + + if deadline in self._commands: + raise IllegalCommandUse( + f"The deadline command cannot also be in the other commands!" + ) + self.addCommands(deadline) self._deadline = deadline def addCommands(self, *commands: Command): + """ + Adds the given commands to the group. + + :param commands: Commands to add to the group. + + :raises IllegalCommandUse: if the deadline command is already in the composition + """ commands = flatten_args_commands(commands) if not self._finished: raise IllegalCommandUse( @@ -52,7 +84,7 @@ def addCommands(self, *commands: Command): for command in commands: if not command.getRequirements().isdisjoint(self.requirements): raise IllegalCommandUse( - "Multiple comands in a parallel composition cannot require the same subsystems." + "Multiple commands in a parallel composition cannot require the same subsystems." ) self._commands[command] = False @@ -96,3 +128,10 @@ def runsWhenDisabled(self) -> bool: def getInterruptionBehavior(self) -> InterruptionBehavior: return self._interruptBehavior + + def initSendable(self, builder: SendableBuilder): + super().initSendable(builder) + + builder.addStringProperty( + "deadline", lambda: self._deadline.getName(), lambda _: None + ) diff --git a/commands2/parallelracegroup.py b/commands2/parallelracegroup.py index 3b66c14a..ef4d3e6b 100644 --- a/commands2/parallelracegroup.py +++ b/commands2/parallelracegroup.py @@ -1,23 +1,23 @@ +# validated: 2024-01-19 DS aaea85ff1656 ParallelRaceGroup.java from __future__ import annotations from typing import Set -from commands2.command import Command, InterruptionBehavior - from .command import Command, InterruptionBehavior -from .commandgroup import * from .commandscheduler import CommandScheduler +from .exceptions import IllegalCommandUse from .util import flatten_args_commands -class ParallelRaceGroup(CommandGroup): +class ParallelRaceGroup(Command): """ A composition that runs a set of commands in parallel, ending when any one of the commands ends and interrupting all the others. The rules for command compositions apply: command instances that are passed to it cannot be added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" + subsystems its components require. + """ def __init__(self, *commands: Command): """ @@ -25,15 +25,21 @@ def __init__(self, *commands: Command): "race to the finish" - the first command to finish ends the entire command, with all other commands being interrupted. - :param commands: the commands to include in this composition.""" + :param commands: the commands to include in this composition. + """ super().__init__() self._commands: Set[Command] = set() self._runsWhenDisabled = True - self._interruptBehavior = InterruptionBehavior.kCancelIncoming self._finished = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming self.addCommands(*commands) def addCommands(self, *commands: Command): + """ + Adds the given commands to the group. + + :param commands: Commands to add to the group. + """ commands = flatten_args_commands(commands) if not self._finished: raise IllegalCommandUse( diff --git a/commands2/perpetualcommand.py b/commands2/perpetualcommand.py deleted file mode 100644 index 1e535e34..00000000 --- a/commands2/perpetualcommand.py +++ /dev/null @@ -1,46 +0,0 @@ -from __future__ import annotations - -from .command import Command -from .commandgroup import * -from .commandscheduler import CommandScheduler - - -class PerpetualCommand(Command): - """ - A command that runs another command in perpetuity, ignoring that command's end conditions. While - this class does not extend CommandGroupBase, it is still considered a composition, as it - allows one to compose another command within it; the command instances that are passed to it - cannot be added to any other groups, or scheduled individually. - - As a rule, CommandGroups require the union of the requirements of their component commands. - - This class is provided by the NewCommands VendorDep - - @deprecated PerpetualCommand violates the assumption that execute() doesn't get called after - isFinished() returns true -- an assumption that should be valid. This was unsafe/undefined - behavior from the start, and RepeatCommand provides an easy way to achieve similar end - results with slightly different (and safe) semantics.""" - - def __init__(self, command: Command): - """ - Creates a new PerpetualCommand. Will run another command in perpetuity, ignoring that command's - end conditions, unless this command itself is interrupted. - - :param command: the command to run perpetually""" - super().__init__() - - CommandScheduler.getInstance().registerComposedCommands([command]) - self._command = command - self.addRequirements(*command.getRequirements()) - - def initialize(self): - self._command.initialize() - - def execute(self): - self._command.execute - - def end(self, interrupted: bool): - self._command.end(interrupted) - - def runsWhenDisabled(self) -> bool: - return self._command.runsWhenDisabled() diff --git a/commands2/pidcommand.py b/commands2/pidcommand.py index 339764c0..325dec52 100644 --- a/commands2/pidcommand.py +++ b/commands2/pidcommand.py @@ -1,3 +1,4 @@ +# validated: 2024-01-19 DS f29a7d2e501b PIDCommand.java # 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. @@ -13,7 +14,7 @@ class PIDCommand(Command): """ - A command that controls an output with a PIDController. Runs forever by default - to add + A command that controls an output with a :class:`wpimath.controller.PIDController`. Runs forever by default - to add exit conditions and/or other behavior, subclass this class. The controller calculation and output are performed synchronously in the command's execute() method. """ @@ -27,7 +28,7 @@ def __init__( *requirements: Subsystem, ): """ - Creates a new PIDCommand, which controls the given output with a PIDController. + Creates a new PIDCommand, which controls the given output with a :class:`wpimath.controller.PIDController`. :param controller: the controller that controls the output. :param measurementSource: the measurement of the process variable @@ -38,6 +39,9 @@ def __init__( """ super().__init__() + assert callable(measurementSource) + assert callable(useOutput) + self._controller = controller self._useOutput = useOutput self._measurement = measurementSource diff --git a/commands2/pidsubsystem.py b/commands2/pidsubsystem.py index c63469a3..ba26528f 100644 --- a/commands2/pidsubsystem.py +++ b/commands2/pidsubsystem.py @@ -1,3 +1,4 @@ +# validated: 2024-01-19 DS f29a7d2e501b PIDSubsystem.java # 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. @@ -10,7 +11,7 @@ class PIDSubsystem(Subsystem): """ - A subsystem that uses a {@link PIDController} to control an output. The + A subsystem that uses a :class:`wpimath.controller.PIDController` to control an output. The controller is run synchronously from the subsystem's periodic() method. """ diff --git a/commands2/printcommand.py b/commands2/printcommand.py index 082a8c7f..5327df73 100644 --- a/commands2/printcommand.py +++ b/commands2/printcommand.py @@ -1,3 +1,4 @@ +# validated: 2024-01-19 DS 8ac45f20bb47 PrintCommand.java from __future__ import annotations from .instantcommand import InstantCommand @@ -5,13 +6,15 @@ class PrintCommand(InstantCommand): """ - A command that prints a string when initialized.""" + A command that prints a string when initialized. + """ def __init__(self, message: str): """ Creates a new a PrintCommand. - :param message: the message to print""" + :param message: the message to print + """ super().__init__(lambda: print(message)) def runsWhenDisabled(self) -> bool: diff --git a/commands2/proxycommand.py b/commands2/proxycommand.py index 630e7b52..759b3504 100644 --- a/commands2/proxycommand.py +++ b/commands2/proxycommand.py @@ -1,9 +1,11 @@ +# validated: 2024-01-19 DS 192a28af4731 ProxyCommand.java from __future__ import annotations from typing import Callable, overload +from wpiutil import SendableBuilder + from .command import Command -from .commandgroup import * from .util import format_args_kwargs @@ -21,7 +23,8 @@ def __init__(self, supplier: Callable[[], Command]): Creates a new ProxyCommand that schedules the supplied command when initialized, and ends when it is no longer scheduled. Useful for lazily creating commands at runtime. - :param supplier: the command supplier""" + :param supplier: the command supplier + """ ... @overload @@ -30,16 +33,19 @@ def __init__(self, command: Command): Creates a new ProxyCommand that schedules the given command when initialized, and ends when it is no longer scheduled. - :param command: the command to run by proxy""" + :param command: the command to run by proxy + """ ... def __init__(self, *args, **kwargs): super().__init__() def init_supplier(supplier: Callable[[], Command]): + assert callable(supplier) self._supplier = supplier def init_command(command: Command): + self.setName(f"Proxy({command.getName()})") self._supplier = lambda: command num_args = len(args) + len(kwargs) @@ -70,7 +76,8 @@ def initialize(self): self._command.schedule() def end(self, interrupted: bool): - if interrupted and self._command is not None: + assert self._command is not None + if interrupted: self._command.cancel() self._command = None @@ -78,6 +85,9 @@ def execute(self): pass def isFinished(self) -> bool: + # because we're between `initialize` and `end`, `self._command` is necessarily not None + # but if called otherwise and m_command is None, + # it's UB, so we can do whatever we want -- like return true. return self._command is None or not self._command.isScheduled() def runsWhenDisabled(self) -> bool: @@ -88,3 +98,11 @@ def runsWhenDisabled(self) -> bool: :returns: true. Otherwise, this proxy would cancel commands that do run when disabled. """ return True + + def initSendable(self, builder: SendableBuilder): + super().initSendable(builder) + builder.addStringProperty( + "proxied", + lambda: "null" if self._command is None else self._command.getName(), + lambda _: None, + ) diff --git a/commands2/proxyschedulecommand.py b/commands2/proxyschedulecommand.py deleted file mode 100644 index 23b9d8a4..00000000 --- a/commands2/proxyschedulecommand.py +++ /dev/null @@ -1,41 +0,0 @@ -from __future__ import annotations - -from .command import Command -from .commandgroup import * - - -class ProxyScheduleCommand(Command): - """ - Schedules the given commands when this command is initialized, and ends when all the commands are - no longer scheduled. Useful for forking off from CommandGroups. If this command is interrupted, - it will cancel all the commands. - """ - - def __init__(self, *toSchedule: Command): - """ - Creates a new ProxyScheduleCommand that schedules the given commands when initialized, and ends - when they are all no longer scheduled. - - :param toSchedule: the commands to schedule - @deprecated Replace with ProxyCommand, composing multiple of them in a {@link - ParallelRaceGroup} if needed.""" - super().__init__() - self.toSchedule = set(toSchedule) - self._finished = False - - def initialize(self): - for command in self.toSchedule: - command.schedule() - - def end(self, interrupted: bool): - if interrupted: - for command in self.toSchedule: - command.cancel() - - def execute(self): - self._finished = True - for command in self.toSchedule: - self._finished = self._finished and not command.isScheduled() - - def isFinished(self) -> bool: - return self._finished diff --git a/commands2/repeatcommand.py b/commands2/repeatcommand.py index dc7dc991..21cf019b 100644 --- a/commands2/repeatcommand.py +++ b/commands2/repeatcommand.py @@ -1,9 +1,10 @@ +# validated: 2024-01-19 DS 6e58db398d63 RepeatCommand.java from __future__ import annotations -from commands2.command import InterruptionBehavior +from wpiutil import SendableBuilder -from .command import Command -from .commandgroup import * +from .command import Command, InterruptionBehavior +from .commandscheduler import CommandScheduler class RepeatCommand(Command): @@ -14,16 +15,21 @@ class RepeatCommand(Command): The rules for command compositions apply: command instances that are passed to it cannot be added to any other composition or scheduled individually,and the composition requires all - subsystems its components require.""" + subsystems its components require. + """ def __init__(self, command: Command): """ Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it ends, until this command is interrupted. - :param command: the command to run repeatedly""" + :param command: the command to run repeatedly + """ super().__init__() self._command = command + CommandScheduler.getInstance().registerComposedCommands([command]) + self.requirements.update(command.getRequirements()) + self.setName(f"Repeat({command.getName()})") def initialize(self): self._ended = False @@ -37,6 +43,7 @@ def execute(self): self._command.execute() if self._command.isFinished(): + # restart command self._command.end(False) self._ended = True @@ -44,6 +51,8 @@ def isFinished(self) -> bool: return False def end(self, interrupted: bool): + # Make sure we didn't already call end() (which would happen if the command finished in the + # last call to our execute()) if not self._ended: self._command.end(interrupted) self._ended = True @@ -53,3 +62,7 @@ def runsWhenDisabled(self) -> bool: def getInterruptionBehavior(self) -> InterruptionBehavior: return self._command.getInterruptionBehavior() + + def initSendable(self, builder: SendableBuilder) -> None: + super().initSendable(builder) + builder.addStringProperty("command", self._command.getName, lambda _: None) diff --git a/commands2/runcommand.py b/commands2/runcommand.py index 231567f5..4dc13807 100644 --- a/commands2/runcommand.py +++ b/commands2/runcommand.py @@ -1,25 +1,28 @@ +# validated: 2024-01-19 DS 5cf961edb973 RunCommand.java from __future__ import annotations from typing import Any, Callable -from .commandgroup import * from .functionalcommand import FunctionalCommand from .subsystem import Subsystem class RunCommand(FunctionalCommand): """ - A command that runs a Runnable continuously. Has no end condition as-is; either subclass it or - use Command#withTimeout(double) or Command#until(BooleanSupplier) to give it one. - If you only wish to execute a Runnable once, use InstantCommand.""" + A command that runs a Callable continuously. Has no end condition as-is; either subclass it or + use :func:`commands2.Command.withTimeout` or :func:`commands2.Command.until` to give it one. + If you only wish to execute a Callable once, use :class:`commands2.InstantCommand`. + """ def __init__(self, toRun: Callable[[], Any], *requirements: Subsystem): """ - Creates a new RunCommand. The Runnable will be run continuously until the command ends. Does + Creates a new RunCommand. The Callable will be run continuously until the command ends. Does not run when disabled. - :param toRun: the Runnable to run - :param requirements: the subsystems to require""" + :param toRun: the Callable to run + :param requirements: the subsystems to require + """ + assert callable(toRun) super().__init__( lambda: None, toRun, lambda interrupted: None, lambda: False, *requirements ) diff --git a/commands2/schedulecommand.py b/commands2/schedulecommand.py index 1d987e6d..d6c36108 100644 --- a/commands2/schedulecommand.py +++ b/commands2/schedulecommand.py @@ -1,7 +1,7 @@ +# validated: 2024-01-19 DS aaea85ff1656 ScheduleCommand.java from __future__ import annotations from .command import Command -from .commandgroup import * class ScheduleCommand(Command): @@ -15,12 +15,13 @@ def __init__(self, *commands: Command): """ Creates a new ScheduleCommand that schedules the given commands when initialized. - :param toSchedule: the commands to schedule""" + :param toSchedule: the commands to schedule + """ super().__init__() - self.toSchedule = set(commands) + self._toSchedule = set(commands) def initialize(self): - for command in self.toSchedule: + for command in self._toSchedule: command.schedule() def isFinished(self) -> bool: diff --git a/commands2/selectcommand.py b/commands2/selectcommand.py index 56710dcd..4b1a0fe3 100644 --- a/commands2/selectcommand.py +++ b/commands2/selectcommand.py @@ -1,11 +1,11 @@ +# validated: 2024-01-19 DS a4a8ad9c753e SelectCommand.java from __future__ import annotations from typing import Callable, Dict, Hashable -from commands2.command import InterruptionBehavior +from wpiutil import SendableBuilder from .command import Command, InterruptionBehavior -from .commandgroup import * from .commandscheduler import CommandScheduler from .printcommand import PrintCommand @@ -17,7 +17,8 @@ class SelectCommand(Command): The rules for command compositions apply: command instances that are passed to it cannot be added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" + subsystems its components require. + """ def __init__( self, @@ -28,16 +29,27 @@ def __init__( Creates a new SelectCommand. :param commands: the map of commands to choose from - :param selector: the selector to determine which command to run""" + :param selector: the selector to determine which command to run + """ super().__init__() - self._commands = commands - self._selector = selector + assert callable(selector) - CommandScheduler.getInstance().registerComposedCommands(commands.values()) + self._defaultCommand = PrintCommand( + "SelectCommand selector value does not correspond to any command!" + ) + self._commands = commands + self._selector = selector + # This is slightly different than Java but avoids UB + self._selectedCommand = self._defaultCommand self._runsWhenDisabled = True self._interruptBehavior = InterruptionBehavior.kCancelIncoming + + scheduler = CommandScheduler.getInstance() + scheduler.registerComposedCommands([self._defaultCommand]) + scheduler.registerComposedCommands(commands.values()) + for command in commands.values(): self.addRequirements(*command.getRequirements()) self._runsWhenDisabled = ( @@ -47,12 +59,9 @@ def __init__( self._interruptBehavior = InterruptionBehavior.kCancelSelf def initialize(self): - if self._selector() not in self._commands: - self._selectedCommand = PrintCommand( - "SelectCommand selector value does not correspond to any command!" - ) - return - self._selectedCommand = self._commands[self._selector()] + self._selectedCommand = self._commands.get( + self._selector(), self._defaultCommand + ) self._selectedCommand.initialize() def execute(self): @@ -69,3 +78,9 @@ def runsWhenDisabled(self) -> bool: def getInterruptionBehavior(self) -> InterruptionBehavior: return self._interruptBehavior + + def initSendable(self, builder: SendableBuilder) -> None: + super().initSendable(builder) + builder.addStringProperty( + "selected", lambda: self._defaultCommand.getName(), lambda _: None + ) diff --git a/commands2/sequentialcommandgroup.py b/commands2/sequentialcommandgroup.py index ef6984d4..e7ecbf78 100644 --- a/commands2/sequentialcommandgroup.py +++ b/commands2/sequentialcommandgroup.py @@ -1,37 +1,45 @@ +# validated: 2024-01-19 DS aaea85ff1656 SequentialCommandGroup.java from __future__ import annotations from typing import List -from commands2.command import Command, InterruptionBehavior +from wpiutil import SendableBuilder from .command import Command, InterruptionBehavior -from .commandgroup import * from .commandscheduler import CommandScheduler +from .exceptions import IllegalCommandUse from .util import flatten_args_commands -class SequentialCommandGroup(CommandGroup): +class SequentialCommandGroup(Command): """ A command composition that runs a list of commands in sequence. The rules for command compositions apply: command instances that are passed to it cannot be added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" + subsystems its components require. + """ def __init__(self, *commands: Command): """ Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the composition finishing when the last command finishes. - :param commands: the commands to include in this composition.""" + :param commands: the commands to include in this composition. + """ super().__init__() self._commands: List[Command] = [] + self._currentCommandIndex = -1 self._runsWhenDisabled = True self._interruptBehavior = InterruptionBehavior.kCancelIncoming - self._currentCommandIndex = -1 self.addCommands(*commands) def addCommands(self, *commands: Command): + """ + Adds the given commands to the group. + + :param commands: Commands to add to the group. + """ commands = flatten_args_commands(commands) if self._currentCommandIndex != -1: raise IllegalCommandUse( @@ -68,16 +76,13 @@ def execute(self): self._commands[self._currentCommandIndex].initialize() def end(self, interrupted: bool): - if not interrupted: - return - if not self._commands: - return - if not self._currentCommandIndex > -1: - return - if not self._currentCommandIndex < len(self._commands): - return + if ( + interrupted + and self._commands + and -1 < self._currentCommandIndex < len(self._commands) + ): + self._commands[self._currentCommandIndex].end(True) - self._commands[self._currentCommandIndex].end(True) self._currentCommandIndex = -1 def isFinished(self) -> bool: @@ -88,3 +93,9 @@ def runsWhenDisabled(self) -> bool: def getInterruptionBehavior(self) -> InterruptionBehavior: return self._interruptBehavior + + def initSendable(self, builder: SendableBuilder) -> None: + super().initSendable(builder) + builder.addIntegerProperty( + "index", lambda: self._currentCommandIndex, lambda _: None + ) diff --git a/commands2/startendcommand.py b/commands2/startendcommand.py index 1ea896af..6059095b 100644 --- a/commands2/startendcommand.py +++ b/commands2/startendcommand.py @@ -1,18 +1,18 @@ +# validated: 2024-01-19 DS b390cad09505 StartEndCommand.java from __future__ import annotations from typing import Any, Callable -from .commandgroup import * from .functionalcommand import FunctionalCommand from .subsystem import Subsystem class StartEndCommand(FunctionalCommand): """ - A command that runs a given runnable when it is initialized, and another runnable when it ends. + A command that runs a given Callable when it is initialized, and another Callable when it ends. Useful for running and then stopping a motor, or extending and then retracting a solenoid. Has no - end condition as-is; either subclass it or use Command#withTimeout(double) or {@link - Command#until(java.util.function.BooleanSupplier)} to give it one. + end condition as-is; either subclass it or use :func:`commands2.Command.withTimeout` + or :func:`commands2.Command.until` to give it one. """ def __init__( @@ -22,12 +22,15 @@ def __init__( *requirements: Subsystem, ): """ - Creates a new StartEndCommand. Will run the given runnables when the command starts and when it + Creates a new StartEndCommand. Will run the given Callables when the command starts and when it ends. - :param onInit: the Runnable to run on command init - :param onEnd: the Runnable to run on command end - :param requirements: the subsystems required by this command""" + :param onInit: the Callable to run on command init + :param onEnd: the Callable to run on command end + :param requirements: the subsystems required by this command + """ + assert callable(onInit) + assert callable(onEnd) super().__init__( onInit, lambda: None, lambda _: onEnd(), lambda: False, *requirements ) diff --git a/commands2/subsystem.py b/commands2/subsystem.py index 56724ebd..3cb2792b 100644 --- a/commands2/subsystem.py +++ b/commands2/subsystem.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 1144115da01f Subsystem.java # Don't import stuff from the package here, it'll cause a circular import from __future__ import annotations @@ -19,14 +20,12 @@ class Subsystem(Sendable): provide methods through which they can be used by Commands. Subsystems are used by the CommandScheduler's resource management system to ensure multiple robot actions are not "fighting" over the same hardware; Commands that use a subsystem should include that subsystem in - their Command#getRequirements() method, and resources used within a subsystem should + their :func:`commands2.Command.getRequirements` method, and resources used within a subsystem should generally remain encapsulated and not be shared by other parts of the robot. - Subsystems must be registered with the scheduler with the {@link - CommandScheduler#registerSubsystem(Subsystem...)} method in order for the {@link - Subsystem#periodic()} method to be called. It is recommended that this method be called from the - constructor of users' Subsystem implementations. The SubsystemBase class offers a simple - base for user implementations that handles this. + Subsystems must be registered with the scheduler with the :func:`commands2.CommandScheduler.registerSubsystem` + method in order for the :func:`.periodic` method to be called. It is recommended that this method be called from the + constructor of users' Subsystem implementations. """ def __new__(cls, *arg, **kwargs) -> Self: @@ -47,14 +46,14 @@ def periodic(self) -> None: This method is called periodically by the CommandScheduler. Useful for updating subsystem-specific state that you don't want to offload to a Command. Teams should try to be consistent within their own codebases about which responsibilities will be handled by - Commands, and which will be handled here.""" + Commands, and which will be handled here. + """ pass def simulationPeriodic(self) -> None: """ This method is called periodically by the CommandScheduler. Useful for updating - subsystem-specific state that needs to be maintained for simulations, such as for updating - edu.wpi.first.wpilibj.simulation classes and setting simulated sensor readings. + subsystem-specific state that needs to be maintained for simulations, such as for updating simulation classes and setting simulated sensor readings. """ pass @@ -62,11 +61,11 @@ def setDefaultCommand(self, command: Command) -> None: """ Sets the default Command of the subsystem. The default command will be automatically scheduled when no other commands are scheduled that require the subsystem. Default commands - should generally not end on their own, i.e. their Command#isFinished() method should - always return false. Will automatically register this subsystem with the {@link - CommandScheduler}. + should generally not end on their own, i.e. their :func:`commands2.Command.isFinished` method should + always return false. Will automatically register this subsystem with the CommandScheduler. - :param defaultCommand: the default command to associate with this subsystem""" + :param defaultCommand: the default command to associate with this subsystem + """ from .commandscheduler import CommandScheduler CommandScheduler.getInstance().setDefaultCommand(self, command) @@ -74,35 +73,48 @@ def setDefaultCommand(self, command: Command) -> None: def removeDefaultCommand(self) -> None: """ Removes the default command for the subsystem. This will not cancel the default command if it - is currently running.""" + is currently running. + """ CommandScheduler.getInstance().removeDefaultCommand(self) def getDefaultCommand(self) -> Optional[Command]: """ - Gets the default command for this subsystem. Returns null if no default command is currently + Gets the default command for this subsystem. Returns None if no default command is currently associated with the subsystem. - :returns: the default command associated with this subsystem""" + :returns: the default command associated with this subsystem + """ from .commandscheduler import CommandScheduler return CommandScheduler.getInstance().getDefaultCommand(self) def getCurrentCommand(self) -> Optional[Command]: """ - Returns the command currently running on this subsystem. Returns null if no command is + Returns the command currently running on this subsystem. Returns None if no command is currently scheduled that requires this subsystem. - :returns: the scheduled command currently requiring this subsystem""" + :returns: the scheduled command currently requiring this subsystem + """ from .commandscheduler import CommandScheduler return CommandScheduler.getInstance().requiring(self) + def register(self): + """ + Registers this subsystem with the :class:`.CommandScheduler`, allowing its + :func:`.periodic` method to be called when the scheduler runs. + """ + from .commandscheduler import CommandScheduler + + return CommandScheduler.getInstance().registerSubsystem(self) + def runOnce(self, action: Callable[[], None]) -> Command: """ Constructs a command that runs an action once and finishes. Requires this subsystem. :param action: the action to run - :return: the command""" + :return: the command + """ from .cmd import runOnce return runOnce(action, self) @@ -125,7 +137,8 @@ def startEnd(self, start: Callable[[], None], end: Callable[[], None]) -> Comman :param start: the action to run on start :param end: the action to run on interrupt - :returns: the command""" + :returns: the command + """ from .cmd import startEnd return startEnd(start, end, self) @@ -137,21 +150,53 @@ def runEnd(self, run: Callable[[], None], end: Callable[[], None]) -> Command: :param run: the action to run every iteration :param end: the action to run on interrupt - :returns: the command""" + :returns: the command + """ from .cmd import runEnd return runEnd(run, end, self) + # + # From SubsystemBase + # + def getName(self) -> str: + """ + Gets the name of this Subsystem. + + :returns: Name + """ return SendableRegistry.getName(self) def setName(self, name: str) -> None: + """ + Set the name of this Subsystem. + """ SendableRegistry.setName(self, name) def getSubsystem(self) -> str: + """ + Gets the subsystem name of this Subsystem. + + :returns: Subsystem name + """ return SendableRegistry.getSubsystem(self) + def setSubsystem(self, subsystem: str): + """ + Sets the subsystem name of this Subsystem. + + :param subsystem: subsystem name + """ + SendableRegistry.setSubsystem(self, subsystem) + def addChild(self, name: str, child: Sendable) -> None: + """ + Associates a :class:`wpiutil.Sendable` with this Subsystem. Also update the child's name. + + :param name: name to give child + :param child: sendable + """ SendableRegistry.addLW(child, self.getSubsystem(), name) def initSendable(self, builder: SendableBuilder) -> None: diff --git a/commands2/swervecontrollercommand.py b/commands2/swervecontrollercommand.py index fac15df7..a1e5a697 100644 --- a/commands2/swervecontrollercommand.py +++ b/commands2/swervecontrollercommand.py @@ -1,9 +1,10 @@ +# validated: 2024-01-20 DS 192a28af4731 SwerveControllerCommand.java # 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. from __future__ import annotations from typing import Callable, Optional, Union, Tuple, Sequence -from typing_extensions import overload + from wpimath.controller import ( HolonomicDriveController, ) @@ -24,10 +25,11 @@ class SwerveControllerCommand(Command): """ - A command that uses two PID controllers (PIDController) and a ProfiledPIDController - (ProfiledPIDController) to follow a trajectory (Trajectory) with a swerve drive. + A command that uses two PID controllers (:class:`wpimath.controller.PIDController`) + and a HolonomicDriveController (:class:`wpimath.controller.HolonomicDriveController`) + to follow a trajectory (:class:`wpimath.trajectory.Trajectory`) with a swerve drive. - This command outputs the raw desired Swerve Module States (SwerveModuleState) in an + This command outputs the raw desired Swerve Module States (:class:`wpimath.kinematics.SwerveModuleState`) in an array. The desired wheel and module rotation velocities should be taken from those and used in velocity PIDs. diff --git a/commands2/sysid/sysidroutine.py b/commands2/sysid/sysidroutine.py index b7d998a6..72036b75 100644 --- a/commands2/sysid/sysidroutine.py +++ b/commands2/sysid/sysidroutine.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 707cb061057f sysid/SysIdRoutine.java from dataclasses import dataclass from enum import Enum diff --git a/commands2/timedcommandrobot.py b/commands2/timedcommandrobot.py index 4a4b3bc5..a012e4f8 100644 --- a/commands2/timedcommandrobot.py +++ b/commands2/timedcommandrobot.py @@ -1,3 +1,4 @@ +# notrack from wpilib import TimedRobot from .commandscheduler import CommandScheduler diff --git a/commands2/trapezoidprofilesubsystem.py b/commands2/trapezoidprofilesubsystem.py index 2b40fd53..cf6af638 100644 --- a/commands2/trapezoidprofilesubsystem.py +++ b/commands2/trapezoidprofilesubsystem.py @@ -1,3 +1,4 @@ +# validated: 2024-01-20 DS 192a28af4731 TrapezoidProfileSubsystem.java # 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. @@ -5,10 +6,13 @@ from typing import Union -from .subsystem import Subsystem +from wpimath import units from wpimath.trajectory import TrapezoidProfile +from .subsystem import Subsystem + + class TrapezoidProfileSubsystem(Subsystem): """ A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies @@ -19,7 +23,7 @@ def __init__( self, constraints: TrapezoidProfile.Constraints, initial_position: float = 0.0, - period: float = 0.02, + period: units.seconds = 0.02, ): """ Creates a new TrapezoidProfileSubsystem. @@ -49,8 +53,8 @@ def setGoal(self, goal: Union[TrapezoidProfile.State, float]): Sets the goal state for the subsystem. Goal velocity assumed to be zero. :param goal: The goal position for the subsystem's motion profile. The goal - can either be a `TrapezoidProfile.State` or `float`. If float is provided, - the assumed velocity for the goal will be 0. + can either be a `TrapezoidProfile.State` or `float`. If float is provided, + the assumed velocity for the goal will be 0. """ # If we got a float, instantiate the state if isinstance(goal, (float, int)): diff --git a/commands2/util.py b/commands2/util.py index 617a5762..e0178c94 100644 --- a/commands2/util.py +++ b/commands2/util.py @@ -1,3 +1,4 @@ +# notrack from __future__ import annotations from typing import Iterable, List, Tuple, Union diff --git a/commands2/waitcommand.py b/commands2/waitcommand.py index 4f7ed04d..87c0337e 100644 --- a/commands2/waitcommand.py +++ b/commands2/waitcommand.py @@ -1,27 +1,31 @@ +# validated: 2024-01-20 DS f29a7d2e501b WaitCommand.java from __future__ import annotations from wpilib import Timer +from wpimath import units +from wpiutil import SendableBuilder from .command import Command -from .commandgroup import * class WaitCommand(Command): """ - A command that does nothing but takes a specified amount of time to finish.""" + A command that does nothing but takes a specified amount of time to finish. + """ - def __init__(self, seconds: float): + def __init__(self, seconds: units.seconds): """ Creates a new WaitCommand. This command will do nothing, and end after the specified duration. - :param seconds: the time to wait, in seconds""" + :param seconds: the time to wait, in seconds + """ super().__init__() self._duration = seconds self._timer = Timer() + self.setName(f"{self.getName()}: {seconds}") def initialize(self): - self._timer.reset() - self._timer.start() + self._timer.restart() def end(self, interrupted: bool): self._timer.stop() @@ -31,3 +35,7 @@ def isFinished(self) -> bool: def runsWhenDisabled(self) -> bool: return True + + def initSendable(self, builder: SendableBuilder) -> None: + super().initSendable(builder) + builder.addDoubleProperty("duration", lambda: self._duration, lambda _: None) diff --git a/commands2/waituntilcommand.py b/commands2/waituntilcommand.py index 34839c9f..d9752677 100644 --- a/commands2/waituntilcommand.py +++ b/commands2/waituntilcommand.py @@ -1,8 +1,10 @@ +# validated: 2024-01-20 DS aaea85ff1656 WaitUntilCommand.java from __future__ import annotations from typing import Callable, overload from wpilib import Timer +from wpimath import units from .command import Command from .util import format_args_kwargs @@ -11,7 +13,8 @@ class WaitUntilCommand(Command): """ A command that does nothing but ends after a specified match time or condition. Useful for - CommandGroups.""" + CommandGroups. + """ _condition: Callable[[], bool] @@ -20,11 +23,12 @@ def __init__(self, condition: Callable[[], bool]): """ Creates a new WaitUntilCommand that ends after a given condition becomes true. - :param condition: the condition to determine when to end""" + :param condition: the condition to determine when to end + """ ... @overload - def __init__(self, time: float): + def __init__(self, time: units.seconds): """ Creates a new WaitUntilCommand that ends after a given match time. @@ -32,13 +36,15 @@ def __init__(self, time: float): guarantee that the time at which the action is performed will be judged to be legal by the referees. When in doubt, add a safety factor or time the action manually. - :param time: the match time at which to end, in seconds""" + :param time: the match time at which to end, in seconds + """ ... def __init__(self, *args, **kwargs): super().__init__() def init_condition(condition: Callable[[], bool]) -> None: + assert callable(condition) self._condition = condition def init_time(time: float) -> None: @@ -61,8 +67,8 @@ def init_time(time: float) -> None: f""" TypeError: WaitUntilCommand(): incompatible function arguments. The following argument types are supported: 1. (self: WaitUntilCommand, condition: () -> bool) - 2. (self: WaitUntilCommand, time: float) - + 2. (self: WaitUntilCommand, time: wpimath.units.seconds) + Invoked with: {format_args_kwargs(self, *args, **kwargs)} """ ) diff --git a/commands2/wrappercommand.py b/commands2/wrappercommand.py index 43c50372..45c98b75 100644 --- a/commands2/wrappercommand.py +++ b/commands2/wrappercommand.py @@ -1,9 +1,9 @@ +# validated: 2024-01-20 DS ad0859a8c9ec WrapperCommand.java from __future__ import annotations from typing import Set from .command import Command, InterruptionBehavior -from .commandgroup import * from .commandscheduler import CommandScheduler @@ -14,14 +14,16 @@ class WrapperCommand(Command): The rules for command compositions apply: command instances that are passed to it cannot be added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" + subsystems its components require. + """ def __init__(self, command: Command): """ Wrap a command. - :param command: the command being wrapped. Trying to directly schedule this command or add it to - a composition will throw an exception.""" + :param command: the command being wrapped. Trying to directly schedule this + command or add it to a composition will throw an exception. + """ super().__init__() CommandScheduler.getInstance().registerComposedCommands([command]) @@ -29,11 +31,15 @@ def __init__(self, command: Command): self.setName(self._command.getName()) def initialize(self): - """The initial subroutine of a command. Called once when the command is initially scheduled.""" + """ + The initial subroutine of a command. Called once when the command is initially scheduled. + """ self._command.initialize() def execute(self): - """The main body of a command. Called repeatedly while the command is scheduled.""" + """ + The main body of a command. Called repeatedly while the command is scheduled. + """ self._command.execute() def end(self, interrupted: bool): @@ -41,10 +47,11 @@ def end(self, interrupted: bool): The action to take when the command ends. Called when either the command finishes normally, or when it interrupted/canceled. - Do not schedule commands here that share requirements with this command. Use {@link - #andThen(Command...)} instead. + Do not schedule commands here that share requirements with this command. Use + :func:`commands2.Command.andThen` instead. - :param interrupted: whether the command was interrupted/canceled""" + :param interrupted: whether the command was interrupted/canceled + """ self._command.end(interrupted) def isFinished(self) -> bool: @@ -52,7 +59,8 @@ def isFinished(self) -> bool: Whether the command has finished. Once a command finishes, the scheduler will call its end() method and un-schedule it. - :returns: whether the command has finished.""" + :returns: whether the command has finished. + """ return self._command.isFinished() def getRequirements(self) -> Set: @@ -65,7 +73,8 @@ def getRequirements(self) -> 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. - :returns: the set of subsystems that are required""" + :returns: the set of subsystems that are required + """ return self._command.getRequirements() def runsWhenDisabled(self) -> bool: @@ -73,7 +82,8 @@ def runsWhenDisabled(self) -> bool: Whether the given command should run when the robot is disabled. Override to return true if the command should run when disabled. - :returns: whether the command should run when the robot is disabled""" + :returns: whether the command should run when the robot is disabled + """ return self._command.runsWhenDisabled() def getInterruptionBehavior(self) -> InterruptionBehavior: diff --git a/docs/.gitignore b/docs/.gitignore index 4080f00f..22f03be7 100644 --- a/docs/.gitignore +++ b/docs/.gitignore @@ -8,4 +8,8 @@ /commands2.rst /commands2 /commands2.button.rst -/commands2.button \ No newline at end of file +/commands2.button +/commands2.cmd.rst +/commands2.cmd/ +/commands2.sysid.rst +/commands2.sysid/ diff --git a/docs/api.rst b/docs/api.rst index faf88f70..2694d563 100644 --- a/docs/api.rst +++ b/docs/api.rst @@ -22,3 +22,4 @@ using this pattern, we recommend that you consult the Java version of the commands2 commands2.button commands2.cmd + commands2.sysid diff --git a/docs/conf.py b/docs/conf.py index 0f2e83a8..28dccc74 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -8,6 +8,8 @@ import os from os.path import abspath, dirname +from importlib.metadata import version as get_version + # Project must be built+installed to generate docs import commands2 @@ -51,25 +53,15 @@ # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. -# -# The short X.Y version. -version = ".".join(commands2.__version__.split(".")[:2]) -# The full version, including alpha/beta/rc tags. -release = commands2.__version__ + +release: str = get_version("robotpy-commands-v2") +version: str = ".".join(release.split(".")[:2]) autoclass_content = "both" intersphinx_mapping = { - "networktables": ( - f"https://robotpy.readthedocs.io/projects/pynetworktables/en/{rtd_version}/", - None, - ), - "wpilib": ( - f"https://robotpy.readthedocs.io/projects/wpilib/en/{rtd_version}/", - None, - ), - "wpimath": ( - f"https://robotpy.readthedocs.io/projects/wpimath/en/{rtd_version}/", + "robotpy": ( + f"https://robotpy.readthedocs.io/projects/robotpy/en/{rtd_version}/", None, ), } @@ -83,13 +75,7 @@ # -- Options for HTML output ---------------------------------------------- -if not on_rtd: # only import and set the theme if we're building docs locally - import sphinx_rtd_theme - - html_theme = "sphinx_rtd_theme" - html_theme_path = [sphinx_rtd_theme.get_html_theme_path()] -else: - html_theme = "default" +html_theme = "sphinx_rtd_theme" # Output file base name for HTML help builder. htmlhelp_basename = "RobotPyCommandDoc" @@ -156,3 +142,4 @@ gen_package(root, "commands2") gen_package(root, "commands2.button") gen_package(root, "commands2.cmd") +gen_package(root, "commands2.sysid") diff --git a/docs/requirements.txt b/docs/requirements.txt index 10a9f290..e09966cf 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -2,4 +2,4 @@ sphinx sphinx-rtd-theme robotpy-sphinx-plugin --e docs \ No newline at end of file +-e . \ No newline at end of file diff --git a/docs/setup.py b/docs/setup.py deleted file mode 100644 index 1786ba89..00000000 --- a/docs/setup.py +++ /dev/null @@ -1,23 +0,0 @@ -# this is a dirty hack to convince readthedocs to install a specific -# version of our software. We assume that this will always be triggered -# on a specific version - -from setuptools import setup - -try: - from setuptools_scm import get_version -except ImportError: - import subprocess - - def get_version(*args, **kwargs): - return subprocess.check_output(["git", "describe", "--tags"], encoding="utf-8") - - -package = "robotpy-commands-v2" -version = get_version(root="..", relative_to=__file__) - -setup( - name="dummy-package", - version="1.0", - install_requires=["%s==%s" % (package, version)], -) diff --git a/setup.py b/setup.py index efa8c01c..d2a7366f 100644 --- a/setup.py +++ b/setup.py @@ -12,7 +12,7 @@ url="https://github.com/robotpy/robotpy-commands-v2", packages=["commands2"], install_requires=[ - "wpilib<2025,>=2024.0.0b2", + "wpilib<2025,>=2024.2.1.2", "typing_extensions>=4.1.0,<5", ], license="BSD-3-Clause", diff --git a/tests/compositiontestbase.py b/tests/compositiontestbase.py index ca1bfad4..5739cb81 100644 --- a/tests/compositiontestbase.py +++ b/tests/compositiontestbase.py @@ -8,147 +8,159 @@ from util import * -if not IS_OLD_COMMANDS: - class SingleCompositionTestBase: - def composeSingle(self, member: commands2.Command): - raise NotImplementedError +class SingleCompositionTestBase: + def composeSingle(self, member: commands2.Command): + raise NotImplementedError - @pytest.mark.parametrize( - "interruptionBehavior", - [ + @pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelSelf, + commands2.InterruptionBehavior.kCancelIncoming, + ], + ) + def test_interruptible(self, interruptionBehavior: commands2.InterruptionBehavior): + command = self.composeSingle( + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + interruptionBehavior + ) + ) + assert command.getInterruptionBehavior() == interruptionBehavior + + @pytest.mark.parametrize("runsWhenDisabled", [True, False]) + def test_runWhenDisabled(self, runsWhenDisabled: bool): + command = self.composeSingle( + commands2.WaitUntilCommand(lambda: False).ignoringDisable(runsWhenDisabled) + ) + assert command.runsWhenDisabled() == runsWhenDisabled + + def test_command_in_other_composition(self): + command = commands2.InstantCommand() + wrapped = commands2.WrapperCommand(command) + with pytest.raises(commands2.IllegalCommandUse): + self.composeSingle(command) + + def test_command_in_multiple_compositions(self): + command = commands2.InstantCommand() + self.composeSingle(command) + with pytest.raises(commands2.IllegalCommandUse): + self.composeSingle(command) + + def test_compose_then_schedule(self, scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + self.composeSingle(command) + with pytest.raises(commands2.IllegalCommandUse): + scheduler.schedule(command) + + def test_schedule_then_compose(self, scheduler: commands2.CommandScheduler): + command = commands2.RunCommand(lambda: None) + scheduler.schedule(command) + with pytest.raises(commands2.IllegalCommandUse): + self.composeSingle(command) + + +class MultiCompositionTestBase(SingleCompositionTestBase): + def compose(self, *members: commands2.Command): + raise NotImplementedError + + def composeSingle(self, member: commands2.Command): + return self.compose(member) + + @pytest.mark.parametrize( + "expected,command1,command2,command3", + [ + pytest.param( commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + id="AllCancelSelf", + ), + pytest.param( commands2.InterruptionBehavior.kCancelIncoming, - ], - ) - def test_interruptible( - self, interruptionBehavior: commands2.InterruptionBehavior - ): - command = self.composeSingle( commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - interruptionBehavior - ) - ) - assert command.getInterruptionBehavior() == interruptionBehavior - - @pytest.mark.parametrize("runsWhenDisabled", [True, False]) - def test_runWhenDisabled(self, runsWhenDisabled: bool): - command = self.composeSingle( - commands2.WaitUntilCommand(lambda: False).ignoringDisable( - runsWhenDisabled - ) - ) - assert command.runsWhenDisabled() == runsWhenDisabled - - class MultiCompositionTestBase(SingleCompositionTestBase): - def compose(self, *members: commands2.Command): - raise NotImplementedError - - def composeSingle(self, member: commands2.Command): - return self.compose(member) - - @pytest.mark.parametrize( - "expected,command1,command2,command3", - [ - pytest.param( - commands2.InterruptionBehavior.kCancelSelf, - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - id="AllCancelSelf", + commands2.InterruptionBehavior.kCancelIncoming ), - pytest.param( - commands2.InterruptionBehavior.kCancelIncoming, - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - id="AllCancelIncoming", + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming ), - pytest.param( - commands2.InterruptionBehavior.kCancelSelf, - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - id="TwoCancelSelfOneIncoming", + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming ), - pytest.param( - commands2.InterruptionBehavior.kCancelSelf, - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - id="TwoCancelIncomingOneSelf", + id="AllCancelIncoming", + ), + pytest.param( + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf ), - ], - ) - def test_interruptible(self, expected, command1, command2, command3): - command = self.compose(command1, command2, command3) - assert command.getInterruptionBehavior() == expected - - @pytest.mark.parametrize( - "expected,command1,command2,command3", - [ - pytest.param( - False, - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - id="AllFalse", + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf ), - pytest.param( - True, - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - id="AllTrue", + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming ), - pytest.param( - False, - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - id="TwoTrueOneFalse", + id="TwoCancelSelfOneIncoming", + ), + pytest.param( + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming ), - pytest.param( - False, - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - id="TwoFalseOneTrue", + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming ), - ], - ) - def test_runWhenDisabled(self, expected, command1, command2, command3): - command = self.compose(command1, command2, command3) - assert command.runsWhenDisabled() == expected - -else: - - class SingleCompositionTestBase: - ... + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + id="TwoCancelIncomingOneSelf", + ), + ], + ) + def test_interruptible(self, expected, command1, command2, command3): + command = self.compose(command1, command2, command3) + assert command.getInterruptionBehavior() == expected - class MultiCompositionTestBase: - ... + @pytest.mark.parametrize( + "expected,command1,command2,command3", + [ + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + id="AllFalse", + ), + pytest.param( + True, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + id="AllTrue", + ), + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + id="TwoTrueOneFalse", + ), + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + id="TwoFalseOneTrue", + ), + ], + ) + def test_runWhenDisabled(self, expected, command1, command2, command3): + command = self.compose(command1, command2, command3) + assert command.runsWhenDisabled() == expected diff --git a/tests/test_command_decorators.py b/tests/test_command_decorators.py index 9c89ff9c..cd047717 100644 --- a/tests/test_command_decorators.py +++ b/tests/test_command_decorators.py @@ -33,7 +33,6 @@ def test_until(scheduler: commands2.CommandScheduler): assert not command.isScheduled() -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_only_while(scheduler: commands2.CommandScheduler): condition = OOBoolean(True) command = commands2.WaitCommand(10).onlyWhile(condition) @@ -45,7 +44,6 @@ def test_only_while(scheduler: commands2.CommandScheduler): assert not command.isScheduled() -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_ignoringDisable(scheduler: commands2.CommandScheduler): command = commands2.RunCommand(lambda: None).ignoringDisable(True) DriverStationSim.setEnabled(False) @@ -127,17 +125,6 @@ def test_raceWith(scheduler: commands2.CommandScheduler): assert not group.isScheduled() -def test_perpetually(scheduler: commands2.CommandScheduler): - command = commands2.InstantCommand() - perpetual = command.perpetually() - scheduler.schedule(perpetual) - scheduler.run() - scheduler.run() - scheduler.run() - assert perpetual.isScheduled() - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_unless(scheduler: commands2.CommandScheduler): unlessCondition = OOBoolean(True) hasRunCondition = OOBoolean(False) @@ -155,7 +142,6 @@ def test_unless(scheduler: commands2.CommandScheduler): assert hasRunCondition == True -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_onlyIf(scheduler: commands2.CommandScheduler): onlyIfCondition = OOBoolean(False) hasRunCondition = OOBoolean(False) @@ -173,7 +159,6 @@ def test_onlyIf(scheduler: commands2.CommandScheduler): assert hasRunCondition == True -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_finallyDo(scheduler: commands2.CommandScheduler): first = OOInteger(0) second = OOInteger(0) @@ -193,7 +178,6 @@ def test_finallyDo(scheduler: commands2.CommandScheduler): assert second == 2 -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_handleInterrupt(scheduler: commands2.CommandScheduler): first = OOInteger(0) second = OOInteger(0) @@ -214,7 +198,6 @@ def test_handleInterrupt(scheduler: commands2.CommandScheduler): assert second == 2 -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_withName(scheduler: commands2.CommandScheduler): command = commands2.InstantCommand() name = "Named" diff --git a/tests/test_command_requirements.py b/tests/test_command_requirements.py index 0215e0bc..c25f2d20 100644 --- a/tests/test_command_requirements.py +++ b/tests/test_command_requirements.py @@ -34,7 +34,6 @@ def test_requirementInterrupt(scheduler: commands2.CommandScheduler): assert interrupter.isScheduled() -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_requirementUninterruptible(scheduler: commands2.CommandScheduler): requirement = commands2.Subsystem() notInterrupted = commands2.RunCommand( diff --git a/tests/test_commandgroup_error.py b/tests/test_commandgroup_error.py index 36008f1a..ed4beac8 100644 --- a/tests/test_commandgroup_error.py +++ b/tests/test_commandgroup_error.py @@ -28,11 +28,10 @@ def test_commandInGroupExternallyScheduled(scheduler: commands2.CommandScheduler scheduler.schedule(command1) -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_redecoratedCommandError(scheduler: commands2.CommandScheduler): command = commands2.InstantCommand() command.withTimeout(10).until(lambda: False) with pytest.raises(commands2.IllegalCommandUse): command.withTimeout(10) - scheduler.removeComposedCommands([command]) + scheduler.removeComposedCommand(command) command.withTimeout(10) diff --git a/tests/test_notifier_command.py b/tests/test_notifier_command.py index 3022c20c..5b6d5670 100644 --- a/tests/test_notifier_command.py +++ b/tests/test_notifier_command.py @@ -9,7 +9,6 @@ import pytest -@pytest.mark.skip(reason="NotifierCommand is broken") def test_notifierCommandScheduler(scheduler: commands2.CommandScheduler): with ManualSimTime() as sim: counter = OOInteger(0) diff --git a/tests/test_perpetualcommand.py b/tests/test_perpetualcommand.py deleted file mode 100644 index 55284ff1..00000000 --- a/tests/test_perpetualcommand.py +++ /dev/null @@ -1,18 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_perpetualCommandSchedule(scheduler: commands2.CommandScheduler): - command = commands2.PerpetualCommand(commands2.InstantCommand()) - - scheduler.schedule(command) - scheduler.run() - - assert scheduler.isScheduled(command) diff --git a/tests/test_proxycommand.py b/tests/test_proxycommand.py index 5f1add10..496ffe9a 100644 --- a/tests/test_proxycommand.py +++ b/tests/test_proxycommand.py @@ -9,7 +9,6 @@ import pytest -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_proxyCommandSchedule(scheduler: commands2.CommandScheduler): command1 = commands2.Command() start_spying_on(command1) @@ -21,7 +20,6 @@ def test_proxyCommandSchedule(scheduler: commands2.CommandScheduler): verify(command1).schedule() -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") def test_proxyCommandEnd(scheduler: commands2.CommandScheduler): cond = OOBoolean() diff --git a/tests/test_robotdisabledcommand.py b/tests/test_robotdisabledcommand.py index d34915f3..120dc070 100644 --- a/tests/test_robotdisabledcommand.py +++ b/tests/test_robotdisabledcommand.py @@ -6,9 +6,6 @@ if TYPE_CHECKING: from .util import * -if IS_OLD_COMMANDS: - import commands2.cmd - import pytest from wpilib import RobotState diff --git a/tests/test_scheduler.py b/tests/test_scheduler.py index 4ee7f32c..de8fdae6 100644 --- a/tests/test_scheduler.py +++ b/tests/test_scheduler.py @@ -35,6 +35,69 @@ def test_schedulerInterruptLambda(scheduler: commands2.CommandScheduler): assert counter == 1 +def test_scheduler_interrupt_no_cause_lambda(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + def on_interrupt(interrupted, cause): + assert cause is None + counter.incrementAndGet() + + scheduler.onCommandInterruptWithCause(on_interrupt) + + command = commands2.cmd.run(lambda: {}) + + scheduler.schedule(command) + scheduler.cancel(command) + + assert counter.get() == 1 + + +def test_scheduler_interrupt_cause_lambda(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + subsystem = commands2.Subsystem() + command = subsystem.run(lambda: None) + interruptor = subsystem.runOnce(lambda: None) + + def on_interrupt(interrupted, cause): + assert cause is interruptor + counter.incrementAndGet() + + scheduler.onCommandInterruptWithCause(on_interrupt) + + scheduler.schedule(command) + scheduler.schedule(interruptor) + + assert counter.get() == 1 + + +def test_scheduler_interrupt_cause_lambda_in_run_loop( + scheduler: commands2.CommandScheduler, +): + counter = OOInteger() + + subsystem = commands2.Subsystem() + command = subsystem.run(lambda: None) + interruptor = subsystem.runOnce(lambda: None) + # This command will schedule interruptor in execute() inside the run loop + interruptor_scheduler = commands2.cmd.runOnce( + lambda: scheduler.schedule(interruptor) + ) + + def on_interrupt(interrupted, cause): + assert cause is interruptor + counter.incrementAndGet() + + scheduler.onCommandInterruptWithCause(on_interrupt) + + scheduler.schedule(command) + scheduler.schedule(interruptor_scheduler) + + scheduler.run() + + assert counter.get() == 1 + + def test_unregisterSubsystem(scheduler: commands2.CommandScheduler): system = commands2.Subsystem() scheduler.registerSubsystem(system) @@ -44,7 +107,11 @@ def test_unregisterSubsystem(scheduler: commands2.CommandScheduler): def test_schedulerCancelAll(scheduler: commands2.CommandScheduler): counter = OOInteger() + def on_interrupt(command, interruptor): + assert interruptor is None + scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet()) + scheduler.onCommandInterruptWithCause(on_interrupt) command = commands2.WaitCommand(10) command2 = commands2.WaitCommand(10) diff --git a/tests/util.py b/tests/util.py index 0c1a835f..03318403 100644 --- a/tests/util.py +++ b/tests/util.py @@ -29,43 +29,6 @@ def full_subclass_of(cls: Type[Y]) -> Type[Y]: return retlist[0] -################### -# Compat for wrapped commands -IS_OLD_COMMANDS = False -try: - if commands2.__version__ == "2023.4.3.0": # type: ignore - IS_OLD_COMMANDS = True -except AttributeError: - pass - -if IS_OLD_COMMANDS: - # not needed for pure but works in pure - import commands2.button - - # In Java, Trigger is in the same package as Button - # I did rexport it in commands so using - # the incorrect `commands2.Trigger` instead of `commands2.button.Trigger` works - commands2.button.Trigger = commands2.Trigger - - # I moved this so its not a nested Enum. - # The old one is still there for compat - commands2.InterruptionBehavior = commands2.Command.InterruptionBehavior - - commands2.Command = commands2.CommandBase - - for name in dir(commands2): - if name == "CommandScheduler": - continue - value = getattr(commands2, name) - if inspect.isclass(value): - setattr(commands2, name, full_subclass_of(value)) - - commands2.IllegalCommandUse = RuntimeError - - -################### - - class ManualSimTime: def __enter__(self) -> "ManualSimTime": pauseTiming()