Skip to content

Commit

Permalink
make loops preemptable
Browse files Browse the repository at this point in the history
  • Loading branch information
Davide Faconti committed Mar 13, 2019
1 parent f590d0f commit 2dc84b1
Show file tree
Hide file tree
Showing 5 changed files with 152 additions and 154 deletions.
62 changes: 30 additions & 32 deletions src/controls/fallback_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@ namespace BT
{

FallbackNode::FallbackNode(const std::string& name)
: ControlNode::ControlNode(name, {} )
,current_child_idx_(0)
: ControlNode::ControlNode(name, {} )
,current_child_idx_(0)
{
setRegistrationID("Fallback");
}
Expand All @@ -29,44 +29,42 @@ NodeStatus FallbackNode::tick()

setStatus(NodeStatus::RUNNING);

while (current_child_idx_ < children_count)
{
TreeNode* current_child_node = children_nodes_[current_child_idx_];
const NodeStatus child_status = current_child_node->executeTick();

switch (child_status)
TreeNode* current_child_node = children_nodes_[current_child_idx_];
const NodeStatus child_status = current_child_node->executeTick();

switch (child_status)
{
case NodeStatus::RUNNING:
{
return child_status;
}
case NodeStatus::SUCCESS:
{
case NodeStatus::RUNNING:
haltChildren(0);
current_child_idx_ = 0;
return child_status;
}
case NodeStatus::FAILURE:
{
current_child_idx_++;
if( current_child_idx_ < children_count )
{
return child_status;
return NodeStatus::RUNNING;
}
case NodeStatus::SUCCESS:
{
else{
haltChildren(0);
current_child_idx_ = 0;
return child_status;
}
case NodeStatus::FAILURE:
{
current_child_idx_++;
}
break;

case NodeStatus::IDLE:
{
throw LogicError("A child node must never return IDLE");
return NodeStatus::FAILURE;
}
} // end switch
} // end while loop
}

// The entire while loop completed. This means that all the children returned FAILURE.
if (current_child_idx_ == children_count)
{
haltChildren(0);
current_child_idx_ = 0;
}

return NodeStatus::FAILURE;
case NodeStatus::IDLE:
{
throw LogicError("A child node must never return IDLE");
}
} // end switch
// never reached
}

void FallbackNode::halt()
Expand Down
60 changes: 29 additions & 31 deletions src/controls/sequence_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ namespace BT


