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.HAL; 010import edu.wpi.first.util.sendable.Sendable; 011import edu.wpi.first.util.sendable.SendableBuilder; 012import edu.wpi.first.util.sendable.SendableRegistry; 013import edu.wpi.first.wpilibj.DriverStation; 014import edu.wpi.first.wpilibj.RobotBase; 015import edu.wpi.first.wpilibj.RobotState; 016import edu.wpi.first.wpilibj.TimedRobot; 017import edu.wpi.first.wpilibj.Watchdog; 018import edu.wpi.first.wpilibj.event.EventLoop; 019import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; 020import java.io.PrintWriter; 021import java.io.StringWriter; 022import java.util.ArrayList; 023import java.util.Arrays; 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.reportUsage("CommandScheduler", ""); 101 SendableRegistry.add(this, "Scheduler"); 102 } 103 104 /** 105 * Changes the period of the loop overrun watchdog. This should be kept in sync with the 106 * TimedRobot period. 107 * 108 * @param period Period in seconds. 109 */ 110 public void setPeriod(double period) { 111 m_watchdog.setTimeout(period); 112 } 113 114 @Override 115 public void close() { 116 SendableRegistry.remove(this); 117 } 118 119 /** 120 * Get the default button poll. 121 * 122 * @return a reference to the default {@link EventLoop} object polling buttons. 123 */ 124 public EventLoop getDefaultButtonLoop() { 125 return m_defaultButtonLoop; 126 } 127 128 /** 129 * Get the active button poll. 130 * 131 * @return a reference to the current {@link EventLoop} object polling buttons. 132 */ 133 public EventLoop getActiveButtonLoop() { 134 return m_activeButtonLoop; 135 } 136 137 /** 138 * Replace the button poll with another one. 139 * 140 * @param loop the new button polling loop object. 141 */ 142 public void setActiveButtonLoop(EventLoop loop) { 143 m_activeButtonLoop = 144 requireNonNullParam(loop, "loop", "CommandScheduler" + ".replaceButtonEventLoop"); 145 } 146 147 /** 148 * Initializes a given command, adds its requirements to the list, and performs the init actions. 149 * 150 * @param command The command to initialize 151 * @param requirements The command requirements 152 */ 153 private void initCommand(Command command, Set<Subsystem> requirements) { 154 m_scheduledCommands.add(command); 155 for (Subsystem requirement : requirements) { 156 m_requirements.put(requirement, command); 157 } 158 command.initialize(); 159 for (Consumer<Command> action : m_initActions) { 160 action.accept(command); 161 } 162 163 m_watchdog.addEpoch(command.getName() + ".initialize()"); 164 } 165 166 /** 167 * Schedules a command for execution. Does nothing if the command is already scheduled. If a 168 * command's requirements are not available, it will only be started if all the commands currently 169 * using those requirements have been scheduled as interruptible. If this is the case, they will 170 * be interrupted and the command will be scheduled. 171 * 172 * <p>WARNING: using this function directly can often lead to unexpected behavior and should be 173 * avoided. Instead Triggers should be used to schedule Commands. 174 * 175 * @param command the command to schedule. If null, no-op. 176 */ 177 private void schedule(Command command) { 178 if (command == null) { 179 DriverStation.reportWarning("Tried to schedule a null command", true); 180 return; 181 } 182 if (m_inRunLoop) { 183 m_toSchedule.add(command); 184 return; 185 } 186 187 requireNotComposed(command); 188 189 // Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't 190 // run when disabled, or the command is already scheduled. 191 if (m_disabled 192 || isScheduled(command) 193 || RobotState.isDisabled() && !command.runsWhenDisabled()) { 194 return; 195 } 196 197 Set<Subsystem> requirements = command.getRequirements(); 198 199 // Schedule the command if the requirements are not currently in-use. 200 if (Collections.disjoint(m_requirements.keySet(), requirements)) { 201 initCommand(command, requirements); 202 } else { 203 // Else check if the requirements that are in use have all have interruptible commands, 204 // and if so, interrupt those commands and schedule the new command. 205 for (Subsystem requirement : requirements) { 206 Command requiring = requiring(requirement); 207 if (requiring != null 208 && requiring.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) { 209 return; 210 } 211 } 212 for (Subsystem requirement : requirements) { 213 Command requiring = requiring(requirement); 214 if (requiring != null) { 215 cancel(requiring, Optional.of(command)); 216 } 217 } 218 initCommand(command, requirements); 219 } 220 } 221 222 /** 223 * Schedules multiple commands for execution. Does nothing for commands already scheduled. 224 * 225 * <p>WARNING: using this function directly can often lead to unexpected behavior and should be 226 * avoided. Instead Triggers should be used to schedule Commands. 227 * 228 * @param commands the commands to schedule. No-op on null. 229 */ 230 public void schedule(Command... commands) { 231 for (Command command : commands) { 232 schedule(command); 233 } 234 } 235 236 /** 237 * Runs a single iteration of the scheduler. The execution occurs in the following order: 238 * 239 * <p>Subsystem periodic methods are called. 240 * 241 * <p>Button bindings are polled, and new commands are scheduled from them. 242 * 243 * <p>Currently-scheduled commands are executed. 244 * 245 * <p>End conditions are checked on currently-scheduled commands, and commands that are finished 246 * have their end methods called and are removed. 247 * 248 * <p>Any subsystems not being used as requirements have their default methods started. 249 */ 250 public void run() { 251 if (m_disabled) { 252 return; 253 } 254 m_watchdog.reset(); 255 256 // Run the periodic method of all registered subsystems. 257 for (Subsystem subsystem : m_subsystems.keySet()) { 258 subsystem.periodic(); 259 if (RobotBase.isSimulation()) { 260 subsystem.simulationPeriodic(); 261 } 262 m_watchdog.addEpoch(subsystem.getName() + ".periodic()"); 263 } 264 265 // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from 266 // inside the button bindings. 267 EventLoop loopCache = m_activeButtonLoop; 268 // Poll buttons for new commands to add. 269 loopCache.poll(); 270 m_watchdog.addEpoch("buttons.run()"); 271 272 m_inRunLoop = true; 273 boolean isDisabled = RobotState.isDisabled(); 274 // Run scheduled commands, remove finished commands. 275 for (Iterator<Command> iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) { 276 Command command = iterator.next(); 277 278 if (isDisabled && !command.runsWhenDisabled()) { 279 cancel(command, kNoInterruptor); 280 continue; 281 } 282 283 command.execute(); 284 for (Consumer<Command> action : m_executeActions) { 285 action.accept(command); 286 } 287 m_watchdog.addEpoch(command.getName() + ".execute()"); 288 if (command.isFinished()) { 289 m_endingCommands.add(command); 290 command.end(false); 291 for (Consumer<Command> action : m_finishActions) { 292 action.accept(command); 293 } 294 m_endingCommands.remove(command); 295 iterator.remove(); 296 297 m_requirements.keySet().removeAll(command.getRequirements()); 298 m_watchdog.addEpoch(command.getName() + ".end(false)"); 299 } 300 } 301 m_inRunLoop = false; 302 303 // Schedule/cancel commands from queues populated during loop 304 for (Command command : m_toSchedule) { 305 schedule(command); 306 } 307 308 for (int i = 0; i < m_toCancelCommands.size(); i++) { 309 cancel(m_toCancelCommands.get(i), m_toCancelInterruptors.get(i)); 310 } 311 312 m_toSchedule.clear(); 313 m_toCancelCommands.clear(); 314 m_toCancelInterruptors.clear(); 315 316 // Add default commands for un-required registered subsystems. 317 for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) { 318 if (!m_requirements.containsKey(subsystemCommand.getKey()) 319 && subsystemCommand.getValue() != null) { 320 schedule(subsystemCommand.getValue()); 321 } 322 } 323 324 m_watchdog.disable(); 325 if (m_watchdog.isExpired()) { 326 System.out.println("CommandScheduler loop overrun"); 327 m_watchdog.printEpochs(); 328 } 329 } 330 331 /** 332 * Registers subsystems with the scheduler. This must be called for the subsystem's periodic block 333 * to run when the scheduler is run, and for the subsystem's default command to be scheduled. It 334 * is recommended to call this from the constructor of your subsystem implementations. 335 * 336 * @param subsystems the subsystem to register 337 */ 338 public void registerSubsystem(Subsystem... subsystems) { 339 for (Subsystem subsystem : subsystems) { 340 if (subsystem == null) { 341 DriverStation.reportWarning("Tried to register a null subsystem", true); 342 continue; 343 } 344 if (m_subsystems.containsKey(subsystem)) { 345 DriverStation.reportWarning("Tried to register an already-registered subsystem", true); 346 continue; 347 } 348 m_subsystems.put(subsystem, null); 349 } 350 } 351 352 /** 353 * Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic 354 * block called, and will not have its default command scheduled. 355 * 356 * @param subsystems the subsystem to un-register 357 */ 358 public void unregisterSubsystem(Subsystem... subsystems) { 359 m_subsystems.keySet().removeAll(Set.of(subsystems)); 360 } 361 362 /** 363 * Un-registers all registered Subsystems with the scheduler. All currently registered subsystems 364 * will no longer have their periodic block called, and will not have their default command 365 * scheduled. 366 */ 367 public void unregisterAllSubsystems() { 368 m_subsystems.clear(); 369 } 370 371 /** 372 * Sets the default command for a subsystem. Registers that subsystem if it is not already 373 * registered. Default commands will run whenever there is no other command currently scheduled 374 * that requires the subsystem. Default commands should be written to never end (i.e. their {@link 375 * Command#isFinished()} method should return false), as they would simply be re-scheduled if they 376 * do. Default commands must also require their subsystem. 377 * 378 * @param subsystem the subsystem whose default command will be set 379 * @param defaultCommand the default command to associate with the subsystem 380 */ 381 public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) { 382 if (subsystem == null) { 383 DriverStation.reportWarning("Tried to set a default command for a null subsystem", true); 384 return; 385 } 386 if (defaultCommand == null) { 387 DriverStation.reportWarning("Tried to set a null default command", true); 388 return; 389 } 390 391 requireNotComposed(defaultCommand); 392 393 if (!defaultCommand.getRequirements().contains(subsystem)) { 394 throw new IllegalArgumentException("Default commands must require their subsystem!"); 395 } 396 397 if (defaultCommand.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) { 398 DriverStation.reportWarning( 399 "Registering a non-interruptible default command!\n" 400 + "This will likely prevent any other commands from requiring this subsystem.", 401 true); 402 // Warn, but allow -- there might be a use case for this. 403 } 404 405 m_subsystems.put(subsystem, defaultCommand); 406 } 407 408 /** 409 * Removes the default command for a subsystem. The current default command will run until another 410 * command is scheduled that requires the subsystem, at which point the current default command 411 * will not be re-scheduled. 412 * 413 * @param subsystem the subsystem whose default command will be removed 414 */ 415 public void removeDefaultCommand(Subsystem subsystem) { 416 if (subsystem == null) { 417 DriverStation.reportWarning("Tried to remove a default command for a null subsystem", true); 418 return; 419 } 420 421 m_subsystems.put(subsystem, null); 422 } 423 424 /** 425 * Gets the default command associated with this subsystem. Null if this subsystem has no default 426 * command associated with it. 427 * 428 * @param subsystem the subsystem to inquire about 429 * @return the default command associated with the subsystem 430 */ 431 public Command getDefaultCommand(Subsystem subsystem) { 432 return m_subsystems.get(subsystem); 433 } 434 435 /** 436 * Cancels commands. The scheduler will only call {@link Command#end(boolean)} method of the 437 * canceled command with {@code true}, indicating they were canceled (as opposed to finishing 438 * normally). 439 * 440 * <p>Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}. 441 * 442 * @param commands the commands to cancel 443 */ 444 public void cancel(Command... commands) { 445 for (Command command : commands) { 446 cancel(command, kNoInterruptor); 447 } 448 } 449 450 /** 451 * Cancels a command. The scheduler will only call {@link Command#end(boolean)} method of the 452 * canceled command with {@code true}, indicating they were canceled (as opposed to finishing 453 * normally). 454 * 455 * <p>Commands will be canceled regardless of {@link InterruptionBehavior interruption behavior}. 456 * 457 * @param command the command to cancel 458 * @param interruptor the interrupting command, if any 459 */ 460 private void cancel(Command command, Optional<Command> interruptor) { 461 if (command == null) { 462 DriverStation.reportWarning("Tried to cancel a null command", true); 463 return; 464 } 465 if (m_endingCommands.contains(command)) { 466 return; 467 } 468 if (m_inRunLoop) { 469 m_toCancelCommands.add(command); 470 m_toCancelInterruptors.add(interruptor); 471 return; 472 } 473 if (!isScheduled(command)) { 474 return; 475 } 476 477 m_endingCommands.add(command); 478 command.end(true); 479 for (BiConsumer<Command, Optional<Command>> action : m_interruptActions) { 480 action.accept(command, interruptor); 481 } 482 m_endingCommands.remove(command); 483 m_scheduledCommands.remove(command); 484 m_requirements.keySet().removeAll(command.getRequirements()); 485 m_watchdog.addEpoch(command.getName() + ".end(true)"); 486 } 487 488 /** Cancels all commands that are currently scheduled. */ 489 public void cancelAll() { 490 // Copy to array to avoid concurrent modification. 491 cancel(m_scheduledCommands.toArray(new Command[0])); 492 } 493 494 /** 495 * Whether the given commands are running. Note that this only works on commands that are directly 496 * scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler 497 * does not see them. 498 * 499 * @param commands multiple commands to check 500 * @return whether all of the commands are currently scheduled 501 */ 502 public boolean isScheduled(Command... commands) { 503 for (var cmd : commands) { 504 if (!isScheduled(cmd)) { 505 return false; 506 } 507 } 508 return true; 509 } 510 511 /** 512 * Whether the given commands are running. Note that this only works on commands that are directly 513 * scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler 514 * does not see them. 515 * 516 * @param command a single command to check 517 * @return whether all of the commands are currently scheduled 518 */ 519 public boolean isScheduled(Command command) { 520 return m_scheduledCommands.contains(command); 521 } 522 523 /** 524 * Returns the command currently requiring a given subsystem. Null if no command is currently 525 * requiring the subsystem 526 * 527 * @param subsystem the subsystem to be inquired about 528 * @return the command currently requiring the subsystem, or null if no command is currently 529 * scheduled 530 */ 531 public Command requiring(Subsystem subsystem) { 532 return m_requirements.get(subsystem); 533 } 534 535 /** Disables the command scheduler. */ 536 public void disable() { 537 m_disabled = true; 538 } 539 540 /** Enables the command scheduler. */ 541 public void enable() { 542 m_disabled = false; 543 } 544 545 /** Prints list of epochs added so far and their times. */ 546 public void printWatchdogEpochs() { 547 m_watchdog.printEpochs(); 548 } 549 550 /** 551 * Adds an action to perform on the initialization of any command by the scheduler. 552 * 553 * @param action the action to perform 554 */ 555 public void onCommandInitialize(Consumer<Command> action) { 556 m_initActions.add(requireNonNullParam(action, "action", "onCommandInitialize")); 557 } 558 559 /** 560 * Adds an action to perform on the execution of any command by the scheduler. 561 * 562 * @param action the action to perform 563 */ 564 public void onCommandExecute(Consumer<Command> action) { 565 m_executeActions.add(requireNonNullParam(action, "action", "onCommandExecute")); 566 } 567 568 /** 569 * Adds an action to perform on the interruption of any command by the scheduler. 570 * 571 * @param action the action to perform 572 */ 573 public void onCommandInterrupt(Consumer<Command> action) { 574 requireNonNullParam(action, "action", "onCommandInterrupt"); 575 m_interruptActions.add((command, interruptor) -> action.accept(command)); 576 } 577 578 /** 579 * Adds an action to perform on the interruption of any command by the scheduler. The action 580 * receives the interrupted command and an Optional containing the interrupting command, or 581 * Optional.empty() if it was not canceled by a command (e.g., by {@link 582 * CommandScheduler#cancel}). 583 * 584 * @param action the action to perform 585 */ 586 public void onCommandInterrupt(BiConsumer<Command, Optional<Command>> action) { 587 m_interruptActions.add(requireNonNullParam(action, "action", "onCommandInterrupt")); 588 } 589 590 /** 591 * Adds an action to perform on the finishing of any command by the scheduler. 592 * 593 * @param action the action to perform 594 */ 595 public void onCommandFinish(Consumer<Command> action) { 596 m_finishActions.add(requireNonNullParam(action, "action", "onCommandFinish")); 597 } 598 599 /** 600 * Register commands as composed. An exception will be thrown if these commands are scheduled 601 * directly or added to a composition. 602 * 603 * @param commands the commands to register 604 * @throws IllegalArgumentException if the given commands have already been composed, or the array 605 * of commands has duplicates. 606 */ 607 public void registerComposedCommands(Command... commands) { 608 Set<Command> commandSet; 609 try { 610 commandSet = Set.of(commands); 611 } catch (IllegalArgumentException e) { 612 throw new IllegalArgumentException( 613 "Cannot compose a command twice in the same composition! (Original exception: " 614 + e 615 + ")"); 616 } 617 requireNotComposedOrScheduled(commandSet); 618 var exception = new Exception("Originally composed at:"); 619 exception.fillInStackTrace(); 620 for (var command : commands) { 621 m_composedCommands.put(command, exception); 622 } 623 } 624 625 /** 626 * Clears the list of composed commands, allowing all commands to be freely used again. 627 * 628 * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use 629 * this unless you fully understand what you are doing. 630 */ 631 public void clearComposedCommands() { 632 m_composedCommands.clear(); 633 } 634 635 /** 636 * Removes a single command from the list of composed commands, allowing it to be freely used 637 * again. 638 * 639 * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use 640 * this unless you fully understand what you are doing. 641 * 642 * @param command the command to remove from the list of grouped commands 643 */ 644 public void removeComposedCommand(Command command) { 645 m_composedCommands.remove(command); 646 } 647 648 /** 649 * Strip additional leading stack trace elements that are in the framework package. 650 * 651 * @param stacktrace the original stacktrace 652 * @return the stacktrace stripped of leading elements so there is at max one leading element from 653 * the edu.wpi.first.wpilibj2.command package. 654 */ 655 private StackTraceElement[] stripFrameworkStackElements(StackTraceElement[] stacktrace) { 656 int i = stacktrace.length - 1; 657 for (; i > 0; i--) { 658 if (stacktrace[i].getClassName().startsWith("edu.wpi.first.wpilibj2.command.")) { 659 break; 660 } 661 } 662 return Arrays.copyOfRange(stacktrace, i, stacktrace.length); 663 } 664 665 /** 666 * Requires that the specified command hasn't already been added to a composition. 667 * 668 * @param commands The commands to check 669 * @throws IllegalArgumentException if the given commands have already been composed. 670 */ 671 public void requireNotComposed(Command... commands) { 672 for (var command : commands) { 673 var exception = m_composedCommands.getOrDefault(command, null); 674 if (exception != null) { 675 exception.setStackTrace(stripFrameworkStackElements(exception.getStackTrace())); 676 var buffer = new StringWriter(); 677 var writer = new PrintWriter(buffer); 678 writer.println( 679 "Commands that have been composed may not be added to another composition or scheduled " 680 + "individually!"); 681 exception.printStackTrace(writer); 682 var thrownException = new IllegalArgumentException(buffer.toString()); 683 thrownException.setStackTrace(stripFrameworkStackElements(thrownException.getStackTrace())); 684 throw thrownException; 685 } 686 } 687 } 688 689 /** 690 * Requires that the specified commands have not already been added to a composition. 691 * 692 * @param commands The commands to check 693 * @throws IllegalArgumentException if the given commands have already been composed. 694 */ 695 public void requireNotComposed(Collection<Command> commands) { 696 requireNotComposed(commands.toArray(Command[]::new)); 697 } 698 699 /** 700 * Requires that the specified command hasn't already been added to a composition, and is not 701 * currently scheduled. 702 * 703 * @param command The command to check 704 * @throws IllegalArgumentException if the given command has already been composed or scheduled. 705 */ 706 public void requireNotComposedOrScheduled(Command command) { 707 if (isScheduled(command)) { 708 throw new IllegalArgumentException( 709 "Commands that have been scheduled individually may not be added to a composition!"); 710 } 711 requireNotComposed(command); 712 } 713 714 /** 715 * Requires that the specified commands have not already been added to a composition, and are not 716 * currently scheduled. 717 * 718 * @param commands The commands to check 719 * @throws IllegalArgumentException if the given commands have already been composed or scheduled. 720 */ 721 public void requireNotComposedOrScheduled(Collection<Command> commands) { 722 for (var command : commands) { 723 requireNotComposedOrScheduled(command); 724 } 725 } 726 727 /** 728 * Check if the given command has been composed. 729 * 730 * @param command The command to check 731 * @return true if composed 732 */ 733 public boolean isComposed(Command command) { 734 return getComposedCommands().contains(command); 735 } 736 737 Set<Command> getComposedCommands() { 738 return m_composedCommands.keySet(); 739 } 740 741 @Override 742 public void initSendable(SendableBuilder builder) { 743 builder.setSmartDashboardType("Scheduler"); 744 builder.addStringArrayProperty( 745 "Names", 746 () -> { 747 String[] names = new String[m_scheduledCommands.size()]; 748 int i = 0; 749 for (Command command : m_scheduledCommands) { 750 names[i] = command.getName(); 751 i++; 752 } 753 return names; 754 }, 755 null); 756 builder.addIntegerArrayProperty( 757 "Ids", 758 () -> { 759 long[] ids = new long[m_scheduledCommands.size()]; 760 int i = 0; 761 for (Command command : m_scheduledCommands) { 762 ids[i] = command.hashCode(); 763 i++; 764 } 765 return ids; 766 }, 767 null); 768 builder.addIntegerArrayProperty( 769 "Cancel", 770 () -> new long[] {}, 771 toCancel -> { 772 Map<Long, Command> ids = new LinkedHashMap<>(); 773 for (Command command : m_scheduledCommands) { 774 long id = command.hashCode(); 775 ids.put(id, command); 776 } 777 for (long hash : toCancel) { 778 cancel(ids.get(hash)); 779 } 780 }); 781 } 782}