00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include "urdf/link.h"
00039 #include <ros/ros.h>
00040 #include <boost/lexical_cast.hpp>
00041
00042 namespace urdf{
00043
00044 boost::shared_ptr<Geometry> parseGeometry(TiXmlElement *g)
00045 {
00046 boost::shared_ptr<Geometry> geom;
00047 if (!g) return geom;
00048
00049 TiXmlElement *shape = g->FirstChildElement();
00050 if (!shape)
00051 {
00052 ROS_ERROR("Geometry tag contains no child element.");
00053 return geom;
00054 }
00055
00056 std::string type_name = shape->ValueStr();
00057 if (type_name == "sphere")
00058 geom.reset(new Sphere);
00059 else if (type_name == "box")
00060 geom.reset(new Box);
00061 else if (type_name == "cylinder")
00062 geom.reset(new Cylinder);
00063 else if (type_name == "mesh")
00064 geom.reset(new Mesh);
00065 else
00066 {
00067 ROS_ERROR("Unknown geometry type '%s'", type_name.c_str());
00068 return geom;
00069 }
00070
00071
00072 if (!geom->initXml(shape)){
00073 ROS_ERROR("Geometry failed to parse");
00074 geom.reset();
00075 }
00076
00077 return geom;
00078 }
00079
00080 bool Material::initXml(TiXmlElement *config)
00081 {
00082 bool has_rgb = false;
00083 bool has_filename = false;
00084
00085 this->clear();
00086
00087 if (!config->Attribute("name"))
00088 {
00089 ROS_ERROR("Material must contain a name attribute");
00090 return false;
00091 }
00092
00093 this->name = config->Attribute("name");
00094
00095
00096 TiXmlElement *t = config->FirstChildElement("texture");
00097 if (t)
00098 {
00099 if (t->Attribute("filename"))
00100 {
00101 this->texture_filename = t->Attribute("filename");
00102 has_filename = true;
00103 }
00104 else
00105 {
00106 ROS_ERROR("texture has no filename for Material %s",this->name.c_str());
00107 }
00108 }
00109
00110
00111 TiXmlElement *c = config->FirstChildElement("color");
00112 if (c)
00113 {
00114 if (c->Attribute("rgba"))
00115 {
00116 if (!this->color.init(c->Attribute("rgba")))
00117 {
00118 ROS_ERROR("Material %s has malformed color rgba values.",this->name.c_str());
00119 this->color.clear();
00120 return false;
00121 }
00122 else
00123 has_rgb = true;
00124 }
00125 else
00126 {
00127 ROS_ERROR("Material %s color has no rgba",this->name.c_str());
00128 }
00129 }
00130
00131 return (has_rgb || has_filename);
00132 }
00133
00134 bool Inertial::initXml(TiXmlElement *config)
00135 {
00136 this->clear();
00137
00138
00139 TiXmlElement *o = config->FirstChildElement("origin");
00140 if (!o)
00141 {
00142 ROS_DEBUG("Origin tag not present for inertial element, using default (Identity)");
00143 this->origin.clear();
00144 }
00145 else
00146 {
00147 if (!this->origin.initXml(o))
00148 {
00149 ROS_ERROR("Inertial has a malformed origin tag");
00150 this->origin.clear();
00151 return false;
00152 }
00153 }
00154
00155 TiXmlElement *mass_xml = config->FirstChildElement("mass");
00156 if (!mass_xml)
00157 {
00158 ROS_ERROR("Inertial element must have mass element");
00159 return false;
00160 }
00161 if (!mass_xml->Attribute("value"))
00162 {
00163 ROS_ERROR("Inertial: mass element must have value attributes");
00164 return false;
00165 }
00166
00167 try
00168 {
00169 mass = boost::lexical_cast<double>(mass_xml->Attribute("value"));
00170 }
00171 catch (boost::bad_lexical_cast &e)
00172 {
00173 ROS_ERROR("mass (%s) is not a float",mass_xml->Attribute("value"));
00174 return false;
00175 }
00176
00177 TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
00178 if (!inertia_xml)
00179 {
00180 ROS_ERROR("Inertial element must have inertia element");
00181 return false;
00182 }
00183 if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
00184 inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
00185 inertia_xml->Attribute("izz")))
00186 {
00187 ROS_ERROR("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
00188 return false;
00189 }
00190 try
00191 {
00192 ixx = boost::lexical_cast<double>(inertia_xml->Attribute("ixx"));
00193 ixy = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
00194 ixz = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
00195 iyy = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
00196 iyz = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
00197 izz = boost::lexical_cast<double>(inertia_xml->Attribute("izz"));
00198 }
00199 catch (boost::bad_lexical_cast &e)
00200 {
00201 ROS_ERROR("one of the inertia elements: ixx (%s) ixy (%s) ixz (%s) iyy (%s) iyz (%s) izz (%s) is not a valid double",
00202 inertia_xml->Attribute("ixx"),
00203 inertia_xml->Attribute("ixy"),
00204 inertia_xml->Attribute("ixz"),
00205 inertia_xml->Attribute("iyy"),
00206 inertia_xml->Attribute("iyz"),
00207 inertia_xml->Attribute("izz"));
00208 return false;
00209 }
00210
00211 return true;
00212 }
00213
00214 bool Visual::initXml(TiXmlElement *config)
00215 {
00216 this->clear();
00217
00218
00219 TiXmlElement *o = config->FirstChildElement("origin");
00220 if (!o)
00221 {
00222 ROS_DEBUG("Origin tag not present for visual element, using default (Identity)");
00223 this->origin.clear();
00224 }
00225 else if (!this->origin.initXml(o))
00226 {
00227 ROS_ERROR("Visual has a malformed origin tag");
00228 this->origin.clear();
00229 return false;
00230 }
00231
00232
00233 TiXmlElement *geom = config->FirstChildElement("geometry");
00234 geometry = parseGeometry(geom);
00235 if (!geometry)
00236 {
00237 ROS_ERROR("Malformed geometry for Visual element");
00238 return false;
00239 }
00240
00241
00242 TiXmlElement *mat = config->FirstChildElement("material");
00243 if (!mat)
00244 {
00245 ROS_DEBUG("visual element has no material tag.");
00246 }
00247 else
00248 {
00249
00250 if (!mat->Attribute("name"))
00251 {
00252 ROS_ERROR("Visual material must contain a name attribute");
00253 return false;
00254 }
00255 this->material_name = mat->Attribute("name");
00256
00257
00258 this->material.reset(new Material);
00259 if (!this->material->initXml(mat))
00260 {
00261 ROS_DEBUG("Could not parse material element in Visual block, maybe defined outside.");
00262 this->material.reset();
00263 }
00264 else
00265 {
00266 ROS_DEBUG("Parsed material element in Visual block.");
00267 }
00268 }
00269
00270 return true;
00271 }
00272
00273 bool Collision::initXml(TiXmlElement* config)
00274 {
00275 this->clear();
00276
00277
00278 TiXmlElement *o = config->FirstChildElement("origin");
00279 if (!o)
00280 {
00281 ROS_DEBUG("Origin tag not present for collision element, using default (Identity)");
00282 this->origin.clear();
00283 }
00284 else if (!this->origin.initXml(o))
00285 {
00286 ROS_ERROR("Collision has a malformed origin tag");
00287 this->origin.clear();
00288 return false;
00289 }
00290
00291
00292 TiXmlElement *geom = config->FirstChildElement("geometry");
00293 geometry = parseGeometry(geom);
00294 if (!geometry)
00295 {
00296 ROS_ERROR("Malformed geometry for Collision element");
00297 return false;
00298 }
00299
00300 return true;
00301 }
00302
00303 bool Sphere::initXml(TiXmlElement *c)
00304 {
00305 this->clear();
00306
00307 this->type = SPHERE;
00308 if (!c->Attribute("radius"))
00309 {
00310 ROS_ERROR("Sphere shape must have a radius attribute");
00311 return false;
00312 }
00313
00314 try
00315 {
00316 radius = boost::lexical_cast<double>(c->Attribute("radius"));
00317 }
00318 catch (boost::bad_lexical_cast &e)
00319 {
00320 ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
00321 return false;
00322 }
00323
00324 return true;
00325 }
00326
00327 bool Box::initXml(TiXmlElement *c)
00328 {
00329 this->clear();
00330
00331 this->type = BOX;
00332 if (!c->Attribute("size"))
00333 {
00334 ROS_ERROR("Box shape has no size attribute");
00335 return false;
00336 }
00337 if (!dim.init(c->Attribute("size")))
00338 {
00339 ROS_ERROR("Box shape has malformed size attribute");
00340 dim.clear();
00341 return false;
00342 }
00343 return true;
00344 }
00345
00346 bool Cylinder::initXml(TiXmlElement *c)
00347 {
00348 this->clear();
00349
00350 this->type = CYLINDER;
00351 if (!c->Attribute("length") ||
00352 !c->Attribute("radius"))
00353 {
00354 ROS_ERROR("Cylinder shape must have both length and radius attributes");
00355 return false;
00356 }
00357
00358 try
00359 {
00360 length = boost::lexical_cast<double>(c->Attribute("length"));
00361 }
00362 catch (boost::bad_lexical_cast &e)
00363 {
00364 ROS_ERROR("length (%s) is not a valid float",c->Attribute("length"));
00365 return false;
00366 }
00367
00368 try
00369 {
00370 radius = boost::lexical_cast<double>(c->Attribute("radius"));
00371 }
00372 catch (boost::bad_lexical_cast &e)
00373 {
00374 ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius"));
00375 return false;
00376 }
00377
00378 return true;
00379 }
00380
00381 bool Mesh::initXml(TiXmlElement *c)
00382 {
00383 this->clear();
00384
00385 this->type = MESH;
00386 if (!c->Attribute("filename"))
00387 {
00388 ROS_ERROR("Mesh must contain a filename attribute");
00389 return false;
00390 }
00391
00392 filename = c->Attribute("filename");
00393
00394 if (c->Attribute("scale"))
00395 {
00396 if (!this->scale.init(c->Attribute("scale")))
00397 {
00398 ROS_ERROR("Mesh scale was specified, but could not be parsed");
00399 this->scale.clear();
00400 return false;
00401 }
00402 }
00403 else
00404 ROS_DEBUG("Mesh scale was not specified, default to (1,1,1)");
00405
00406 return true;
00407 }
00408
00409
00410 bool Link::initXml(TiXmlElement* config)
00411 {
00412 this->clear();
00413
00414 const char *name_char = config->Attribute("name");
00415 if (!name_char)
00416 {
00417 ROS_ERROR("No name given for the link.");
00418 return false;
00419 }
00420 name = std::string(name_char);
00421
00422
00423 TiXmlElement *i = config->FirstChildElement("inertial");
00424 if (i)
00425 {
00426 inertial.reset(new Inertial);
00427 if (!inertial->initXml(i))
00428 {
00429 ROS_ERROR("Could not parse inertial element for Link '%s'", this->name.c_str());
00430 return false;
00431 }
00432 }
00433
00434
00435 TiXmlElement *v = config->FirstChildElement("visual");
00436 if (v)
00437 {
00438 visual.reset(new Visual);
00439 if (!visual->initXml(v))
00440 {
00441 ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str());
00442 return false;
00443 }
00444 }
00445
00446
00447 TiXmlElement *col = config->FirstChildElement("collision");
00448 if (col)
00449 {
00450 collision.reset(new Collision);
00451 if (!collision->initXml(col))
00452 {
00453 ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str());
00454 return false;
00455 }
00456 }
00457
00458 return true;
00459 }
00460
00461 void Link::setParent(boost::shared_ptr<Link> parent)
00462 {
00463 this->parent_link_ = parent;
00464 ROS_DEBUG("set parent Link '%s' for Link '%s'", parent->name.c_str(), this->name.c_str());
00465 }
00466
00467 void Link::setParentJoint(boost::shared_ptr<Joint> parent)
00468 {
00469 this->parent_joint = parent;
00470 ROS_DEBUG("set parent joint '%s' to Link '%s'", parent->name.c_str(), this->name.c_str());
00471 }
00472
00473 void Link::addChild(boost::shared_ptr<Link> child)
00474 {
00475 this->child_links.push_back(child);
00476 ROS_DEBUG("added child Link '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
00477 }
00478
00479 void Link::addChildJoint(boost::shared_ptr<Joint> child)
00480 {
00481 this->child_joints.push_back(child);
00482 ROS_DEBUG("added child Joint '%s' to Link '%s'", child->name.c_str(), this->name.c_str());
00483 }
00484
00485
00486
00487 }
00488