16 #if defined(__linux) || defined(__linux__) 17 #pragma GCC diagnostic push 18 #pragma GCC diagnostic ignored "-Wattributes" 22 #pragma warning(disable : 4996) // do not complain about sprintf 40 auto StrEqual = [](
const char* str1,
const char* str2) ->
bool {
41 return strcmp(str1, str2) == 0;
51 void recursivelyCreateTree(
const std::string& tree_ID,
56 void getPortsRecursively(
const XMLElement* element, std::vector<std::string> &output_ports);
61 std::unordered_map<std::string,const XMLElement*>
tree_roots;
71 current_path( filesystem::
path::getcwd() ),
78 current_path = filesystem::path::getcwd();
79 opened_documents.clear();
85 #if defined(__linux) || defined(__linux__) 86 #pragma GCC diagnostic pop 90 _p( new
Pimpl(factory) )
106 filesystem::path file_path( filename );
117 doc->
Parse(xml_text.c_str(), xml_text.size());
127 sprintf(buffer,
"Error parsing the XML: %s", doc->
ErrorName() );
135 include_node !=
nullptr;
139 filesystem::path file_path( include_node->Attribute(
"path") );
141 if( include_node->Attribute(
"ros_pkg") )
144 if( file_path.is_absolute() )
146 std::cout <<
"WARNING: <include path=\"...\"> containes an absolute path.\n" 147 <<
"Attribute [ros_pkg] will be ignored."<< std::endl;
151 file_path = filesystem::path( ros_pkg_path ) / file_path;
154 throw RuntimeError(
"Using attribute [ros_pkg] in <include>, but this library was compiled " 155 "without ROS support. Recompile the BehaviorTree.CPP using catkin");
159 if( !file_path.is_absolute() )
161 file_path = current_path / file_path;
166 next_doc->
LoadFile(file_path.str().c_str());
167 loadDocImpl(next_doc);
174 std::string tree_name;
175 if (bt_node->Attribute(
"ID"))
177 tree_name = bt_node->Attribute(
"ID");
182 tree_roots.insert( {tree_name, bt_node} );
185 std::set<std::string> registered_nodes;
187 doc->
Print(&printer);
190 for(
const auto& it: factory.manifests())
192 registered_nodes.insert( it.first );
194 for(
const auto& it: tree_roots)
196 registered_nodes.insert( it.first );
203 const std::set<std::string>& registered_nodes)
207 auto xml_error = doc.
Parse( xml_text.c_str(), xml_text.size());
211 sprintf(buffer,
"Error parsing the XML: %s", doc.
ErrorName() );
216 auto ThrowError = [&](
int line_num,
const std::string& text) {
218 sprintf(buffer,
"Error at line %d: -> %s", line_num, text.c_str());
222 auto ChildrenCount = [](
const XMLElement* parent_node) {
224 for (
auto node = parent_node->FirstChildElement(); node !=
nullptr;
225 node = node->NextSiblingElement())
237 throw RuntimeError(
"The XML must have a root node called <root>");
241 auto meta_sibling = models_root ? models_root->
NextSiblingElement(
"TreeNodesModel") :
nullptr;
245 ThrowError(meta_sibling->GetLineNum(),
246 " Only a single node <TreeNodesModel> is supported");
255 const char* name = node->
Name();
259 const char* ID = node->Attribute(
"ID");
262 ThrowError(node->GetLineNum(),
263 "Error at line %d: -> The attribute [ID] is mandatory");
271 std::function<void(const XMLElement*)> recursiveStep;
274 const int children_count = ChildrenCount(node);
275 const char* name = node->Name();
278 if (children_count != 1)
280 ThrowError(node->GetLineNum(),
281 "The node <Decorator> must have exactly 1 child");
283 if (!node->Attribute(
"ID"))
285 ThrowError(node->GetLineNum(),
286 "The node <Decorator> must have the attribute [ID]");
291 if (children_count != 0)
293 ThrowError(node->GetLineNum(),
294 "The node <Action> must not have any child");
296 if (!node->Attribute(
"ID"))
298 ThrowError(node->GetLineNum(),
299 "The node <Action> must have the attribute [ID]");
302 else if (
StrEqual(name,
"Condition"))
304 if (children_count != 0)
306 ThrowError(node->GetLineNum(),
307 "The node <Condition> must not have any child");
309 if (!node->Attribute(
"ID"))
311 ThrowError(node->GetLineNum(),
312 "The node <Condition> must have the attribute [ID]");
317 if (children_count == 0)
319 ThrowError(node->GetLineNum(),
320 "The node <Control> must have at least 1 child");
322 if (!node->Attribute(
"ID"))
324 ThrowError(node->GetLineNum(),
325 "The node <Control> must have the attribute [ID]");
328 else if (
StrEqual(name,
"Sequence") ||
332 if (children_count == 0)
334 ThrowError(node->GetLineNum(),
335 "A Control node must have at least 1 child");
340 auto child = node->FirstChildElement();
344 if (
StrEqual(child->Name(),
"remap"))
346 ThrowError(node->GetLineNum(),
"<remap> was deprecated");
349 ThrowError(node->GetLineNum(),
"<SubTree> should not have any child");
353 if (!node->Attribute(
"ID"))
355 ThrowError(node->GetLineNum(),
356 "The node <SubTree> must have the attribute [ID]");
362 bool found = ( registered_nodes.find(name) != registered_nodes.end() );
365 ThrowError(node->GetLineNum(),
366 std::string(
"Node not recognized: ") + name);
370 if (
StrEqual(name,
"SubTree") ==
false)
372 for (
auto child = node->FirstChildElement(); child !=
nullptr;
373 child = child->NextSiblingElement())
375 recursiveStep(child);
380 std::vector<std::string> tree_names;
383 for (
auto bt_root = xml_root->
FirstChildElement(
"BehaviorTree"); bt_root !=
nullptr;
387 if (bt_root->Attribute(
"ID"))
389 tree_names.emplace_back(bt_root->Attribute(
"ID"));
391 if (ChildrenCount(bt_root) != 1)
393 ThrowError(bt_root->GetLineNum(),
394 "The node <BehaviorTree> must have exactly 1 child");
398 recursiveStep(bt_root->FirstChildElement());
402 if (xml_root->
Attribute(
"main_tree_to_execute"))
404 std::string main_tree = xml_root->
Attribute(
"main_tree_to_execute");
405 if (std::find(tree_names.begin(), tree_names.end(), main_tree) == tree_names.end())
407 throw RuntimeError(
"The tree specified in [main_tree_to_execute] can't be found");
414 throw RuntimeError(
"If you don't specify the attribute [main_tree_to_execute], " 415 "Your file must contain a single BehaviorTree");
426 std::string main_tree_ID;
427 if (xml_root->
Attribute(
"main_tree_to_execute"))
429 main_tree_ID = xml_root->
Attribute(
"main_tree_to_execute");
436 throw RuntimeError(
"[main_tree_to_execute] was not specified correctly");
439 if( !root_blackboard )
441 throw RuntimeError(
"XMLParser::instantiateTree needs a non-empty root_blackboard");
457 const std::string element_name = element->
Name();
459 std::string instance_name;
462 if (element_name ==
"Action" || element_name ==
"Decorator" ||
463 element_name ==
"Condition" || element_name ==
"Control")
472 const char* attr_alias = element->
Attribute(
"name");
475 instance_name = attr_alias;
484 if (element_name ==
"SubTree" ||
485 element_name ==
"SubTreePlus" )
487 instance_name = element->
Attribute(
"ID");
493 const std::string attribute_name = att->Name();
494 if (attribute_name !=
"ID" && attribute_name !=
"name")
496 port_remap[attribute_name] = att->Value();
506 if( factory.builders().count(ID) != 0)
508 const auto&
manifest = factory.manifests().at(ID);
511 for(
const auto& remap_it: port_remap)
513 if(
manifest.ports.count( remap_it.first ) == 0 )
515 throw RuntimeError(
"Possible typo? In the XML, you tried to remap port \"",
516 remap_it.first,
"\" in node [", ID,
" / ", instance_name,
517 "], but the manifest of this node does not contain a port with this name.");
522 for(
const auto& port_it:
manifest.ports)
524 const std::string& port_name = port_it.first;
525 const auto& port_info = port_it.second;
527 auto remap_it = port_remap.find(port_name);
528 if( remap_it == port_remap.end())
536 const auto port_key =
static_cast<std::string
>(param_res.value());
538 auto prev_info = blackboard->portInfo( port_key );
542 blackboard->setPortInfo( port_key, port_info );
546 if( prev_info->type() && port_info.type() &&
547 *prev_info->type() != *port_info.type())
549 blackboard->debugMessage();
551 throw RuntimeError(
"The creation of the tree failed because the port [", port_key,
552 "] was initially created with type [",
demangle( prev_info->type() ),
553 "] and, later type [",
demangle( port_info.type() ),
554 "] was used somewhere else." );
561 for(
const auto& remap_it: port_remap)
563 const auto& port_name = remap_it.first;
564 auto port_it =
manifest.ports.find( port_name );
565 if( port_it !=
manifest.ports.end() )
567 auto direction = port_it->second.direction();
580 for (
const auto& port_it:
manifest.ports)
582 const std::string& port_name = port_it.first;
583 const PortInfo& port_info = port_it.second;
594 child_node = factory.instantiateTreeNode(instance_name, ID, config);
596 else if( tree_roots.count(ID) != 0) {
597 child_node = std::make_unique<SubtreeNode>( instance_name );
600 throw RuntimeError( ID,
" is not a registered node, nor a Subtree");
605 if (
auto control_parent = dynamic_cast<ControlNode*>(node_parent.get()))
607 control_parent->addChild(child_node.get());
609 if (
auto decorator_parent = dynamic_cast<DecoratorNode*>(node_parent.get()))
611 decorator_parent->setChild(child_node.get());
622 std::function<void(const TreeNode::Ptr&, const XMLElement*)> recursiveStep;
628 auto node = createNodeFromXML(element, blackboard, parent);
629 output_tree.
nodes.push_back(node);
633 if( dynamic_cast<const SubtreeNode*>(node.get()) )
635 bool is_isolated =
true;
637 for (
const XMLAttribute* attr = element->FirstAttribute(); attr !=
nullptr; attr = attr->
Next())
639 if( strcmp(attr->Name(),
"__shared_blackboard") == 0 &&
648 recursivelyCreateTree( node->name(), output_tree, blackboard, node );
654 for (
const XMLAttribute* attr = element->FirstAttribute(); attr !=
nullptr; attr = attr->
Next())
656 if( strcmp(attr->Name(),
"ID") == 0 )
660 new_bb->addSubtreeRemapping( attr->Name(), attr->Value() );
663 recursivelyCreateTree( node->name(), output_tree, new_bb, node );
666 else if( dynamic_cast<const SubtreePlusNode*>(node.get()) )
670 std::set<StringView> mapped_keys;
672 bool do_autoremap =
false;
674 for (
const XMLAttribute* attr = element->FirstAttribute(); attr !=
nullptr; attr = attr->
Next())
676 const char* attr_name = attr->Name();
677 const char* attr_value = attr->Value();
683 if(
StrEqual(attr_name,
"__autoremap") )
693 new_bb->addSubtreeRemapping( attr_name, port_name );
694 mapped_keys.insert(attr_name);
698 new_bb->set(attr_name, static_cast<std::string>(attr_value) );
699 mapped_keys.insert(attr_name);
705 std::vector<std::string> remapped_ports;
706 auto new_root_element = tree_roots[node->name()]->FirstChildElement();
708 getPortsRecursively( new_root_element, remapped_ports );
709 for(
const auto& port: remapped_ports)
711 if( mapped_keys.count(port) == 0)
713 new_bb->addSubtreeRemapping( port, port );
718 recursivelyCreateTree( node->name(), output_tree, new_bb, node );
723 for (
auto child_element = element->FirstChildElement(); child_element;
724 child_element = child_element->NextSiblingElement())
726 recursiveStep(node, child_element);
731 auto root_element = tree_roots[tree_ID]->FirstChildElement();
734 recursiveStep(root_parent, root_element);
738 std::vector<std::string>& output_ports)
742 const char* attr_name = attr->Name();
743 const char* attr_value = attr->Value();
749 output_ports.push_back( static_cast<std::string>(port_name) );
756 getPortsRecursively(child_element, output_ports);
773 for (
auto& model_it : factory.
manifests())
775 const auto& registration_ID = model_it.first;
776 const auto& model = model_it.second;
778 if( factory.
builtinNodes().count( registration_ID ) != 0)
788 element->
SetAttribute(
"ID", model.registration_ID.c_str());
790 for (
auto& port : model.ports)
792 const auto& port_name = port.first;
793 const auto& port_info = port.second;
796 switch( port_info.direction() )
804 if( port_info.type() )
808 if( !port_info.defaultValue().empty() )
810 port_element->
SetAttribute(
"default", port_info.defaultValue().c_str() );
813 if( !port_info.description().empty() )
815 port_element->
SetText( port_info.description().c_str() );
826 return std::string(printer.
CStr(), size_t(printer.
CStrSize() - 1));
Tree buildTreeFromText(const BehaviorTreeFactory &factory, const std::string &text, const Blackboard::Ptr &blackboard)
void SetAttribute(const char *name, const char *value)
Sets the named attribute to value.
Pimpl(const BehaviorTreeFactory &fact)
static Blackboard::Ptr create(Blackboard::Ptr parent={})
const XMLElement * NextSiblingElement(const char *name=0) const
Get the next (right) sibling element of this node, with an optionally supplied name.
PortDirection direction() const
void recursivelyCreateTree(const std::string &tree_ID, Tree &output_tree, Blackboard::Ptr blackboard, const TreeNode::Ptr &root_parent)
static raw_event_t * buffer
std::string toStr(T value)
const XMLAttribute * Next() const
The next attribute in the list.
std::unordered_map< std::string, const XMLElement * > tree_roots
std::vector< Blackboard::Ptr > blackboard_stack
filesystem::path current_path
XMLNode * InsertFirstChild(XMLNode *addThis)
void loadDocImpl(BT_TinyXML2::XMLDocument *doc)
const XMLElement * FirstChildElement(const char *name=0) const
std::shared_ptr< Blackboard > Ptr
const std::set< std::string > & builtinNodes() const
List of builtin IDs.
Blackboard::Ptr blackboard
XMLElement * NewElement(const char *name)
void SetText(const char *inText)
XMLError LoadFile(const char *filename)
std::string demangle(char const *name)
XMLElement * RootElement()
bool Error() const
Return true if there was an error parsing the document.
The BehaviorTreeFactory is used to create instances of a TreeNode at run-time.
const std::string & defaultValue() const
const char * CStr() const
nonstd::string_view StringView
void Print(XMLPrinter *streamer=0) const
const BehaviorTreeFactory & factory
const XMLAttribute * FirstAttribute() const
Return the first attribute in the list.
bool convertFromString< bool >(StringView str)
XMLError Parse(const char *xml, size_t nBytes=(size_t)(-1))
PortsRemapping output_ports
Struct used to store a tree. If this object goes out of scope, the tree is destroyed.
Tree instantiateTree(const Blackboard::Ptr &root_blackboard) override
Simple class for manipulating paths on Linux/Windows/Mac OS.
ROSLIB_DECL std::string getPath(const std::string &package_name)
std::vector< TreeNode::Ptr > nodes
static volatile int count
static StringView stripBlackboardPointer(StringView str)
const std::unordered_map< std::string, TreeNodeManifest > & manifests() const
Manifests of all the registered TreeNodes.
void getPortsRecursively(const XMLElement *element, std::vector< std::string > &output_ports)
std::string writeTreeNodesModelXML(const BehaviorTreeFactory &factory)
The XMLParser is a class used to read the model of a BehaviorTree from file or text and instantiate t...
static bool isBlackboardPointer(StringView str)
static const char * xml_text
void loadFromText(const std::string &xml_text) override
const char * ErrorName() const
std::list< std::unique_ptr< BT_TinyXML2::XMLDocument > > opened_documents
XMLParser(const BehaviorTreeFactory &factory)
const char * Name() const
Get the name of an element (which is the Value() of the node.)
std::shared_ptr< TreeNode > Ptr
void VerifyXML(const std::string &xml_text, const std::set< std::string > ®istered_nodes)
const char * Attribute(const char *name, const char *value=0) const
TreeNode::Ptr createNodeFromXML(const XMLElement *element, const Blackboard::Ptr &blackboard, const TreeNode::Ptr &node_parent)
void loadFromFile(const std::string &filename) override
std::basic_string< CharT, Traits > to_string(basic_string_view< CharT, Traits > v)
PortsRemapping input_ports
Tree buildTreeFromFile(const BehaviorTreeFactory &factory, const std::string &filename, const Blackboard::Ptr &blackboard)
XMLNode * InsertEndChild(XMLNode *addThis)
static Optional< StringView > getRemappedKey(StringView port_name, StringView remapping_value)
std::unordered_map< std::string, std::string > PortsRemapping