SequenceNode::SequenceNode(const std::string& name)
: ControlNode::ControlNode(name, {} )
: ControlNode::ControlNode(name, {} )
, current_child_idx_(0)
{
setRegistrationID("Sequence");
Expand All @@ -37,44 +37,42 @@ NodeStatus SequenceNode::tick()

setStatus(NodeStatus::RUNNING);

while (current_child_idx_ < children_count)
{
TreeNode* current_child_node = children_nodes_[current_child_idx_];
const NodeStatus child_status = current_child_node->executeTick();
TreeNode* current_child_node = children_nodes_[current_child_idx_];
const NodeStatus child_status = current_child_node->executeTick();

switch (child_status)
switch (child_status)
{
case NodeStatus::RUNNING:
{
return child_status;
}
case NodeStatus::FAILURE:
{
case NodeStatus::RUNNING:
// Reset on failure
haltChildren(0);
current_child_idx_ = 0;
return child_status;
}
case NodeStatus::SUCCESS:
{
current_child_idx_++;
if( current_child_idx_ < children_count )
{
return child_status;
return NodeStatus::RUNNING;
}
case NodeStatus::FAILURE:
{
// Reset on failure
else{
haltChildren(0);
current_child_idx_ = 0;
return child_status;
}
case NodeStatus::SUCCESS:
{
current_child_idx_++;
}
break;

case NodeStatus::IDLE:
{
throw LogicError("A child node must never return IDLE");
return NodeStatus::SUCCESS;
}
} // end switch
} // end while loop
}

// The entire while loop completed. This means that all the children returned SUCCESS.
if (current_child_idx_ == children_count)
{
haltChildren(0);
current_child_idx_ = 0;
}
return NodeStatus::SUCCESS;
case NodeStatus::IDLE:
{
throw LogicError("A child node must never return IDLE");
}
} // end switch
// never reached
}

}
62 changes: 30 additions & 32 deletions src/controls/sequence_star_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ namespace BT
{

SequenceStarNode::SequenceStarNode(const std::string& name)
: ControlNode::ControlNode(name, {} )
: ControlNode::ControlNode(name, {} )
, current_child_idx_(0)
{
setRegistrationID("SequenceStar");
Expand All @@ -29,43 +29,41 @@ NodeStatus SequenceStarNode::tick()

setStatus(NodeStatus::RUNNING);

while (current_child_idx_ < children_count)
{
TreeNode* current_child_node = children_nodes_[current_child_idx_];
const NodeStatus child_status = current_child_node->executeTick();
TreeNode* current_child_node = children_nodes_[current_child_idx_];
const NodeStatus child_status = current_child_node->executeTick();

switch (child_status)
switch (child_status)
{
case NodeStatus::RUNNING:
{
case NodeStatus::RUNNING:
{
return child_status;
}
case NodeStatus::FAILURE:
{
// DO NOT reset on failure
haltChildren(current_child_idx_);
return child_status;
}
case NodeStatus::SUCCESS:
return child_status;
}
case NodeStatus::FAILURE:
{
// DO NOT reset on failure
haltChildren(current_child_idx_);
return child_status;
}
case NodeStatus::SUCCESS:
{
current_child_idx_++;
if( current_child_idx_ < children_count )
{
current_child_idx_++;
return NodeStatus::RUNNING;
}
break;

case NodeStatus::IDLE:
{
throw LogicError("A child node must never return IDLE");
else{
haltChildren(0);
current_child_idx_ = 0;
return NodeStatus::SUCCESS;
}
} // end switch
} // end while loop
}

// The entire while loop completed. This means that all the children returned SUCCESS.
if (current_child_idx_ == children_count)
{
haltChildren(0);
current_child_idx_ = 0;
}
return NodeStatus::SUCCESS;
case NodeStatus::IDLE:
{
throw LogicError("A child node must never return IDLE");
}
} // end switch
// never reached
}

void SequenceStarNode::halt()
Expand Down
62 changes: 32 additions & 30 deletions src/decorators/repeat_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,19 +18,19 @@ namespace BT
constexpr const char* RepeatNode::NUM_CYCLES;

RepeatNode::RepeatNode(const std::string& name, unsigned int NTries)
: DecoratorNode(name, {} ),
num_cycles_(NTries),
try_index_(0),
read_parameter_from_ports_(false)
: DecoratorNode(name, {} ),
num_cycles_(NTries),
try_index_(0),
read_parameter_from_ports_(false)
{
setRegistrationID("Repeat");
setRegistrationID("Repeat");
}

RepeatNode::RepeatNode(const std::string& name, const NodeConfiguration& config)
: DecoratorNode(name, config),
num_cycles_(0),
try_index_(0),
read_parameter_from_ports_(true)
num_cycles_(0),
try_index_(0),
read_parameter_from_ports_(true)
{

}
Expand All @@ -47,38 +47,40 @@ NodeStatus RepeatNode::tick()

setStatus(NodeStatus::RUNNING);

while (try_index_ < num_cycles_)
{
NodeStatus child_state = child_node_->executeTick();
NodeStatus child_state = child_node_->executeTick();

switch (child_state)
switch (child_state)
{
case NodeStatus::SUCCESS:
{
case NodeStatus::SUCCESS:
try_index_++;
if( try_index_ < num_cycles_)
{
try_index_++;
return NodeStatus::RUNNING;
}
break;

case NodeStatus::FAILURE:
{
else{
try_index_ = 0;
return (NodeStatus::FAILURE);
return NodeStatus::SUCCESS;
}
}

case NodeStatus::RUNNING:
{
return NodeStatus::RUNNING;
}
case NodeStatus::FAILURE:
{
try_index_ = 0;
return (NodeStatus::FAILURE);
}

default:
{
throw LogicError("A child node must never return IDLE");
}
case NodeStatus::RUNNING:
{
return NodeStatus::RUNNING;
}
}

try_index_ = 0;
return NodeStatus::SUCCESS;
default:
{
throw LogicError("A child node must never return IDLE");
}
}
// never reached
}

void RepeatNode::halt()
Expand Down
Loading

0 comments on commit 2dc84b1

Please sign in to comment.