diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 537176519..899a32acb 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -49,6 +49,7 @@ enum class PreCond SUCCESS_IF, SKIP_IF, WHILE_TRUE, + ALWAYS, COUNT_ }; diff --git a/src/tree_node.cpp b/src/tree_node.cpp index 7b7a4ac88..28767f8c6 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -208,24 +208,28 @@ Expected 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()) + auto execute_result = parse_executor(env); + // always + if(preID == PreCond::ALWAYS) { - 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(""); + } + auto bool_result = execute_result.cast(); + // 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; } @@ -476,6 +480,8 @@ std::string toStr(const PreCond& pre) return "_skipIf"; case PreCond::WHILE_TRUE: return "_while"; + case PreCond::ALWAYS: + return "_onStart"; default: return "Undefined"; } diff --git a/tests/gtest_preconditions.cpp b/tests/gtest_preconditions.cpp index 7a3df89f7..be1b030d3 100644 --- a/tests/gtest_preconditions.cpp +++ b/tests/gtest_preconditions.cpp @@ -397,3 +397,32 @@ TEST(Preconditions, WhileCallsOnHalt) ASSERT_EQ(status, BT::NodeStatus::SKIPPED); ASSERT_EQ(tree.rootBlackboard()->get("B"), 69); } + +TEST(Preconditions, OnStart) +{ + BehaviorTreeFactory factory; + std::array counters; + RegisterTestTick(factory, "Test", counters); + + static constexpr auto xml_text = R"( + + + + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + const auto status = tree.tickWhileRunning(); + ASSERT_EQ(status, NodeStatus::SUCCESS); + ASSERT_EQ(tree.rootBlackboard()->get("B"), 70); + ASSERT_EQ(tree.rootBlackboard()->get("C"), 70); + ASSERT_EQ(counters[0], 1); + ASSERT_EQ(counters[1], 0); + ASSERT_EQ(counters[2], 1); +}