fix scheduledCommands.erase() being called in the same sets iterator

wpilibsuite · May 6, 2024 · 3388b06 · 3388b06
1 parent 6a73ca8
commit 3388b06
Showing 1 changed file with 8 additions and 1 deletion.
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -196,6 +196,7 @@ void CommandScheduler::Run() {
   m_impl->inRunLoop = true;
   bool isDisabled = frc::RobotState::IsDisabled();
   // Run scheduled commands, remove finished commands.
+  auto toRemove = std::vector<Command*>(m_impl->scheduledCommands.size());
   for (Command* command : m_impl->scheduledCommands) {
     if (isDisabled && !command->RunsWhenDisabled()) {
       Cancel(command, std::nullopt);
@@ -216,14 +217,20 @@ void CommandScheduler::Run() {
       }
       m_impl->endingCommands.erase(command);
 
-      m_impl->scheduledCommands.erase(command);
+      toRemove.push_back(command);
       for (auto&& requirement : command->GetRequirements()) {
         m_impl->requirements.erase(requirement);
       }
 
       m_watchdog.AddEpoch(command->GetName() + ".End(false)");
     }
   }
+
+  // remove the commands that should be removed
+  for (auto&& command : toRemove) {
+    m_impl->scheduledCommands.erase(command);
+  }
+
   m_impl->inRunLoop = false;
 
   for (Command* command : m_impl->toSchedule) {