[commands] CommandCompositionError: Include stacktrace of original composition (#5984)

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

@@ -61,7 +61,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
 
   private static final Optional<Command> kNoInterruptor = Optional.empty();
 
-  private final Set<Command> m_composedCommands = Collections.newSetFromMap(new WeakHashMap<>());
+  private final Map<Command, Exception> m_composedCommands = new WeakHashMap<>();
 
   // A set of the currently-running commands.
   private final Set<Command> m_scheduledCommands = new LinkedHashSet<>();
@@ -586,7 +586,11 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
   public void registerComposedCommands(Command... commands) {
     var commandSet = Set.of(commands);
     requireNotComposed(commandSet);
-    m_composedCommands.addAll(commandSet);
+    var exception = new Exception("Originally composed at:");
+    exception.fillInStackTrace();
+    for (var command : commands) {
+      m_composedCommands.put(command, exception);
+    }
   }
 
   /**
@@ -615,14 +619,18 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
   /**
    * Requires that the specified command hasn't been already added to a composition.
    *
-   * @param command The command to check
+   * @param commands The commands to check
    * @throws IllegalArgumentException if the given commands have already been composed.
    */
-  public void requireNotComposed(Command command) {
-    if (m_composedCommands.contains(command)) {
-      throw new IllegalArgumentException(
-          "Commands that have been composed may not be added to another composition or scheduled "
-              + "individually!");
+  public void requireNotComposed(Command... commands) {
+    for (var command : commands) {
+      var exception = m_composedCommands.getOrDefault(command, null);
+      if (exception != null) {
+        throw new IllegalArgumentException(
+            "Commands that have been composed may not be added to another composition or scheduled "
+                + "individually!",
+            exception);
+      }
     }
   }
 
@@ -633,11 +641,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
    * @throws IllegalArgumentException if the given commands have already been composed.
    */
   public void requireNotComposed(Collection<Command> commands) {
-    if (!Collections.disjoint(commands, getComposedCommands())) {
-      throw new IllegalArgumentException(
-          "Commands that have been composed may not be added to another composition or scheduled "
-              + "individually!");
-    }
+    requireNotComposed(commands.toArray(Command[]::new));
   }
 
   /**
@@ -651,7 +655,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
   }
 
   Set<Command> getComposedCommands() {
-    return m_composedCommands;
+    return m_composedCommands.keySet();
   }
 
   @Override