[commands] Merge CommandBase into Command and SubsystemBase into Subsystem (#5392)

Moves all CommandBase functionality into Command and deprecates CommandBase for removal.
Moves all SubsystemBase functionality into Subsystem and deprecates SubsystemBase for removal.
Adds a function to CommandScheduler to remove all registered Subsystems.

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

@@ -4,6 +4,9 @@
 
 #include "frc2/command/Command.h"
 
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
 #include "frc2/command/CommandHelper.h"
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/ConditionalCommand.h"
@@ -19,6 +22,10 @@
 
 using namespace frc2;
 
+Command::Command() {
+  wpi::SendableRegistry::Add(this, GetTypeName(*this));
+}
+
 Command::~Command() {
   CommandScheduler::GetInstance().Cancel(this);
 }
@@ -32,6 +39,42 @@ void Command::Initialize() {}
 void Command::Execute() {}
 void Command::End(bool interrupted) {}
 
+wpi::SmallSet<Subsystem*, 4> Command::GetRequirements() const {
+  return m_requirements;
+}
+
+void Command::AddRequirements(std::initializer_list<Subsystem*> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void Command::AddRequirements(std::span<Subsystem* const> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void Command::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
+  m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void Command::AddRequirements(Subsystem* requirement) {
+  m_requirements.insert(requirement);
+}
+
+void Command::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
+}
+
+std::string Command::GetName() const {
+  return wpi::SendableRegistry::GetName(this);
+}
+
+std::string Command::GetSubsystem() const {
+  return wpi::SendableRegistry::GetSubsystem(this);
+}
+
+void Command::SetSubsystem(std::string_view subsystem) {
+  wpi::SendableRegistry::SetSubsystem(this, subsystem);
+}
+
 CommandPtr Command::WithTimeout(units::second_t duration) && {
   return std::move(*this).ToPtr().WithTimeout(duration);
 }
@@ -125,12 +168,6 @@ bool Command::HasRequirement(Subsystem* requirement) const {
   return hasRequirement;
 }
 
-std::string Command::GetName() const {
-  return GetTypeName(*this);
-}
-
-void Command::SetName(std::string_view name) {}
-
 bool Command::IsComposed() const {
   return m_isComposed;
 }
@@ -139,6 +176,39 @@ void Command::SetComposed(bool isComposed) {
   m_isComposed = isComposed;
 }
 
+void Command::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Command");
+  builder.AddStringProperty(
+      ".name", [this] { return GetName(); }, nullptr);
+  builder.AddBooleanProperty(
+      "running", [this] { return IsScheduled(); },
+      [this](bool value) {
+        bool isScheduled = IsScheduled();
+        if (value && !isScheduled) {
+          Schedule();
+        } else if (!value && isScheduled) {
+          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);
+}
+
 namespace frc2 {
 bool RequirementsDisjoint(Command* first, Command* second) {
   bool disjoint = true;

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

@@ -3,82 +3,3 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/CommandBase.h"
-
-#include <wpi/sendable/SendableBuilder.h>
-#include <wpi/sendable/SendableRegistry.h>
-
-using namespace frc2;
-
-CommandBase::CommandBase() {
-  wpi::SendableRegistry::Add(this, GetTypeName(*this));
-}
-
-void CommandBase::AddRequirements(
-    std::initializer_list<Subsystem*> requirements) {
-  m_requirements.insert(requirements.begin(), requirements.end());
-}
-
-void CommandBase::AddRequirements(std::span<Subsystem* const> requirements) {
-  m_requirements.insert(requirements.begin(), requirements.end());
-}
-
-void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
-  m_requirements.insert(requirements.begin(), requirements.end());
-}
-
-void CommandBase::AddRequirements(Subsystem* requirement) {
-  m_requirements.insert(requirement);
-}
-
-wpi::SmallSet<Subsystem*, 4> CommandBase::GetRequirements() const {
-  return m_requirements;
-}
-
-void CommandBase::SetName(std::string_view name) {
-  wpi::SendableRegistry::SetName(this, name);
-}
-
-std::string CommandBase::GetName() const {
-  return wpi::SendableRegistry::GetName(this);
-}
-
-std::string CommandBase::GetSubsystem() const {
-  return wpi::SendableRegistry::GetSubsystem(this);
-}
-
-void CommandBase::SetSubsystem(std::string_view subsystem) {
-  wpi::SendableRegistry::SetSubsystem(this, subsystem);
-}
-
-void CommandBase::InitSendable(wpi::SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Command");
-  builder.AddStringProperty(
-      ".name", [this] { return GetName(); }, nullptr);
-  builder.AddBooleanProperty(
-      "running", [this] { return IsScheduled(); },
-      [this](bool value) {
-        bool isScheduled = IsScheduled();
-        if (value && !isScheduled) {
-          Schedule();
-        } else if (!value && isScheduled) {
-          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

@@ -236,12 +236,12 @@ CommandPtr CommandPtr::WithName(std::string_view name) && {
   return std::move(wrapper).ToPtr();
 }
 
-CommandBase* CommandPtr::get() const& {
+Command* CommandPtr::get() const& {
   AssertValid();
   return m_ptr.get();
 }
 
-std::unique_ptr<CommandBase> CommandPtr::Unwrap() && {
+std::unique_ptr<Command> CommandPtr::Unwrap() && {
   AssertValid();
   return std::move(m_ptr);
 }

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

@@ -291,6 +291,10 @@ void CommandScheduler::UnregisterSubsystem(
   }
 }
 
+void CommandScheduler::UnregisterAllSubsystems() {
+  m_impl->subsystems.clear();
+}
+
 void CommandScheduler::SetDefaultCommand(Subsystem* subsystem,
                                          CommandPtr&& defaultCommand) {
   if (!defaultCommand.get()->HasRequirement(subsystem)) {

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

@@ -66,7 +66,7 @@ Command::InterruptionBehavior ConditionalCommand::GetInterruptionBehavior()
 }
 
 void ConditionalCommand::InitSendable(wpi::SendableBuilder& builder) {
-  CommandBase::InitSendable(builder);
+  Command::InitSendable(builder);
   builder.AddStringProperty(
       "onTrue", [this] { return m_onTrue->GetName(); }, nullptr);
   builder.AddStringProperty(

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

@@ -97,7 +97,7 @@ void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
 }
 
 void ParallelDeadlineGroup::InitSendable(wpi::SendableBuilder& builder) {
-  CommandBase::InitSendable(builder);
+  Command::InitSendable(builder);
 
   builder.AddStringProperty(
       "deadline", [this] { return m_deadline->GetName(); }, nullptr);

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

@@ -41,7 +41,7 @@ bool ProxyCommand::IsFinished() {
 }
 
 void ProxyCommand::InitSendable(wpi::SendableBuilder& builder) {
-  CommandBase::InitSendable(builder);
+  Command::InitSendable(builder);
   builder.AddStringProperty(
       "proxied",
       [this] {

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

@@ -161,7 +161,7 @@ bool RamseteCommand::IsFinished() {
 }
 
 void RamseteCommand::InitSendable(wpi::SendableBuilder& builder) {
-  CommandBase::InitSendable(builder);
+  Command::InitSendable(builder);
   builder.AddDoubleProperty(
       "leftVelocity", [this] { return m_prevSpeeds.left.value(); }, nullptr);
   builder.AddDoubleProperty(

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

@@ -56,7 +56,7 @@ Command::InterruptionBehavior RepeatCommand::GetInterruptionBehavior() const {
 }
 
 void RepeatCommand::InitSendable(wpi::SendableBuilder& builder) {
-  CommandBase::InitSendable(builder);
+  Command::InitSendable(builder);
   builder.AddStringProperty(
       "command", [this] { return m_command->GetName(); }, nullptr);
 }

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

@@ -83,7 +83,7 @@ void SequentialCommandGroup::AddCommands(
 }
 
 void SequentialCommandGroup::InitSendable(wpi::SendableBuilder& builder) {
-  CommandBase::InitSendable(builder);
+  Command::InitSendable(builder);
   builder.AddIntegerProperty(
       "index", [this] { return m_currentCommandIndex; }, nullptr);
 }

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

@@ -8,6 +8,12 @@
 #include "frc2/command/Commands.h"
 
 using namespace frc2;
+
+Subsystem::Subsystem() {
+  wpi::SendableRegistry::AddLW(this, GetTypeName(*this));
+  CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
 Subsystem::~Subsystem() {
   CommandScheduler::GetInstance().UnregisterSubsystem(this);
 }
@@ -33,6 +39,26 @@ Command* Subsystem::GetCurrentCommand() const {
   return CommandScheduler::GetInstance().Requiring(this);
 }
 
+std::string Subsystem::GetName() const {
+  return wpi::SendableRegistry::GetName(this);
+}
+
+void Subsystem::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
+}
+
+std::string Subsystem::GetSubsystem() const {
+  return wpi::SendableRegistry::GetSubsystem(this);
+}
+
+void Subsystem::SetSubsystem(std::string_view name) {
+  wpi::SendableRegistry::SetSubsystem(this, name);
+}
+
+void Subsystem::AddChild(std::string name, wpi::Sendable* child) {
+  wpi::SendableRegistry::AddLW(child, GetSubsystem(), name);
+}
+
 void Subsystem::Register() {
   return CommandScheduler::GetInstance().RegisterSubsystem(this);
 }
@@ -54,3 +80,33 @@ CommandPtr Subsystem::RunEnd(std::function<void()> run,
                              std::function<void()> end) {
   return cmd::RunEnd(std::move(run), std::move(end), {this});
 }
+
+void Subsystem::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Subsystem");
+  builder.AddBooleanProperty(
+      ".hasDefault", [this] { return GetDefaultCommand() != nullptr; },
+      nullptr);
+  builder.AddStringProperty(
+      ".default",
+      [this]() -> std::string {
+        auto command = GetDefaultCommand();
+        if (command == nullptr) {
+          return "none";
+        }
+        return command->GetName();
+      },
+      nullptr);
+  builder.AddBooleanProperty(
+      ".hasCommand", [this] { return GetCurrentCommand() != nullptr; },
+      nullptr);
+  builder.AddStringProperty(
+      ".command",
+      [this]() -> std::string {
+        auto command = GetCurrentCommand();
+        if (command == nullptr) {
+          return "none";
+        }
+        return command->GetName();
+      },
+      nullptr);
+}

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

@@ -12,57 +12,4 @@
 
 using namespace frc2;
 
-SubsystemBase::SubsystemBase() {
-  wpi::SendableRegistry::AddLW(this, GetTypeName(*this));
-  CommandScheduler::GetInstance().RegisterSubsystem({this});
-}
-
-void SubsystemBase::InitSendable(wpi::SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Subsystem");
-  builder.AddBooleanProperty(
-      ".hasDefault", [this] { return GetDefaultCommand() != nullptr; },
-      nullptr);
-  builder.AddStringProperty(
-      ".default",
-      [this]() -> std::string {
-        auto command = GetDefaultCommand();
-        if (command == nullptr) {
-          return "none";
-        }
-        return command->GetName();
-      },
-      nullptr);
-  builder.AddBooleanProperty(
-      ".hasCommand", [this] { return GetCurrentCommand() != nullptr; },
-      nullptr);
-  builder.AddStringProperty(
-      ".command",
-      [this]() -> std::string {
-        auto command = GetCurrentCommand();
-        if (command == nullptr) {
-          return "none";
-        }
-        return command->GetName();
-      },
-      nullptr);
-}
-
-std::string SubsystemBase::GetName() const {
-  return wpi::SendableRegistry::GetName(this);
-}
-
-void SubsystemBase::SetName(std::string_view name) {
-  wpi::SendableRegistry::SetName(this, name);
-}
-
-std::string SubsystemBase::GetSubsystem() const {
-  return wpi::SendableRegistry::GetSubsystem(this);
-}
-
-void SubsystemBase::SetSubsystem(std::string_view name) {
-  wpi::SendableRegistry::SetSubsystem(this, name);
-}
-
-void SubsystemBase::AddChild(std::string name, wpi::Sendable* child) {
-  wpi::SendableRegistry::AddLW(child, GetSubsystem(), name);
-}
+SubsystemBase::SubsystemBase() {}

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

@@ -30,7 +30,7 @@ bool WaitCommand::RunsWhenDisabled() const {
 }
 
 void WaitCommand::InitSendable(wpi::SendableBuilder& builder) {
-  CommandBase::InitSendable(builder);
+  Command::InitSendable(builder);
   builder.AddDoubleProperty(
       "duration", [this] { return m_duration.value(); }, nullptr);
 }