diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 8c528ccf6..dca440acc 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -429,6 +429,7 @@ class TreeNode friend class BehaviorTreeFactory; friend class DecoratorNode; friend class ControlNode; + friend class ThreadedAction; friend class Tree; [[nodiscard]] NodeConfig& config(); diff --git a/src/action_node.cpp b/src/action_node.cpp index 8e47ad940..bda304ee4 100644 --- a/src/action_node.cpp +++ b/src/action_node.cpp @@ -14,6 +14,8 @@ #define MINICORO_IMPL #include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/basic_types.h" + // minicoro.h includes this inside extern "C": include it first, at global scope. #ifdef __EMSCRIPTEN__ #include @@ -21,6 +23,7 @@ #include "minicoro.h" +#include #include using namespace BT; @@ -207,6 +210,25 @@ void StatefulActionNode::halt() NodeStatus BT::ThreadedAction::executeTick() { using lock_type = std::unique_lock; + + // This method overrides TreeNode::executeTick(), so it must handle the + // pre-conditions (_skipIf, _failureIf, _successIf, _while) and the + // post-conditions itself, as TreeNode::executeTick() does. + if(auto precond = checkPreConditions()) + { + const NodeStatus precond_status = precond.value(); + if(isStatusCompleted(precond_status)) + { + checkPostConditions(precond_status); + } + // preserve the IDLE state if skipped, but communicate SKIPPED to parent + if(precond_status != NodeStatus::SKIPPED) + { + setStatus(precond_status); + } + return precond_status; + } + //send signal to other thread. // The other thread is in charge for changing the status if(status() == NodeStatus::IDLE) @@ -236,16 +258,24 @@ NodeStatus BT::ThreadedAction::executeTick() }); } - const lock_type l(mutex_); - if(exptr_) + NodeStatus new_status = NodeStatus::IDLE; { - // The official interface of std::exception_ptr does not define any move - // semantics. Thus, we copy and reset exptr_ manually. - const auto exptr_copy = exptr_; - exptr_ = nullptr; - std::rethrow_exception(exptr_copy); + const lock_type l(mutex_); + if(exptr_) + { + // The official interface of std::exception_ptr does not define any move + // semantics. Thus, we copy and reset exptr_ manually. + const auto exptr_copy = exptr_; + exptr_ = nullptr; + std::rethrow_exception(exptr_copy); + } + new_status = status(); } - return status(); + if(isStatusCompleted(new_status)) + { + checkPostConditions(new_status); + } + return new_status; } void ThreadedAction::halt() diff --git a/tests/gtest_skipping.cpp b/tests/gtest_skipping.cpp index a33ad2731..3420bfdfc 100644 --- a/tests/gtest_skipping.cpp +++ b/tests/gtest_skipping.cpp @@ -1,9 +1,12 @@ #include "action_test_node.h" #include "test_helper.hpp" +#include "behaviortree_cpp/action_node.h" #include "behaviortree_cpp/basic_types.h" #include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/tree_node.h" +#include #include #include @@ -219,3 +222,55 @@ TEST(SkippingLogic, WhileSkip) // counters[1] contains the number of times TestB was ticked ASSERT_EQ(counters[1], 0); } + +namespace +{ +class ThreadedTickCounter : public ThreadedAction +{ +public: + ThreadedTickCounter(const std::string& name, const NodeConfig& config, + std::atomic_int* ticks) + : ThreadedAction(name, config), ticks_(ticks) + {} + + NodeStatus tick() override + { + ++(*ticks_); + return NodeStatus::SUCCESS; + } + +private: + std::atomic_int* ticks_; +}; +} // namespace + +TEST(SkippingLogic, ThreadedActionPreAndPostConditions) +{ + // ThreadedAction overrides executeTick(), it must still honor the + // pre-conditions and run the post-conditions. + BehaviorTreeFactory factory; + std::atomic_int ticks = 0; + factory.registerNodeType("Threaded", PortsList{}, &ticks); + + const std::string xml_text = R"( + + + +