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 #include <boost/algorithm/string.hpp>
00038 #include <ros/ros.h>
00039 #include <vector>
00040 #include "urdf/model.h"
00041
00042 namespace urdf{
00043
00044
00045 Model::Model()
00046 {
00047 this->clear();
00048 }
00049
00050 void Model::clear()
00051 {
00052 name_.clear();
00053 this->links_.clear();
00054 this->joints_.clear();
00055 this->materials_.clear();
00056 this->root_link_.reset();
00057 }
00058
00059
00060 bool Model::initFile(const std::string& filename)
00061 {
00062 TiXmlDocument xml_doc;
00063 xml_doc.LoadFile(filename);
00064
00065 return initXml(&xml_doc);
00066 }
00067
00068
00069 bool Model::initParam(const std::string& param)
00070 {
00071 ros::NodeHandle nh;
00072 std::string xml_string;
00073
00074
00075 std::string full_param;
00076 if (!nh.searchParam(param, full_param)){
00077 ROS_ERROR("Could not find parameter %s on parameter server", param.c_str());
00078 return false;
00079 }
00080
00081
00082 if (!nh.getParam(full_param, xml_string)){
00083 ROS_ERROR("Could read parameter %s on parameter server", full_param.c_str());
00084 return false;
00085 }
00086 return initString(xml_string);
00087 }
00088
00089
00090 bool Model::initString(const std::string& xml_string)
00091 {
00092 TiXmlDocument xml_doc;
00093 xml_doc.Parse(xml_string.c_str());
00094
00095 return initXml(&xml_doc);
00096 }
00097
00098
00099 bool Model::initXml(TiXmlDocument *xml_doc)
00100 {
00101 if (!xml_doc)
00102 {
00103 ROS_ERROR("Could not parse the xml");
00104 return false;
00105 }
00106
00107 TiXmlElement *robot_xml = xml_doc->FirstChildElement("robot");
00108 if (!robot_xml)
00109 {
00110 ROS_ERROR("Could not find the 'robot' element in the xml file");
00111 return false;
00112 }
00113 return initXml(robot_xml);
00114 }
00115
00116 bool Model::initXml(TiXmlElement *robot_xml)
00117 {
00118 this->clear();
00119
00120 ROS_DEBUG("Parsing robot xml");
00121 if (!robot_xml) return false;
00122
00123
00124 const char *name = robot_xml->Attribute("name");
00125 if (!name)
00126 {
00127 ROS_ERROR("No name given for the robot.");
00128 return false;
00129 }
00130 this->name_ = std::string(name);
00131
00132
00133 for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
00134 {
00135 boost::shared_ptr<Material> material;
00136 material.reset(new Material);
00137
00138 if (material->initXml(material_xml))
00139 {
00140 if (this->getMaterial(material->name))
00141 {
00142 ROS_ERROR("material '%s' is not unique.", material->name.c_str());
00143 material.reset();
00144 return false;
00145 }
00146 else
00147 {
00148 this->materials_.insert(make_pair(material->name,material));
00149 ROS_DEBUG("successfully added a new material '%s'", material->name.c_str());
00150 }
00151 }
00152 else
00153 {
00154 ROS_ERROR("material xml is not initialized correctly");
00155 material.reset();
00156 return false;
00157 }
00158 }
00159
00160
00161 for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
00162 {
00163 boost::shared_ptr<Link> link;
00164 link.reset(new Link);
00165
00166 if (link->initXml(link_xml))
00167 {
00168 if (this->getLink(link->name))
00169 {
00170 ROS_ERROR("link '%s' is not unique.", link->name.c_str());
00171 link.reset();
00172 return false;
00173 }
00174 else
00175 {
00176
00177 ROS_DEBUG("setting link '%s' material", link->name.c_str());
00178 if (link->visual)
00179 {
00180 if (!link->visual->material_name.empty())
00181 {
00182 if (this->getMaterial(link->visual->material_name))
00183 {
00184 ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str());
00185 link->visual->material = this->getMaterial( link->visual->material_name.c_str() );
00186 }
00187 else
00188 {
00189 if (link->visual->material)
00190 {
00191 ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str());
00192 this->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
00193 }
00194 else
00195 {
00196 ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
00197 link.reset();
00198 return false;
00199 }
00200 }
00201 }
00202 }
00203
00204 this->links_.insert(make_pair(link->name,link));
00205 ROS_DEBUG("successfully added a new link '%s'", link->name.c_str());
00206 }
00207 }
00208 else
00209 {
00210 ROS_ERROR("link xml is not initialized correctly");
00211 link.reset();
00212 return false;
00213 }
00214 }
00215 if (this->links_.empty()){
00216 ROS_ERROR("No link elements found in urdf file");
00217 return false;
00218 }
00219
00220
00221 for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
00222 {
00223 boost::shared_ptr<Joint> joint;
00224 joint.reset(new Joint);
00225
00226 if (joint->initXml(joint_xml))
00227 {
00228 if (this->getJoint(joint->name))
00229 {
00230 ROS_ERROR("joint '%s' is not unique.", joint->name.c_str());
00231 joint.reset();
00232 return false;
00233 }
00234 else
00235 {
00236 this->joints_.insert(make_pair(joint->name,joint));
00237 ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str());
00238 }
00239 }
00240 else
00241 {
00242 ROS_ERROR("joint xml is not initialized correctly");
00243 joint.reset();
00244 return false;
00245 }
00246 }
00247
00248
00249
00250
00251 std::map<std::string, std::string> parent_link_tree;
00252 parent_link_tree.clear();
00253
00254
00255 if (!this->initTree(parent_link_tree))
00256 {
00257 ROS_ERROR("failed to build tree");
00258 return false;
00259 }
00260
00261
00262 if (!this->initRoot(parent_link_tree))
00263 {
00264 ROS_ERROR("failed to find root link");
00265 return false;
00266 }
00267
00268 return true;
00269 }
00270
00271 bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
00272 {
00273
00274 for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
00275 {
00276 std::string parent_link_name = joint->second->parent_link_name;
00277 std::string child_link_name = joint->second->child_link_name;
00278
00279 ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());
00280 if (parent_link_name.empty() || child_link_name.empty())
00281 {
00282 ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str());
00283 return false;
00284 }
00285 else
00286 {
00287
00288 boost::shared_ptr<Link> child_link, parent_link;
00289 this->getLink(child_link_name, child_link);
00290 if (!child_link)
00291 {
00292 ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() );
00293 return false;
00294 }
00295 this->getLink(parent_link_name, parent_link);
00296 if (!parent_link)
00297 {
00298 ROS_ERROR(" parent link '%s' of joint '%s' not found. The Boxturtle urdf parser used to automatically add this link for you, but this is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint from your urdf file, or add \"<link name=\"%s\" />\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() );
00299 return false;
00300 }
00301
00302
00303 child_link->setParent(parent_link);
00304
00305
00306 child_link->setParentJoint(joint->second);
00307
00308
00309 parent_link->addChildJoint(joint->second);
00310
00311
00312 parent_link->addChild(child_link);
00313
00314
00315 parent_link_tree[child_link->name] = parent_link_name;
00316
00317 ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
00318 }
00319 }
00320
00321 return true;
00322 }
00323
00324
00325
00326 bool Model::initRoot(std::map<std::string, std::string> &parent_link_tree)
00327 {
00328
00329 this->root_link_.reset();
00330
00331
00332 for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++)
00333 {
00334 std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first);
00335 if (parent == parent_link_tree.end())
00336 {
00337
00338 if (!this->root_link_)
00339 {
00340 getLink(l->first, this->root_link_);
00341 }
00342
00343 else{
00344 ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str());
00345 return false;
00346 }
00347 }
00348 }
00349 if (!this->root_link_)
00350 {
00351 ROS_ERROR("No root link found. The robot xml is not a valid tree.");
00352 return false;
00353 }
00354 ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
00355
00356 return true;
00357 }
00358
00359 boost::shared_ptr<Material> Model::getMaterial(const std::string& name) const
00360 {
00361 boost::shared_ptr<Material> ptr;
00362 if (this->materials_.find(name) == this->materials_.end())
00363 ptr.reset();
00364 else
00365 ptr = this->materials_.find(name)->second;
00366 return ptr;
00367 }
00368
00369 boost::shared_ptr<const Link> Model::getLink(const std::string& name) const
00370 {
00371 boost::shared_ptr<const Link> ptr;
00372 if (this->links_.find(name) == this->links_.end())
00373 ptr.reset();
00374 else
00375 ptr = this->links_.find(name)->second;
00376 return ptr;
00377 }
00378
00379 void Model::getLinks(std::vector<boost::shared_ptr<Link> >& links) const
00380 {
00381 for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
00382 {
00383 links.push_back(link->second);
00384 }
00385 }
00386
00387 void Model::getLink(const std::string& name,boost::shared_ptr<Link> &link) const
00388 {
00389 boost::shared_ptr<Link> ptr;
00390 if (this->links_.find(name) == this->links_.end())
00391 ptr.reset();
00392 else
00393 ptr = this->links_.find(name)->second;
00394 link = ptr;
00395 }
00396
00397 boost::shared_ptr<const Joint> Model::getJoint(const std::string& name) const
00398 {
00399 boost::shared_ptr<const Joint> ptr;
00400 if (this->joints_.find(name) == this->joints_.end())
00401 ptr.reset();
00402 else
00403 ptr = this->joints_.find(name)->second;
00404 return ptr;
00405 }
00406
00407 }
00408