/** * @function parseWorldToEntityPaths */ void DartLoader::parseWorldToEntityPaths(const std::string &_xml_string) { TiXmlDocument xml_doc; xml_doc.Parse(_xml_string.c_str()); TiXmlElement *world_xml = xml_doc.FirstChildElement("world"); if( !world_xml ) { return; } // Get all include filenames std::map<std::string, std::string> includedFiles; for( TiXmlElement* include_xml = world_xml->FirstChildElement("include"); include_xml; include_xml = include_xml->NextSiblingElement("include") ) { const char *filename = include_xml->Attribute("filename"); const char *model_name = include_xml->Attribute("model_name"); std::string string_filename( filename ); std::string string_filepath = string_filename.substr( 0, string_filename.rfind("/") + 1 ); std::string string_model_name( model_name ); includedFiles[string_model_name] = string_filepath; } // Get all entities for( TiXmlElement* entity_xml = world_xml->FirstChildElement("entity"); entity_xml; entity_xml = entity_xml->NextSiblingElement("entity") ) { // Find model and name for entity, if not, error const char* entity_model = entity_xml->Attribute("model"); const char* entity_name = entity_xml->Attribute("name"); if( entity_name && entity_model ) { std::string string_entity_model( entity_model ); std::string string_entity_name( entity_name ); // Find the model if( includedFiles.find( string_entity_model ) == includedFiles.end() ) { std::cout<<"[!] Did not find entity model included. Weird things may happen"<<std::endl; return; } // Add it else { std::string string_entity_filepath = includedFiles.find( string_entity_model )->second; mWorld_To_Entity_Paths[string_entity_name] = string_entity_filepath; } } // If no name or model is defined else { std::cout<< "[!] Entity was not defined. Weird things will happen" <<std::endl; } } // for all entities }
/** * @function parseWorldURDF */ World* parseWorldURDF(const std::string &_xml_string, std::string _root_to_world_path) { World* world = new World(); TiXmlDocument xml_doc; xml_doc.Parse( _xml_string.c_str() ); TiXmlElement *world_xml = xml_doc.FirstChildElement("world"); if( !world_xml ) { printf ( "[parseWorldURDF] ERROR: Could not find a <world> element in XML, exiting and not loading! \n" ); return NULL; } // Get world name const char *name = world_xml->Attribute("name"); if(!name) { printf ("[parseWorldURDF] ERROR: World does not have a name specified. Exiting and not loading! \n"); return NULL; } world->name = std::string(name); if(debug) std::cout<< "World name: "<< world->name << std::endl; // Get all include filenames int count = 0; std::map<std::string, std::string> includedFiles; for( TiXmlElement* include_xml = world_xml->FirstChildElement("include"); include_xml; include_xml = include_xml->NextSiblingElement("include") ) { count++; const char *filename = include_xml->Attribute("filename"); const char *model_name = include_xml->Attribute("model_name"); std::string string_filename( filename ); std::string string_model_name( model_name ); includedFiles[string_model_name] = string_filename; if(debug) std::cout<<"Include: Model name: "<< model_name <<" filename: "<< filename <<std::endl; } if(debug) std::cout<<"Found "<<count<<" include filenames "<<std::endl; // Get all entities count = 0; for( TiXmlElement* entity_xml = world_xml->FirstChildElement("entity"); entity_xml; entity_xml = entity_xml->NextSiblingElement("entity") ) { count++; Entity entity; try { const char* entity_model = entity_xml->Attribute("model"); std::string string_entity_model( entity_model ); // Find the model if( includedFiles.find( string_entity_model ) == includedFiles.end() ) { std::cout<<"[parseWorldURDF] ERROR: I cannot find the model you want to use, did you write the name right? Exiting and not loading! \n"<<std::endl; return NULL; } else { std::string fileName = includedFiles.find( string_entity_model )->second; std::string fileFullName = _root_to_world_path; fileFullName.append( fileName ); if(debug) std::cout<< "Entity full filename: "<< fileFullName << std::endl; // Parse model std::string xml_model_string; std::fstream xml_file( fileFullName.c_str(), std::fstream::in ); while( xml_file.good() ) { std::string line; std::getline( xml_file, line ); xml_model_string += (line + "\n"); } xml_file.close(); ModelInterface* model = parseURDF( xml_model_string ).get(); if( !model ) { std::cout<< "[parseWorldURDF] Model in "<<fileFullName<<" not found. Exiting and not loading!" <<std::endl; return NULL; } else { entity.model.reset(model); // Parse location TiXmlElement *o = entity_xml->FirstChildElement("origin"); if( o ) { if( !parsePose( entity.origin, o ) ) { printf ("[ERROR] Write the pose for your entity! \n"); return world; } } // If name is defined const char* entity_name = entity_xml->Attribute("name"); if( entity_name ) { std::string string_entity_name( entity_name ); entity.model->name_ = string_entity_name; } // Store in world world->models.push_back( entity ); } } // end of include read } catch( ParseError &e ) { if(debug) printf ("Entity xml not initialized correctly \n"); //entity->reset(); //world->reset(); return world; } } // end for if(debug) printf ("Found %d entities \n", count); return world; }