ParallelGroup.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
package org.wpilib.command3;
 
import static org.wpilib.util.ErrorMessages.requireNonNullParam;
 
import java.util.Collection;
import java.util.HashSet;
import java.util.LinkedHashSet;
import java.util.Set;

public final class ParallelGroup implements Command {
  private final Collection<Command> m_requiredCommands = new HashSet<>();
  private final Collection<Command> m_optionalCommands = new LinkedHashSet<>();
  private final Set<Mechanism> m_requirements = new HashSet<>();
  private final String m_name;
  private final int m_priority;

  ParallelGroup(
      String name, Collection<Command> requiredCommands, Collection<Command> optionalCommands) {
    requireNonNullParam(name, "name", "ParallelGroup");
    requireNonNullParam(requiredCommands, "requiredCommands", "ParallelGroup");
    requireNonNullParam(optionalCommands, "optionalCommands", "ParallelGroup");
 
    int i = 0;
    for (Command requiredCommand : requiredCommands) {
      requireNonNullParam(requiredCommand, "requiredCommands[" + i + "]", "ParallelGroup");
      i++;
    }
 
    i = 0;
    for (Command c : optionalCommands) {
      requireNonNullParam(c, "optionalCommands[" + i + "]", "ParallelGroup");
      i++;
    }
 
    var allCommands = new LinkedHashSet<Command>();
    allCommands.addAll(requiredCommands);
    allCommands.addAll(optionalCommands);
 
    ConflictDetector.throwIfConflicts(allCommands);
 
    m_name = name;
    m_optionalCommands.addAll(optionalCommands);
    m_requiredCommands.addAll(requiredCommands);
 
    for (var command : allCommands) {
      m_requirements.addAll(command.requirements());
    }
 
    m_priority =
        allCommands.stream().mapToInt(Command::priority).max().orElse(Command.DEFAULT_PRIORITY);
  }
 
  @Override
  public void run(Coroutine coroutine) {
    coroutine.fork(m_optionalCommands);
 
    if (m_requiredCommands.isEmpty()) {
      // No required commands - just wait for the first optional command to finish
      coroutine.awaitAny(m_optionalCommands);
    } else {
      // Wait for every required command to finish
      coroutine.awaitAll(m_requiredCommands);
    }
 
    // The scheduler will cancel any optional child commands that are still running when
    // the `run` method returns
  }
 
  @Override
  public String name() {
    return m_name;
  }
 
  @Override
  public Set<Mechanism> requirements() {
    return m_requirements;
  }
 
  @Override
  public int priority() {
    return m_priority;
  }
 
  @Override
  public String toString() {
    return "ParallelGroup[name=" + m_name + "]";
  }
 
  // package-private for testing
 
  Collection<Command> getRequiredCommands() {
    return m_requiredCommands;
  }
 
  Collection<Command> getOptionalCommands() {
    return m_optionalCommands;
  }
}