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