[commands] Enhance Command Sendable implementations (#4822)

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

@@ -105,8 +105,7 @@ CommandPtr Command::HandleInterrupt(std::function<void(void)> handler) && {
 }
 
 CommandPtr Command::WithName(std::string_view name) && {
-  SetName(name);
-  return std::move(*this).ToPtr();
+  return std::move(*this).ToPtr().WithName(name);
 }
 
 void Command::Schedule() {

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

@@ -64,4 +64,21 @@ void CommandBase::InitSendable(wpi::SendableBuilder& builder) {
           Cancel();
         }
       });
+  builder.AddBooleanProperty(
+      ".isParented", [this] { return IsComposed(); }, nullptr);
+  builder.AddStringProperty(
+      "interruptBehavior",
+      [this] {
+        switch (GetInterruptionBehavior()) {
+          case Command::InterruptionBehavior::kCancelIncoming:
+            return "kCancelIncoming";
+          case Command::InterruptionBehavior::kCancelSelf:
+            return "kCancelSelf";
+          default:
+            return "Invalid";
+        }
+      },
+      nullptr);
+  builder.AddBooleanProperty(
+      "runsWhenDisabled", [this] { return RunsWhenDisabled(); }, nullptr);
 }

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

@@ -221,8 +221,9 @@ CommandPtr CommandPtr::HandleInterrupt(std::function<void(void)> handler) && {
 
 CommandPtr CommandPtr::WithName(std::string_view name) && {
   AssertValid();
-  m_ptr->SetName(name);
-  return std::move(*this);
+  WrapperCommand wrapper{std::move(m_ptr)};
+  wrapper.SetName(name);
+  return std::move(wrapper).ToPtr();
 }
 
 CommandBase* CommandPtr::get() const {

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

@@ -4,6 +4,8 @@
 
 #include "frc2/command/ConditionalCommand.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+
 using namespace frc2;
 
 ConditionalCommand::ConditionalCommand(std::unique_ptr<Command>&& onTrue,
@@ -50,3 +52,21 @@ bool ConditionalCommand::IsFinished() {
 bool ConditionalCommand::RunsWhenDisabled() const {
   return m_runsWhenDisabled;
 }
+
+void ConditionalCommand::InitSendable(wpi::SendableBuilder& builder) {
+  CommandBase::InitSendable(builder);
+  builder.AddStringProperty(
+      "onTrue", [this] { return m_onTrue->GetName(); }, nullptr);
+  builder.AddStringProperty(
+      "onFalse", [this] { return m_onFalse->GetName(); }, nullptr);
+  builder.AddStringProperty(
+      "selected",
+      [this] {
+        if (m_selectedCommand) {
+          return m_selectedCommand->GetName();
+        } else {
+          return std::string{"null"};
+        }
+      },
+      nullptr);
+}

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

@@ -4,6 +4,8 @@
 
 #include "frc2/command/ParallelDeadlineGroup.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+
 using namespace frc2;
 
 ParallelDeadlineGroup::ParallelDeadlineGroup(
@@ -93,3 +95,10 @@ void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
   AddRequirements(m_deadline->GetRequirements());
   m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
 }
+
+void ParallelDeadlineGroup::InitSendable(wpi::SendableBuilder& builder) {
+  CommandBase::InitSendable(builder);
+
+  builder.AddStringProperty(
+      "deadline", [this] { return m_deadline->GetName(); }, nullptr);
+}

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

@@ -4,13 +4,17 @@
 
 #include "frc2/command/ProxyCommand.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+
 using namespace frc2;
 
 ProxyCommand::ProxyCommand(wpi::unique_function<Command*()> supplier)
     : m_supplier(std::move(supplier)) {}
 
 ProxyCommand::ProxyCommand(Command* command)
-    : m_supplier([command] { return command; }) {}
+    : m_supplier([command] { return command; }) {
+  SetName(std::string{"Proxy("}.append(command->GetName()).append(")"));
+}
 
 ProxyCommand::ProxyCommand(std::unique_ptr<Command> command)
     : m_supplier([command = std::move(command)] { return command.get(); }) {}
@@ -35,3 +39,17 @@ bool ProxyCommand::IsFinished() {
   // do whatever we want -- like return true.
   return m_command == nullptr || !m_command->IsScheduled();
 }
+
+void ProxyCommand::InitSendable(wpi::SendableBuilder& builder) {
+  CommandBase::InitSendable(builder);
+  builder.AddStringProperty(
+      "proxied",
+      [this] {
+        if (m_command) {
+          return m_command->GetName();
+        } else {
+          return std::string{"null"};
+        }
+      },
+      nullptr);
+}

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

@@ -6,6 +6,8 @@
 
 #include <utility>
 
+#include <wpi/sendable/SendableBuilder.h>
+
 using namespace frc2;
 
 RamseteCommand::RamseteCommand(
@@ -158,3 +160,11 @@ void RamseteCommand::End(bool interrupted) {
 bool RamseteCommand::IsFinished() {
   return m_timer.HasElapsed(m_trajectory.TotalTime());
 }
+
+void RamseteCommand::InitSendable(wpi::SendableBuilder& builder) {
+  CommandBase::InitSendable(builder);
+  builder.AddDoubleProperty(
+      "leftVelocity", [this] { return m_prevSpeeds.left.value(); }, nullptr);
+  builder.AddDoubleProperty(
+      "rightVelocity", [this] { return m_prevSpeeds.right.value(); }, nullptr);
+}

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

@@ -4,6 +4,8 @@
 
 #include "frc2/command/RepeatCommand.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+
 using namespace frc2;
 
 RepeatCommand::RepeatCommand(std::unique_ptr<Command>&& command) {
@@ -11,6 +13,7 @@ RepeatCommand::RepeatCommand(std::unique_ptr<Command>&& command) {
   m_command = std::move(command);
   m_command->SetComposed(true);
   AddRequirements(m_command->GetRequirements());
+  SetName(std::string{"Repeat("}.append(m_command->GetName()).append(")"));
 }
 
 void RepeatCommand::Initialize() {
@@ -46,3 +49,9 @@ bool RepeatCommand::RunsWhenDisabled() const {
 Command::InterruptionBehavior RepeatCommand::GetInterruptionBehavior() const {
   return m_command->GetInterruptionBehavior();
 }
+
+void RepeatCommand::InitSendable(wpi::SendableBuilder& builder) {
+  CommandBase::InitSendable(builder);
+  builder.AddStringProperty(
+      "command", [this] { return m_command->GetName(); }, nullptr);
+}

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

@@ -4,6 +4,8 @@
 
 #include "frc2/command/SequentialCommandGroup.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+
 using namespace frc2;
 
 SequentialCommandGroup::SequentialCommandGroup(
@@ -79,3 +81,9 @@ void SequentialCommandGroup::AddCommands(
     m_commands.emplace_back(std::move(command));
   }
 }
+
+void SequentialCommandGroup::InitSendable(wpi::SendableBuilder& builder) {
+  CommandBase::InitSendable(builder);
+  builder.AddIntegerProperty(
+      "index", [this] { return m_currentCommandIndex; }, nullptr);
+}

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

@@ -6,6 +6,7 @@
 
 #include <fmt/format.h>
 #include <frc/fmt/Units.h>
+#include <wpi/sendable/SendableBuilder.h>
 
 using namespace frc2;
 
@@ -29,3 +30,9 @@ bool WaitCommand::IsFinished() {
 bool WaitCommand::RunsWhenDisabled() const {
   return true;
 }
+
+void WaitCommand::InitSendable(wpi::SendableBuilder& builder) {
+  CommandBase::InitSendable(builder);
+  builder.AddDoubleProperty(
+      "duration", [this] { return m_duration.value(); }, nullptr);
+}

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

@@ -12,6 +12,8 @@ WrapperCommand::WrapperCommand(std::unique_ptr<Command>&& command) {
   CommandScheduler::GetInstance().RequireUngrouped(command.get());
   m_command = std::move(command);
   m_command->SetComposed(true);
+  // copy the wrapped command's name
+  SetName(m_command->GetName());
 }
 
 void WrapperCommand::Initialize() {