diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index e61b6f88801..9f040523d14 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -20,7 +20,10 @@ 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 void removeComposedCommand(Command command) { 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 void requireNotComposed(Command... commands) { 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; } } }