00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #include <functional>
00014 #include <list>
00015
00016 #if defined(__linux) || defined(__linux__)
00017 #pragma GCC diagnostic push
00018 #pragma GCC diagnostic ignored "-Wattributes"
00019 #endif
00020
00021 #ifdef _MSC_VER
00022 #pragma warning(disable : 4996) // do not complain about sprintf
00023 #endif
00024
00025 #include "behaviortree_cpp/xml_parsing.h"
00026 #include "private/tinyxml2.h"
00027 #include "filesystem/path.h"
00028
00029 #ifdef USING_ROS
00030 #include <ros/package.h>
00031 #endif
00032
00033 #include "behaviortree_cpp/blackboard.h"
00034 #include "behaviortree_cpp/utils/demangle_util.h"
00035
00036 namespace BT
00037 {
00038 using namespace BT_TinyXML2;
00039
00040 struct XMLParser::Pimpl
00041 {
00042 TreeNode::Ptr createNodeFromXML(const XMLElement* element,
00043 const Blackboard::Ptr& blackboard,
00044 const TreeNode::Ptr& node_parent);
00045
00046 void recursivelyCreateTree(const std::string& tree_ID,
00047 Tree& output_tree,
00048 Blackboard::Ptr blackboard,
00049 const TreeNode::Ptr& root_parent);
00050
00051 void loadDocImpl(BT_TinyXML2::XMLDocument* doc);
00052
00053 std::list<std::unique_ptr<BT_TinyXML2::XMLDocument> > opened_documents;
00054 std::unordered_map<std::string,const XMLElement*> tree_roots;
00055
00056 const BehaviorTreeFactory& factory;
00057
00058 filesystem::path current_path;
00059
00060 int suffix_count;
00061
00062 explicit Pimpl(const BehaviorTreeFactory &fact):
00063 factory(fact),
00064 current_path( filesystem::path::getcwd() ),
00065 suffix_count(0)
00066 {}
00067
00068 void clear()
00069 {
00070 suffix_count = 0;
00071 current_path = filesystem::path::getcwd();
00072 opened_documents.clear();
00073 tree_roots.clear();
00074 }
00075
00076 };
00077
00078 #if defined(__linux) || defined(__linux__)
00079 #pragma GCC diagnostic pop
00080 #endif
00081
00082 XMLParser::XMLParser(const BehaviorTreeFactory &factory) :
00083 _p( new Pimpl(factory) )
00084 {
00085 }
00086
00087 XMLParser::~XMLParser()
00088 {
00089 delete _p;
00090 }
00091
00092 void XMLParser::loadFromFile(const std::string& filename)
00093 {
00094 _p->opened_documents.emplace_back(new BT_TinyXML2::XMLDocument());
00095
00096 BT_TinyXML2::XMLDocument* doc = _p->opened_documents.back().get();
00097 doc->LoadFile(filename.c_str());
00098
00099 filesystem::path file_path( filename );
00100 _p->current_path = file_path.parent_path().make_absolute();
00101
00102 _p->loadDocImpl( doc );
00103 }
00104
00105 void XMLParser::loadFromText(const std::string& xml_text)
00106 {
00107 _p->opened_documents.emplace_back(new BT_TinyXML2::XMLDocument());
00108
00109 BT_TinyXML2::XMLDocument* doc = _p->opened_documents.back().get();
00110 doc->Parse(xml_text.c_str(), xml_text.size());
00111
00112 _p->loadDocImpl( doc );
00113 }
00114
00115 void XMLParser::Pimpl::loadDocImpl(BT_TinyXML2::XMLDocument* doc)
00116 {
00117 if (doc->Error())
00118 {
00119 char buffer[200];
00120 sprintf(buffer, "Error parsing the XML: %s", doc->ErrorName() );
00121 throw RuntimeError(buffer);
00122 }
00123
00124 const XMLElement* xml_root = doc->RootElement();
00125
00126
00127 for (auto include_node = xml_root->FirstChildElement("include");
00128 include_node != nullptr;
00129 include_node = include_node->NextSiblingElement("include"))
00130 {
00131
00132 filesystem::path file_path( include_node->Attribute("path") );
00133
00134 if( include_node->Attribute("ros_pkg") )
00135 {
00136 #ifdef USING_ROS
00137 if( file_path.is_absolute() )
00138 {
00139 std::cout << "WARNING: <include path=\"...\"> containes an absolute path.\n"
00140 << "Attribute [ros_pkg] will be ignored."<< std::endl;
00141 }
00142 else {
00143 auto ros_pkg_path = ros::package::getPath( include_node->Attribute("ros_pkg") );
00144 file_path = filesystem::path( ros_pkg_path ) / file_path;
00145 }
00146 #else
00147 throw RuntimeError("Using attribute [ros_pkg] in <include>, but this library was compiled "
00148 "without ROS support. Recompile the BehaviorTree.CPP using catkin");
00149 #endif
00150 }
00151
00152 if( !file_path.is_absolute() )
00153 {
00154 file_path = current_path / file_path;
00155 }
00156
00157 opened_documents.emplace_back(new BT_TinyXML2::XMLDocument());
00158 BT_TinyXML2::XMLDocument* next_doc = opened_documents.back().get();
00159 next_doc->LoadFile(file_path.str().c_str());
00160 loadDocImpl(next_doc);
00161 }
00162
00163 for (auto bt_node = xml_root->FirstChildElement("BehaviorTree");
00164 bt_node != nullptr;
00165 bt_node = bt_node->NextSiblingElement("BehaviorTree"))
00166 {
00167 std::string tree_name;
00168 if (bt_node->Attribute("ID"))
00169 {
00170 tree_name = bt_node->Attribute("ID");
00171 }
00172 else{
00173 tree_name = "BehaviorTree_" + std::to_string( suffix_count++ );
00174 }
00175 tree_roots.insert( {tree_name, bt_node} );
00176 }
00177
00178 std::set<std::string> registered_nodes;
00179 XMLPrinter printer;
00180 doc->Print(&printer);
00181 auto xml_text = std::string(printer.CStr(), size_t(printer.CStrSize() - 1));
00182
00183 for( const auto& it: factory.manifests())
00184 {
00185 registered_nodes.insert( it.first );
00186 }
00187 for( const auto& it: tree_roots)
00188 {
00189 registered_nodes.insert( it.first );
00190 }
00191
00192 VerifyXML(xml_text, registered_nodes);
00193 }
00194
00195 void VerifyXML(const std::string& xml_text,
00196 const std::set<std::string>& registered_nodes)
00197 {
00198
00199 BT_TinyXML2::XMLDocument doc;
00200 auto xml_error = doc.Parse( xml_text.c_str(), xml_text.size());
00201 if (xml_error)
00202 {
00203 char buffer[200];
00204 sprintf(buffer, "Error parsing the XML: %s", doc.ErrorName() );
00205 throw RuntimeError( buffer );
00206 }
00207
00208
00209 auto StrEqual = [](const char* str1, const char* str2) -> bool {
00210 return strcmp(str1, str2) == 0;
00211 };
00212
00213 auto ThrowError = [&](int line_num, const std::string& text) {
00214 char buffer[256];
00215 sprintf(buffer, "Error at line %d: -> %s", line_num, text.c_str());
00216 throw RuntimeError( buffer );
00217 };
00218
00219 auto ChildrenCount = [](const XMLElement* parent_node) {
00220 int count = 0;
00221 for (auto node = parent_node->FirstChildElement(); node != nullptr;
00222 node = node->NextSiblingElement())
00223 {
00224 count++;
00225 }
00226 return count;
00227 };
00228
00229
00230 const XMLElement* xml_root = doc.RootElement();
00231
00232 if (!xml_root || !StrEqual(xml_root->Name(), "root"))
00233 {
00234 throw RuntimeError("The XML must have a root node called <root>");
00235 }
00236
00237 auto models_root = xml_root->FirstChildElement("TreeNodesModel");
00238 auto meta_sibling = models_root ? models_root->NextSiblingElement("TreeNodesModel") : nullptr;
00239
00240 if (meta_sibling)
00241 {
00242 ThrowError(meta_sibling->GetLineNum(),
00243 " Only a single node <TreeNodesModel> is supported");
00244 }
00245 if (models_root)
00246 {
00247
00248
00249 for (auto node = xml_root->FirstChildElement(); node != nullptr;
00250 node = node->NextSiblingElement())
00251 {
00252 const char* name = node->Name();
00253 if (StrEqual(name, "Action") || StrEqual(name, "Decorator") ||
00254 StrEqual(name, "SubTree") || StrEqual(name, "Condition"))
00255 {
00256 const char* ID = node->Attribute("ID");
00257 if (!ID)
00258 {
00259 ThrowError(node->GetLineNum(),
00260 "Error at line %d: -> The attribute [ID] is mandatory");
00261 }
00262 }
00263 }
00264 }
00265
00266
00267
00268 std::function<void(const XMLElement*)> recursiveStep;
00269
00270 recursiveStep = [&](const XMLElement* node) {
00271 const int children_count = ChildrenCount(node);
00272 const char* name = node->Name();
00273 if (StrEqual(name, "Decorator"))
00274 {
00275 if (children_count != 1)
00276 {
00277 ThrowError(node->GetLineNum(),
00278 "The node <Decorator> must have exactly 1 child");
00279 }
00280 if (!node->Attribute("ID"))
00281 {
00282 ThrowError(node->GetLineNum(),
00283 "The node <Decorator> must have the attribute [ID]");
00284 }
00285 }
00286 else if (StrEqual(name, "Action"))
00287 {
00288 if (children_count != 0)
00289 {
00290 ThrowError(node->GetLineNum(),
00291 "The node <Action> must not have any child");
00292 }
00293 if (!node->Attribute("ID"))
00294 {
00295 ThrowError(node->GetLineNum(),
00296 "The node <Action> must have the attribute [ID]");
00297 }
00298 }
00299 else if (StrEqual(name, "Condition"))
00300 {
00301 if (children_count != 0)
00302 {
00303 ThrowError(node->GetLineNum(),
00304 "The node <Condition> must not have any child");
00305 }
00306 if (!node->Attribute("ID"))
00307 {
00308 ThrowError(node->GetLineNum(),
00309 "The node <Condition> must have the attribute [ID]");
00310 }
00311 }
00312 else if (StrEqual(name, "Sequence") ||
00313 StrEqual(name, "SequenceStar") ||
00314 StrEqual(name, "Fallback") )
00315 {
00316 if (children_count == 0)
00317 {
00318 ThrowError(node->GetLineNum(),
00319 "A Control node must have at least 1 child");
00320 }
00321 }
00322 else if (StrEqual(name, "SubTree"))
00323 {
00324 for (auto child = node->FirstChildElement(); child != nullptr;
00325 child = child->NextSiblingElement())
00326 {
00327 if( StrEqual(child->Name(), "remap") )
00328 {
00329 ThrowError(node->GetLineNum(), "<remap> was deprecated");
00330 }
00331 else{
00332 ThrowError(node->GetLineNum(), "<SubTree> should not have any child");
00333 }
00334 }
00335
00336 if (!node->Attribute("ID"))
00337 {
00338 ThrowError(node->GetLineNum(),
00339 "The node <SubTree> must have the attribute [ID]");
00340 }
00341 }
00342 else
00343 {
00344
00345 bool found = ( registered_nodes.find(name) != registered_nodes.end() );
00346 if (!found)
00347 {
00348 ThrowError(node->GetLineNum(),
00349 std::string("Node not recognized: ") + name);
00350 }
00351 }
00352
00353 if (StrEqual(name, "SubTree") == false)
00354 {
00355 for (auto child = node->FirstChildElement(); child != nullptr;
00356 child = child->NextSiblingElement())
00357 {
00358 recursiveStep(child);
00359 }
00360 }
00361 };
00362
00363 std::vector<std::string> tree_names;
00364 int tree_count = 0;
00365
00366 for (auto bt_root = xml_root->FirstChildElement("BehaviorTree"); bt_root != nullptr;
00367 bt_root = bt_root->NextSiblingElement("BehaviorTree"))
00368 {
00369 tree_count++;
00370 if (bt_root->Attribute("ID"))
00371 {
00372 tree_names.emplace_back(bt_root->Attribute("ID"));
00373 }
00374 if (ChildrenCount(bt_root) != 1)
00375 {
00376 ThrowError(bt_root->GetLineNum(),
00377 "The node <BehaviorTree> must have exactly 1 child");
00378 }
00379 else
00380 {
00381 recursiveStep(bt_root->FirstChildElement());
00382 }
00383 }
00384
00385 if (xml_root->Attribute("main_tree_to_execute"))
00386 {
00387 std::string main_tree = xml_root->Attribute("main_tree_to_execute");
00388 if (std::find(tree_names.begin(), tree_names.end(), main_tree) == tree_names.end())
00389 {
00390 throw RuntimeError("The tree specified in [main_tree_to_execute] can't be found");
00391 }
00392 }
00393 else
00394 {
00395 if (tree_count != 1)
00396 {
00397 throw RuntimeError("If you don't specify the attribute [main_tree_to_execute], "
00398 "Your file must contain a single BehaviorTree");
00399 }
00400 }
00401 }
00402
00403 Tree XMLParser::instantiateTree(const Blackboard::Ptr& root_blackboard)
00404 {
00405 Tree output_tree;
00406
00407 XMLElement* xml_root = _p->opened_documents.front()->RootElement();
00408
00409 std::string main_tree_ID;
00410 if (xml_root->Attribute("main_tree_to_execute"))
00411 {
00412 main_tree_ID = xml_root->Attribute("main_tree_to_execute");
00413 }
00414 else if( _p->tree_roots.size() == 1)
00415 {
00416 main_tree_ID = _p->tree_roots.begin()->first;
00417 }
00418 else{
00419 throw RuntimeError("[main_tree_to_execute] was not specified correctly");
00420 }
00421
00422 if( !root_blackboard )
00423 {
00424 throw RuntimeError("XMLParser::instantiateTree needs a non-empty root_blackboard");
00425 }
00426
00427 output_tree.blackboard_stack.push_back( root_blackboard );
00428
00429 _p->recursivelyCreateTree(main_tree_ID,
00430 output_tree,
00431 root_blackboard,
00432 TreeNode::Ptr() );
00433
00434 if( output_tree.nodes.size() > 0)
00435 {
00436 output_tree.root_node = output_tree.nodes.front().get();
00437 }
00438 return output_tree;
00439 }
00440
00441 TreeNode::Ptr XMLParser::Pimpl::createNodeFromXML(const XMLElement *element,
00442 const Blackboard::Ptr &blackboard,
00443 const TreeNode::Ptr &node_parent)
00444 {
00445 const std::string element_name = element->Name();
00446 std::string ID;
00447 std::string instance_name;
00448
00449
00450 if (element_name == "Action" || element_name == "Decorator" || element_name == "Condition")
00451 {
00452 ID = element->Attribute("ID");
00453 }
00454 else
00455 {
00456 ID = element_name;
00457 }
00458
00459 const char* attr_alias = element->Attribute("name");
00460 if (attr_alias)
00461 {
00462 instance_name = attr_alias;
00463 }
00464 else
00465 {
00466 instance_name = ID;
00467 }
00468
00469 if (element_name == "SubTree")
00470 {
00471 instance_name = element->Attribute("ID");
00472 }
00473
00474 PortsRemapping remapping_parameters;
00475
00476 if (element_name != "SubTree")
00477 {
00478 for (const XMLAttribute* att = element->FirstAttribute(); att; att = att->Next())
00479 {
00480 const std::string attribute_name = att->Name();
00481 if (attribute_name != "ID" && attribute_name != "name")
00482 {
00483 remapping_parameters[attribute_name] = att->Value();
00484 }
00485 }
00486 }
00487 NodeConfiguration config;
00488 config.blackboard = blackboard;
00489
00490
00491 TreeNode::Ptr child_node;
00492
00493 if( factory.builders().count(ID) != 0)
00494 {
00495 const auto& manifest = factory.manifests().at(ID);
00496
00497
00498 for(const auto& remapping_it: remapping_parameters)
00499 {
00500 if( manifest.ports.count( remapping_it.first ) == 0 )
00501 {
00502 throw RuntimeError("Possible typo? In the XML, you tried to remap port \"",
00503 remapping_it.first, "\" in node [", ID," / ", instance_name,
00504 "], but the manifest of this node does not contain a port with this name.");
00505 }
00506 }
00507
00508
00509 for(const auto& port_it: manifest.ports)
00510 {
00511 const std::string& port_name = port_it.first;
00512 const auto& port_info = port_it.second;
00513
00514 auto remap_it = remapping_parameters.find(port_name);
00515 if( remap_it == remapping_parameters.end())
00516 {
00517 continue;
00518 }
00519 StringView remapping_value = remap_it->second;
00520 auto remapped_res = TreeNode::getRemappedKey(port_name, remapping_value);
00521 if( remapped_res )
00522 {
00523 const auto& port_key = remapped_res.value().to_string();
00524
00525 auto prev_info = blackboard->portInfo( port_key );
00526 if( !prev_info )
00527 {
00528
00529 blackboard->setPortInfo( port_key, port_info );
00530 }
00531 else{
00532
00533 if( prev_info->type() && port_info.type() &&
00534 prev_info->type()!= port_info.type())
00535 {
00536 blackboard->debugMessage();
00537
00538 throw RuntimeError( "The creation of the tree failed because the port [", port_key,
00539 "] was initially created with type [", demangle( prev_info->type() ),
00540 "] and, later type [", demangle( port_info.type() ),
00541 "] was used somewhere else." );
00542 }
00543 }
00544 }
00545 }
00546
00547
00548 for(const auto& remap_it: remapping_parameters)
00549 {
00550 const auto& port_name = remap_it.first;
00551 auto port_it = manifest.ports.find( port_name );
00552 if( port_it != manifest.ports.end() )
00553 {
00554 auto direction = port_it->second.direction();
00555 if( direction != PortDirection::OUTPUT )
00556 {
00557 config.input_ports.insert( remap_it );
00558 }
00559 if( direction != PortDirection::INPUT )
00560 {
00561 config.output_ports.insert( remap_it );
00562 }
00563 }
00564 }
00565
00566 for (const auto& port_it: manifest.ports)
00567 {
00568 const std::string& port_name = port_it.first;
00569 const PortInfo& port_info = port_it.second;
00570
00571 auto direction = port_info.direction();
00572 if( direction != PortDirection::INPUT &&
00573 config.input_ports.count(port_name) == 0 &&
00574 port_info.defaultValue().empty() == false)
00575 {
00576 config.input_ports.insert( { port_name, port_info.defaultValue() } );
00577 }
00578 }
00579 child_node = factory.instantiateTreeNode(instance_name, ID, config);
00580 }
00581 else if( tree_roots.count(ID) != 0) {
00582 child_node = std::make_unique<DecoratorSubtreeNode>( instance_name );
00583 }
00584 else{
00585 throw RuntimeError( ID, " is not a registered node, nor a Subtree");
00586 }
00587
00588 if (node_parent)
00589 {
00590 if (auto control_parent = dynamic_cast<ControlNode*>(node_parent.get()))
00591 {
00592 control_parent->addChild(child_node.get());
00593 }
00594 if (auto decorator_parent = dynamic_cast<DecoratorNode*>(node_parent.get()))
00595 {
00596 decorator_parent->setChild(child_node.get());
00597 }
00598 }
00599 return child_node;
00600 }
00601
00602 void BT::XMLParser::Pimpl::recursivelyCreateTree(const std::string& tree_ID,
00603 Tree& output_tree,
00604 Blackboard::Ptr blackboard,
00605 const TreeNode::Ptr& root_parent)
00606 {
00607 std::function<void(const TreeNode::Ptr&, const XMLElement*)> recursiveStep;
00608
00609 recursiveStep = [&](const TreeNode::Ptr& parent,
00610 const XMLElement* element)
00611 {
00612 auto node = createNodeFromXML(element, blackboard, parent);
00613 output_tree.nodes.push_back(node);
00614
00615 if( node->type() == NodeType::SUBTREE )
00616 {
00617 auto new_bb = Blackboard::create(blackboard);
00618
00619 for (const XMLAttribute* attr = element->FirstAttribute(); attr != nullptr; attr = attr->Next())
00620 {
00621 new_bb->addSubtreeRemapping( attr->Name(), attr->Value() );
00622 }
00623
00624 output_tree.blackboard_stack.emplace_back(new_bb);
00625 recursivelyCreateTree( node->name(), output_tree, new_bb, node );
00626 }
00627 else
00628 {
00629 for (auto child_element = element->FirstChildElement(); child_element;
00630 child_element = child_element->NextSiblingElement())
00631 {
00632 recursiveStep(node, child_element);
00633 }
00634 }
00635 };
00636
00637 auto root_element = tree_roots[tree_ID]->FirstChildElement();
00638
00639
00640 recursiveStep(root_parent, root_element);
00641 }
00642
00643
00644 std::string writeTreeNodesModelXML(const BehaviorTreeFactory& factory)
00645 {
00646 using namespace BT_TinyXML2;
00647
00648 BT_TinyXML2::XMLDocument doc;
00649
00650 XMLElement* rootXML = doc.NewElement("root");
00651 doc.InsertFirstChild(rootXML);
00652
00653 XMLElement* model_root = doc.NewElement("TreeNodesModel");
00654 rootXML->InsertEndChild(model_root);
00655
00656 for (auto& model_it : factory.manifests())
00657 {
00658 const auto& registration_ID = model_it.first;
00659 const auto& model = model_it.second;
00660
00661 if( factory.builtinNodes().count( registration_ID ) != 0)
00662 {
00663 continue;
00664 }
00665
00666 if (model.type == NodeType::CONTROL)
00667 {
00668 continue;
00669 }
00670 XMLElement* element = doc.NewElement( toStr(model.type).c_str() );
00671 element->SetAttribute("ID", model.registration_ID.c_str());
00672
00673 for (auto& port : model.ports)
00674 {
00675 const auto& port_name = port.first;
00676 const auto& port_info = port.second;
00677
00678 XMLElement* port_element = nullptr;
00679 switch( port_info.direction() )
00680 {
00681 case PortDirection::INPUT: port_element = doc.NewElement("input_port"); break;
00682 case PortDirection::OUTPUT: port_element = doc.NewElement("output_port"); break;
00683 case PortDirection::INOUT: port_element = doc.NewElement("inout_port"); break;
00684 }
00685
00686 port_element->SetAttribute("name", port_name.c_str() );
00687 if( port_info.type() )
00688 {
00689 port_element->SetAttribute("type", BT::demangle( port_info.type() ).c_str() );
00690 }
00691 if( !port_info.defaultValue().empty() )
00692 {
00693 port_element->SetAttribute("default", port_info.defaultValue().c_str() );
00694 }
00695
00696 if( !port_info.description().empty() )
00697 {
00698 port_element->SetText( port_info.description().c_str() );
00699 }
00700
00701 element->InsertEndChild(port_element);
00702 }
00703
00704 model_root->InsertEndChild(element);
00705 }
00706
00707 XMLPrinter printer;
00708 doc.Print(&printer);
00709 return std::string(printer.CStr(), size_t(printer.CStrSize() - 1));
00710 }
00711
00712 Tree buildTreeFromText(const BehaviorTreeFactory& factory, const std::string& text,
00713 const Blackboard::Ptr& blackboard)
00714 {
00715 XMLParser parser(factory);
00716 parser.loadFromText(text);
00717 return parser.instantiateTree(blackboard);
00718 }
00719
00720 Tree buildTreeFromFile(const BehaviorTreeFactory& factory, const std::string& filename,
00721 const Blackboard::Ptr& blackboard)
00722 {
00723 XMLParser parser(factory);
00724 parser.loadFromFile(filename);
00725 return parser.instantiateTree(blackboard);
00726 }
00727
00728 }