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

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