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