diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index bd77c4c72..c89769413 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -50,11 +50,12 @@ enum class PreCond SUCCESS_IF, SKIP_IF, WHILE_TRUE, + ON_START, COUNT_ }; -static const std::array PreCondNames = { // - "_failureIf", "_successIf", "_skipIf", "_while" +static const std::array PreCondNames = { // + "_failureIf", "_successIf", "_skipIf", "_while", "_onStart" }; enum class PostCond diff --git a/src/tree_node.cpp b/src/tree_node.cpp index 0d4dd66bd..a1dae2042 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -208,24 +208,27 @@ 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); + 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(); + // 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; } diff --git a/tests/gtest_preconditions.cpp b/tests/gtest_preconditions.cpp index 7a3df89f7..6d86cf6ef 100644 --- a/tests/gtest_preconditions.cpp +++ b/tests/gtest_preconditions.cpp @@ -397,3 +397,30 @@ 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); +}