[commands] Clarified error messages for parallel composition commands

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");

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();

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();

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();

wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp

@@ -6,6 +6,7 @@
 
 #include <string>
 #include <utility>
+#include <vector>
 
 #include <wpi/StackTrace.h>
 #include <wpi/sendable/SendableBuilder.h>
@@ -185,6 +186,31 @@ std::optional<std::string> 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 +242,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

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp

@@ -4,6 +4,7 @@
 
 #include "frc2/command/ParallelCommandGroup.h"
 
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -75,19 +76,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);
   }
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp

@@ -5,6 +5,7 @@
 #include "frc2/command/ParallelDeadlineGroup.h"
 
 #include <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -75,20 +76,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);
   }
 }
 

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp

@@ -4,6 +4,7 @@
 
 #include "frc2/command/ParallelRaceGroup.h"
 
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -62,19 +63,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));
   }
 }

wpilibNewCommands/src/main/native/include/frc2/command/Command.h

@@ -8,6 +8,7 @@
 #include <memory>
 #include <optional>
 #include <string>
+#include <vector>
 
 #include <units/time.h>
 #include <wpi/Demangle.h>
@@ -484,19 +485,21 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
  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<Subsystem*, 4> m_requirements;
 
   std::optional<std::string> 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