From e19e94893f63dcf0606902e0797b353b54dc31c1 Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Sun, 27 Dec 2020 11:55:34 -0300 Subject: [PATCH 1/3] 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 7dedc941f..43464f8fb 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -826,20 +826,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 45404a31187498fd3fadc47524be1d282623e66f Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Sun, 29 Aug 2021 21:33:09 -0300 Subject: [PATCH 2/3] Add feature to mock subtrees and instantiate trees starting from a subtree node --- 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 | 78 +++++++ include/behaviortree_cpp_v3/xml_parsing.h | 12 +- src/bt_factory.cpp | 19 +- src/decorators/subtree_node.cpp | 8 +- src/xml_parsing.cpp | 217 ++++++++++++------ 8 files changed, 305 insertions(+), 85 deletions(-) create mode 100644 include/behaviortree_cpp_v3/decorators/subtreemock_node.h 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..d60029d20 --- /dev/null +++ b/include/behaviortree_cpp_v3/decorators/subtreemock_node.h @@ -0,0 +1,78 @@ +#ifndef ACTION_SUBTREEMOCK_NODE_H +#define ACTION_SUBTREEMOCK_NODE_H + +#include "behaviortree_cpp_v3/action_node.h" + +#include +#include + +namespace BT +{ +/** + * @brief SubtreeMockBlackboardProxy is a wrapper class that provided limited access to + * the blackboard to mocked nodes. The mock callback receives modifiable reference to + * a SubtreeMockBlackboardProxy instance that can be used to access the + * BT blackboard. + */ +class SubtreeMockBlackboardProxy +{ + public: + explicit SubtreeMockBlackboardProxy(TreeNode* mock_node) : mock_node_{mock_node} + { + } + + // thou shall not propagate or store this object + SubtreeMockBlackboardProxy(const SubtreeMockBlackboardProxy&) = delete; + SubtreeMockBlackboardProxy(SubtreeMockBlackboardProxy&&) = 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_; +}; + +/** + * @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: + using CallbackFunction = std::function; + + SubtreeMockNode(const std::string& name, const NodeConfiguration& config, + const CallbackFunction& callback) + : SyncActionNode{name, config}, callback_{callback} + { + } + + private: + CallbackFunction callback_; + + BT::NodeStatus tick() override + { + SubtreeMockBlackboardProxy 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 43464f8fb..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,90 +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 - 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; + + for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; + attr = attr->Next()) + { + const char* attr_name = attr->Name(); + const char* attr_value = attr->Value(); + + if( StrEqual(attr_name, "ID") ) + { + continue; + } + if( StrEqual(attr_name, "__autoremap") ) + { + if (convertFromString(attr->Value())) + { + do_autoremap = true; + } + continue; + } - getPortsRecursively( new_root_element, remapped_ports ); - for( const auto& port: remapped_ports) + 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); + } + } + + if (do_autoremap) { - if( mapped_keys.count(port) == 0) + 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) { - new_bb->addSubtreeRemapping( port, port ); + if( mapped_keys.count(port) == 0) + { + new_bb->addSubtreeRemapping( port, port ); + } } } - } - recursivelyCreateTree( node->name(), output_tree, new_bb, node ); + recursivelyCreateTree(node->name(), output_tree, new_bb, node, mock_subtrees); + } } } else @@ -728,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 From 868a94496d7d9d1c058b8e7bb64f1dd8b2bd23a4 Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Sun, 29 Aug 2021 21:33:33 -0300 Subject: [PATCH 3/3] Add tests for the subtree mocking feature --- tests/CMakeLists.txt | 1 + tests/gtest_subtreemock.cpp | 241 +++++++++++++++++++++ tests/include/gtet_subtreemock_aux_nodes.h | 159 ++++++++++++++ 3 files changed, 401 insertions(+) create mode 100644 tests/gtest_subtreemock.cpp create mode 100644 tests/include/gtet_subtreemock_aux_nodes.h 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..19465648d --- /dev/null +++ b/tests/gtest_subtreemock.cpp @@ -0,0 +1,241 @@ +/* Copyright (C) 2021 Gerardo Puga - All Rights Reserved +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), +* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, +* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +// stdlib +#include +#include + +// gtest +#include + +// behaviortree.cpp +#include "behaviortree_cpp_v3/decorators/subtreemock_node.h" +#include "behaviortree_cpp_v3/bt_factory.h" + +// the auxiliar tree nodes used in this test +#include "gtet_subtreemock_aux_nodes.h" + +using namespace BT; + +class SubtreeMockTests : public ::testing::Test +{ + public: + const double tolerance_{1e-6}; + const std::string xml_text = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory_; + + std::map mock_callbacks_; + + void SetUp() override + { + // fill in the factory with the nodes used by the tree + factory_.registerNodeType("SumNode"); + factory_.registerNodeType("DiffNode"); + factory_.registerNodeType("MultNode"); + factory_.registerNodeType("DivNode"); + factory_.registerNodeType("SqrtNode"); + + // fill the calbacks map with the mock mock_callbacks_ that will replace branches in some of the tests + mock_callbacks_["SimpleSolutionSolver"] = [](SubtreeMockBlackboardProxy& 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; + }; + + mock_callbacks_["QuadraticEquationSolver"] = [](SubtreeMockBlackboardProxy& p) { + p.setOutput("res1", 97.0); + p.setOutput("res2", 99.0); + return NodeStatus::FAILURE; + }; + } +}; + +TEST_F(SubtreeMockTests, FullRootTree) +{ + // Test the full tree, starting from the default root. + // Read reasults from the blackboard and test final values. + 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 the full tree, mocking the values returned by the QuadraticEquationSolver subtree. + // Read reasults from the blackboard and test final values. + NodeBuilder mock_builder = [this](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(name, config, mock_callbacks_.at(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, + NodeBuilder mock_builder = [this](const std::string& name, const NodeConfiguration& config) { + return std::make_unique(name, config, mock_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 builder 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); +} diff --git a/tests/include/gtet_subtreemock_aux_nodes.h b/tests/include/gtet_subtreemock_aux_nodes.h new file mode 100644 index 000000000..dc7cb7bb7 --- /dev/null +++ b/tests/include/gtet_subtreemock_aux_nodes.h @@ -0,0 +1,159 @@ +/* Copyright (C) 2021 Gerardo Puga - All Rights Reserved +* +* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), +* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, +* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +*/ + +#ifndef GTEST_SUBTREEMOCK_AUX_NODES_H +#define GTEST_SUBTREEMOCK_AUX_NODES_H + +// stdlib +#include + +// behaviortree.cpp +#include "behaviortree_cpp_v3/bt_factory.h" + +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")}; + } +}; + +#endif