Command: Use SmallPtrSet for requirements instead of std::set

wpilibc/src/main/native/cpp/commands/CommandGroup.cpp

@@ -26,7 +26,7 @@ void CommandGroup::AddSequential(Command* command) {
 
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  for (auto& requirement : command->GetRequirements()) Requires(requirement);
+  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
 }
 
 void CommandGroup::AddSequential(Command* command, double timeout) {
@@ -47,7 +47,7 @@ void CommandGroup::AddSequential(Command* command, double timeout) {
 
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  for (auto& requirement : command->GetRequirements()) Requires(requirement);
+  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
 }
 
 void CommandGroup::AddParallel(Command* command) {
@@ -63,7 +63,7 @@ void CommandGroup::AddParallel(Command* command) {
 
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  for (auto& requirement : command->GetRequirements()) Requires(requirement);
+  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
 }
 
 void CommandGroup::AddParallel(Command* command, double timeout) {
@@ -84,7 +84,7 @@ void CommandGroup::AddParallel(Command* command, double timeout) {
 
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  for (auto& requirement : command->GetRequirements()) Requires(requirement);
+  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
 }
 
 bool CommandGroup::IsInterruptible() const {
@@ -229,7 +229,7 @@ void CommandGroup::CancelConflicts(Command* command) {
     Command* child = (*childIter)->m_command;
     bool erased = false;
 
-    for (auto& requirement : command->GetRequirements()) {
+    for (auto&& requirement : command->GetRequirements()) {
       if (child->DoesRequire(requirement)) {
         child->_Cancel();
         child->Removed();

wpilibc/src/main/native/cpp/commands/Scheduler.cpp

@@ -8,7 +8,6 @@
 #include "frc/commands/Scheduler.h"
 
 #include <algorithm>
-#include <set>
 
 #include <hal/HAL.h>
 
@@ -100,7 +99,7 @@ void Scheduler::Remove(Command* command) {
 
   if (!m_commands.erase(command)) return;
 
-  for (auto& requirement : command->GetRequirements()) {
+  for (auto&& requirement : command->GetRequirements()) {
     requirement->SetCurrentCommand(nullptr);
   }
 
@@ -186,7 +185,7 @@ void Scheduler::ProcessCommandAddition(Command* command) {
   auto found = m_commands.find(command);
   if (found == m_commands.end()) {
     // Check that the requirements can be had
-    Command::SubsystemSet requirements = command->GetRequirements();
+    const auto& requirements = command->GetRequirements();
     for (const auto& requirement : requirements) {
       if (requirement->GetCurrentCommand() != nullptr &&
           !requirement->GetCurrentCommand()->IsInterruptible())
@@ -195,7 +194,7 @@ void Scheduler::ProcessCommandAddition(Command* command) {
 
     // Give it the requirements
     m_adding = true;
-    for (auto& requirement : requirements) {
+    for (auto&& requirement : requirements) {
       if (requirement->GetCurrentCommand() != nullptr) {
         requirement->GetCurrentCommand()->Cancel();
         Remove(requirement->GetCurrentCommand());

wpilibc/src/main/native/include/frc/commands/Command.h

@@ -8,18 +8,18 @@
 #pragma once
 
 #include <memory>
-#include <set>
 #include <string>
 
+#include <wpi/SmallPtrSet.h>
 #include <wpi/Twine.h>
 
 #include "frc/ErrorBase.h"
+#include "frc/commands/Subsystem.h"
 #include "frc/smartdashboard/SendableBase.h"
 
 namespace frc {
 
 class CommandGroup;
-class Subsystem;
 
 /**
  * The Command class is at the very core of the entire command framework.
@@ -186,7 +186,7 @@ class Command : public ErrorBase, public SendableBase {
    */
   bool DoesRequire(Subsystem* subsystem) const;
 
-  typedef std::set<Subsystem*> SubsystemSet;
+  typedef wpi::SmallPtrSetImpl<Subsystem*> SubsystemSet;
 
   /**
    * Returns the requirements (as an std::set of Subsystem pointers) of this
@@ -393,7 +393,7 @@ class Command : public ErrorBase, public SendableBase {
   bool m_initialized = false;
 
   // The requirements (or null if no requirements)
-  SubsystemSet m_requirements;
+  wpi::SmallPtrSet<Subsystem*, 4> m_requirements;
 
   // Whether or not it is running
   bool m_running = false;

wpilibc/src/main/native/include/frc/commands/Scheduler.h

@@ -95,7 +95,8 @@ class Scheduler : public ErrorBase, public SendableBase {
 
   void ProcessCommandAddition(Command* command);
 
-  Command::SubsystemSet m_subsystems;
+  typedef std::set<Subsystem*> SubsystemSet;
+  SubsystemSet m_subsystems;
   wpi::mutex m_buttonsMutex;
   typedef std::vector<std::unique_ptr<ButtonScheduler>> ButtonVector;
   ButtonVector m_buttons;