bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(visual.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(visual.m_geometry,geom,logger))
{
return false;
}
const char *name_char = config->Attribute("name");
if (name_char)
visual.m_name = name_char;
visual.m_hasLocalMaterial = false;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
//todo(erwincoumans) skip materials in SDF for now (due to complexity)
if (mat && !m_parseSDF)
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
}
return true;
}
bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(visual.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(visual.m_geometry,geom,logger))
{
return false;
}
const char *name_char = config->Attribute("name");
if (name_char)
visual.m_name = name_char;
visual.m_hasLocalMaterial = false;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (mat)
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
visual.m_hasLocalMaterial = true;
}
}
}
return true;
}
static void parseScene( MeshModel *root ){
_log( "Scene" );
enterChunk();
while( int id=nextChunk() ){
switch( id ){
case CHUNK_OBJECT:
parseObject( root );
break;
case CHUNK_MATERIAL:
if( !animonly ) parseMaterial();
break;
}
}
leaveChunk();
}
void SceneParser::parseSphere(void)
{
parseToken(Scanner::LeftCurly);
Vector3 center;
parseVector(center);
float radius;
parseNumber(radius);
Sphere* s = new Sphere(center, radius);
raytracer->addObject(s);
advance();
parseMaterial(s);
parseTransform(s);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parseBox(void)
{
parseToken(Scanner::LeftCurly);
Vector3 minCorner;
parseVector(minCorner);
Vector3 maxCorner;
parseVector(maxCorner);
Box* b = new Box(minCorner, maxCorner);
raytracer->addObject(b);
advance();
parseMaterial(b);
parseTransform(b);
acceptToken(Scanner::RightCurly);
}
bool parseVisual(Visual &vis, TiXmlElement *config)
{
vis.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o) {
if (!parsePose(vis.origin, o))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
vis.geometry = parseGeometry(geom);
if (!vis.geometry)
return false;
const char *name_char = config->Attribute("name");
if (name_char)
vis.name = name_char;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (mat) {
// get material name
if (!mat->Attribute("name")) {
logError("Visual material must contain a name attribute");
return false;
}
vis.material_name = mat->Attribute("name");
// try to parse material element in place
resetPtr(vis.material,new Material());
if (!parseMaterial(*vis.material, mat, true))
{
logDebug("urdfdom: material has only name, actual material definition may be in the model");
}
}
return true;
}
void SceneParser::parseMaterials() {
char token[MAX_PARSER_TOKEN_LENGTH];
getToken(token); assert (!strcmp(token, "{"));
// read in the number of objects
getToken(token); assert (!strcmp(token, "numMaterials"));
num_materials = readInt();
materials = new Material*[num_materials];
// read in the objects
int count = 0;
while (num_materials > count) {
getToken(token);
if (!strcmp(token, "Material")) {
materials[count] = parseMaterial();
} else {
printf ("Unknown token in parseMaterial: '%s'\n", token);
exit(0);
}
count++;
}
getToken(token); assert (!strcmp(token, "}"));
}
void SceneParser::parseCylinder(void)
{
parseToken(Scanner::LeftCurly);
Vector3 base;
parseVector(base);
float height;
parseNumber(height);
float radius;
parseNumber(radius);
Cylinder* c = new Cylinder(base, height, radius);
raytracer->addObject(c);
advance();
parseMaterial(c);
parseTransform(c);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parsePlane(void)
{
parseToken(Scanner::LeftCurly);
Vector3 center;
parseVector(center);
Vector3 right;
parseVector(right);
Vector3 up;
parseVector(up);
Plane* p = new Plane(center, right, up);
raytracer->addObject(p);
advance();
parseMaterial(p);
parseTransform(p);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parseTriangle(void)
{
parseToken(Scanner::LeftCurly);
Vector3* p1 = new Vector3();
parseVector(*p1);
Vector3* p2 = new Vector3();
parseVector(*p2);
Vector3* p3 = new Vector3();
parseVector(*p3);
Triangle* t = new Triangle(p1, p2, p3, true);
raytracer->addObject(t);
advance();
parseMaterial(t);
parseTransform(t);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parseMesh(void)
{
parseToken(Scanner::LeftCurly);
parseToken(Scanner::String);
string fileName = scanner.tokenText();
vector<Triangle*>* newMesh = new vector<Triangle*>;
vector<Vector3>* newPoints = new vector<Vector3>;
vector<Vector3>* newNormals = new vector<Vector3>;
if(!parser->loadObj(fileName, *newMesh, *newPoints, *newNormals))
errorFlag = true;
Mesh* m = new Mesh(newMesh, newPoints, newNormals);
raytracer->addObject(m);
advance();
parseMaterial(m);
parseTransform(m);
acceptToken(Scanner::RightCurly);
}
Group* SceneParser::parseGroup() {
//
// each group starts with an integer that specifies
// the number of objects in the group
//
// the material node sets the color of all objects which
// follow, until the next material node
//
char token[MAX_PARSER_TOKEN_LENGTH];
getToken(token); assert (!strcmp(token, "{"));
// read in the number of objects
getToken(token); assert (!strcmp(token, "numObjects"));
int num_objects = readInt();
Group *answer = new Group(num_objects);
// read in the objects
int count = 0;
while (num_objects > count) {
getToken(token);
if (!strcmp(token, "Material")) {
// materials don't count
parseMaterial();
} else {
// everything else does count
Object3D *object = parseObject(token);
assert (object != NULL);
answer->addObject(count,object);
count++;
}
}
getToken(token); assert (!strcmp(token, "}"));
// return the group
return answer;
}
void Model3DS::parseEdit()
{
cout << "parseEdit" << endl;
DWord length = currentChunk.length;
DWord n = cfg3ds::chunkHeaderSize;
numTextures = 0;
while (n < length)
{
readChunkHeader();
n += currentChunk.length;
switch (currentChunk.id)
{
case chunks::EDIT_OBJECT:
parseObject();
break;
case chunks::EDIT_MATERIAL:
parseMaterial();
break;
default:
skipChunk();
}
}
textures = new GLuint[numTextures];
glGenTextures(numTextures, textures);
for (list<Material *>::iterator it = materials.begin(); it != materials.end(); ++it) {
if ((*it)->texmapFile == NULL)
continue;
char *texmapFileName = new char[strlen(path) + strlen((*it)->texmapFile) + 1];
sprintf(texmapFileName, "%s%s", path, (*it)->texmapFile);
sf::Image image;
bool result = image.LoadFromFile(texmapFileName);
delete [] texmapFileName;
if (result == false) {
cout << "Can't read texture file!" << endl;
(*it)->texmapFile = NULL;
continue;
}
glBindTexture(GL_TEXTURE_2D, textures[(*it)->textureRef]);
// select modulate to mix texture with color for shading
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
// when texture area is small, bilinear filter the closest mipmap
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
// when texture area is large, bilinear filter the original
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
// the texture wraps over at the edges (repeat)
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
gluBuild2DMipmaps(GL_TEXTURE_2D, GL_RGBA, image.GetWidth(), image.GetHeight(), GL_RGBA, GL_UNSIGNED_BYTE, image.GetPixelsPtr());
}
}
bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
TiXmlDocument xml_doc;
xml_doc.Parse(sdfText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
xml_doc.ClearError();
return false;
}
TiXmlElement *sdf_xml = xml_doc.FirstChildElement("sdf");
if (!sdf_xml)
{
logger->reportError("expected an sdf element");
return false;
}
TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
if (!world_xml)
{
logger->reportError("expected a world element");
return false;
}
// Get all model (robot) elements
for (TiXmlElement* robot_xml = world_xml->FirstChildElement("model"); robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
{
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
logger->reportError("Expected a name for robot");
return false;
}
localModel->m_name = name;
TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose");
if (0==pose_xml)
{
localModel->m_rootTransformInWorld.setIdentity();
}
else
{
parseTransform(localModel->m_rootTransformInWorld,pose_xml,logger,m_parseSDF);
}
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
UrdfMaterial* material = new UrdfMaterial;
parseMaterial(*material, material_xml, logger);
UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
localModel->m_materials.insert(material->m_name.c_str(),material);
}
}
// char msg[1024];
// sprintf(msg,"Num materials=%d", m_model.m_materials.size());
// logger->printMessage(msg);
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
UrdfLink* link = new UrdfLink;
if (parseLink(*localModel, *link, link_xml,logger))
{
if (localModel->m_links.find(link->m_name.c_str()))
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
return false;
} else
{
//copy model material into link material, if link has no local material
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
} else
{
//logger->reportError("Cannot find material with name:");
//logger->reportError(vis.m_materialName.c_str());
}
}
}
localModel->m_links.insert(link->m_name.c_str(),link);
}
} else
{
logger->reportError("failed to parse link");
delete link;
return false;
}
}
if (localModel->m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
UrdfJoint* joint = new UrdfJoint;
if (parseJoint(*joint, joint_xml,logger))
{
if (localModel->m_joints.find(joint->m_name.c_str()))
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
return false;
}
else
{
localModel->m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
{
logger->reportError("joint xml is not initialized correctly");
return false;
}
}
bool ok(initTreeAndRoot(*localModel,logger));
if (!ok)
{
return false;
}
m_sdfModels.push_back(localModel);
}
return true;
}
bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
{
TiXmlDocument xml_doc;
xml_doc.Parse(urdfText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
xml_doc.ClearError();
return false;
}
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
logger->reportError("expected a robot element");
return false;
}
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
logger->reportError("Expected a name for robot");
return false;
}
m_urdf2Model.m_name = name;
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
UrdfMaterial* material = new UrdfMaterial;
parseMaterial(*material, material_xml, logger);
UrdfMaterial** mat =m_urdf2Model.m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
m_urdf2Model.m_materials.insert(material->m_name.c_str(),material);
}
}
// char msg[1024];
// sprintf(msg,"Num materials=%d", m_model.m_materials.size());
// logger->printMessage(msg);
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
UrdfLink* link = new UrdfLink;
if (parseLink(m_urdf2Model,*link, link_xml,logger))
{
if (m_urdf2Model.m_links.find(link->m_name.c_str()))
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
return false;
} else
{
//copy model material into link material, if link has no local material
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
} else
{
//logger->reportError("Cannot find material with name:");
//logger->reportError(vis.m_materialName.c_str());
}
}
}
m_urdf2Model.m_links.insert(link->m_name.c_str(),link);
}
} else
{
logger->reportError("failed to parse link");
delete link;
return false;
}
}
if (m_urdf2Model.m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
UrdfJoint* joint = new UrdfJoint;
if (parseJoint(*joint, joint_xml,logger))
{
if (m_urdf2Model.m_joints.find(joint->m_name.c_str()))
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
return false;
}
else
{
m_urdf2Model.m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
{
logger->reportError("joint xml is not initialized correctly");
return false;
}
}
bool ok(initTreeAndRoot(m_urdf2Model,logger));
if (!ok)
{
return false;
}
if (forceFixedBase)
{
for (int i=0;i<m_urdf2Model.m_rootLinks.size();i++)
{
UrdfLink* link(m_urdf2Model.m_rootLinks.at(i));
link->m_inertia.m_mass = 0.0;
link->m_inertia.m_ixx = 0.0;
link->m_inertia.m_ixy = 0.0;
link->m_inertia.m_ixz = 0.0;
link->m_inertia.m_iyy = 0.0;
link->m_inertia.m_iyz = 0.0;
link->m_inertia.m_izz = 0.0;
}
}
return true;
}
bool Scene::parse(const string &filename) {
QFile* file = new QFile(filename.c_str());
if (!file->open(QIODevice::ReadOnly | QIODevice::Text)) {
QMessageBox::critical(0,
"FermaRenderer",
"Couldn't open the selected scene file.",
QMessageBox::Ok);
return false;
}
QXmlStreamReader xml(file);
while(!xml.atEnd() && !xml.hasError()) {
QXmlStreamReader::TokenType token = xml.readNext();
if(token == QXmlStreamReader::StartDocument) {
continue;
}
if(token == QXmlStreamReader::StartElement) {
// parse different parts
if(xml.name() == "material") {
materials.push_back(parseMaterial(xml));
}
if(xml.name() == "shape") {
shapes.push_back(parseShape(xml));
}
if(xml.name() == "object") {
objects.push_back(parseObject(xml));
}
if(xml.name() == "config") {
config = parseConfigurations(xml);
}
if(xml.name() == "camera") {
camera = parseCamera(xml);
}
}
}
if(xml.hasError()) {
QMessageBox::critical(0,
"FermaRenderer",
xml.errorString(),
QMessageBox::Ok);
return false;
}
xml.clear();
cout << "done parsing." << endl;
return true;
}
Actor* Parser::parseMesh(xml_node_iterator sceneElement)
{
// setting the mesh
const char* filename = sceneElement->attribute("file").value();
TriangleMesh* mesh = MeshReader().execute(filename);
Primitive* primitive = new TriangleMeshShape(mesh);
xml_node op;
if ((op = sceneElement->child("transform")) != NULL) {
vec3 position(0, 0, 0);
quat q(vec3(0, 0, 0));
vec3 scale(1, 1, 1);
float x, y, z;
xml_object_range<xml_node_iterator> transformations = op.children();
for (xml_node_iterator transformation = transformations.begin(); transformation != transformations.end(); ++transformation)
{
if (strcmp(transformation->name(), "position") == 0)
{
const char * stringTranslation = transformation->text().as_string();
sscanf(stringTranslation, "%f %f %f", &x, &y, &z);
vec3 translationVec(x, y, z);
position = translationVec;
}
else if (strcmp(transformation->name(), "scale") == 0)
{
float s_ = transformation->text().as_float();
vec3 newS(s_, s_, s_);
scale = newS;
}
else if (strcmp(transformation->name(), "rotation") == 0)
{
float angle = transformation->child("angle").text().as_float();
const char * _Axis = transformation->child("axis").text().as_string();
sscanf(_Axis, "%f %f %f", &x, &y, &z);
vec3 axis(x, y, z);
q = quat(axis, angle);
}
mesh->transform(mat4::TRS(position, q, scale));
}
}
if ((op = sceneElement->child("material")) != NULL) {
Material * material = parseMaterial(op);
primitive->setMaterial(material);
}
else
{
Material * material = MaterialFactory::New();
primitive->setMaterial(material->getDefault());
}
Actor* act = new Actor(*primitive);
act->setName(filename);
return act;
}
Actor* Parser::parseSphere(xml_node_iterator sceneElement)
{
// default values
vec3 center(0, 0, 0);
REAL radius = 1.0;
int meridians = 16;
// opt values
xml_node op;
op = sceneElement->child("center");
REAL x, y, z;
if (op != NULL)
{
const char* center_vector = op.text().as_string();
sscanf(center_vector, "%f %f %f", &x, &y, &z);
center.set(x, y, z);
}
op = sceneElement->child("radius");
if (op != NULL)
radius = op.text().as_float();
op = sceneElement->child("meridians");
if (op != NULL)
meridians = op.text().as_int();
// now, lets make the mesh of Sphere
TriangleMesh* sphereMesh = MeshSweeper::makeSphere(center, radius, meridians);
Primitive* primitive = new TriangleMeshShape(sphereMesh);
if ((op = sceneElement->child("transform")) != NULL) {
vec3 position(0, 0, 0);
quat q(vec3(0, 0, 0));
vec3 scale(1, 1, 1);
float x, y, z;
xml_object_range<xml_node_iterator> transformations = op.children();
for (xml_node_iterator transformation = transformations.begin(); transformation != transformations.end(); ++transformation)
{
if (strcmp(transformation->name(), "position") == 0)
{
const char * stringTranslation = transformation->text().as_string();
sscanf(stringTranslation, "%f %f %f", &x, &y, &z);
vec3 translationVec(x, y, z);
position = translationVec;
}
else if (strcmp(transformation->name(), "scale") == 0)
{
float s_ = transformation->text().as_float();
vec3 newS(s_, s_, s_);
scale = newS;
}
else if (strcmp(transformation->name(), "rotation") == 0)
{
float angle = transformation->child("angle").text().as_float();
const char * _Axis = transformation->child("axis").text().as_string();
sscanf(_Axis, "%f %f %f", &x, &y, &z);
vec3 axis(x, y, z);
q = quat(axis, angle);
}
sphereMesh->transform(mat4::TRS(position, q, scale));
}
}
if ((op = sceneElement->child("material")) != NULL) {
Material * material = parseMaterial(op);
primitive->setMaterial(material);
}
else
{
Material * material = MaterialFactory::New();
primitive->setMaterial(material->getDefault());
}
Actor* act = new Actor(*primitive);
act->setName("sphere");
return act;
}
ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
{
ModelInterfaceSharedPtr model(new ModelInterface);
model->clear();
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
if (xml_doc.Error())
{
CONSOLE_BRIDGE_logError(xml_doc.ErrorDesc());
xml_doc.ClearError();
model.reset();
return model;
}
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file");
model.reset();
return model;
}
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
CONSOLE_BRIDGE_logError("No name given for the robot.");
model.reset();
return model;
}
model->name_ = std::string(name);
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
MaterialSharedPtr material;
material.reset(new Material);
try {
parseMaterial(*material, material_xml, false); // material needs to be fully defined here
if (model->getMaterial(material->name))
{
CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
material.reset();
model.reset();
return model;
}
else
{
model->materials_.insert(make_pair(material->name,material));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
}
}
catch (ParseError &/*e*/) {
CONSOLE_BRIDGE_logError("material xml is not initialized correctly");
material.reset();
model.reset();
return model;
}
}
// Get all Link elements
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
LinkSharedPtr link;
link.reset(new Link);
try {
parseLink(*link, link_xml);
if (model->getLink(link->name))
{
CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
model.reset();
return model;
}
else
{
// set link visual(s) material
CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
if (link->visual)
{
assignMaterial(link->visual, model, link->name.c_str());
}
for (const auto& visual : link->visual_array)
{
assignMaterial(visual, model, link->name.c_str());
}
model->links_.insert(make_pair(link->name,link));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
}
}
catch (ParseError &/*e*/) {
CONSOLE_BRIDGE_logError("link xml is not initialized correctly");
model.reset();
return model;
}
}
if (model->links_.empty()){
CONSOLE_BRIDGE_logError("No link elements found in urdf file");
model.reset();
return model;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
JointSharedPtr joint;
joint.reset(new Joint);
if (parseJoint(*joint, joint_xml))
{
if (model->getJoint(joint->name))
{
CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str());
model.reset();
return model;
}
else
{
model->joints_.insert(make_pair(joint->name,joint));
CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
}
}
else
{
CONSOLE_BRIDGE_logError("joint xml is not initialized correctly");
model.reset();
return model;
}
}
// every link has children links and joints, but no parents, so we create a
// local convenience data structure for keeping child->parent relations
std::map<std::string, std::string> parent_link_tree;
parent_link_tree.clear();
// building tree: name mapping
try
{
model->initTree(parent_link_tree);
}
catch(ParseError &e)
{
CONSOLE_BRIDGE_logError("Failed to build tree: %s", e.what());
model.reset();
return model;
}
// find the root link
try
{
model->initRoot(parent_link_tree);
}
catch(ParseError &e)
{
CONSOLE_BRIDGE_logError("Failed to find root link: %s", e.what());
model.reset();
return model;
}
return model;
}
