From e42d77bd55d16b2af76d282873e6ba9087379f9b Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Sat, 26 Dec 2020 16:07:18 -0300 Subject: [PATCH 1/4] Formatting fixes --- src/xml_parsing.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index 7dedc941f..55c7e75de 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -647,8 +647,9 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, { recursivelyCreateTree( node->name(), output_tree, blackboard, node ); } - else{ - // Creating an isolated + else + { + // Creating an isolated blackboard auto new_bb = Blackboard::create(blackboard); for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) From 29aff66659617ca6916cc07b64c1038b1561def2 Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Mon, 28 Dec 2020 13:26:05 -0300 Subject: [PATCH 2/4] Improve subtree tests --- tests/gtest_subtree.cpp | 112 ++++++++++++++++++++++++++-------------- 1 file changed, 73 insertions(+), 39 deletions(-) diff --git a/tests/gtest_subtree.cpp b/tests/gtest_subtree.cpp index c79fd8182..605cfd045 100644 --- a/tests/gtest_subtree.cpp +++ b/tests/gtest_subtree.cpp @@ -77,14 +77,14 @@ TEST(SubTree, GoodRemapping) - - - + + + - + )"; @@ -92,9 +92,12 @@ TEST(SubTree, GoodRemapping) factory.registerNodeType("SaySomething"); factory.registerNodeType("CopyPorts"); - Tree tree = factory.createTreeFromText(xml_text); + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = factory.createTreeFromText(xml_text, blackboard); auto ret = tree.tickRoot(); ASSERT_EQ(ret, NodeStatus::SUCCESS ); + auto out_key_value = blackboard->get("base_bb_out"); + ASSERT_EQ("hello", out_key_value); } TEST(SubTree, BadRemapping) @@ -142,7 +145,7 @@ TEST(SubTree, BadRemapping) EXPECT_ANY_THROW( tree_bad_out.tickRoot() ); } -TEST(SubTree, SubtreePlusA) +TEST(SubTree, SubtreeSharedBlackboard) { static const char* xml_text = R"( @@ -150,59 +153,80 @@ TEST(SubTree, SubtreePlusA) - - - - - + + + - + + + + )"; BehaviorTreeFactory factory; - factory.registerNodeType("SaySomething"); + factory.registerNodeType("CopyPorts"); - Tree tree = factory.createTreeFromText(xml_text); + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = factory.createTreeFromText(xml_text, blackboard); auto ret = tree.tickRoot(); ASSERT_EQ(ret, NodeStatus::SUCCESS ); + + auto shared_bb_out_1_value = blackboard->get("shared_bb_out_1"); + auto shared_bb_out_2_value = blackboard->get("shared_bb_out_2"); + ASSERT_EQ("abc", shared_bb_out_1_value); + ASSERT_EQ("def", shared_bb_out_2_value); } -TEST(SubTree, SubtreePlusB) +TEST(SubTree, SubtreePlusA) { - static const char* xml_text = R"( + static const char* xml_text = R"( - - - + + + + + - - - - - - + + + + + + + + + + )"; - BehaviorTreeFactory factory; - factory.registerNodeType("SaySomething"); + BehaviorTreeFactory factory; + factory.registerNodeType("CopyPorts"); - Tree tree = factory.createTreeFromText(xml_text); - auto ret = tree.tickRoot(); - ASSERT_EQ(ret, NodeStatus::SUCCESS ); + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = factory.createTreeFromText(xml_text, blackboard); + auto ret = tree.tickRoot(); + ASSERT_EQ(ret, NodeStatus::SUCCESS ); + + auto abc_base_bb_out_value = blackboard->get("abc_base_bb_out"); + auto def_base_bb_out_value = blackboard->get("def_base_bb_out"); + auto ghi_base_bb_out_value = blackboard->get("ghi_base_bb_out"); + ASSERT_EQ("abc", abc_base_bb_out_value); + ASSERT_EQ("def", def_base_bb_out_value); + ASSERT_EQ("ghi", ghi_base_bb_out_value); } -TEST(SubTree, SubtreePlusC) +TEST(SubTree, SubtreePlusB) { static const char* xml_text = R"( @@ -210,26 +234,35 @@ TEST(SubTree, SubtreePlusC) - - - + + + - - + + + )"; BehaviorTreeFactory factory; - factory.registerNodeType("SaySomething"); + factory.registerNodeType("CopyPorts"); - Tree tree = factory.createTreeFromText(xml_text); + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = factory.createTreeFromText(xml_text, blackboard); auto ret = tree.tickRoot(); - ASSERT_EQ(ret, NodeStatus::SUCCESS ); + ASSERT_EQ(ret, NodeStatus::SUCCESS); + + auto abc_base_bb_out_value = blackboard->get("auto_mapped_out_1"); + auto def_base_bb_out_value = blackboard->get("auto_mapped_out_2"); + auto ghi_base_bb_out_value = blackboard->get("auto_mapped_out_3"); + ASSERT_EQ("abc", abc_base_bb_out_value); + ASSERT_EQ("def", def_base_bb_out_value); + ASSERT_EQ("ghi", ghi_base_bb_out_value); } @@ -277,3 +310,4 @@ TEST(SubTree, SubtreePlusD) + From 488ca9a67819d15302aece5d47948b1df557813e Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Sun, 27 Dec 2020 11:55:34 -0300 Subject: [PATCH 3/4] Remove dead code --- src/xml_parsing.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index 55c7e75de..95a3975f3 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -827,20 +827,4 @@ std::string writeTreeNodesModelXML(const BehaviorTreeFactory& factory) return std::string(printer.CStr(), size_t(printer.CStrSize() - 1)); } -Tree buildTreeFromText(const BehaviorTreeFactory& factory, const std::string& text, - const Blackboard::Ptr& blackboard) -{ - XMLParser parser(factory); - parser.loadFromText(text); - return parser.instantiateTree(blackboard); -} - -Tree buildTreeFromFile(const BehaviorTreeFactory& factory, const std::string& filename, - const Blackboard::Ptr& blackboard) -{ - XMLParser parser(factory); - parser.loadFromFile(filename); - return parser.instantiateTree(blackboard); -} - } From 7902cafcf37b95a6d628da6760c397885e38c852 Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Sun, 27 Dec 2020 11:58:04 -0300 Subject: [PATCH 4/4] Add the possibility to override builtin nodes and set the root subtree --- include/behaviortree_cpp_v3/bt_factory.h | 28 +- include/behaviortree_cpp_v3/bt_parser.h | 12 +- .../decorators/subtree_node.h | 16 +- .../decorators/subtreemock_node.h | 70 ++++ include/behaviortree_cpp_v3/xml_parsing.h | 12 +- src/bt_factory.cpp | 19 +- src/decorators/subtree_node.cpp | 8 +- src/xml_parsing.cpp | 218 +++++++---- tests/CMakeLists.txt | 1 + tests/gtest_subtreemock.cpp | 360 ++++++++++++++++++ 10 files changed, 658 insertions(+), 86 deletions(-) create mode 100644 include/behaviortree_cpp_v3/decorators/subtreemock_node.h create mode 100644 tests/gtest_subtreemock.cpp diff --git a/include/behaviortree_cpp_v3/bt_factory.h b/include/behaviortree_cpp_v3/bt_factory.h index ac7c424d2..212c6c3e0 100644 --- a/include/behaviortree_cpp_v3/bt_factory.h +++ b/include/behaviortree_cpp_v3/bt_factory.h @@ -368,11 +368,33 @@ class BehaviorTreeFactory /// List of builtin IDs. const std::set& builtinNodes() const; + /** + * @brief Creates a tree from the xml description in a file. + * @param[in] text String with the xml description of the tree. + * @param[in] blackboard Root blackboard, used by the root of the tree. Defaults to creating a new one. + * @param[in] root_subtree_id Id of the subtree used as the root to build the tree from. Defaults to empty, + * which causes the default root discovery algorithm to be used. + * @param[in] mock_subtrees Replaces subtree instantiations with a special node that can be configured + * to call mock callbacks for testing. + */ Tree createTreeFromText(const std::string& text, - Blackboard::Ptr blackboard = Blackboard::create()); - + Blackboard::Ptr blackboard = Blackboard::create(), + const std::string &root_subtree_id = "", + const bool mock_subtrees = false); + + /** + * @brief Creates a tree from the xml description in a file. + * @param[in] file_path Path to the file containing the xml description of the tree. + * @param[in] blackboard Root blackboard, used by the root of the tree. Defaults to creating a new one. + * @param[in] root_subtree_id Id of the subtree used as the root to build the tree from. Defaults to empty, + * which causes the default root discovery algorithm to be used. + * @param[in] mock_subtrees Replaces subtree instantiations with a special node that can be configured + * to call mock callbacks for testing. + */ Tree createTreeFromFile(const std::string& file_path, - Blackboard::Ptr blackboard = Blackboard::create()); + Blackboard::Ptr blackboard = Blackboard::create(), + const std::string &root_subtree_id = "", + const bool mock_subtrees = false); private: std::unordered_map builders_; diff --git a/include/behaviortree_cpp_v3/bt_parser.h b/include/behaviortree_cpp_v3/bt_parser.h index c5f979928..950b47a1c 100644 --- a/include/behaviortree_cpp_v3/bt_parser.h +++ b/include/behaviortree_cpp_v3/bt_parser.h @@ -25,7 +25,17 @@ class Parser virtual void loadFromText(const std::string& xml_text) = 0; - virtual Tree instantiateTree(const Blackboard::Ptr &root_blackboard) = 0; + /** + * @brief Instantiates a BehaviorTreee from the source, using a given blackboard and given root subtree. + * @param[in] root_blackboard Root blackboard, used by the root of the tree. + * @param[in] root_subtree_id Id of the subtree used as the root to build the tree from. Defaults to empty, + * which causes the default root discovery algorithm to be used. + * @param[in] mock_subtrees Replaces subtree instantiations with a special node that can be configured + * to call mock callbacks for testing. + */ + virtual Tree instantiateTree(const Blackboard::Ptr &root_blackboard, + const std::string &root_subtree = "", + const bool mock_subtrees = false) = 0; }; } diff --git a/include/behaviortree_cpp_v3/decorators/subtree_node.h b/include/behaviortree_cpp_v3/decorators/subtree_node.h index f9ae66d40..2e6e292f5 100644 --- a/include/behaviortree_cpp_v3/decorators/subtree_node.h +++ b/include/behaviortree_cpp_v3/decorators/subtree_node.h @@ -15,13 +15,10 @@ namespace BT class SubtreeNode : public DecoratorNode { public: - SubtreeNode(const std::string& name); + SubtreeNode(const std::string& name, const NodeConfiguration& config); virtual ~SubtreeNode() override = default; - private: - virtual BT::NodeStatus tick() override; - static PortsList providedPorts() { return { InputPort("__shared_blackboard", false, @@ -29,6 +26,9 @@ class SubtreeNode : public DecoratorNode "need to do port remapping to connect it to the parent") }; } + private: + virtual BT::NodeStatus tick() override; + virtual NodeType type() const override final { return NodeType::SUBTREE; @@ -80,19 +80,19 @@ class SubtreeNode : public DecoratorNode class SubtreePlusNode : public DecoratorNode { public: - SubtreePlusNode(const std::string& name); + SubtreePlusNode(const std::string& name, const NodeConfiguration& config); virtual ~SubtreePlusNode() override = default; -private: - virtual BT::NodeStatus tick() override; - static PortsList providedPorts() { return { InputPort("__autoremap", false, "If true, all the ports with the same name will be remapped") }; } +private: + virtual BT::NodeStatus tick() override; + virtual NodeType type() const override final { return NodeType::SUBTREE; diff --git a/include/behaviortree_cpp_v3/decorators/subtreemock_node.h b/include/behaviortree_cpp_v3/decorators/subtreemock_node.h new file mode 100644 index 000000000..3708466fd --- /dev/null +++ b/include/behaviortree_cpp_v3/decorators/subtreemock_node.h @@ -0,0 +1,70 @@ +#ifndef ACTION_SUBTREEMOCK_NODE_H +#define ACTION_SUBTREEMOCK_NODE_H + +#include "behaviortree_cpp_v3/action_node.h" + +#include +#include +#include + +namespace BT +{ +/** + * @brief The SubtreeMockNode is a node that replaces a whole subtree, allowing to replace its + * execution with the execution of a standin callback function. This allows mocking subtrees + * to simplify the testing of large behavior trees. + */ +class SubtreeMockNode : public SyncActionNode +{ + public: + class BlackboardProxy + { + public: + explicit BlackboardProxy(TreeNode* mock_node) : mock_node_{mock_node} + {} + + // thou shall not propagate or store this object + BlackboardProxy(const BlackboardProxy&) = delete; + BlackboardProxy(BlackboardProxy&&) = delete; + + template + Result getInput(const std::string& key, T& destination) const + { + return mock_node_->getInput(key, destination); + } + + template + Optional getInput(const std::string& key) const + { + return mock_node_->getInput(key); + } + + template + Result setOutput(const std::string& key, const T& value) + { + return mock_node_->setOutput(key, value); + } + + private: + TreeNode* mock_node_; + }; + + using CallbackFunction = std::function; + using CallbackMap = std::unordered_map; + + SubtreeMockNode(const std::string& name, const NodeConfiguration& config, + const CallbackFunction& callback) + : SyncActionNode{name, config}, callback_{callback} {} + + private: + CallbackFunction callback_; + + BT::NodeStatus tick() override { + BlackboardProxy bp(this); + return callback_(bp); + } +}; + +} // namespace BT + +#endif // ACTION_SUBTREEMOCK_NODE_H diff --git a/include/behaviortree_cpp_v3/xml_parsing.h b/include/behaviortree_cpp_v3/xml_parsing.h index 401dd1e11..0036212b7 100644 --- a/include/behaviortree_cpp_v3/xml_parsing.h +++ b/include/behaviortree_cpp_v3/xml_parsing.h @@ -25,7 +25,17 @@ class XMLParser: public Parser void loadFromText(const std::string& xml_text) override; - Tree instantiateTree(const Blackboard::Ptr &root_blackboard) override; + /** + * @brief Instantiates a BehaviorTreee from the source, using a given blackboard and given root subtree. + * @param[in] root_blackboard Root blackboard, used by the root of the tree. + * @param[in] root_subtree_id Id of the subtree used as the root to build the tree from. If left empty + * the default root discovery algorithm to be used. + * @param[in] mock_subtrees Replaces subtree instantiations with a special node that can be configured + * to call mock callbacks for testing. + */ + Tree instantiateTree(const Blackboard::Ptr& root_blackboard, + const std::string& root_subtree, + const bool mock_subtrees) override; private: diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index dbf3e2333..419f078ca 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -67,6 +67,9 @@ BehaviorTreeFactory::BehaviorTreeFactory() { builtin_IDs_.insert( it.first ); } + + // Register the SubtreeMock node as builtin to disallow unregistering it. + builtin_IDs_.insert("SubtreeMock"); } bool BehaviorTreeFactory::unregisterBuilder(const std::string& ID) @@ -248,21 +251,29 @@ const std::set &BehaviorTreeFactory::builtinNodes() const } Tree BehaviorTreeFactory::createTreeFromText(const std::string &text, - Blackboard::Ptr blackboard) + Blackboard::Ptr blackboard, + const std::string &root_subtree_id, + const bool mock_subtrees) { + if (mock_subtrees && (builders_.count("SubtreeMock") == 0)) { + throw RuntimeError("mock_subtrees is true but not SubtreeMock node has been registered " + "with the mock callbacks"); + } XMLParser parser(*this); parser.loadFromText(text); - auto tree = parser.instantiateTree(blackboard); + auto tree = parser.instantiateTree(blackboard, root_subtree_id, mock_subtrees); tree.manifests = this->manifests(); return tree; } Tree BehaviorTreeFactory::createTreeFromFile(const std::string &file_path, - Blackboard::Ptr blackboard) + Blackboard::Ptr blackboard, + const std::string &root_subtree_id, + const bool mock_subtrees) { XMLParser parser(*this); parser.loadFromFile(file_path); - auto tree = parser.instantiateTree(blackboard); + auto tree = parser.instantiateTree(blackboard, root_subtree_id, mock_subtrees); tree.manifests = this->manifests(); return tree; } diff --git a/src/decorators/subtree_node.cpp b/src/decorators/subtree_node.cpp index 20ffaf502..7671d8042 100644 --- a/src/decorators/subtree_node.cpp +++ b/src/decorators/subtree_node.cpp @@ -1,8 +1,8 @@ #include "behaviortree_cpp_v3/decorators/subtree_node.h" -BT::SubtreeNode::SubtreeNode(const std::string &name) : - DecoratorNode(name, {} ) +BT::SubtreeNode::SubtreeNode(const std::string& name, const NodeConfiguration& config) : + DecoratorNode(name, config) { setRegistrationID("SubTree"); } @@ -19,8 +19,8 @@ BT::NodeStatus BT::SubtreeNode::tick() //-------------------------------- -BT::SubtreePlusNode::SubtreePlusNode(const std::string &name) : - DecoratorNode(name, {} ) +BT::SubtreePlusNode::SubtreePlusNode(const std::string& name, const NodeConfiguration& config) : + DecoratorNode(name, config) { setRegistrationID("SubTreePlus"); } diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index 95a3975f3..25c8bd5ac 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -51,7 +51,8 @@ struct XMLParser::Pimpl void recursivelyCreateTree(const std::string& tree_ID, Tree& output_tree, Blackboard::Ptr blackboard, - const TreeNode::Ptr& root_parent); + const TreeNode::Ptr& root_parent, + const bool mock_subtrees); void getPortsRecursively(const XMLElement* element, std::vector &output_ports); @@ -417,14 +418,18 @@ void VerifyXML(const std::string& xml_text, } } -Tree XMLParser::instantiateTree(const Blackboard::Ptr& root_blackboard) +Tree XMLParser::instantiateTree(const Blackboard::Ptr& root_blackboard, const std::string &root_subtree_id, const bool mock_subtrees) { Tree output_tree; XMLElement* xml_root = _p->opened_documents.front()->RootElement(); std::string main_tree_ID; - if (xml_root->Attribute("main_tree_to_execute")) + + if (!root_subtree_id.empty()) { + main_tree_ID = root_subtree_id; + } + else if (xml_root->Attribute("main_tree_to_execute")) { main_tree_ID = xml_root->Attribute("main_tree_to_execute"); } @@ -446,7 +451,8 @@ Tree XMLParser::instantiateTree(const Blackboard::Ptr& root_blackboard) _p->recursivelyCreateTree(main_tree_ID, output_tree, root_blackboard, - TreeNode::Ptr() ); + TreeNode::Ptr(), + mock_subtrees ); return output_tree; } @@ -594,7 +600,7 @@ TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement *element, child_node = factory.instantiateTreeNode(instance_name, ID, config); } else if( tree_roots.count(ID) != 0) { - child_node = std::make_unique( instance_name ); + child_node = factory.instantiateTreeNode(instance_name, ID, config); } else{ throw RuntimeError( ID, " is not a registered node, nor a Subtree"); @@ -617,8 +623,9 @@ TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement *element, void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, Tree& output_tree, Blackboard::Ptr blackboard, - const TreeNode::Ptr& root_parent) -{ + const TreeNode::Ptr& root_parent, + const bool mock_subtrees) +{ std::function recursiveStep; recursiveStep = [&](const TreeNode::Ptr& parent, @@ -632,91 +639,169 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, { if( dynamic_cast(node.get()) ) { - bool is_isolated = true; + if (mock_subtrees) { + // build a set of builtin attributes for SubTree + std::set builtin_attributes{"ID"}; + for (const auto it: SubtreeNode::providedPorts()) { + builtin_attributes.emplace(it.first); + } - for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) - { - if( strcmp(attr->Name(), "__shared_blackboard") == 0 && - convertFromString(attr->Value()) == true ) + NodeConfiguration config; + config.blackboard = blackboard; + for (auto attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) { - is_isolated = false; + if (builtin_attributes.count(attr->Name())) { + continue; + } + /* subtree port have not inherent direction, so set it as inout */ + config.input_ports.insert({attr->Name(), "="}); + config.output_ports.insert({attr->Name(), "="}); } - } - if( !is_isolated ) - { - recursivelyCreateTree( node->name(), output_tree, blackboard, node ); - } - else - { - // Creating an isolated blackboard - auto new_bb = Blackboard::create(blackboard); + TreeNode::Ptr subtree_mock_node = factory.instantiateTreeNode(node->name(), "SubtreeMock", config); + // the parent is a decorator, because it's the subtree node. + auto decorator_parent = dynamic_cast(node.get()); + decorator_parent->setChild(subtree_mock_node.get()); + output_tree.nodes.push_back(subtree_mock_node); + } else { + bool is_isolated = true; - for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) + for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; + attr = attr->Next()) { - if( strcmp(attr->Name(), "ID") == 0 ) + if (strcmp(attr->Name(), "__shared_blackboard") == 0 && + convertFromString(attr->Value()) == true) { - continue; + is_isolated = false; } - new_bb->addSubtreeRemapping( attr->Name(), attr->Value() ); } - output_tree.blackboard_stack.emplace_back(new_bb); - recursivelyCreateTree( node->name(), output_tree, new_bb, node ); + + if (!is_isolated) + { + recursivelyCreateTree(node->name(), output_tree, blackboard, node, + mock_subtrees); + } + else + { + // Creating an isolated blackboard + auto new_bb = Blackboard::create(blackboard); + + for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; + attr = attr->Next()) + { + if (strcmp(attr->Name(), "ID") == 0) + { + continue; + } + new_bb->addSubtreeRemapping(attr->Name(), attr->Value()); + } + output_tree.blackboard_stack.emplace_back(new_bb); + recursivelyCreateTree(node->name(), output_tree, new_bb, node, + mock_subtrees); + } } } else if( dynamic_cast(node.get()) ) { - auto new_bb = Blackboard::create(blackboard); - output_tree.blackboard_stack.emplace_back(new_bb); - std::set mapped_keys; - - bool do_autoremap = false; - - for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next()) + if (mock_subtrees) { - const char* attr_name = attr->Name(); - const char* attr_value = attr->Value(); - - if( StrEqual(attr_name, "ID") ) + // build a set of builtin attributes for SubTree + std::set builtin_attributes{"ID"}; + for (const auto it : SubtreePlusNode::providedPorts()) { - continue; - } - if( StrEqual(attr_name, "__autoremap") ) - { - do_autoremap = convertFromString(attr_value); - continue; + builtin_attributes.emplace(it.first); } - if( TreeNode::isBlackboardPointer(attr_value)) + NodeConfiguration config; + config.blackboard = blackboard; + for (auto attr = element->FirstAttribute(); attr != nullptr; + attr = attr->Next()) { - // do remapping - StringView port_name = TreeNode::stripBlackboardPointer(attr_value); - new_bb->addSubtreeRemapping( attr_name, port_name ); - mapped_keys.insert(attr_name); - } - else{ - // constant string: just set that constant value into the BB - new_bb->set(attr_name, static_cast(attr_value) ); - mapped_keys.insert(attr_name); + if (builtin_attributes.count(attr->Name())) + { + continue; + } + // TODO find a way to support __autoremap while mocking + if (std::string{attr->Name()} == "__autoremap") + { + throw RuntimeError("__autoremap is not supported when mocking " + "subtrees"); + } + StringView str = attr->Value(); + if (TreeNode::isBlackboardPointer(str)) { + std::string external_port_name{TreeNode::stripBlackboardPointer(str)}; + /* subtree ports have not inherent direction, so set it as inout */ + config.input_ports.insert({external_port_name, "="}); + config.output_ports.insert({external_port_name, "="}); + } } - } - if( do_autoremap ) + TreeNode::Ptr subtree_mock_node = + factory.instantiateTreeNode(node->name(), "SubtreeMock", config); + // the parent is a decorator, because is the subtree node. + auto decorator_parent = dynamic_cast(node.get()); + decorator_parent->setChild(subtree_mock_node.get()); + output_tree.nodes.push_back(subtree_mock_node); + } + else { - std::vector remapped_ports; - auto new_root_element = tree_roots[node->name()]->FirstChildElement(); + auto new_bb = Blackboard::create(blackboard); + output_tree.blackboard_stack.emplace_back(new_bb); + std::set mapped_keys; + + bool do_autoremap = false; - getPortsRecursively( new_root_element, remapped_ports ); - for( const auto& port: remapped_ports) + for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; + attr = attr->Next()) { - if( mapped_keys.count(port) == 0) + const char* attr_name = attr->Name(); + const char* attr_value = attr->Value(); + + if( StrEqual(attr_name, "ID") ) { - new_bb->addSubtreeRemapping( port, port ); + continue; + } + if( StrEqual(attr_name, "__autoremap") ) + { + if (convertFromString(attr->Value())) + { + do_autoremap = true; + } + continue; + } + + if( TreeNode::isBlackboardPointer(attr_value)) + { + // do remapping + StringView port_name = TreeNode::stripBlackboardPointer(attr_value); + new_bb->addSubtreeRemapping( attr_name, port_name ); + mapped_keys.insert(attr_name); + } + else + { + // constant string: just set that constant value into the BB + new_bb->set(attr_name, static_cast(attr_value) ); + mapped_keys.insert(attr_name); } } - } - recursivelyCreateTree( node->name(), output_tree, new_bb, node ); + if (do_autoremap) + { + std::vector remapped_ports; + auto new_root_element = tree_roots[node->name()]->FirstChildElement(); + + getPortsRecursively( new_root_element, remapped_ports ); + for( const auto& port: remapped_ports) + { + if( mapped_keys.count(port) == 0) + { + new_bb->addSubtreeRemapping( port, port ); + } + } + } + + recursivelyCreateTree(node->name(), output_tree, new_bb, node, mock_subtrees); + } } } else @@ -729,6 +814,9 @@ void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID, } }; + if (!tree_roots.count(tree_ID)) { + throw RuntimeError( "The creation of the tree failed because the subtree id is unknown: ", tree_ID); + } auto root_element = tree_roots[tree_ID]->FirstChildElement(); // start recursion diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ca86ba31c..0e0437a19 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -16,6 +16,7 @@ set(BT_TESTS gtest_ports.cpp navigation_test.cpp gtest_subtree.cpp + gtest_subtreemock.cpp gtest_switch.cpp ) diff --git a/tests/gtest_subtreemock.cpp b/tests/gtest_subtreemock.cpp new file mode 100644 index 000000000..d355c538b --- /dev/null +++ b/tests/gtest_subtreemock.cpp @@ -0,0 +1,360 @@ +#include +#include +#include "behaviortree_cpp_v3/decorators/subtreemock_node.h" +#include "behaviortree_cpp_v3/bt_factory.h" + +using namespace BT; + +class SumNode : public BT::SyncActionNode +{ + public: + SumNode(const std::string& name, const BT::NodeConfiguration& config) + : BT::SyncActionNode(name, config) + { + } + + BT::NodeStatus tick() override + { + auto op1 = getInput("op1"); + auto op2 = getInput("op2"); + if (!op1 || !op2) + { + throw BT::RuntimeError("missing required input for ", __PRETTY_FUNCTION__); + } + setOutput("out", op1.value() + op2.value()); + return BT::NodeStatus::SUCCESS; + } + + static BT::PortsList providedPorts() + { + return {BT::InputPort("op1"), BT::InputPort("op2"), + BT::OutputPort("out")}; + } +}; + +class DiffNode : public BT::SyncActionNode +{ + public: + DiffNode(const std::string& name, const BT::NodeConfiguration& config) + : BT::SyncActionNode(name, config) + { + } + + BT::NodeStatus tick() override + { + auto op1 = getInput("op1"); + auto op2 = getInput("op2"); + if (!op1 || !op2) + { + throw BT::RuntimeError("missing required input for ", __PRETTY_FUNCTION__); + } + setOutput("out", op1.value() - op2.value()); + return BT::NodeStatus::SUCCESS; + } + + static BT::PortsList providedPorts() + { + return {BT::InputPort("op1"), BT::InputPort("op2"), + BT::OutputPort("out")}; + } +}; + +class MultNode : public BT::SyncActionNode +{ + public: + MultNode(const std::string& name, const BT::NodeConfiguration& config) + : BT::SyncActionNode(name, config) + { + } + + BT::NodeStatus tick() override + { + auto op1 = getInput("op1"); + auto op2 = getInput("op2"); + if (!op1 || !op2) + { + throw BT::RuntimeError("missing required input for ", __PRETTY_FUNCTION__); + } + setOutput("out", op1.value() * op2.value()); + return BT::NodeStatus::SUCCESS; + } + + static BT::PortsList providedPorts() + { + return {BT::InputPort("op1"), BT::InputPort("op2"), + BT::OutputPort("out")}; + } +}; + +class DivNode : public BT::SyncActionNode +{ + public: + DivNode(const std::string& name, const BT::NodeConfiguration& config) + : BT::SyncActionNode(name, config) + { + } + + BT::NodeStatus tick() override + { + auto op1 = getInput("op1"); + auto op2 = getInput("op2"); + if (!op1 || !op2) + { + throw BT::RuntimeError("missing required input for ", __PRETTY_FUNCTION__); + } + setOutput("out", op1.value() / op2.value()); + return BT::NodeStatus::SUCCESS; + } + + static BT::PortsList providedPorts() + { + return {BT::InputPort("op1"), BT::InputPort("op2"), + BT::OutputPort("out")}; + } +}; + +class SqrtNode : public BT::SyncActionNode +{ + public: + SqrtNode(const std::string& name, const BT::NodeConfiguration& config) + : BT::SyncActionNode(name, config) + { + } + + BT::NodeStatus tick() override + { + auto op = getInput("in"); + if (!op) + { + throw BT::RuntimeError("missing required input for ", __PRETTY_FUNCTION__); + } + if (op.value() < 0.0) + { + return BT::NodeStatus::FAILURE; + } + setOutput("out", std::sqrt(op.value())); + return BT::NodeStatus::SUCCESS; + } + + static BT::PortsList providedPorts() + { + return {BT::InputPort("in"), BT::OutputPort("out")}; + } +}; + +class SubtreeMockTests : public ::testing::Test +{ + public: + const double tolerance_{1e-6}; + const std::string xml_text = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory_; + + void SetUp() override + { + factory_.registerNodeType("SumNode"); + factory_.registerNodeType("DiffNode"); + factory_.registerNodeType("MultNode"); + factory_.registerNodeType("DivNode"); + factory_.registerNodeType("SqrtNode"); + } +}; + +TEST_F(SubtreeMockTests, FullRootTree) +{ + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = factory_.createTreeFromText(xml_text.c_str(), blackboard); + auto ret = tree.tickRoot(); + ASSERT_EQ(ret, NodeStatus::SUCCESS); + + auto sol_1_value = blackboard->get("res1"); + auto sol_2_value = blackboard->get("res2"); + ASSERT_NEAR(-1.0, sol_1_value, tolerance_); + ASSERT_NEAR(-3.0, sol_2_value, tolerance_); +} + +TEST_F(SubtreeMockTests, TestRootMockingSubtrees) +{ + // Test root tree, mocking the values returned by QuadraticEquationSolver + SubtreeMockNode::CallbackMap callbacks{{ + "QuadraticEquationSolver", + [](SubtreeMockNode::BlackboardProxy& p) { + p.setOutput("res1", 97.0); + p.setOutput("res2", 99.0); + return NodeStatus::FAILURE; + }, + }}; + NodeBuilder mock_builder = [&callbacks](const std::string& name, + const NodeConfiguration& config) { + if (callbacks.count(name) == 0) + { + throw RuntimeError("Requested generation for unmocked subbranch!"); + } + return std::make_unique(name, config, callbacks[name]); + }; + + factory_.registerBuilder("SubtreeMock", mock_builder); + + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = factory_.createTreeFromText(xml_text.c_str(), blackboard, "MainTree", true); + auto ret = tree.tickRoot(); + + ASSERT_EQ(ret, NodeStatus::FAILURE); + auto sol_1_value = blackboard->get("res1"); + auto sol_2_value = blackboard->get("res2"); + ASSERT_NEAR(97.0, sol_1_value, tolerance_); + ASSERT_NEAR(99.0, sol_2_value, tolerance_); +} + +TEST_F(SubtreeMockTests, TestFullSubtree) +{ + // Test the QuadraticEquationSolver subtree, without mocking any subtrees. + // Inputs get set on the bb before running the tree. + // Inputs are such that the subtree returns SUCCEESS and provides results. + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = + factory_.createTreeFromText(xml_text.c_str(), blackboard, "QuadraticEquationSolver"); + + blackboard->set("a", 1.0); + blackboard->set("b", -20.0); + blackboard->set("c", 99.0); + auto ret = tree.tickRoot(); + + ASSERT_EQ(ret, NodeStatus::SUCCESS); + auto sol_1_value = blackboard->get("res1"); + auto sol_2_value = blackboard->get("res2"); + ASSERT_NEAR(11.0, sol_1_value, tolerance_); + ASSERT_NEAR(9.0, sol_2_value, tolerance_); +} + +TEST_F(SubtreeMockTests, TestSubtreeWithInputThatWouldCauseFAILURE) +{ + // Test QuadraticEquationSolver subtree, without mocking any subtrees. + // Inputs get set on the bb before running the tree. + // Inputs are such that the subtree returns FAILURE with no results in the blackboard. + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = + factory_.createTreeFromText(xml_text.c_str(), blackboard, "QuadraticEquationSolver"); + + // these inputs cause the determinant to be negative + blackboard->set("a", 2.0); + blackboard->set("b", 1.0); + blackboard->set("c", 2.0); + auto ret = tree.tickRoot(); + + ASSERT_EQ(ret, NodeStatus::FAILURE); +} + +TEST_F(SubtreeMockTests, TestSubtreeMockingChildSubtrees) +{ + // Test QuadraticEquationSolver subtree, mocking subtrees called from there, + SubtreeMockNode::CallbackMap callbacks{{ + "SimpleSolutionSolver", + [](SubtreeMockNode::BlackboardProxy& p) { + static int state = 0; + auto a = p.getInput("a").value(); + auto b = p.getInput("b").value(); + auto c = p.getInput("c").value(); + + switch (state++) + { + case 0: + p.setOutput("res1", a + b + c); + break; + case 1: + p.setOutput("res2", a - b - c); + break; + default: + break; + } + return NodeStatus::SUCCESS; + }, + }}; + NodeBuilder mock_builder = [&callbacks](const std::string& name, + const NodeConfiguration& config) { + return std::make_unique(name, config, callbacks[name]); + }; + + factory_.registerBuilder("SubtreeMock", mock_builder); + + Blackboard::Ptr blackboard = Blackboard::create(); + Tree tree = + factory_.createTreeFromText(xml_text.c_str(), blackboard, "QuadraticEquationSolver", true); + + blackboard->set("a", 0.0); + blackboard->set("b", 32.0); + blackboard->set("c", 10.0); + auto ret = tree.tickRoot(); + + ASSERT_EQ(ret, NodeStatus::SUCCESS); + auto sol_1_value = blackboard->get("res1"); + auto sol_2_value = blackboard->get("res2"); + ASSERT_NEAR(42.0, sol_1_value, tolerance_); + ASSERT_NEAR(-42.0, sol_2_value, tolerance_); +} + +TEST_F(SubtreeMockTests, TestMockingWithNoSubtreeMockNodeThrows) +{ + // Tests that attempting to set mock_subtrees to true causes the creation of + // the tree to throw if no SubtreeMock bulder has been set in the factory. + Blackboard::Ptr blackboard = Blackboard::create(); + ASSERT_THROW( + { + Tree tree = factory_.createTreeFromText(xml_text.c_str(), blackboard, + "QuadraticEquationSolver", true); + }, + RuntimeError); +} + +TEST_F(SubtreeMockTests, TestBadSubtreeIdThrows) +{ + Blackboard::Ptr blackboard = Blackboard::create(); + ASSERT_THROW( + { + Tree tree = factory_.createTreeFromText(xml_text.c_str(), blackboard, + "ThisSubtreeDoesNotExist"); + }, + RuntimeError); +}