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}