001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj2.command; 006 007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; 008 009import edu.wpi.first.hal.FRCNetComm.tInstances; 010import edu.wpi.first.hal.FRCNetComm.tResourceType; 011import edu.wpi.first.hal.HAL; 012import edu.wpi.first.util.sendable.Sendable; 013import edu.wpi.first.util.sendable.SendableBuilder; 014import edu.wpi.first.util.sendable.SendableRegistry; 015import edu.wpi.first.wpilibj.DriverStation; 016import edu.wpi.first.wpilibj.RobotBase; 017import edu.wpi.first.wpilibj.RobotState; 018import edu.wpi.first.wpilibj.TimedRobot; 019import edu.wpi.first.wpilibj.Watchdog; 020import edu.wpi.first.wpilibj.event.EventLoop; 021import edu.wpi.first.wpilibj.livewindow.LiveWindow; 022import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; 023import java.util.ArrayList; 024import java.util.Collection; 025import java.util.Collections; 026import java.util.Iterator; 027import java.util.LinkedHashMap; 028import java.util.LinkedHashSet; 029import java.util.List; 030import java.util.Map; 031import java.util.Optional; 032import java.util.Set; 033import java.util.WeakHashMap; 034import java.util.function.BiConsumer; 035import java.util.function.Consumer; 036 037/** 038 * The scheduler responsible for running {@link Command}s. A Command-based robot should call {@link 039 * CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands 040 * synchronously from the main loop. Subsystems should be registered with the scheduler using {@link 041 * CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link Subsystem#periodic()} 042 * methods to be called and for their default commands to be scheduled. 043 * 044 * <p>This class is provided by the NewCommands VendorDep 045 */ 046public final class CommandScheduler implements Sendable, AutoCloseable { 047 /** The Singleton Instance. */ 048 private static CommandScheduler instance; 049 050 /** 051 * Returns the Scheduler instance. 052 * 053 * @return the instance 054 */ 055 public static synchronized CommandScheduler getInstance() { 056 if (instance == null) { 057 instance = new CommandScheduler(); 058 } 059 return instance; 060 } 061 062 private static final Optional<Command> kNoInterruptor = Optional.empty(); 063 064 private final Map<Command, Exception> m_composedCommands = new WeakHashMap<>(); 065 066 // A set of the currently-running commands. 067 private final Set<Command> m_scheduledCommands = new LinkedHashSet<>(); 068 069 // A map from required subsystems to their requiring commands. Also used as a set of the 070 // currently-required subsystems. 071 private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>(); 072 073 // A map from subsystems registered with the scheduler to their default commands. Also used 074 // as a list of currently-registered subsystems. 075 private final Map<Subsystem, Command> m_subsystems = new LinkedHashMap<>(); 076 077 private final EventLoop m_defaultButtonLoop = new EventLoop(); 078 // The set of currently-registered buttons that will be polled every iteration. 079 private EventLoop m_activeButtonLoop = m_defaultButtonLoop; 080 081 private boolean m_disabled; 082 083 // Lists of user-supplied actions to be executed on scheduling events for every command. 084 private final List<Consumer<Command>> m_initActions = new ArrayList<>(); 085 private final List<Consumer<Command>> m_executeActions = new ArrayList<>(); 086 private final List<BiConsumer<Command, Optional<Command>>> m_interruptActions = new ArrayList<>(); 087 private final List<Consumer<Command>> m_finishActions = new ArrayList<>(); 088 089 // Flag and queues for avoiding ConcurrentModificationException if commands are 090 // scheduled/canceled during run 091 private boolean m_inRunLoop; 092 private final Set<Command> m_toSchedule = new LinkedHashSet<>(); 093 private final List<Command> m_toCancelCommands = new ArrayList<>(); 094 private final List<Optional<Command>> m_toCancelInterruptors = new ArrayList<>(); 095 private final Set<Command> m_endingCommands = new LinkedHashSet<>(); 096 097 private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {}); 098 099 CommandScheduler() { 100 HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler); 101 SendableRegistry.addLW(this, "Scheduler"); 102 LiveWindow.setEnabledListener( 103 () -> { 104 disable(); 105 cancelAll(); 106 }); 107 LiveWindow.setDisabledListener(this::enable); 108 } 109 110 /** 111 * Changes the period of the loop overrun watchdog. This should be kept in sync with the 112 * TimedRobot period. 113 * 114 * @param period Period in seconds. 115 */ 116 public void setPeriod(double period) { 117 m_watchdog.setTimeout(period); 118 } 119 120 @Override 121 public void close() { 122 SendableRegistry.remove(this); 123 LiveWindow.setEnabledListener(null); 124 LiveWindow.setDisabledListener(null); 125 } 126 127 /** 128 * Get the default button poll. 129 * 130 * @return a reference to the default {@link EventLoop} object polling buttons. 131 */ 132 public EventLoop getDefaultButtonLoop() { 133 return m_defaultButtonLoop; 134 } 135 136 /** 137 * Get the active button poll. 138 * 139 * @return a reference to the current {@link EventLoop} object polling buttons. 140 */ 141 public EventLoop getActiveButtonLoop() { 142 return m_activeButtonLoop; 143 } 144 145 /** 146 * Replace the button poll with another one. 147 * 148 * @param loop the new button polling loop object. 149 */ 150 public void setActiveButtonLoop(EventLoop loop) { 151 m_activeButtonLoop = 152 requireNonNullParam(loop, "loop", "CommandScheduler" + ".replaceButtonEventLoop"); 153 } 154 155 /** 156 * Initializes a given command, adds its requirements to the list, and performs the init actions. 157 * 158 * @param command The command to initialize 159 * @param requirements The command requirements 160 */ 161 private void initCommand(Command command, Set<Subsystem> requirements) { 162 m_scheduledCommands.add(command); 163 for (Subsystem requirement : requirements) { 164 m_requirements.put(requirement, command); 165 } 166 command.initialize(); 167 for (Consumer<Command> action : m_initActions) { 168 action.accept(command); 169 } 170 171 m_watchdog.addEpoch(command.getName() + ".initialize()"); 172 } 173 174 /** 175 * Schedules a command for execution. Does nothing if the command is already scheduled. If a 176 * command's requirements are not available, it will only be started if all the commands currently 177 * using those requirements have been scheduled as interruptible. If this is the case, they will 178 * be interrupted and the command will be scheduled. 179 * 180 * @param command the command to schedule. If null, no-op. 181 */ 182 private void schedule(Command command) { 183 if (command == null) { 184 DriverStation.reportWarning("Tried to schedule a null command", true); 185 return; 186 } 187 if (m_inRunLoop) { 188 m_toSchedule.add(command); 189 return; 190 } 191 192 requireNotComposed(command); 193 194 // Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't 195 // run when disabled, or the command is already scheduled. 196 if (m_disabled 197 || isScheduled(command) 198 || RobotState.isDisabled() && !command.runsWhenDisabled()) { 199 return; 200 } 201 202 Set<Subsystem> requirements = command.getRequirements(); 203 204 // Schedule the command if the requirements are not currently in-use. 205 if (Collections.disjoint(m_requirements.keySet(), requirements)) { 206 initCommand(command, requirements); 207 } else { 208 // Else check if the requirements that are in use have all have interruptible commands, 209 // and if so, interrupt those commands and schedule the new command. 210 for (Subsystem requirement : requirements) { 211 Command requiring = requiring(requirement); 212 if (requiring != null 213 && requiring.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) { 214 return; 215 } 216 } 217 for (Subsystem requirement : requirements) { 218 Command requiring = requiring(requirement); 219 if (requiring != null) { 220 cancel(requiring, Optional.of(command)); 221 } 222 } 223 initCommand(command, requirements); 224 } 225 } 226 227 /** 228 * Schedules multiple commands for execution. Does nothing for commands already scheduled. 229 * 230 * @param commands the commands to schedule. No-op on null. 231 */ 232 public void schedule(Command... commands) { 233 for (Command command : commands) { 234 schedule(command); 235 } 236 } 237 238 /** 239 * Runs a single iteration of the scheduler. The execution occurs in the following order: 240 * 241 * <p>Subsystem periodic methods are called. 242 * 243 * <p>Button bindings are polled, and new commands are scheduled from them. 244 * 245 * <p>Currently-scheduled commands are executed. 246 * 247 * <p>End conditions are checked on currently-scheduled commands, and commands that are finished 248 * have their end methods called and are removed. 249 * 250 * <p>Any subsystems not being used as requirements have their default methods started. 251 */ 252 public void run() { 253 if (m_disabled) { 254 return; 255 } 256 m_watchdog.reset(); 257 258 // Run the periodic method of all registered subsystems. 259 for (Subsystem subsystem : m_subsystems.keySet()) { 260 subsystem.periodic(); 261 if (RobotBase.isSimulation()) { 262 subsystem.simulationPeriodic(); 263 } 264 m_watchdog.addEpoch(subsystem.getName() + ".periodic()"); 265 } 266 267 // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from 268 // inside the button bindings. 269 EventLoop loopCache = m_activeButtonLoop; 270 // Poll buttons for new commands to add. 271 loopCache.poll(); 272 m_watchdog.addEpoch("buttons.run()"); 273 274 m_inRunLoop = true; 275 boolean isDisabled = RobotState.isDisabled(); 276 // Run scheduled commands, remove finished commands. 277 for (Iterator<Command> iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) { 278 Command command = iterator.next(); 279 280 if (isDisabled && !command.runsWhenDisabled()) { 281 cancel(command, kNoInterruptor); 282 continue; 283 } 284 285 command.execute(); 286 for (Consumer<Command> action : m_executeActions) { 287 action.accept(command); 288 } 289 m_watchdog.addEpoch(command.getName() + ".execute()"); 290 if (command.isFinished()) { 291 m_endingCommands.add(command); 292 command.end(false); 293 for (Consumer<Command> action : m_finishActions) { 294 action.accept(command); 295 } 296 m_endingCommands.remove(command); 297 iterator.remove(); 298 299 m_requirements.keySet().removeAll(command.getRequirements()); 300 m_watchdog.addEpoch(command.getName() + ".end(false)"); 301 } 302 } 303 m_inRunLoop = false; 304 305 // Schedule/cancel commands from queues populated during loop 306 for (Command command : m_toSchedule) { 307 schedule(command); 308 } 309 310 for (int i = 0; i < m_toCancelCommands.size(); i++) { 311 cancel(m_toCancelCommands.get(i), m_toCancelInterruptors.get(i)); 312 } 313 314 m_toSchedule.clear(); 315 m_toCancelCommands.clear(); 316 m_toCancelInterruptors.clear(); 317 318 // Add default commands for un-required registered subsystems. 319 for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) { 320 if (!m_requirements.containsKey(subsystemCommand.getKey()) 321 && subsystemCommand.getValue() != null) { 322 schedule(subsystemCommand.getValue()); 323 } 324 } 325 326 m_watchdog.disable(); 327 if (m_watchdog.isExpired()) { 328 System.out.println("CommandScheduler loop overrun"); 329 m_watchdog.printEpochs(); 330 } 331 } 332 333 /** 334 * Registers subsystems with the scheduler. This must be called for the subsystem's periodic block 335 * to run when the scheduler is run, and for the subsystem's default command to be scheduled. It 336 * is recommended to call this from the constructor of your subsystem implementations. 337 * 338 * @param subsystems the subsystem to register 339 */ 340 public void registerSubsystem(Subsystem... subsystems) { 341 for (Subsystem subsystem : subsystems) { 342 if (subsystem == null) { 343 DriverStation.reportWarning("Tried to register a null subsystem", true); 344 continue; 345 } 346 if (m_subsystems.containsKey(subsystem)) { 347 DriverStation.reportWarning("Tried to register an already-registered subsystem", true); 348 continue; 349 } 350 m_subsystems.put(subsystem, null); 351 } 352 } 353 354 /** 355 * Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic 356 * block called, and will not have its default command scheduled. 357 * 358 * @param subsystems the subsystem to un-register 359 */ 360 public void unregisterSubsystem(Subsystem... subsystems) { 361 m_subsystems.keySet().removeAll(Set.of(subsystems)); 362 } 363 364 /** 365 * Un-registers all registered Subsystems with the scheduler. All currently registered subsystems 366 * will no longer have their periodic block called, and will not have their default command 367 * scheduled. 368 */ 369 public void unregisterAllSubsystems() { 370 m_subsystems.clear(); 371 } 372 373 /** 374 * Sets the default command for a subsystem. Registers that subsystem if it is not already 375 * registered. Default commands will run whenever there is no other command currently scheduled 376 * that requires the subsystem. Default commands should be written to never end (i.e. their {@link 377 * Command#isFinished()} method should return false), as they would simply be re-scheduled if they 378 * do. Default commands must also require their subsystem. 379 * 380 * @param subsystem the subsystem whose default command will be set 381 * @param defaultCommand the default command to associate with the subsystem 382 */ 383 public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) { 384 if (subsystem == null) { 385 DriverStation.reportWarning("Tried to set a default command for a null subsystem", true); 386 return; 387 } 388 if (defaultCommand == null) { 389 DriverStation.reportWarning("Tried to set a null default command", true); 390 return; 391 } 392 393 requireNotComposed(defaultCommand); 394 395 if (!defaultCommand.getRequirements().contains(subsystem)) { 396 throw new IllegalArgumentException("Default commands must require their subsystem!"); 397 } 398 399 if (defaultCommand.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) { 400 DriverStation.reportWarning( 401 "Registering a non-interruptible default command!\n" 402 + "This will likely prevent any other commands from requiring this subsystem.", 403 true); 404 // Warn, but allow -- there might be a use case for this. 405 } 406 407 m_subsystems.put(subsystem, defaultCommand); 408 } 409 410 /** 411 * Removes the default command for a subsystem. The current default command will run until another 412 * command is scheduled that requires the subsystem, at which point the current default command 413 * will not be re-scheduled. 414 * 415 * @param subsystem the subsystem whose default command will be removed 416 */ 417 public void removeDefaultCommand(Subsystem subsystem) { 418 if (subsystem == null) { 419 DriverStation.reportWarning("Tried to remove a default command for a null subsystem", true); 420 return; 421 } 422 423 m_subsystems.put(subsystem, null); 424 } 425 426 /** 427 * Gets the default command associated with this subsystem. Null if this subsystem has no default 428 * command associated with it. 429 * 430 * @param subsystem the subsystem to inquire about 431 * @return the default command associated with the subsystem 432 */ 433 public Command getDefaultCommand(Subsystem subsystem) { 434 return m_subsystems.get(subsystem); 435 } 436 437 /** 438 * Cancels commands. The scheduler will only call {@link Command#end(boolean)} method of the 439 * canceled command with {@code true}, indicating they were canceled (as opposed to finishing 440 * normally). 441 * 442 * <p>Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}. 443 * 444 * @param commands the commands to cancel 445 */ 446 public void cancel(Command... commands) { 447 for (Command command : commands) { 448 cancel(command, kNoInterruptor); 449 } 450 } 451 452 /** 453 * Cancels a command. The scheduler will only call {@link Command#end(boolean)} method of the 454 * canceled command with {@code true}, indicating they were canceled (as opposed to finishing 455 * normally). 456 * 457 * <p>Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}. 458 * 459 * @param command the command to cancel 460 * @param interruptor the interrupting command, if any 461 */ 462 private void cancel(Command command, Optional<Command> interruptor) { 463 if (command == null) { 464 DriverStation.reportWarning("Tried to cancel a null command", true); 465 return; 466 } 467 if (m_endingCommands.contains(command)) { 468 return; 469 } 470 if (m_inRunLoop) { 471 m_toCancelCommands.add(command); 472 m_toCancelInterruptors.add(interruptor); 473 return; 474 } 475 if (!isScheduled(command)) { 476 return; 477 } 478 479 m_endingCommands.add(command); 480 command.end(true); 481 for (BiConsumer<Command, Optional<Command>> action : m_interruptActions) { 482 action.accept(command, interruptor); 483 } 484 m_endingCommands.remove(command); 485 m_scheduledCommands.remove(command); 486 m_requirements.keySet().removeAll(command.getRequirements()); 487 m_watchdog.addEpoch(command.getName() + ".end(true)"); 488 } 489 490 /** Cancels all commands that are currently scheduled. */ 491 public void cancelAll() { 492 // Copy to array to avoid concurrent modification. 493 cancel(m_scheduledCommands.toArray(new Command[0])); 494 } 495 496 /** 497 * Whether the given commands are running. Note that this only works on commands that are directly 498 * scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler 499 * does not see them. 500 * 501 * @param commands the command to query 502 * @return whether the command is currently scheduled 503 */ 504 public boolean isScheduled(Command... commands) { 505 return m_scheduledCommands.containsAll(Set.of(commands)); 506 } 507 508 /** 509 * Returns the command currently requiring a given subsystem. Null if no command is currently 510 * requiring the subsystem 511 * 512 * @param subsystem the subsystem to be inquired about 513 * @return the command currently requiring the subsystem, or null if no command is currently 514 * scheduled 515 */ 516 public Command requiring(Subsystem subsystem) { 517 return m_requirements.get(subsystem); 518 } 519 520 /** Disables the command scheduler. */ 521 public void disable() { 522 m_disabled = true; 523 } 524 525 /** Enables the command scheduler. */ 526 public void enable() { 527 m_disabled = false; 528 } 529 530 /** 531 * Adds an action to perform on the initialization of any command by the scheduler. 532 * 533 * @param action the action to perform 534 */ 535 public void onCommandInitialize(Consumer<Command> action) { 536 m_initActions.add(requireNonNullParam(action, "action", "onCommandInitialize")); 537 } 538 539 /** 540 * Adds an action to perform on the execution of any command by the scheduler. 541 * 542 * @param action the action to perform 543 */ 544 public void onCommandExecute(Consumer<Command> action) { 545 m_executeActions.add(requireNonNullParam(action, "action", "onCommandExecute")); 546 } 547 548 /** 549 * Adds an action to perform on the interruption of any command by the scheduler. 550 * 551 * @param action the action to perform 552 */ 553 public void onCommandInterrupt(Consumer<Command> action) { 554 requireNonNullParam(action, "action", "onCommandInterrupt"); 555 m_interruptActions.add((command, interruptor) -> action.accept(command)); 556 } 557 558 /** 559 * Adds an action to perform on the interruption of any command by the scheduler. The action 560 * receives the interrupted command and an Optional containing the interrupting command, or 561 * Optional.empty() if it was not canceled by a command (e.g., by {@link 562 * CommandScheduler#cancel}). 563 * 564 * @param action the action to perform 565 */ 566 public void onCommandInterrupt(BiConsumer<Command, Optional<Command>> action) { 567 m_interruptActions.add(requireNonNullParam(action, "action", "onCommandInterrupt")); 568 } 569 570 /** 571 * Adds an action to perform on the finishing of any command by the scheduler. 572 * 573 * @param action the action to perform 574 */ 575 public void onCommandFinish(Consumer<Command> action) { 576 m_finishActions.add(requireNonNullParam(action, "action", "onCommandFinish")); 577 } 578 579 /** 580 * Register commands as composed. An exception will be thrown if these commands are scheduled 581 * directly or added to a composition. 582 * 583 * @param commands the commands to register 584 * @throws IllegalArgumentException if the given commands have already been composed. 585 */ 586 public void registerComposedCommands(Command... commands) { 587 var commandSet = Set.of(commands); 588 requireNotComposed(commandSet); 589 var exception = new Exception("Originally composed at:"); 590 exception.fillInStackTrace(); 591 for (var command : commands) { 592 m_composedCommands.put(command, exception); 593 } 594 } 595 596 /** 597 * Clears the list of composed commands, allowing all commands to be freely used again. 598 * 599 * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use 600 * this unless you fully understand what you are doing. 601 */ 602 public void clearComposedCommands() { 603 m_composedCommands.clear(); 604 } 605 606 /** 607 * Removes a single command from the list of composed commands, allowing it to be freely used 608 * again. 609 * 610 * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use 611 * this unless you fully understand what you are doing. 612 * 613 * @param command the command to remove from the list of grouped commands 614 */ 615 public void removeComposedCommand(Command command) { 616 m_composedCommands.remove(command); 617 } 618 619 /** 620 * Requires that the specified command hasn't been already added to a composition. 621 * 622 * @param commands The commands to check 623 * @throws IllegalArgumentException if the given commands have already been composed. 624 */ 625 public void requireNotComposed(Command... commands) { 626 for (var command : commands) { 627 var exception = m_composedCommands.getOrDefault(command, null); 628 if (exception != null) { 629 throw new IllegalArgumentException( 630 "Commands that have been composed may not be added to another composition or scheduled " 631 + "individually!", 632 exception); 633 } 634 } 635 } 636 637 /** 638 * Requires that the specified commands not have been already added to a composition. 639 * 640 * @param commands The commands to check 641 * @throws IllegalArgumentException if the given commands have already been composed. 642 */ 643 public void requireNotComposed(Collection<Command> commands) { 644 requireNotComposed(commands.toArray(Command[]::new)); 645 } 646 647 /** 648 * Check if the given command has been composed. 649 * 650 * @param command The command to check 651 * @return true if composed 652 */ 653 public boolean isComposed(Command command) { 654 return getComposedCommands().contains(command); 655 } 656 657 Set<Command> getComposedCommands() { 658 return m_composedCommands.keySet(); 659 } 660 661 @Override 662 public void initSendable(SendableBuilder builder) { 663 builder.setSmartDashboardType("Scheduler"); 664 builder.addStringArrayProperty( 665 "Names", 666 () -> { 667 String[] names = new String[m_scheduledCommands.size()]; 668 int i = 0; 669 for (Command command : m_scheduledCommands) { 670 names[i] = command.getName(); 671 i++; 672 } 673 return names; 674 }, 675 null); 676 builder.addIntegerArrayProperty( 677 "Ids", 678 () -> { 679 long[] ids = new long[m_scheduledCommands.size()]; 680 int i = 0; 681 for (Command command : m_scheduledCommands) { 682 ids[i] = command.hashCode(); 683 i++; 684 } 685 return ids; 686 }, 687 null); 688 builder.addIntegerArrayProperty( 689 "Cancel", 690 () -> new long[] {}, 691 toCancel -> { 692 Map<Long, Command> ids = new LinkedHashMap<>(); 693 for (Command command : m_scheduledCommands) { 694 long id = command.hashCode(); 695 ids.put(id, command); 696 } 697 for (long hash : toCancel) { 698 cancel(ids.get(hash)); 699 } 700 }); 701 } 702}