diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java index a53d792d0e4..364140134a1 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java @@ -581,6 +581,37 @@ public WrapperCommand withName(String name) { return wrapper; } + /** + * Throws an error if a parallel group already shares + * one or more requirements with a command + * that will be added to it. + * + * @param toAdd The command that will be added to the parallel group. + */ + protected void ensureDisjointRequirements(Command toAdd) { + var sharedRequirements = new HashSet<>(getRequirements()); + sharedRequirements.retainAll(toAdd.getRequirements()); + if (!sharedRequirements.isEmpty()) { + StringBuilder sharedRequirementsStr = new StringBuilder(); + boolean first = true; + for (Subsystem requirement: sharedRequirements) { + if (first) { + first = false; + } else { + sharedRequirementsStr.append(", "); + } + sharedRequirementsStr.append(requirement.getName()); + } + throw new IllegalArgumentException( + String.format( + "Command %s could not be added to this parallel group" + + " because the subsystems [%s] are already required in this command." + + " Multiple commands in a parallel composition cannot require" + + " the same subsystems.", + toAdd.getName(), sharedRequirementsStr)); + } + } + @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Command"); diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java index 0fc31876ce4..b77f30adcec 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj2.command; -import java.util.Collections; import java.util.LinkedHashMap; import java.util.Map; @@ -51,10 +50,7 @@ public final void addCommands(Command... commands) { CommandScheduler.getInstance().registerComposedCommands(commands); for (Command command : commands) { - if (!Collections.disjoint(command.getRequirements(), getRequirements())) { - throw new IllegalArgumentException( - "Multiple commands in a parallel composition cannot require the same subsystems"); - } + ensureDisjointRequirements(command); m_commands.put(command, false); addRequirements(command.getRequirements()); m_runWhenDisabled &= command.runsWhenDisabled(); diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java index 2286cee7de8..cf923113c64 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java @@ -5,7 +5,6 @@ package edu.wpi.first.wpilibj2.command; import edu.wpi.first.util.sendable.SendableBuilder; -import java.util.Collections; import java.util.LinkedHashMap; import java.util.Map; @@ -81,10 +80,7 @@ public final void addCommands(Command... commands) { CommandScheduler.getInstance().registerComposedCommands(commands); for (Command command : commands) { - if (!Collections.disjoint(command.getRequirements(), getRequirements())) { - throw new IllegalArgumentException( - "Multiple commands in a parallel group cannot require the same subsystems"); - } + ensureDisjointRequirements(command); m_commands.put(command, false); addRequirements(command.getRequirements()); m_runWhenDisabled &= command.runsWhenDisabled(); diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java index 36eaf275431..becfc20e4fa 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj2.command; -import java.util.Collections; import java.util.LinkedHashSet; import java.util.Set; @@ -52,10 +51,7 @@ public final void addCommands(Command... commands) { CommandScheduler.getInstance().registerComposedCommands(commands); for (Command command : commands) { - if (!Collections.disjoint(command.getRequirements(), getRequirements())) { - throw new IllegalArgumentException( - "Multiple commands in a parallel composition cannot require the same subsystems"); - } + ensureDisjointRequirements(command); m_commands.add(command); addRequirements(command.getRequirements()); m_runWhenDisabled &= command.runsWhenDisabled(); diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp index 365cf80a4ea..84d241a57ab 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp @@ -185,6 +185,31 @@ std::optional Command::GetPreviousCompositionSite() const { return m_previousComposition; } +void Command::EnsureDisjointRequirements(Command* toAdd) { + std::string sharedRequirementsStr = ""; + bool hasSharedRequirements = false; + auto&& requirementsToAdd = toAdd->GetRequirements(); + for (auto&& requirement : this->GetRequirements()) { + if (requirementsToAdd.find(requirement) != requirementsToAdd.end()) { + if (!hasSharedRequirements) { + hasSharedRequirements = true; // ensures formatting like "a, b, c" + } else { + sharedRequirementsStr.append(", "); + } + sharedRequirementsStr.append(requirement->GetName()); + } + } + if (hasSharedRequirements) { + throw FRC_MakeError( + frc::err::CommandIllegalUse, + "Command {} could not be added to this Parallel Group" + " because the subsystems [{}] are already required in this command." + " Multiple commands in a parallel composition cannot require the " + "same subsystems.", + toAdd->GetName(), sharedRequirementsStr); + } +} + void Command::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("Command"); builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr); @@ -216,14 +241,3 @@ void Command::InitSendable(wpi::SendableBuilder& builder) { builder.AddBooleanProperty( "runsWhenDisabled", [this] { return RunsWhenDisabled(); }, nullptr); } - -namespace frc2 { -bool RequirementsDisjoint(Command* first, Command* second) { - bool disjoint = true; - auto&& requirements = second->GetRequirements(); - for (auto&& requirement : first->GetRequirements()) { - disjoint &= requirements.find(requirement) == requirements.end(); - } - return disjoint; -} -} // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp index b433eb445a1..eda400b099b 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp @@ -75,19 +75,14 @@ void ParallelCommandGroup::AddCommands( } for (auto&& command : commands) { - if (RequirementsDisjoint(this, command.get())) { - command->SetComposed(true); - AddRequirements(command->GetRequirements()); - m_runWhenDisabled &= command->RunsWhenDisabled(); - if (command->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace_back(std::move(command), false); - } else { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Multiple commands in a parallel group cannot " - "require the same subsystems"); + EnsureDisjointRequirements(command.get()); + command->SetComposed(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + if (command->GetInterruptionBehavior() == + Command::InterruptionBehavior::kCancelSelf) { + m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; } + m_commands.emplace_back(std::move(command), false); } } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp index 90669fe20be..ab100550deb 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp @@ -75,20 +75,15 @@ void ParallelDeadlineGroup::AddCommands( } for (auto&& command : commands) { - if (RequirementsDisjoint(this, command.get())) { - command->SetComposed(true); - AddRequirements(command->GetRequirements()); - m_runWhenDisabled &= command->RunsWhenDisabled(); - if (command->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace_back(std::move(command), false); - } else { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Multiple commands in a parallel group cannot " - "require the same subsystems"); + EnsureDisjointRequirements(command.get()); + command->SetComposed(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + if (command->GetInterruptionBehavior() == + Command::InterruptionBehavior::kCancelSelf) { + m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; } + m_commands.emplace_back(std::move(command), false); } } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp index 5896dd88aaa..47a1de7903d 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp @@ -62,19 +62,14 @@ void ParallelRaceGroup::AddCommands( } for (auto&& command : commands) { - if (RequirementsDisjoint(this, command.get())) { - command->SetComposed(true); - AddRequirements(command->GetRequirements()); - m_runWhenDisabled &= command->RunsWhenDisabled(); - if (command->GetInterruptionBehavior() == - Command::InterruptionBehavior::kCancelSelf) { - m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; - } - m_commands.emplace_back(std::move(command)); - } else { - throw FRC_MakeError(frc::err::CommandIllegalUse, - "Multiple commands in a parallel group cannot " - "require the same subsystems"); + EnsureDisjointRequirements(command.get()); + command->SetComposed(true); + AddRequirements(command->GetRequirements()); + m_runWhenDisabled &= command->RunsWhenDisabled(); + if (command->GetInterruptionBehavior() == + Command::InterruptionBehavior::kCancelSelf) { + m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf; } + m_commands.emplace_back(std::move(command)); } } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index c4af1afe81b..1d73ea43862 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -484,6 +484,16 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { protected: Command(); + /** + * Throws an error if a parallel group already shares + * one or more requirements with a command + * that will be added to it. + * + * @param parallelGroup The parallel group command. + * @param toAdd The command that will be added to the parallel group. + */ + void EnsureDisjointRequirements(Command* toAdd); + private: /// Requirements set. wpi::SmallSet m_requirements; @@ -491,12 +501,4 @@ class Command : public wpi::Sendable, public wpi::SendableHelper { std::optional m_previousComposition; }; -/** - * Checks if two commands have disjoint requirement sets. - * - * @param first The first command to check. - * @param second The second command to check. - * @return False if first and second share a requirement. - */ -bool RequirementsDisjoint(Command* first, Command* second); -} // namespace frc2 +} // namespace frc2 \ No newline at end of file