/**
* @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;
}
