Clean up Command container iteration code (#73)

wpilibc/src/main/native/cpp/Commands/Command.cpp

@@ -114,7 +114,7 @@ bool Command::DoesRequire(Subsystem* system) const {
   return m_requirements.count(system) > 0;
 }
 
-Command::SubsystemSet Command::GetRequirements() const {
+const Command::SubsystemSet& Command::GetRequirements() const {
   return m_requirements;
 }
 

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

@@ -20,15 +20,13 @@ void CommandGroup::AddSequential(Command* command) {
   }
   if (!AssertUnlocked("Cannot add new command to command group")) return;
 
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
+
   command->SetParent(this);
 
-  m_commands.push_back(
-      CommandGroupEntry(command, CommandGroupEntry::kSequence_InSequence));
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  Command::SubsystemSet requirements = command->GetRequirements();
-  for (auto iter = requirements.begin(); iter != requirements.end(); iter++)
-    Requires(*iter);
+  for (auto& requirement : command->GetRequirements()) Requires(requirement);
 }
 
 void CommandGroup::AddSequential(Command* command, double timeout) {
@@ -42,15 +40,14 @@ void CommandGroup::AddSequential(Command* command, double timeout) {
     return;
   }
 
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
+                          timeout);
+
   command->SetParent(this);
 
-  m_commands.push_back(CommandGroupEntry(
-      command, CommandGroupEntry::kSequence_InSequence, timeout));
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  Command::SubsystemSet requirements = command->GetRequirements();
-  for (auto iter = requirements.begin(); iter != requirements.end(); iter++)
-    Requires(*iter);
+  for (auto& requirement : command->GetRequirements()) Requires(requirement);
 }
 
 void CommandGroup::AddParallel(Command* command) {
@@ -60,15 +57,13 @@ void CommandGroup::AddParallel(Command* command) {
   }
   if (!AssertUnlocked("Cannot add new command to command group")) return;
 
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
+
   command->SetParent(this);
 
-  m_commands.push_back(
-      CommandGroupEntry(command, CommandGroupEntry::kSequence_BranchChild));
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  Command::SubsystemSet requirements = command->GetRequirements();
-  for (auto iter = requirements.begin(); iter != requirements.end(); iter++)
-    Requires(*iter);
+  for (auto& requirement : command->GetRequirements()) Requires(requirement);
 }
 
 void CommandGroup::AddParallel(Command* command, double timeout) {
@@ -82,15 +77,14 @@ void CommandGroup::AddParallel(Command* command, double timeout) {
     return;
   }
 
+  m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
+                          timeout);
+
   command->SetParent(this);
 
