bool Model::initString(const std::string& xml_string)
{
boost::shared_ptr<ModelInterface> model;
// necessary for COLLADA compatibility
if( IsColladaData(xml_string) ) {
ROS_DEBUG("Parsing robot collada xml string");
static boost::mutex PARSER_PLUGIN_LOCK;
static boost::scoped_ptr<pluginlib::ClassLoader<urdf::URDFParser> > PARSER_PLUGIN_LOADER;
boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK);
try
{
if (!PARSER_PLUGIN_LOADER)
PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader<urdf::URDFParser>("urdf_parser_plugin", "urdf::URDFParser"));
const std::vector<std::string> &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses();
bool found = false;
for (std::size_t i = 0 ; i < classes.size() ; ++i)
if (classes[i].find("urdf/ColladaURDFParser") != std::string::npos)
{
boost::shared_ptr<urdf::URDFParser> instance = PARSER_PLUGIN_LOADER->createInstance(classes[i]);
if (instance)
model = instance->parse(xml_string);
found = true;
break;
}
if (!found)
ROS_ERROR_STREAM("No URDF parser plugin found for Collada files. Did you install the corresponding package?");
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what() << ". Will not parse Collada file.");
}
}
else {
ROS_DEBUG("Parsing robot urdf xml string");
model = parseURDF(xml_string);
}
// copy data from model into this object
if (model){
this->links_ = model->links_;
this->joints_ = model->joints_;
this->materials_ = model->materials_;
this->name_ = model->name_;
this->root_link_ = model->root_link_;
return true;
}
else
return false;
}
/** Initialize the model using an URDF string
* @param xml_string The robot description in URDF format
* @return true if the model was intialized successfully
*/
bool Model::initString(const std::string& xml_string)
{
ModelInterfaceSharedPtr model;
if( IsColladaData(xml_string) ) {
// currently, support for Collada is not implemented
throw URDFColladaNotSupportedException();
}
else {
model = parseURDF(xml_string);
}
// copy data from model into this object
if (model){
this->links_ = model->links_;
this->joints_ = model->joints_;
this->materials_ = model->materials_;
this->name_ = model->name_;
this->root_link_ = model->root_link_;
return true;
}
else
return false;
}
/**
* @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;
}
