diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 59b059cdd..be7560fbf 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -52,3 +52,5 @@ if(CURSES_FOUND) target_link_libraries(t12_ncurses_manual_selector ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes ) endif() +add_executable(t13_load_multiple_BTs t13_load_multiple_BTs.cpp ) +target_link_libraries(t13_load_multiple_BTs ${BEHAVIOR_TREE_LIBRARY} bt_sample_nodes ) diff --git a/examples/t13_load_multiple_BTs.cpp b/examples/t13_load_multiple_BTs.cpp new file mode 100644 index 000000000..ef05e1ae2 --- /dev/null +++ b/examples/t13_load_multiple_BTs.cpp @@ -0,0 +1,84 @@ +#include "dummy_nodes.h" +#include "behaviortree_cpp_v3/bt_factory.h" + + +/** This example show how it is possible to: + * - load BehaviorTrees from multiple files manually (without the tag) + * - instantiate a specific tree, instead of the one specified by [main_tree_to_execute] + */ + +// clang-format off + +static const char* xml_text_main = R"( + + + + + + + + + )"; + +static const char* xml_text_subA = R"( + + + + + )"; + +static const char* xml_text_subB = R"( + + + + + )"; + +// clang-format on + +using namespace BT; + +int main() +{ + BT::BehaviorTreeFactory factory; + factory.registerNodeType("SaySomething"); + + // Register the behavior tree definitions, but do not instantiate them yet. + // Order is not important. + factory.registerBehaviorTreeFromText(xml_text_subA); + factory.registerBehaviorTreeFromText(xml_text_subB); + factory.registerBehaviorTreeFromText(xml_text_main); + + //Check that the BTs have been registered correctly + std::cout << "Registered BehaviorTrees:" << std::endl; + for(const std::string& bt_name: factory.registeredBehaviorTrees()) + { + std::cout << " - " << bt_name << std::endl; + } + + // You can create the MainTree and the subtrees will be added automatically. + std::cout << "----- MainTree tick ----" << std::endl; + auto main_tree = factory.createdTree("MainTree"); + main_tree.tickRoot(); + + // ... or you can create only one of the subtree + std::cout << "----- SubA tick ----" << std::endl; + auto subA_tree = factory.createdTree("SubA"); + subA_tree.tickRoot(); + + return 0; +} +/* Expected output: + +Registered BehaviorTrees: + - MainTree + - SubA + - SubB +----- MainTree tick ---- +Robot says: starting MainTree +Robot says: Executing SubA +Robot says: Executing SubB +----- SubA tick ---- +Robot says: Executing SubA + +*/ diff --git a/include/behaviortree_cpp_v3/bt_factory.h b/include/behaviortree_cpp_v3/bt_factory.h index ac7c424d2..f0c69fdf0 100644 --- a/include/behaviortree_cpp_v3/bt_factory.h +++ b/include/behaviortree_cpp_v3/bt_factory.h @@ -22,7 +22,6 @@ #include #include - #include "behaviortree_cpp_v3/behavior_tree.h" namespace BT @@ -197,6 +196,8 @@ class Tree }; +class Parser; + /** * @brief The BehaviorTreeFactory is used to create instances of a * TreeNode at run-time. @@ -270,6 +271,12 @@ class BehaviorTreeFactory */ void registerFromROSPlugins(); + void registerBehaviorTreeFromFile(const std::string& filename); + + void registerBehaviorTreeFromText(const std::string& xml_text); + + std::vector registeredBehaviorTrees() const; + /** * @brief instantiateTreeNode creates an instance of a previously registered TreeNode. * @@ -374,11 +381,16 @@ class BehaviorTreeFactory Tree createTreeFromFile(const std::string& file_path, Blackboard::Ptr blackboard = Blackboard::create()); + Tree createdTree(const std::string& tree_name, + Blackboard::Ptr blackboard = Blackboard::create()); + private: std::unordered_map builders_; std::unordered_map manifests_; std::set builtin_IDs_; + std::unordered_map behavior_tree_definitions_; + std::shared_ptr parser_; // clang-format on }; diff --git a/include/behaviortree_cpp_v3/bt_parser.h b/include/behaviortree_cpp_v3/bt_parser.h index c5f979928..46e9b3aa4 100644 --- a/include/behaviortree_cpp_v3/bt_parser.h +++ b/include/behaviortree_cpp_v3/bt_parser.h @@ -21,11 +21,13 @@ class Parser Parser(const Parser& other) = delete; Parser& operator=(const Parser& other) = delete; - virtual void loadFromFile(const std::string& filename) = 0; + virtual void loadFromFile(const std::string& filename, bool add_includes = true) = 0; - virtual void loadFromText(const std::string& xml_text) = 0; + virtual void loadFromText(const std::string& xml_text, bool add_includes = true) = 0; - virtual Tree instantiateTree(const Blackboard::Ptr &root_blackboard) = 0; + virtual std::vector registeredBehaviorTrees() const = 0; + + virtual Tree instantiateTree(const Blackboard::Ptr &root_blackboard, std::string tree_name = {}) = 0; }; } diff --git a/include/behaviortree_cpp_v3/xml_parsing.h b/include/behaviortree_cpp_v3/xml_parsing.h index 401dd1e11..45b196c78 100644 --- a/include/behaviortree_cpp_v3/xml_parsing.h +++ b/include/behaviortree_cpp_v3/xml_parsing.h @@ -16,16 +16,19 @@ class XMLParser: public Parser public: XMLParser(const BehaviorTreeFactory& factory); - ~XMLParser(); + ~XMLParser() override; XMLParser(const XMLParser& other) = delete; XMLParser& operator=(const XMLParser& other) = delete; - void loadFromFile(const std::string& filename) override; + void loadFromFile(const std::string& filename, bool add_includes = true) override; - void loadFromText(const std::string& xml_text) override; + void loadFromText(const std::string& xml_text, bool add_includes = true) override; - Tree instantiateTree(const Blackboard::Ptr &root_blackboard) override; + std::vector registeredBehaviorTrees() const override; + + Tree instantiateTree(const Blackboard::Ptr &root_blackboard, + std::string main_tree_to_execute = {}) override; private: diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index c31226571..12ecdbc1e 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -10,9 +10,11 @@ * 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. */ +#include #include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/utils/shared_library.h" #include "behaviortree_cpp_v3/xml_parsing.h" +#include "private/tinyxml2.h" #ifdef USING_ROS #include "filesystem/path.h" @@ -23,6 +25,7 @@ namespace BT { BehaviorTreeFactory::BehaviorTreeFactory() { + parser_ = std::make_shared(*this); registerNodeType("Fallback"); registerNodeType("Sequence"); registerNodeType("SequenceStar"); @@ -212,6 +215,22 @@ void BehaviorTreeFactory::registerFromROSPlugins() } #endif + +void BehaviorTreeFactory::registerBehaviorTreeFromFile(const std::string &filename) +{ + parser_->loadFromFile(filename); +} + +void BehaviorTreeFactory::registerBehaviorTreeFromText(const std::string &xml_text) +{ + parser_->loadFromText(xml_text); +} + +std::vector BehaviorTreeFactory::registeredBehaviorTrees() const +{ + return parser_->registeredBehaviorTrees(); +} + std::unique_ptr BehaviorTreeFactory::instantiateTreeNode( const std::string& name, const std::string& ID, @@ -268,6 +287,13 @@ Tree BehaviorTreeFactory::createTreeFromFile(const std::string &file_path, return tree; } +Tree BehaviorTreeFactory::createdTree(const std::string &tree_name, Blackboard::Ptr blackboard) +{ + auto tree = parser_->instantiateTree(blackboard, tree_name); + tree.manifests = this->manifests(); + return tree; +} + Tree::~Tree() { haltTree(); diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index f51472b52..b839b0435 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -22,6 +22,7 @@ #pragma warning(disable : 4996) // do not complain about sprintf #endif +#include #include "behaviortree_cpp_v3/xml_parsing.h" #include "private/tinyxml2.h" #include "filesystem/path.h" @@ -59,10 +60,10 @@ struct XMLParser::Pimpl void getPortsRecursively(const XMLElement* element, std::vector &output_ports); - void loadDocImpl(BT_TinyXML2::XMLDocument* doc); + void loadDocImpl(BT_TinyXML2::XMLDocument* doc, bool add_includes); std::list > opened_documents; - std::unordered_map tree_roots; + std::map tree_roots; const BehaviorTreeFactory& factory; @@ -100,7 +101,7 @@ XMLParser::~XMLParser() delete _p; } -void XMLParser::loadFromFile(const std::string& filename) +void XMLParser::loadFromFile(const std::string& filename, bool add_includes) { _p->opened_documents.emplace_back(new BT_TinyXML2::XMLDocument()); @@ -110,20 +111,30 @@ void XMLParser::loadFromFile(const std::string& filename) filesystem::path file_path( filename ); _p->current_path = file_path.parent_path().make_absolute(); - _p->loadDocImpl( doc ); + _p->loadDocImpl( doc, add_includes ); } -void XMLParser::loadFromText(const std::string& xml_text) +void XMLParser::loadFromText(const std::string& xml_text, bool add_includes) { _p->opened_documents.emplace_back(new BT_TinyXML2::XMLDocument()); BT_TinyXML2::XMLDocument* doc = _p->opened_documents.back().get(); doc->Parse(xml_text.c_str(), xml_text.size()); - _p->loadDocImpl( doc ); + _p->loadDocImpl( doc, add_includes ); } -void XMLParser::Pimpl::loadDocImpl(BT_TinyXML2::XMLDocument* doc) +std::vector XMLParser::registeredBehaviorTrees() const +{ + std::vector out; + for(const auto& it: _p->tree_roots) + { + out.push_back(it.first); + } + return out; +} + +void XMLParser::Pimpl::loadDocImpl(BT_TinyXML2::XMLDocument* doc, bool add_includes) { if (doc->Error()) { @@ -139,10 +150,13 @@ void XMLParser::Pimpl::loadDocImpl(BT_TinyXML2::XMLDocument* doc) include_node != nullptr; include_node = include_node->NextSiblingElement("include")) { + if( !add_includes ) + { + break; + } filesystem::path file_path( include_node->Attribute("path") ); const char* ros_pkg_relative_path = include_node->Attribute("ros_pkg"); - std::string ros_pkg_path; if( ros_pkg_relative_path ) { @@ -151,17 +165,18 @@ void XMLParser::Pimpl::loadDocImpl(BT_TinyXML2::XMLDocument* doc) std::cout << "WARNING: contains an absolute path.\n" << "Attribute [ros_pkg] will be ignored."<< std::endl; } - else + else { + std::string ros_pkg_path; #ifdef USING_ROS - ros_pkg_path = ros::package::getPath(ros_pkg_relative_path); + ros_pkg_path = ros::package::getPath(ros_pkg_relative_path); #elif defined USING_ROS2 - ros_pkg_path = ament_index_cpp::get_package_share_directory(ros_pkg_relative_path); + ros_pkg_path = ament_index_cpp::get_package_share_directory(ros_pkg_relative_path); + file_path = filesystem::path( ros_pkg_path ) / file_path; #else - throw RuntimeError("Using attribute [ros_pkg] in , but this library was compiled " - "without ROS support. Recompile the BehaviorTree.CPP using catkin"); + throw RuntimeError("Using attribute [ros_pkg] in , but this library was compiled " + "without ROS support. Recompile the BehaviorTree.CPP using catkin"); #endif - file_path = filesystem::path( ros_pkg_path ) / file_path; } } @@ -173,7 +188,7 @@ void XMLParser::Pimpl::loadDocImpl(BT_TinyXML2::XMLDocument* doc) opened_documents.emplace_back(new BT_TinyXML2::XMLDocument()); BT_TinyXML2::XMLDocument* next_doc = opened_documents.back().get(); next_doc->LoadFile(file_path.str().c_str()); - loadDocImpl(next_doc); + loadDocImpl(next_doc, add_includes); } for (auto bt_node = xml_root->FirstChildElement("BehaviorTree"); @@ -407,41 +422,40 @@ void VerifyXML(const std::string& xml_text, recursiveStep(bt_root->FirstChildElement()); } } - - if (xml_root->Attribute("main_tree_to_execute")) - { - std::string main_tree = xml_root->Attribute("main_tree_to_execute"); - if (std::find(tree_names.begin(), tree_names.end(), main_tree) == tree_names.end()) - { - throw RuntimeError("The tree specified in [main_tree_to_execute] can't be found"); - } - } - else - { - if (tree_count != 1) - { - throw RuntimeError("If you don't specify the attribute [main_tree_to_execute], " - "Your file must contain a single BehaviorTree"); - } - } } -Tree XMLParser::instantiateTree(const Blackboard::Ptr& root_blackboard) +Tree XMLParser::instantiateTree(const Blackboard::Ptr& root_blackboard, + std::string main_tree_to_execute) { Tree output_tree; + std::string main_tree_ID = main_tree_to_execute; - XMLElement* xml_root = _p->opened_documents.front()->RootElement(); - - std::string main_tree_ID; - if (xml_root->Attribute("main_tree_to_execute")) + if( main_tree_ID.empty() ) { - main_tree_ID = xml_root->Attribute("main_tree_to_execute"); + for( const auto& doc: _p->opened_documents) + { + XMLElement* xml_root = doc->RootElement(); + if (xml_root->Attribute("main_tree_to_execute")) + { + if(!main_tree_ID.empty()) + { + throw RuntimeError("The attribute [main_tree_to_execute] has been " + "found multiple times. You must specify explicitly the name " + "of the to instantiate."); + } + main_tree_ID = xml_root->Attribute("main_tree_to_execute"); + } + } } - else if( _p->tree_roots.size() == 1) + + // special case: no name, but there is only one registered BT. + if( main_tree_ID.empty() && _p->tree_roots.size() == 1) { main_tree_ID = _p->tree_roots.begin()->first; } - else{ + + if( main_tree_ID.empty() ) + { throw RuntimeError("[main_tree_to_execute] was not specified correctly"); } //-------------------------------------- diff --git a/tests/gtest_factory.cpp b/tests/gtest_factory.cpp index 406c5e22e..e909ce0cc 100644 --- a/tests/gtest_factory.cpp +++ b/tests/gtest_factory.cpp @@ -71,8 +71,65 @@ static const char* xml_text_subtree = R"( )"; +static const char* xml_text_subtree_part1 = R"( + + + + + + + + + )"; + +static const char* xml_text_subtree_part2 = R"( + + + + + + + + + + + + + )"; + // clang-format on + +TEST(BehaviorTreeFactory, XMLParsingOrder) +{ + BehaviorTreeFactory factory; + CrossDoor::RegisterNodes(factory); + + { + XMLParser parser(factory); + parser.loadFromText(xml_text_subtree); + auto trees = parser.registeredBehaviorTrees(); + ASSERT_EQ(trees[0], "CrossDoorSubtree"); + ASSERT_EQ(trees[1], "MainTree"); + } + { + XMLParser parser(factory); + parser.loadFromText(xml_text_subtree_part1); + parser.loadFromText(xml_text_subtree_part2); + auto trees = parser.registeredBehaviorTrees(); + ASSERT_EQ(trees[0], "CrossDoorSubtree"); + ASSERT_EQ(trees[1], "MainTree"); + } + { + XMLParser parser(factory); + parser.loadFromText(xml_text_subtree_part2); + parser.loadFromText(xml_text_subtree_part1); + auto trees = parser.registeredBehaviorTrees(); + ASSERT_EQ(trees[0], "CrossDoorSubtree"); + ASSERT_EQ(trees[1], "MainTree"); + } +} + TEST(BehaviorTreeFactory, VerifyLargeTree) { BehaviorTreeFactory factory; @@ -114,6 +171,8 @@ TEST(BehaviorTreeFactory, VerifyLargeTree) ASSERT_EQ(decorator->child()->name(), "IsDoorOpen"); } + + TEST(BehaviorTreeFactory, Subtree) { BehaviorTreeFactory factory; @@ -244,3 +303,4 @@ TEST(BehaviorTreeFactory, SubTreeWithRemapping) ASSERT_FALSE( talk_bb->getAny("talk_out") ); } +