-  m_commands.push_back(CommandGroupEntry(
-      command, CommandGroupEntry::kSequence_BranchChild, timeout));
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  Command::SubsystemSet requirements = command->GetRequirements();
-  for (auto iter = requirements.begin(); iter != requirements.end(); iter++)
-    Requires(*iter);
+  for (auto& requirement : command->GetRequirements()) Requires(requirement);
 }
 
 bool CommandGroup::IsInterruptible() const {
@@ -102,8 +96,8 @@ bool CommandGroup::IsInterruptible() const {
     if (!cmd->IsInterruptible()) return false;
   }
 
-  for (auto iter = m_children.cbegin(); iter != m_children.cend(); iter++) {
-    if (!iter->m_command->IsInterruptible()) return false;
+  for (const auto& child : m_children) {
+    if (!child->m_command->IsInterruptible()) return false;
   }
 
   return true;
@@ -127,7 +121,7 @@ void CommandGroup::Interrupted() {}
 void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
 
 void CommandGroup::_Execute() {
-  CommandGroupEntry entry;
+  CommandGroupEntry* entry;
   Command* cmd = nullptr;
   bool firstRun = false;
 
@@ -136,14 +130,20 @@ void CommandGroup::_Execute() {
     m_currentCommandIndex = 0;
   }
 
+  // While there are still commands in this group to run
   while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+    // If a command is prepared to run
     if (cmd != nullptr) {
-      if (entry.IsTimedOut()) cmd->_Cancel();
+      // If command timed out, cancel it so it's removed from the Scheduler
+      if (entry->IsTimedOut()) cmd->_Cancel();
 
+      // If command finished or was cancelled, remove it from Scheduler
       if (cmd->Run()) {
         break;
       } else {
         cmd->Removed();
+
+        // Advance to next command in group
         m_currentCommandIndex++;
         firstRun = true;
         cmd = nullptr;
@@ -151,12 +151,12 @@ void CommandGroup::_Execute() {
       }
     }
 
-    entry = m_commands[m_currentCommandIndex];
+    entry = &m_commands[m_currentCommandIndex];
     cmd = nullptr;
 
-    switch (entry.m_state) {
+    switch (entry->m_state) {
       case CommandGroupEntry::kSequence_InSequence:
-        cmd = entry.m_command;
+        cmd = entry->m_command;
         if (firstRun) {
           cmd->StartRunning();
           CancelConflicts(cmd);
@@ -165,32 +165,43 @@ void CommandGroup::_Execute() {
         break;
 
       case CommandGroupEntry::kSequence_BranchPeer:
+        // Start executing a parallel command and advance to next entry in group
         m_currentCommandIndex++;
-        entry.m_command->Start();
+        entry->m_command->Start();
         break;
 
       case CommandGroupEntry::kSequence_BranchChild:
         m_currentCommandIndex++;
-        CancelConflicts(entry.m_command);
-        entry.m_command->StartRunning();
+
+        /* Causes scheduler to skip children of current command which require
+         * the same subsystems as it
+         */
+        CancelConflicts(entry->m_command);
+        entry->m_command->StartRunning();
+
+        // Add current command entry to list of children of this group
         m_children.push_back(entry);
         break;
     }
   }
 
   // Run Children
-  for (auto iter = m_children.begin(); iter != m_children.end();) {
-    entry = *iter;
-    Command* child = entry.m_command;
-    if (entry.IsTimedOut()) child->_Cancel();
+  for (auto& entry : m_children) {
+    auto child = entry->m_command;
+    if (entry->IsTimedOut()) {
+      child->_Cancel();
+    }
 
+    // If child finished or was cancelled, set it to nullptr. nullptr entries
+    // are removed later.
     if (!child->Run()) {
       child->Removed();
-      iter = m_children.erase(iter);
-    } else {
-      iter++;
+      entry = nullptr;
     }
   }
+
+  m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
+                   m_children.end());
 }
 
 void CommandGroup::_End() {
@@ -203,8 +214,8 @@ void CommandGroup::_End() {
     cmd->Removed();
   }
 
-  for (auto iter = m_children.begin(); iter != m_children.end(); iter++) {
-    Command* cmd = iter->m_command;
+  for (auto& child : m_children) {
+    Command* cmd = child->m_command;
     cmd->_Cancel();
     cmd->Removed();
   }
@@ -215,13 +226,11 @@ void CommandGroup::_Interrupted() { _End(); }
 
 void CommandGroup::CancelConflicts(Command* command) {
   for (auto childIter = m_children.begin(); childIter != m_children.end();) {
-    Command* child = childIter->m_command;
+    Command* child = (*childIter)->m_command;
     bool erased = false;
 
-    Command::SubsystemSet requirements = command->GetRequirements();
-    for (auto requirementIter = requirements.begin();
-         requirementIter != requirements.end(); requirementIter++) {
-      if (child->DoesRequire(*requirementIter)) {
+    for (auto& requirement : command->GetRequirements()) {
+      if (child->DoesRequire(requirement)) {
         child->_Cancel();
         child->Removed();
         childIter = m_children.erase(childIter);

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

@@ -33,7 +33,7 @@ void Scheduler::AddCommand(Command* command) {
 
 void Scheduler::AddButton(ButtonScheduler* button) {
   std::lock_guard<wpi::mutex> lock(m_buttonsMutex);
-  m_buttons.push_back(button);
+  m_buttons.emplace_back(button);
 }
 
 void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
@@ -50,27 +50,23 @@ void Scheduler::Run() {
     if (!m_enabled) return;
 
     std::lock_guard<wpi::mutex> lock(m_buttonsMutex);
-    for (auto rButtonIter = m_buttons.rbegin(); rButtonIter != m_buttons.rend();
-         rButtonIter++) {
-      (*rButtonIter)->Execute();
+    for (auto& button : m_buttons) {
+      button->Execute();
     }
   }
 
   // Call every subsystem's periodic method
-  for (auto subsystemIter = m_subsystems.begin();
-       subsystemIter != m_subsystems.end(); subsystemIter++) {
-    Subsystem* subsystem = *subsystemIter;
+  for (auto& subsystem : m_subsystems) {
     subsystem->Periodic();
   }
 
   m_runningCommandsChanged = false;
 
   // Loop through the commands
-  for (auto commandIter = m_commands.begin();
-       commandIter != m_commands.end();) {
-    Command* command = *commandIter;
+  for (auto cmdIter = m_commands.begin(); cmdIter != m_commands.end();) {
+    Command* command = *cmdIter;
     // Increment before potentially removing to keep the iterator valid
-    ++commandIter;
+    ++cmdIter;
     if (!command->Run()) {
       Remove(command);
       m_runningCommandsChanged = true;
@@ -80,21 +76,18 @@ void Scheduler::Run() {
   // Add the new things
   {
     std::lock_guard<wpi::mutex> lock(m_additionsMutex);
-    for (auto additionsIter = m_additions.begin();
-         additionsIter != m_additions.end(); additionsIter++) {
-      ProcessCommandAddition(*additionsIter);
+    for (auto& addition : m_additions) {
+      ProcessCommandAddition(addition);
     }
     m_additions.clear();
   }
 
   // Add in the defaults
-  for (auto subsystemIter = m_subsystems.begin();
-       subsystemIter != m_subsystems.end(); subsystemIter++) {
-    Subsystem* lock = *subsystemIter;
-    if (lock->GetCurrentCommand() == nullptr) {
-      ProcessCommandAddition(lock->GetDefaultCommand());
+  for (auto& subsystem : m_subsystems) {
+    if (subsystem->GetCurrentCommand() == nullptr) {
+      ProcessCommandAddition(subsystem->GetDefaultCommand());
     }
-    lock->ConfirmCommand();
+    subsystem->ConfirmCommand();
   }
 }
 
@@ -106,10 +99,8 @@ void Scheduler::Remove(Command* command) {
 
   if (!m_commands.erase(command)) return;
 
-  Command::SubsystemSet requirements = command->GetRequirements();
-  for (auto iter = requirements.begin(); iter != requirements.end(); iter++) {
-    Subsystem* lock = *iter;
-    lock->SetCurrentCommand(nullptr);
+  for (auto& requirement : command->GetRequirements()) {
+    requirement->SetCurrentCommand(nullptr);
   }
 
   command->Removed();
@@ -149,12 +140,10 @@ void Scheduler::InitSendable(SendableBuilder& builder) {
 
     // Cancel commands whose cancel buttons were pressed on the SmartDashboard
     if (!toCancel.empty()) {
-      for (auto commandIter = m_commands.begin();
-           commandIter != m_commands.end(); ++commandIter) {
-        for (size_t i = 0; i < toCancel.size(); i++) {
-          Command* c = *commandIter;
-          if (c->GetID() == toCancel[i]) {
-            c->Cancel();
+      for (auto& command : m_commands) {
+        for (const auto& cancelled : toCancel) {
+          if (command->GetID() == cancelled) {
+            command->Cancel();
           }
         }
       }
@@ -166,11 +155,9 @@ void Scheduler::InitSendable(SendableBuilder& builder) {
     if (m_runningCommandsChanged) {
       commands.resize(0);
       ids.resize(0);
-      for (auto commandIter = m_commands.begin();
-           commandIter != m_commands.end(); ++commandIter) {
-        Command* c = *commandIter;
-        commands.push_back(c->GetName());
-        ids.push_back(c->GetID());
+      for (const auto& command : m_commands) {
+        commands.emplace_back(command->GetName());
+        ids.emplace_back(command->GetID());
       }
       m_namesEntry.SetStringArray(commands);
       m_idsEntry.SetDoubleArray(ids);
@@ -198,24 +185,20 @@ void Scheduler::ProcessCommandAddition(Command* command) {
   if (found == m_commands.end()) {
     // Check that the requirements can be had
     Command::SubsystemSet requirements = command->GetRequirements();
-    for (Command::SubsystemSet::iterator iter = requirements.begin();
-         iter != requirements.end(); iter++) {
-      Subsystem* lock = *iter;
-      if (lock->GetCurrentCommand() != nullptr &&
-          !lock->GetCurrentCommand()->IsInterruptible())
+    for (const auto& requirement : requirements) {
+      if (requirement->GetCurrentCommand() != nullptr &&
+          !requirement->GetCurrentCommand()->IsInterruptible())
         return;
     }
 
     // Give it the requirements
     m_adding = true;
-    for (Command::SubsystemSet::iterator iter = requirements.begin();
-         iter != requirements.end(); iter++) {
-      Subsystem* lock = *iter;
-      if (lock->GetCurrentCommand() != nullptr) {
-        lock->GetCurrentCommand()->Cancel();
-        Remove(lock->GetCurrentCommand());
+    for (auto& requirement : requirements) {
+      if (requirement->GetCurrentCommand() != nullptr) {
+        requirement->GetCurrentCommand()->Cancel();
+        Remove(requirement->GetCurrentCommand());
       }
-      lock->SetCurrentCommand(command);
+      requirement->SetCurrentCommand(command);
     }
     m_adding = false;
 

wpilibc/src/main/native/cpp/Commands/Subsystem.cpp

@@ -24,16 +24,8 @@ void Subsystem::SetDefaultCommand(Command* command) {
   if (command == nullptr) {
     m_defaultCommand = nullptr;
   } else {
-    bool found = false;
-    Command::SubsystemSet requirements = command->GetRequirements();
-    for (auto iter = requirements.begin(); iter != requirements.end(); iter++) {
-      if (*iter == this) {
-        found = true;
-        break;
-      }
-    }
-
-    if (!found) {
+    const auto& reqs = command->GetRequirements();
+    if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
       wpi_setWPIErrorWithContext(
           CommandIllegalUse, "A default command must require the subsystem");
       return;