From ab7dd6661f0ded23be07012defa2b170dd280761 Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Mon, 22 Jan 2024 03:39:13 -0500 Subject: [PATCH] Temporary commit with command scheduler progress - Need to add tests, make other decisions --- commands2/commandscheduler.py | 326 ++++++++++++++++++++++--------- tests/test_command_schedule.py | 17 ++ tests/test_commandgroup_error.py | 2 +- 3 files changed, 250 insertions(+), 95 deletions(-) diff --git a/commands2/commandscheduler.py b/commands2/commandscheduler.py index 72458ff6..9d4171f3 100644 --- a/commands2/commandscheduler.py +++ b/commands2/commandscheduler.py @@ -1,23 +1,32 @@ +# validated: 2024-01-23 DS 8aeee0362672 CommandScheduler.java from __future__ import annotations 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 ( + DriverStation, + LiveWindow, + RobotBase, + RobotState, + TimedRobot, + Watchdog, + reportWarning, +) from wpilib.event import EventLoop +from wpiutil import Sendable, SendableBuilder, SendableRegistry from .command import Command, InterruptionBehavior from .exceptions import IllegalCommandUse from .subsystem import Subsystem -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 +38,7 @@ def __new__(cls) -> CommandScheduler: return cls._instance @staticmethod - def getInstance() -> "CommandScheduler": + def getInstance() -> CommandScheduler: """ Returns the Scheduler instance. @@ -46,25 +55,40 @@ def resetInstance() -> None: inst = CommandScheduler._instance if inst: inst._defaultButtonLoop.clear() + LiveWindow.setEnabledCallback(lambda: None) + LiveWindow.setDisabledCallback(lambda: None) 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, Exception] = {} + + # 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]] = [] @@ -72,7 +96,10 @@ def __init__(self) -> 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 +107,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 +149,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 +162,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 +173,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 +220,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 +307,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 +323,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 +376,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 +394,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) + + 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 +476,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 +485,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 +494,7 @@ def onCommandInterrupt(self, action: Callable[[Command], Any]) -> None: :param action: the action to perform """ + assert callable(action) self._interruptActions.append(action) def onCommandFinish(self, action: Callable[[Command], Any]) -> None: @@ -416,6 +503,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 +512,72 @@ 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) + try: + raise Exception("originally composed here") + except Exception as e: + for cmd in cmds: + self._composedCommands[cmd] = e 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. """ - 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: + exception = self._composedCommands.get(command) + if exception is not None: + raise IllegalCommandUse( + "Commands that have been composed may not be added to another composition or scheduled individually" + ) from exception + + 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 +586,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 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 getComposedCommands(self) -> Set[Command]: - return self._composedCommands + 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/tests/test_command_schedule.py b/tests/test_command_schedule.py index 62d21823..4d70a007 100644 --- a/tests/test_command_schedule.py +++ b/tests/test_command_schedule.py @@ -88,3 +88,20 @@ def test_notScheduledCancel(scheduler: commands2.CommandScheduler): command = commands2.Command() scheduler.cancel(command) + + +# def test_scheduler_interrupt_no_cause_lambda(scheduler: commands2.CommandScheduler): +# counter = OOInteger() + +# def on_interrupt(interrupted, cause): +# assert cause is None +# counter.incrementAndGet() + +# scheduler.onCommandInterrupt(on_interrupt) + +# command = commands2.cmd.run(lambda: {}) + +# scheduler.schedule(command) +# scheduler.cancel(command) + +# assert counter.get() == 1 diff --git a/tests/test_commandgroup_error.py b/tests/test_commandgroup_error.py index 36008f1a..d75d6ccb 100644 --- a/tests/test_commandgroup_error.py +++ b/tests/test_commandgroup_error.py @@ -34,5 +34,5 @@ def test_redecoratedCommandError(scheduler: commands2.CommandScheduler): command.withTimeout(10).until(lambda: False) with pytest.raises(commands2.IllegalCommandUse): command.withTimeout(10) - scheduler.removeComposedCommands([command]) + scheduler.removeComposedCommand(command) command.withTimeout(10)