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

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;
 };