Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Clean up Command container iteration code (#73)

Browse Source
This commit is contained in:
Tyler Veness
2018-06-23 19:41:45 -05:00
committed by Peter Johnson
parent ea7d11b1db
commit b7807bf9d2
9 changed files with 198 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
wpilibc/src/main/native/cpp/Commands/CommandGroup.cpp
View File

@@ -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);