[commands] Clarified error messages for parallel composition commands

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.Collections;
+import java.util.HashSet;
 import java.util.LinkedHashMap;
 import java.util.Map;
 
@@ -51,9 +51,26 @@ public final void addCommands(Command... commands) {
     CommandScheduler.getInstance().registerComposedCommands(commands);
 
     for (Command command : commands) {
-      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
+      var sharedRequirements = new HashSet<>(this.getRequirements());
+      sharedRequirements.retainAll(command.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(
-            "Multiple commands in a parallel composition cannot require the same subsystems");
+          String.format(
+            "Command %s could not be added to this ParallelCommandGroup"
+            + " because the subsystems [%s] are already required in this command."
+            + " Multiple commands in a parallel composition cannot require"
+            + " the same subsystems.",
+            command.getName(), sharedRequirementsStr));
       }
       m_commands.put(command, false);
       addRequirements(command.getRequirements());

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java

@@ -5,7 +5,7 @@
 package edu.wpi.first.wpilibj2.command;
 
 import edu.wpi.first.util.sendable.SendableBuilder;
-import java.util.Collections;
+import java.util.HashSet;
 import java.util.LinkedHashMap;
 import java.util.Map;
 
@@ -81,9 +81,26 @@ public final void addCommands(Command... commands) {
     CommandScheduler.getInstance().registerComposedCommands(commands);
 
     for (Command command : commands) {
-      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
+      var sharedRequirements = new HashSet<>(this.getRequirements());
+      sharedRequirements.retainAll(command.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(
-            "Multiple commands in a parallel group cannot require the same subsystems");
+          String.format(
+            "Command %s could not be added to this ParallelCommandGroup"
+            + " because the subsystems [%s] are already required in this command."
+            + " Multiple commands in a parallel composition cannot require"
+            + " the same subsystems.",
+            command.getName(), sharedRequirementsStr));
       }
       m_commands.put(command, false);
       addRequirements(command.getRequirements());

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.Collections;
+import java.util.HashSet;
 import java.util.LinkedHashSet;
 import java.util.Set;
 
@@ -52,9 +52,26 @@ public final void addCommands(Command... commands) {
     CommandScheduler.getInstance().registerComposedCommands(commands);
 
     for (Command command : commands) {
-      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
+      var sharedRequirements = new HashSet<>(this.getRequirements());
+      sharedRequirements.retainAll(command.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(
-            "Multiple commands in a parallel composition cannot require the same subsystems");
+          String.format(
+            "Command %s could not be added to this ParallelCommandGroup"
+            + " because the subsystems [%s] are already required in this command."
+            + " Multiple commands in a parallel composition cannot require"
+            + " the same subsystems.",
+            command.getName(), sharedRequirementsStr));
       }
       m_commands.add(command);
       addRequirements(command.getRequirements());

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>
@@ -218,12 +219,15 @@ void Command::InitSendable(wpi::SendableBuilder& builder) {
 }
 
 namespace frc2 {
-bool RequirementsDisjoint(Command* first, Command* second) {
-  bool disjoint = true;
-  auto&& requirements = second->GetRequirements();
+
+std::vector<Subsystem*> GetSharedRequirements(Command* first, Command* second) {
+  std::vector<Subsystem*> shared;
+  auto&& requirementsOfSecond = second->GetRequirements();
   for (auto&& requirement : first->GetRequirements()) {
-    disjoint &= requirements.find(requirement) == requirements.end();
+    if (requirementsOfSecond.find(requirement) != requirementsOfSecond.end()) {
+      shared.push_back(requirement);
+    }
   }
-  return disjoint;
+  return shared;
 }
 }  // 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,7 +76,8 @@ void ParallelCommandGroup::AddCommands(
   }
 
   for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
+    auto sharedRequirements = GetSharedRequirements(this, command.get());
+    if (sharedRequirements.empty()) {
       command->SetComposed(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
@@ -85,9 +87,23 @@ void ParallelCommandGroup::AddCommands(
       }
       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");
+      std::string formattedRequirements = "";
+      bool first = true;
+      for (auto&& requirement : sharedRequirements) {
+        if (first) {
+          first = false;
+        } else {
+          formattedRequirements += ", ";
+        }
+        formattedRequirements += requirement->GetName();
+      }
+      throw FRC_MakeError(
+          frc::err::CommandIllegalUse,
+          "Command {} could not be added to this ParallelCommandGroup"
+          " because the subsystems [{}] are already required in this command."
+          " Multiple commands in a parallel composition cannot require the "
+          "same subsystems.",
+          command->GetName(), formattedRequirements);
     }
   }
 }

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,7 +76,8 @@ void ParallelDeadlineGroup::AddCommands(
   }
 
   for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
+    auto sharedRequirements = GetSharedRequirements(this, command.get());
+    if (sharedRequirements.empty()) {
       command->SetComposed(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
@@ -85,9 +87,23 @@ void ParallelDeadlineGroup::AddCommands(
       }
       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");
+      std::string formattedRequirements = "";
+      bool first = true;
+      for (auto&& requirement : sharedRequirements) {
+        if (first) {
+          first = false;
+        } else {
+          formattedRequirements += ", ";
+        }
+        formattedRequirements += requirement->GetName();
+      }
+      throw FRC_MakeError(
+          frc::err::CommandIllegalUse,
+          "Command {} could not be added to this ParallelCommandGroup"
+          " because the subsystems [{}] are already required in this command."
+          " Multiple commands in a parallel composition cannot require the "
+          "same subsystems.",
+          command->GetName(), formattedRequirements);
     }
   }
 }

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,7 +63,8 @@ void ParallelRaceGroup::AddCommands(
   }
 
   for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
+    auto sharedRequirements = GetSharedRequirements(this, command.get());
+    if (sharedRequirements.empty()) {
       command->SetComposed(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
@@ -72,9 +74,23 @@ void ParallelRaceGroup::AddCommands(
       }
       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");
+      std::string formattedRequirements = "";
+      bool first = true;
+      for (auto&& requirement : sharedRequirements) {
+        if (first) {
+          first = false;
+        } else {
+          formattedRequirements += ", ";
+        }
+        formattedRequirements += requirement->GetName();
+      }
+      throw FRC_MakeError(
+          frc::err::CommandIllegalUse,
+          "Command {} could not be added to this ParallelCommandGroup"
+          " because the subsystems [{}] are already required in this command."
+          " Multiple commands in a parallel composition cannot require the "
+          "same subsystems.",
+          command->GetName(), formattedRequirements);
     }
   }
 }

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>
@@ -492,11 +493,11 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
 };
 
 /**
- * Checks if two commands have disjoint requirement sets.
+ * Gets the shared requirements between two distinct commands.
  *
  * @param first The first command to check.
  * @param second The second command to check.
- * @return False if first and second share a requirement.
+ * @return A vector of shared subsystem requirements.
  */
-bool RequirementsDisjoint(Command* first, Command* second);
+std::vector<Subsystem*> GetSharedRequirements(Command* first, Command* second);
 }  // namespace frc2