[commands] CommandCompositionError: Include stacktrace of original co…

…mposition (#5984)

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

@@ -61,7 +61,7 @@ public static synchronized CommandScheduler getInstance() {
 
   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 void onCommandFinish(Consumer<Command> action) {
   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 void removeComposedCommand(Command command) {
   /**
    * 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 void requireNotComposed(Command command) {
    * @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 boolean isComposed(Command command) {
   }
 
   Set<Command> getComposedCommands() {
-    return m_composedCommands;
+    return m_composedCommands.keySet();
   }
 
   @Override

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

@@ -4,6 +4,7 @@
 
 #include "frc2/command/Command.h"
 
+#include <wpi/StackTrace.h>
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
 
@@ -31,7 +32,7 @@ Command::~Command() {
 }
 
 Command& Command::operator=(const Command& rhs) {
-  m_isComposed = false;
+  SetComposed(false);
   return *this;
 }
 
@@ -156,11 +157,19 @@ bool Command::HasRequirement(Subsystem* requirement) const {
 }
 
 bool Command::IsComposed() const {
-  return m_isComposed;
+  return GetPreviousCompositionSite().has_value();
 }
 
 void Command::SetComposed(bool isComposed) {
-  m_isComposed = isComposed;
+  if (isComposed) {
+    m_previousComposition = wpi::GetStackTrace(1);
+  } else {
+    m_previousComposition.reset();
+  }
+}
+
+std::optional<std::string> Command::GetPreviousCompositionSite() const {
+  return m_previousComposition;
 }
 
 void Command::InitSendable(wpi::SendableBuilder& builder) {

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

@@ -27,10 +27,17 @@ CommandPtr::CommandPtr(std::unique_ptr<Command>&& command)
   AssertValid();
 }
 
+CommandPtr::CommandPtr(CommandPtr&& rhs) {
+  m_ptr = std::move(rhs.m_ptr);
+  AssertValid();
+  rhs.m_moveOutSite = wpi::GetStackTrace(1);
+}
+
 void CommandPtr::AssertValid() const {
   if (!m_ptr) {
     throw FRC_MakeError(frc::err::CommandIllegalUse,
-                        "Moved-from CommandPtr object used!");
+                        "Moved-from CommandPtr object used!\nMoved out at:\n{}",
+                        m_moveOutSite);
   }
 }
 

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

@@ -453,11 +453,13 @@ void CommandScheduler::OnCommandFinish(Action action) {
 }
 
 void CommandScheduler::RequireUngrouped(const Command* command) {
-  if (command->IsComposed()) {
+  auto stacktrace = command->GetPreviousCompositionSite();
+  if (stacktrace.has_value()) {
     throw FRC_MakeError(frc::err::CommandIllegalUse,
                         "Commands that have been composed may not be added to "
-                        "another composition or scheduled "
-                        "individually!");
+                        "another composition or scheduled individually!"
+                        "\nOriginally composed at:\n{}",
+                        stacktrace.value());
   }
 }
 

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

@@ -6,11 +6,13 @@
 
 #include <functional>
 #include <memory>
+#include <optional>
 #include <string>
 
 #include <units/time.h>
 #include <wpi/Demangle.h>
 #include <wpi/SmallSet.h>
+#include <wpi/StackTrace.h>
 #include <wpi/sendable/Sendable.h>
 
 #include "frc2/command/Requirements.h"
@@ -387,6 +389,14 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    */
   void SetComposed(bool isComposed);
 
+  /**
+   * Get the stacktrace of where this command was composed, or an empty
+   * optional. Intended for internal use.
+   *
+   * @return optional string representation of the composition site stack trace.
+   */
+  std::optional<std::string> GetPreviousCompositionSite() const;
+
   /**
    * Whether the given command should run when the robot is disabled.  Override
    * to return true if the command should run when disabled.
@@ -424,7 +434,7 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
    */
   virtual std::unique_ptr<Command> TransferOwnership() && = 0;
 
-  bool m_isComposed = false;
+  std::optional<std::string> m_previousComposition;
 };
 
 /**

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

@@ -7,6 +7,7 @@
 #include <concepts>
 #include <functional>
 #include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -35,7 +36,7 @@ class CommandPtr final {
       : CommandPtr(
             std::make_unique<std::decay_t<T>>(std::forward<T>(command))) {}
 
-  CommandPtr(CommandPtr&&) = default;
+  CommandPtr(CommandPtr&&);
   CommandPtr& operator=(CommandPtr&&) = default;
 
   explicit CommandPtr(std::nullptr_t) = delete;
@@ -327,6 +328,7 @@ class CommandPtr final {
 
  private:
   std::unique_ptr<Command> m_ptr;
+  std::string m_moveOutSite{""};
   void AssertValid() const;
 };