Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions include/behaviortree_cpp/tree_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,12 @@ enum class PreCond
SUCCESS_IF,
SKIP_IF,
WHILE_TRUE,
ON_START,
COUNT_
};

static const std::array<std::string, 4> PreCondNames = { //
"_failureIf", "_successIf", "_skipIf", "_while"
static const std::array<std::string, 5> PreCondNames = { //
"_failureIf", "_successIf", "_skipIf", "_while", "_onStart"
};

enum class PostCond
Expand Down
35 changes: 19 additions & 16 deletions src/tree_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -208,24 +208,27 @@ Expected<NodeStatus> TreeNode::checkPreConditions()
// Some preconditions are applied only when the node state is IDLE or SKIPPED
if(_p->status == NodeStatus::IDLE || _p->status == NodeStatus::SKIPPED)
{
// what to do if the condition is true
if(parse_executor(env).cast<bool>())
auto execute_result = parse_executor(env);
if(preID == PreCond::ON_START)
{
if(preID == PreCond::FAILURE_IF)
{
return NodeStatus::FAILURE;
}
else if(preID == PreCond::SUCCESS_IF)
{
return NodeStatus::SUCCESS;
}
else if(preID == PreCond::SKIP_IF)
{
return NodeStatus::SKIPPED;
}
return nonstd::make_unexpected(""); // no precondition
}
auto bool_result = execute_result.cast<bool>();
// what to do if the condition is true and the precondition is xx_IF
if(bool_result && preID == PreCond::FAILURE_IF)
{
return NodeStatus::FAILURE;
}
if(bool_result && preID == PreCond::SUCCESS_IF)
{
return NodeStatus::SUCCESS;
}
if(bool_result && preID == PreCond::SKIP_IF)
{
return NodeStatus::SKIPPED;
}
// if the conditions is false
else if(preID == PreCond::WHILE_TRUE)
// if the condition is false and the precondition is WHILE_TRUE, skip the node
if(!bool_result && preID == PreCond::WHILE_TRUE)
{
return NodeStatus::SKIPPED;
}
Expand Down
27 changes: 27 additions & 0 deletions tests/gtest_preconditions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -397,3 +397,30 @@ TEST(Preconditions, WhileCallsOnHalt)
ASSERT_EQ(status, BT::NodeStatus::SKIPPED);
ASSERT_EQ(tree.rootBlackboard()->get<int>("B"), 69);
}

TEST(Preconditions, OnStart)
{
BehaviorTreeFactory factory;
std::array<int, 3> counters;
RegisterTestTick(factory, "Test", counters);
static constexpr auto xml_text = R"(
<root BTCPP_format="4">
<BehaviorTree ID="Main">
<Sequence>
<AlwaysSuccess _onStart="B:=69"/>
<TestA/>
<TestB _successIf="B==69"/>
<TestC _onStart="C:=70" _post="B=70"/>
</Sequence>
</BehaviorTree>
</root>
)";
auto tree = factory.createTreeFromText(xml_text);
const auto status = tree.tickWhileRunning();
ASSERT_EQ(status, NodeStatus::SUCCESS);
ASSERT_EQ(tree.rootBlackboard()->get<int>("B"), 70);
ASSERT_EQ(tree.rootBlackboard()->get<int>("C"), 70);
ASSERT_EQ(counters[0], 1);
ASSERT_EQ(counters[1], 0);
ASSERT_EQ(counters[2], 1);
}
Loading