Clean up new C++ commands (#2027)

- Remove use of std::set. The only place std::set was actually used was in ParallelRaceGroup,
but this was of minimal utility as ParallelRaceGroup checked for duplicate subsystem
requirements, so it would be very unusual to end up with duplicate commands
in any case; replaced it with a vector.
- Remove use of std::unordered_map except for SelectCommand. Replaced with vector.
- Use pImpl idiom for CommandScheduler
- Minimize include files (remove unnecessary ones)
- Reformat include file order for consistency

wpilibNewCommands/.styleguide

@@ -0,0 +1,8 @@
+includeOtherLibs {
+  ^HAL/
+  ^networktables/
+  ^frc/
+  ^units/
+  ^wpi/
+  ^frc2/Timer
+}

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

@@ -7,8 +7,6 @@
 
 #include "frc2/command/Command.h"
 
-#include <iostream>
-
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/ParallelCommandGroup.h"

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

@@ -9,8 +9,6 @@
 
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/CommandScheduler.h>
-#include <frc2/command/SetUtilities.h>
 
 using namespace frc2;
 

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

@@ -7,19 +7,10 @@
 
 #include "frc2/command/CommandGroupBase.h"
 
-#include <set>
-
-#include "frc/WPIErrors.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/SequentialCommandGroup.h"
+#include <frc/WPIErrors.h>
 
 using namespace frc2;
-template <typename TMap, typename TKey>
-static bool ContainsKey(const TMap& map, TKey keyToCheck) {
-  return map.find(keyToCheck) != map.end();
-}
+
 bool CommandGroupBase::RequireUngrouped(Command& command) {
   if (command.IsGrouped()) {
     wpi_setGlobalWPIErrorWithContext(

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

@@ -7,39 +7,85 @@
 
 #include "frc2/command/CommandScheduler.h"
 
+#include <hal/HALBase.h>
 #include <frc/RobotState.h>
 #include <frc/WPIErrors.h>
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/CommandGroupBase.h>
-#include <frc2/command/Subsystem.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/DenseMap.h>
 
-#include <hal/HAL.h>
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandState.h"
+#include "frc2/command/Subsystem.h"
 
 using namespace frc2;
+
+class CommandScheduler::Impl {
+ public:
+  // A map from commands to their scheduling state.  Also used as a set of the
+  // currently-running commands.
+  wpi::DenseMap<Command*, CommandState> scheduledCommands;
+
+  // A map from required subsystems to their requiring commands.  Also used as a
+  // set of the currently-required subsystems.
+  wpi::DenseMap<Subsystem*, Command*> requirements;
+
+  // A map from subsystems registered with the scheduler to their default
+  // commands.  Also used as a list of currently-registered subsystems.
+  wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> subsystems;
+
+  // The set of currently-registered buttons that will be polled every
+  // iteration.
+  wpi::SmallVector<wpi::unique_function<void()>, 4> buttons;
+
+  bool disabled{false};
+
+  // NetworkTable entries for use in Sendable impl
+  nt::NetworkTableEntry namesEntry;
+  nt::NetworkTableEntry idsEntry;
+  nt::NetworkTableEntry cancelEntry;
+
+  // Lists of user-supplied actions to be executed on scheduling events for
+  // every command.
+  wpi::SmallVector<Action, 4> initActions;
+  wpi::SmallVector<Action, 4> executeActions;
+  wpi::SmallVector<Action, 4> interruptActions;
+  wpi::SmallVector<Action, 4> finishActions;
+
+  // Flag and queues for avoiding concurrent modification if commands are
+  // scheduled/canceled during run
+
+  bool inRunLoop = false;
+  wpi::DenseMap<Command*, bool> toSchedule;
+  wpi::SmallVector<Command*, 4> toCancel;
+};
+
 template <typename TMap, typename TKey>
 static bool ContainsKey(const TMap& map, TKey keyToCheck) {
   return map.find(keyToCheck) != map.end();
 }
 
-CommandScheduler::CommandScheduler() {
+CommandScheduler::CommandScheduler() : m_impl(new Impl) {
   frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
 }
 
+CommandScheduler::~CommandScheduler() {}
+
 CommandScheduler& CommandScheduler::GetInstance() {
   static CommandScheduler scheduler;
   return scheduler;
 }
 
 void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
-  m_buttons.emplace_back(std::move(button));
+  m_impl->buttons.emplace_back(std::move(button));
 }
 
-void CommandScheduler::ClearButtons() { m_buttons.clear(); }
+void CommandScheduler::ClearButtons() { m_impl->buttons.clear(); }
 
 void CommandScheduler::Schedule(bool interruptible, Command* command) {
-  if (m_inRunLoop) {
-    m_toSchedule.try_emplace(command, interruptible);
+  if (m_impl->inRunLoop) {
+    m_impl->toSchedule.try_emplace(command, interruptible);
     return;
   }
 
@@ -49,9 +95,9 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
                                "cannot be independently scheduled");
     return;
   }
-  if (m_disabled ||
+  if (m_impl->disabled ||
       (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
-      ContainsKey(m_scheduledCommands, command)) {
+      ContainsKey(m_impl->scheduledCommands, command)) {
     return;
   }
 
@@ -61,10 +107,11 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
 
   bool isDisjoint = true;
   bool allInterruptible = true;
-  for (auto&& i1 : m_requirements) {
+  for (auto&& i1 : m_impl->requirements) {
     if (requirements.find(i1.first) != requirements.end()) {
       isDisjoint = false;
-      allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible();
+      allInterruptible &=
+          m_impl->scheduledCommands[i1.second].IsInterruptible();
       intersection.emplace_back(i1.second);
     }
   }
@@ -76,12 +123,12 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) {
       }
     }
     command->Initialize();
-    m_scheduledCommands[command] = CommandState{interruptible};
-    for (auto&& action : m_initActions) {
+    m_impl->scheduledCommands[command] = CommandState{interruptible};
+    for (auto&& action : m_impl->initActions) {
       action(*command);
     }
     for (auto&& requirement : requirements) {
-      m_requirements[requirement] = command;
+      m_impl->requirements[requirement] = command;
     }
   }
 }
@@ -115,24 +162,24 @@ void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
 }
 
 void CommandScheduler::Run() {
-  if (m_disabled) {
+  if (m_impl->disabled) {
     return;
   }
 
   // Run the periodic method of all registered subsystems.
-  for (auto&& subsystem : m_subsystems) {
+  for (auto&& subsystem : m_impl->subsystems) {
     subsystem.getFirst()->Periodic();
   }
 
   // Poll buttons for new commands to add.
-  for (auto&& button : m_buttons) {
+  for (auto&& button : m_impl->buttons) {
     button();
   }
 
-  m_inRunLoop = true;
+  m_impl->inRunLoop = true;
   // Run scheduled commands, remove finished commands.
-  for (auto iterator = m_scheduledCommands.begin();
-       iterator != m_scheduledCommands.end(); iterator++) {
+  for (auto iterator = m_impl->scheduledCommands.begin();
+       iterator != m_impl->scheduledCommands.end(); iterator++) {
     Command* command = iterator->getFirst();
 
     if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
@@ -141,53 +188,53 @@ void CommandScheduler::Run() {
     }
 
     command->Execute();
-    for (auto&& action : m_executeActions) {
+    for (auto&& action : m_impl->executeActions) {
       action(*command);
     }
 
     if (command->IsFinished()) {
       command->End(false);
-      for (auto&& action : m_finishActions) {
+      for (auto&& action : m_impl->finishActions) {
         action(*command);
       }
 
       for (auto&& requirement : command->GetRequirements()) {
-        m_requirements.erase(requirement);
+        m_impl->requirements.erase(requirement);
       }
 
-      m_scheduledCommands.erase(iterator);
+      m_impl->scheduledCommands.erase(iterator);
     }
   }
-  m_inRunLoop = false;
+  m_impl->inRunLoop = false;
 
-  for (auto&& commandInterruptible : m_toSchedule) {
+  for (auto&& commandInterruptible : m_impl->toSchedule) {
     Schedule(commandInterruptible.second, commandInterruptible.first);
   }
 
-  for (auto&& command : m_toCancel) {
+  for (auto&& command : m_impl->toCancel) {
     Cancel(command);
   }
 
-  m_toSchedule.clear();
-  m_toCancel.clear();
+  m_impl->toSchedule.clear();
+  m_impl->toCancel.clear();
 
   // Add default commands for un-required registered subsystems.
-  for (auto&& subsystem : m_subsystems) {
-    auto s = m_requirements.find(subsystem.getFirst());
-    if (s == m_requirements.end() && subsystem.getSecond()) {
+  for (auto&& subsystem : m_impl->subsystems) {
+    auto s = m_impl->requirements.find(subsystem.getFirst());
+    if (s == m_impl->requirements.end() && subsystem.getSecond()) {
       Schedule({subsystem.getSecond().get()});
     }
   }
 }
 
 void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
-  m_subsystems[subsystem] = nullptr;
+  m_impl->subsystems[subsystem] = nullptr;
 }
 
 void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
-  auto s = m_subsystems.find(subsystem);
-  if (s != m_subsystems.end()) {
-    m_subsystems.erase(s);
+  auto s = m_impl->subsystems.find(subsystem);
+  if (s != m_impl->subsystems.end()) {
+    m_impl->subsystems.erase(s);
   }
 }
 
@@ -206,8 +253,8 @@ void CommandScheduler::UnregisterSubsystem(
 }
 
 Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
-  auto&& find = m_subsystems.find(subsystem);
-  if (find != m_subsystems.end()) {
+  auto&& find = m_impl->subsystems.find(subsystem);
+  if (find != m_impl->subsystems.end()) {
     return find->second.get();
   } else {
     return nullptr;
@@ -215,21 +262,21 @@ Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
 }
 
 void CommandScheduler::Cancel(Command* command) {
-  if (m_inRunLoop) {
-    m_toCancel.emplace_back(command);
+  if (m_impl->inRunLoop) {
+    m_impl->toCancel.emplace_back(command);
     return;
   }
 
-  auto find = m_scheduledCommands.find(command);
-  if (find == m_scheduledCommands.end()) return;
+  auto find = m_impl->scheduledCommands.find(command);
+  if (find == m_impl->scheduledCommands.end()) return;
   command->End(true);
-  for (auto&& action : m_interruptActions) {
+  for (auto&& action : m_impl->interruptActions) {
     action(*command);
   }
-  m_scheduledCommands.erase(find);
-  for (auto&& requirement : m_requirements) {
+  m_impl->scheduledCommands.erase(find);
+  for (auto&& requirement : m_impl->requirements) {
     if (requirement.second == command) {
-      m_requirements.erase(requirement.first);
+      m_impl->requirements.erase(requirement.first);
     }
   }
 }
@@ -247,14 +294,14 @@ void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
 }
 
 void CommandScheduler::CancelAll() {
-  for (auto&& command : m_scheduledCommands) {
+  for (auto&& command : m_impl->scheduledCommands) {
     Cancel(command.first);
   }
 }
 
 double CommandScheduler::TimeSinceScheduled(const Command* command) const {
-  auto find = m_scheduledCommands.find(command);
-  if (find != m_scheduledCommands.end()) {
+  auto find = m_impl->scheduledCommands.find(command);
+  if (find != m_impl->scheduledCommands.end()) {
     return find->second.TimeSinceInitialized();
   } else {
     return -1;
@@ -281,65 +328,72 @@ bool CommandScheduler::IsScheduled(
 }
 
 bool CommandScheduler::IsScheduled(const Command* command) const {
-  return m_scheduledCommands.find(command) != m_scheduledCommands.end();
+  return m_impl->scheduledCommands.find(command) !=
+         m_impl->scheduledCommands.end();
 }
 
 Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
-  auto find = m_requirements.find(subsystem);
-  if (find != m_requirements.end()) {
+  auto find = m_impl->requirements.find(subsystem);
+  if (find != m_impl->requirements.end()) {
     return find->second;
   } else {
     return nullptr;
   }
 }
 
-void CommandScheduler::Disable() { m_disabled = true; }
+void CommandScheduler::Disable() { m_impl->disabled = true; }
 
-void CommandScheduler::Enable() { m_disabled = false; }
+void CommandScheduler::Enable() { m_impl->disabled = false; }
 
 void CommandScheduler::OnCommandInitialize(Action action) {
-  m_initActions.emplace_back(std::move(action));
+  m_impl->initActions.emplace_back(std::move(action));
 }
 
 void CommandScheduler::OnCommandExecute(Action action) {
-  m_executeActions.emplace_back(std::move(action));
+  m_impl->executeActions.emplace_back(std::move(action));
 }
 
 void CommandScheduler::OnCommandInterrupt(Action action) {
-  m_interruptActions.emplace_back(std::move(action));
+  m_impl->interruptActions.emplace_back(std::move(action));
 }
 
 void CommandScheduler::OnCommandFinish(Action action) {
-  m_finishActions.emplace_back(std::move(action));
+  m_impl->finishActions.emplace_back(std::move(action));
 }
 
 void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Scheduler");
-  m_namesEntry = builder.GetEntry("Names");
-  m_idsEntry = builder.GetEntry("Ids");
-  m_cancelEntry = builder.GetEntry("Cancel");
+  m_impl->namesEntry = builder.GetEntry("Names");
+  m_impl->idsEntry = builder.GetEntry("Ids");
+  m_impl->cancelEntry = builder.GetEntry("Cancel");
 
   builder.SetUpdateTable([this] {
     double tmp[1];
     tmp[0] = 0;
-    auto toCancel = m_cancelEntry.GetDoubleArray(tmp);
+    auto toCancel = m_impl->cancelEntry.GetDoubleArray(tmp);
     for (auto cancel : toCancel) {
       uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
       Command* command = reinterpret_cast<Command*>(ptrTmp);
-      if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) {
+      if (m_impl->scheduledCommands.find(command) !=
+          m_impl->scheduledCommands.end()) {
         Cancel(command);
       }
-      m_cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+      m_impl->cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
     }
 
     wpi::SmallVector<std::string, 8> names;
     wpi::SmallVector<double, 8> ids;
-    for (auto&& command : m_scheduledCommands) {
+    for (auto&& command : m_impl->scheduledCommands) {
       names.emplace_back(command.first->GetName());
       uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
       ids.emplace_back(static_cast<double>(ptrTmp));
     }
-    m_namesEntry.SetStringArray(names);
-    m_idsEntry.SetDoubleArray(ids);
+    m_impl->namesEntry.SetStringArray(names);
+    m_impl->idsEntry.SetDoubleArray(ids);
   });
 }
+
+void CommandScheduler::SetDefaultCommandImpl(Subsystem* subsystem,
+                                             std::unique_ptr<Command> command) {
+  m_impl->subsystems[subsystem] = std::move(command);
+}

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

@@ -7,7 +7,7 @@
 
 #include "frc2/command/CommandState.h"
 
-#include "frc/Timer.h"
+#include <frc/Timer.h>
 
 using namespace frc2;
 CommandState::CommandState(bool interruptible)

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

@@ -72,7 +72,7 @@ void ParallelCommandGroup::AddCommands(
       command->SetGrouped(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands[std::move(command)] = false;
+      m_commands.emplace_back(std::move(command), false);
     } else {
       wpi_setWPIErrorWithContext(CommandIllegalUse,
                                  "Multiple commands in a parallel group cannot "

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

@@ -67,7 +67,7 @@ void ParallelDeadlineGroup::AddCommands(
       command->SetGrouped(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands[std::move(command)] = false;
+      m_commands.emplace_back(std::move(command), false);
     } else {
       wpi_setWPIErrorWithContext(CommandIllegalUse,
                                  "Multiple commands in a parallel group cannot "
@@ -80,7 +80,7 @@ void ParallelDeadlineGroup::AddCommands(
 void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
   m_deadline = deadline.get();
   m_deadline->SetGrouped(true);
-  m_commands[std::move(deadline)] = false;
+  m_commands.emplace_back(std::move(deadline), false);
   AddRequirements(m_deadline->GetRequirements());
   m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
 }

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

@@ -58,7 +58,7 @@ void ParallelRaceGroup::AddCommands(
       command->SetGrouped(true);
       AddRequirements(command->GetRequirements());
       m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands.emplace(std::move(command));
+      m_commands.emplace_back(std::move(command));
     } else {
       wpi_setWPIErrorWithContext(CommandIllegalUse,
                                  "Multiple commands in a parallel group cannot "

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

@@ -7,6 +7,8 @@
 
 #include "frc2/command/PrintCommand.h"
 
+#include <wpi/raw_ostream.h>
+
 using namespace frc2;
 
 PrintCommand::PrintCommand(const wpi::Twine& message)

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

@@ -9,8 +9,9 @@
 
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/Command.h>
-#include <frc2/command/CommandScheduler.h>
+
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandScheduler.h"
 
 using namespace frc2;
 

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

@@ -7,8 +7,6 @@
 
 #include "frc2/command/TrapezoidProfileCommand.h"
 
-#include <units/units.h>
-
 using namespace frc2;
 
 TrapezoidProfileCommand::TrapezoidProfileCommand(

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

@@ -7,13 +7,15 @@
 
 #include "frc2/command/WaitUntilCommand.h"
 
+#include <frc2/Timer.h>
+
 using namespace frc2;
 
 WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
     : m_condition{std::move(condition)} {}
 
-WaitUntilCommand::WaitUntilCommand(double time)
-    : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {}
+WaitUntilCommand::WaitUntilCommand(units::second_t time)
+    : m_condition{[=] { return Timer::GetMatchTime() - time > 0_s; }} {}
 
 bool WaitUntilCommand::IsFinished() { return m_condition(); }
 

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

@@ -7,18 +7,17 @@
 
 #pragma once
 
-#include <frc/ErrorBase.h>
-#include <frc/WPIErrors.h>
-#include <frc2/command/Subsystem.h>
-
+#include <functional>
 #include <memory>
 #include <string>
 
+#include <frc/ErrorBase.h>
 #include <units/units.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/Demangle.h>
 #include <wpi/SmallSet.h>
-#include <wpi/Twine.h>
+
+#include "frc2/command/Subsystem.h"
 
 namespace frc2 {
 

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

@@ -7,15 +7,15 @@
 
 #pragma once
 
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
+#include <initializer_list>
 #include <string>
 
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
 #include <wpi/SmallSet.h>
 #include <wpi/Twine.h>
 
-#include "Command.h"
+#include "frc2/command/Command.h"
 
 namespace frc2 {
 /**

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

@@ -7,13 +7,13 @@
 
 #pragma once
 
-#include <frc/ErrorBase.h>
-
+#include <initializer_list>
 #include <memory>
-#include <set>
 #include <vector>
 
-#include "CommandBase.h"
+#include <wpi/ArrayRef.h>
+
+#include "frc2/command/CommandBase.h"
 
 namespace frc2 {
 

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

@@ -11,7 +11,7 @@
 #include <type_traits>
 #include <utility>
 
-#include "Command.h"
+#include "frc2/command/Command.h"
 
 namespace frc2 {
 

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

@@ -7,22 +7,16 @@
 
 #pragma once
 
+#include <initializer_list>
+#include <memory>
+#include <utility>
+
 #include <frc/ErrorBase.h>
-#include <frc/RobotState.h>
 #include <frc/WPIErrors.h>
 #include <frc/smartdashboard/Sendable.h>
 #include <frc/smartdashboard/SendableHelper.h>
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/DenseMap.h>
+#include <wpi/ArrayRef.h>
 #include <wpi/FunctionExtras.h>
-#include <wpi/SmallSet.h>
-
-#include "CommandState.h"
 
 namespace frc2 {
 class Command;
@@ -46,6 +40,10 @@ class CommandScheduler final : public frc::Sendable,
    */
   static CommandScheduler& GetInstance();
 
+  ~CommandScheduler();
+  CommandScheduler(const CommandScheduler&) = delete;
+  CommandScheduler& operator=(const CommandScheduler&) = delete;
+
   using Action = std::function<void(const Command&)>;
 
   /**
@@ -186,8 +184,9 @@ class CommandScheduler final : public frc::Sendable,
                                  "Default commands should not end!");
       return;
     }
-    m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
-        std::forward<T>(defaultCommand));
+    SetDefaultCommandImpl(subsystem,
+                          std::make_unique<std::remove_reference_t<T>>(
+                              std::forward<T>(defaultCommand)));
   }
 
   /**
@@ -330,42 +329,11 @@ class CommandScheduler final : public frc::Sendable,
   // Constructor; private as this is a singleton
   CommandScheduler();
 
-  // A map from commands to their scheduling state.  Also used as a set of the
-  // currently-running commands.
-  wpi::DenseMap<Command*, CommandState> m_scheduledCommands;
+  void SetDefaultCommandImpl(Subsystem* subsystem,
+                             std::unique_ptr<Command> command);
 
-  // A map from required subsystems to their requiring commands.  Also used as a
-  // set of the currently-required subsystems.
-  wpi::DenseMap<Subsystem*, Command*> m_requirements;
-
-  // A map from subsystems registered with the scheduler to their default
-  // commands.  Also used as a list of currently-registered subsystems.
-  wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> m_subsystems;
-
-  // The set of currently-registered buttons that will be polled every
-  // iteration.
-  wpi::SmallVector<wpi::unique_function<void()>, 4> m_buttons;
-
-  bool m_disabled{false};
-
-  // NetworkTable entries for use in Sendable impl
-  nt::NetworkTableEntry m_namesEntry;
-  nt::NetworkTableEntry m_idsEntry;
-  nt::NetworkTableEntry m_cancelEntry;
-
-  // Lists of user-supplied actions to be executed on scheduling events for
-  // every command.
-  wpi::SmallVector<Action, 4> m_initActions;
-  wpi::SmallVector<Action, 4> m_executeActions;
-  wpi::SmallVector<Action, 4> m_interruptActions;
-  wpi::SmallVector<Action, 4> m_finishActions;
-
-  // Flag and queues for avoiding concurrent modification if commands are
-  // scheduled/canceled during run
-
-  bool m_inRunLoop = false;
-  wpi::DenseMap<Command*, bool> m_toSchedule;
-  wpi::SmallVector<Command*, 4> m_toCancel;
+  class Impl;
+  std::unique_ptr<Impl> m_impl;
 
   friend class CommandTestBase;
 };

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

@@ -7,13 +7,13 @@
 
 #pragma once
 
-#include <iostream>
+#include <functional>
 #include <memory>
 #include <utility>
 
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,8 +7,10 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,8 +7,11 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,12 +7,14 @@
 
 #pragma once
 
-#include <frc/Notifier.h>
+#include <functional>
+#include <initializer_list>
 
+#include <frc/Notifier.h>
 #include <units/units.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,7 +7,11 @@
 
 #pragma once
 
-#include "frc/controller/PIDController.h"
+#include <functional>
+#include <initializer_list>
+
+#include <frc/controller/PIDController.h>
+
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 

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

@@ -7,7 +7,8 @@
 
 #pragma once
 
-#include "frc/controller/PIDController.h"
+#include <frc/controller/PIDController.h>
+
 #include "frc2/command/SubsystemBase.h"
 
 namespace frc2 {

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

@@ -13,12 +13,11 @@
 #endif
 
 #include <memory>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
@@ -89,7 +88,7 @@ class ParallelCommandGroup
  private:
   void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
 
-  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+  std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
   bool m_runWhenDisabled{true};
   bool isRunning = false;
 };

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

@@ -13,12 +13,11 @@
 #endif
 
 #include <memory>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
@@ -99,7 +98,7 @@ class ParallelDeadlineGroup
 
   void SetDeadline(std::unique_ptr<Command>&& deadline);
 
-  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+  std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
   Command* m_deadline;
   bool m_runWhenDisabled{true};
   bool isRunning = false;

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

@@ -13,13 +13,11 @@
 #endif
 
 #include <memory>
-#include <set>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**
@@ -78,7 +76,7 @@ class ParallelRaceGroup
  private:
   void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
 
-  std::set<std::unique_ptr<Command>> m_commands;
+  std::vector<std::unique_ptr<Command>> m_commands;
   bool m_runWhenDisabled{true};
   bool m_finished{false};
   bool isRunning = false;

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

@@ -15,9 +15,9 @@
 #include <memory>
 #include <utility>
 
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -8,10 +8,9 @@
 #pragma once
 
 #include <wpi/Twine.h>
-#include <wpi/raw_ostream.h>
 
-#include "CommandHelper.h"
-#include "InstantCommand.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/InstantCommand.h"
 
 namespace frc2 {
 /**

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

@@ -7,9 +7,12 @@
 
 #pragma once
 
+#include <functional>
+#include <initializer_list>
+
+#include <frc/controller/ProfiledPIDController.h>
 #include <units/units.h>
 
-#include "frc/controller/ProfiledPIDController.h"
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 

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

@@ -7,9 +7,9 @@
 
 #pragma once
 
+#include <frc/controller/ProfiledPIDController.h>
 #include <units/units.h>
 
-#include "frc/controller/ProfiledPIDController.h"
 #include "frc2/command/SubsystemBase.h"
 
 namespace frc2 {

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

@@ -7,11 +7,12 @@
 
 #pragma once
 
+#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/SetUtilities.h"
 
 namespace frc2 {
 /**

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

@@ -5,21 +5,22 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#pragma once
+
 #include <functional>
+#include <initializer_list>
 #include <memory>
 
+#include <frc/controller/PIDController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/trajectory/Trajectory.h>
+#include <frc2/Timer.h>
 #include <units/units.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc/controller/PIDController.h"
-#include "frc/controller/RamseteController.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/Trajectory.h"
-#include "frc2/Timer.h"
-
-#pragma once
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,8 +7,11 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,11 +7,12 @@
 
 #pragma once
 
+#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/SetUtilities.h"
 
 namespace frc2 {
 /**

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

@@ -15,14 +15,14 @@
 #include <memory>
 #include <unordered_map>
 #include <utility>
+#include <type_traits>
 #include <vector>
 
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "PrintCommand.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/PrintCommand.h"
 
 namespace frc2 {
-template <typename Key>
 /**
  * Runs one of a selection of commands, either using a selector and a key to
  * command mapping, or a supplier that returns the command directly at runtime.
@@ -39,6 +39,7 @@ template <typename Key>
  * <p>As a rule, CommandGroups require the union of the requirements of their
  * component commands.
  */
+template <typename Key>
 class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
  public:
   /**

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

@@ -14,15 +14,16 @@
 
 #include <limits>
 #include <memory>
+#include <type_traits>
 #include <utility>
 #include <vector>
 
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
 #include <wpi/ArrayRef.h>
 
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-#include "frc/ErrorBase.h"
-#include "frc/WPIErrors.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 

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

@@ -7,8 +7,11 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
+#include <functional>
+#include <initializer_list>
+
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,10 +7,11 @@
 
 #pragma once
 
-#include <frc2/command/CommandScheduler.h>
-
+#include <type_traits>
 #include <utility>
 
+#include "frc2/command/CommandScheduler.h"
+
 namespace frc2 {
 class Command;
 /**

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

@@ -7,12 +7,12 @@
 
 #pragma once
 
+#include <string>
+
 #include <frc/smartdashboard/Sendable.h>
 #include <frc/smartdashboard/SendableHelper.h>
 
-#include <string>
-
-#include "Subsystem.h"
+#include "frc2/command/Subsystem.h"
 
 namespace frc2 {
 /**

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

@@ -5,15 +5,16 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#pragma once
+
+#include <functional>
+#include <initializer_list>
+
 #include <frc/trajectory/TrapezoidProfile.h>
 #include <frc2/Timer.h>
 
-#include <functional>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-#pragma once
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,12 +7,11 @@
 
 #pragma once
 
+#include <frc2/Timer.h>
 #include <units/units.h>
-#include <wpi/Twine.h>
 
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc2/Timer.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
 /**

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

@@ -7,8 +7,11 @@
 
 #pragma once
 
-#include "CommandBase.h"
-#include "frc/Timer.h"
+#include <functional>
+
+#include <units/units.h>
+
+#include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
 namespace frc2 {
@@ -36,7 +39,7 @@ class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
    *
    * @param time the match time after which to end, in seconds
    */
-  explicit WaitUntilCommand(double time);
+  explicit WaitUntilCommand(units::second_t time);
 
   WaitUntilCommand(WaitUntilCommand&& other) = default;