[commands] Fix double composition error truncation (#6501)

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

@@ -20,7 +20,10 @@ import edu.wpi.first.wpilibj.Watchdog;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
+import java.io.PrintWriter;
+import java.io.StringWriter;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.Collection;
 import java.util.Collections;
 import java.util.Iterator;
@@ -625,6 +628,23 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
     m_composedCommands.remove(command);
   }
 
+  /**
+   * Strip additional leading stack trace elements that are in the framework package.
+   *
+   * @param stacktrace the original stacktrace
+   * @return the stacktrace stripped of leading elements so there is at max one leading element from
+   *     the edu.wpi.first.wpilibj2.command package.
+   */
+  private StackTraceElement[] stripFrameworkStackElements(StackTraceElement[] stacktrace) {
+    int i = stacktrace.length - 1;
+    for (; i > 0; i--) {
+      if (stacktrace[i].getClassName().startsWith("edu.wpi.first.wpilibj2.command.")) {
+        break;
+      }
+    }
+    return Arrays.copyOfRange(stacktrace, i, stacktrace.length);
+  }
+
   /**
    * Requires that the specified command hasn't already been added to a composition.
    *
@@ -635,10 +655,16 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
     for (var command : commands) {
       var exception = m_composedCommands.getOrDefault(command, null);
       if (exception != null) {
-        throw new IllegalArgumentException(
+        exception.setStackTrace(stripFrameworkStackElements(exception.getStackTrace()));
+        var buffer = new StringWriter();
+        var writer = new PrintWriter(buffer);
+        writer.println(
             "Commands that have been composed may not be added to another composition or scheduled "
-                + "individually!",
-            exception);
+                + "individually!");
+        exception.printStackTrace(writer);
+        var thrownException = new IllegalArgumentException(buffer.toString());
+        thrownException.setStackTrace(stripFrameworkStackElements(thrownException.getStackTrace()));
+        throw thrownException;
       }
     }
   }