bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

major cleanup of urdf, urdf_parser, collada_parser and urdf_interface

This commit is contained in:
Wim Meeussen 2011-06-21 16:29:47 -07:00
parent 6e63977e35
commit 8a357ce772
11 changed files with 2172 additions and 2298 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
collada_parser/include/collada_parser/collada_parser.h
View File

@ -46,30 +46,8 @@
namespace urdf{
class ColladaParser : public ModelInterface
{
public:
/// \brief Load Model from string
bool initCollada(const std::string &xml_string );
protected:
/// non-const get Collada Link()
void getColladaLink(const std::string& name,boost::shared_ptr<Link> &link) const;
/// non-const getColladaMaterial()
//boost::shared_ptr<Material> getColladaMaterial(const std::string& name) const;
/// in initXml(), onece all links are loaded,
/// it's time to build a tree
bool initColladaTree(std::map<std::string, std::string> &parent_link_tree);
/// in initXml(), onece tree is built,
/// it's time to find the root Link
bool initColladaRoot(std::map<std::string, std::string> &parent_link_tree);
friend class ColladaModelReader;
};
boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_string );
}

163
collada_parser/src/collada_parser.cpp
View File

@ -389,7 +389,7 @@ class ColladaModelReader : public daeErrorHandler
};
public:
ColladaModelReader(ColladaParser& model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
ColladaModelReader(boost::shared_ptr<ModelInterface> model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
daeErrorHandler::setErrorHandler(this);
_resourcedir = ".";
}
@ -441,7 +441,7 @@ protected:
/// \extract the first possible robot in the scene
bool _Extract()
{
_model.clear();
_model->clear();
std::list< std::pair<domInstance_kinematics_modelRef, boost::shared_ptr<KinematicsSceneBindings> > > listPossibleBodies;
domCOLLADA::domSceneRef allscene = _dom->getScene();
if( !allscene ) {
@ -482,12 +482,12 @@ protected:
{
std::map<std::string, std::string> parent_link_tree;
// building tree: name mapping
if (!_model.initColladaTree(parent_link_tree)) {
if (!_model->initTree(parent_link_tree)) {
ROS_ERROR("failed to build tree");
}
// find the root link
if (!_model.initColladaRoot(parent_link_tree)) {
if (!_model->initRoot(parent_link_tree)) {
ROS_ERROR("failed to find root link");
}
}
@ -513,17 +513,17 @@ protected:
}
// set the name
if( _model.name_.size() == 0 && !!ias->getName() ) {
_model.name_ = ias->getName();
if( _model->name_.size() == 0 && !!ias->getName() ) {
_model->name_ = ias->getName();
}
if( _model.name_.size() == 0 && !!ias->getSid()) {
_model.name_ = ias->getSid();
if( _model->name_.size() == 0 && !!ias->getSid()) {
_model->name_ = ias->getSid();
}
if( _model.name_.size() == 0 && !!articulated_system->getName() ) {
_model.name_ = articulated_system->getName();
if( _model->name_.size() == 0 && !!articulated_system->getName() ) {
_model->name_ = articulated_system->getName();
}
if( _model.name_.size() == 0 && !!articulated_system->getId()) {
_model.name_ = articulated_system->getId();
if( _model->name_.size() == 0 && !!articulated_system->getId()) {
_model->name_ = articulated_system->getId();
}
if( !!articulated_system->getMotion() ) {
@ -606,11 +606,11 @@ protected:
return false;
}
if( _model.name_.size() == 0 && !!ikm->getName() ) {
_model.name_ = ikm->getName();
if( _model->name_.size() == 0 && !!ikm->getName() ) {
_model->name_ = ikm->getName();
}
if( _model.name_.size() == 0 && !!ikm->getID() ) {
_model.name_ = ikm->getID();
if( _model->name_.size() == 0 && !!ikm->getID() ) {
_model->name_ = ikm->getID();
}
if (!_ExtractKinematicsModel(kmodel, pvisualnode, pmodel, bindings.listAxisBindings)) {
@ -624,7 +624,7 @@ protected:
bool _ExtractKinematicsModel(domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const std::list<JointAxisBinding>& listAxisBindings)
{
std::vector<domJointRef> vdomjoints;
ROS_DEBUG_STREAM(str(boost::format("kinematics model: %s\n")%_model.name_));
ROS_DEBUG_STREAM(str(boost::format("kinematics model: %s\n")%_model->name_));
if( !!pnode ) {
ROS_DEBUG_STREAM(str(boost::format("node name: %s\n")%pnode->getId()));
}
@ -764,12 +764,12 @@ protected:
}
boost::shared_ptr<Link> plink;
_model.getColladaLink(linkname,plink);
_model->getLink(linkname,plink);
if( !plink ) {
plink.reset(new Link());
plink->name = linkname;
plink->visual.reset(new Visual());
_model.links_.insert(std::make_pair(linkname,plink));
_model->links_.insert(std::make_pair(linkname,plink));
}
_getUserData(pdomlink)->p = plink;
@ -867,7 +867,7 @@ protected:
pjoint->name = pdomjoint->getName();
}
else {
pjoint->name = str(boost::format("dummy%d")%_model.joints_.size());
pjoint->name = str(boost::format("dummy%d")%_model->joints_.size());
}
if( !joint_active ) {
@ -886,8 +886,8 @@ protected:
}
_getUserData(pdomjoint)->p = pjoint;
_getUserData(pdomaxis)->p = boost::shared_ptr<int>(new int(_model.joints_.size()));
_model.joints_[pjoint->name] = pjoint;
_getUserData(pdomaxis)->p = boost::shared_ptr<int>(new int(_model->joints_.size()));
_model->joints_[pjoint->name] = pjoint;
vjoints[ic] = pjoint;
}
@ -922,7 +922,7 @@ protected:
ss <<"_dummy" << numjoints;
pchildlink.reset(new Link());
pchildlink->name = ss.str();
_model.links_.insert(std::make_pair(pchildlink->name,pchildlink));
_model->links_.insert(std::make_pair(pchildlink->name,pchildlink));
}
ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic));
@ -2175,7 +2175,7 @@ protected:
return boost::shared_ptr<Joint>();
}
boost::shared_ptr<Joint> pjoint = _model.joints_[std::string(pdomjoint->getName())];
boost::shared_ptr<Joint> pjoint = _model->joints_[std::string(pdomjoint->getName())];
if(!pjoint) {
ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName()));
}
@ -2514,17 +2514,17 @@ protected:
int _nGlobalSensorId, _nGlobalManipulatorId;
std::string _filename;
std::string _resourcedir;
ColladaParser& _model;
boost::shared_ptr<ModelInterface> _model;
};
bool urdfFromColladaFile(std::string const& daefilename, ColladaParser& model)
bool urdfFromColladaFile(std::string const& daefilename, boost::shared_ptr<ModelInterface> model)
{
ColladaModelReader reader(model);
return reader.InitFromFile(daefilename);
}
bool urdfFromColladaData(std::string const& data, ColladaParser& model)
bool urdfFromColladaData(std::string const& data, boost::shared_ptr<ModelInterface> model)
{
ColladaModelReader reader(model);
return reader.InitFromData(data);
@ -2543,107 +2543,16 @@ bool IsColladaData(const std::string& data)
return data.find("<COLLADA") != std::string::npos;
}
bool ColladaParser::initCollada(const std::string &xml_str)
boost::shared_ptr<ModelInterface> parseCollada(const std::string &xml_str)
{
ColladaModelReader reader(*this);
return reader.InitFromData(xml_str);
}
boost::shared_ptr<ModelInterface> model(new ModelInterface);
bool ColladaParser::initColladaTree(std::map<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
{
std::string parent_link_name = joint->second->parent_link_name;
std::string child_link_name = joint->second->child_link_name;
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());
if (parent_link_name.empty() || child_link_name.empty())
{
ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str());
return false;
}
else
{
// find child and parent links
boost::shared_ptr<Link> child_link, parent_link;
this->getColladaLink(child_link_name, child_link);
if (!child_link)
{
ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() );
return false;
}
this->getColladaLink(parent_link_name, parent_link);
if (!parent_link)
{
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() );
return false;
}
//set parent link for child link
child_link->setParent(parent_link);
//set parent joint for child link
child_link->setParentJoint(joint->second);
//set child joint for parent link
parent_link->addChildJoint(joint->second);
//set child link for parent link
parent_link->addChild(child_link);
// fill in child/parent string map
parent_link_tree[child_link->name] = parent_link_name;
ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
}
}
return true;
}
bool ColladaParser::initColladaRoot(std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
// find the links that have no parent in the tree
for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++)
{
std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first);
if (parent == parent_link_tree.end())
{
// store root link
if (!this->root_link_)
{
getColladaLink(l->first, this->root_link_);
}
// we already found a root link
else{
ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str());
return false;
}
}
}
if (!this->root_link_)
{
ROS_ERROR("No root link found. The robot xml is not a valid tree.");
return false;
}
ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
return true;
}
void ColladaParser::getColladaLink(const std::string& name,boost::shared_ptr<Link> &link) const
{
boost::shared_ptr<Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
link = ptr;
ColladaModelReader reader(model);
if (!reader.InitFromData(xml_str))
model.reset();
return model;
}
}

4
urdf/CMakeLists.txt
View File

@ -21,7 +21,9 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
rosbuild_gensrv()
rosbuild_add_library(${PROJECT_NAME} src/model.cpp)
rosbuild_add_executable(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp)
rosbuild_add_gtest_build_flags(test_parser)
target_link_libraries(test_parser ${PROJECT_NAME})
rosbuild_add_gtest_build_flags(test_parser)
rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch)

2
urdf/include/urdf/model.h
View File

@ -45,7 +45,7 @@
namespace urdf{
class Model: public URDFParser, ColladaParser
class Model: public ModelInterface
{
public:
/// \brief Load Model from TiXMLElement

24
urdf/src/model.cpp
View File

@ -38,6 +38,8 @@
#include <ros/ros.h>
#include <vector>
#include "urdf/model.h"
#include <fstream>
#include <iostream>
namespace urdf{
@ -49,7 +51,7 @@ bool Model::initFile(const std::string& filename)
// get the entire file
std::string xml_string;
std::fstream xml_file(filename, std::fstream::in);
std::fstream xml_file(filename.c_str(), std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
@ -113,22 +115,36 @@ bool Model::initXml(TiXmlElement *robot_xml)
}
std::stringstream ss;
ss << *xml_doc;
ss << (*robot_xml);
return Model::initString(ss.str());
}
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");
return this->initCollada(xml_string);
model = parseCollada(xml_string);
}
else {
ROS_DEBUG("Parsing robot urdf xml string");
return this->initURDF(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;
}

121
urdf_interface/include/urdf_interface/model.h
View File

@ -88,14 +88,117 @@ public:
this->root_link_.reset();
};
/// \brief get parent Link of a Link given name
//virtual boost::shared_ptr<const Link> getParentLink(const std::string& name) const = 0;
/// \brief get parent Joint of a Link given name
//virtual boost::shared_ptr<const Joint> getParentJoint(const std::string& name) const = 0;
/// \brief get child Link of a Link given name
//virtual boost::shared_ptr<const Link> getChildLink(const std::string& name) const = 0;
/// \brief get child Joint of a Link given name
//virtual boost::shared_ptr<const Joint> getChildJoint(const std::string& name) const = 0;
/// non-const getLink()
void getLink(const std::string& name,boost::shared_ptr<Link> &link) const
{
boost::shared_ptr<Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
link = ptr;
};
/// non-const getMaterial()
boost::shared_ptr<Material> getMaterial(const std::string& name) const
{
boost::shared_ptr<Material> ptr;
if (this->materials_.find(name) == this->materials_.end())
ptr.reset();
else
ptr = this->materials_.find(name)->second;
return ptr;
};
/// in initXml(), onece all links are loaded,
/// it's time to build a tree
bool initTree(std::map<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
{
std::string parent_link_name = joint->second->parent_link_name;
std::string child_link_name = joint->second->child_link_name;
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());
if (parent_link_name.empty() || child_link_name.empty())
{
ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str());
return false;
}
else
{
// find child and parent links
boost::shared_ptr<Link> child_link, parent_link;
this->getLink(child_link_name, child_link);
if (!child_link)
{
ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() );
return false;
}
this->getLink(parent_link_name, parent_link);
if (!parent_link)
{
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() );
return false;
}
//set parent link for child link
child_link->setParent(parent_link);
//set parent joint for child link
child_link->setParentJoint(joint->second);
//set child joint for parent link
parent_link->addChildJoint(joint->second);
//set child link for parent link
parent_link->addChild(child_link);
// fill in child/parent string map
parent_link_tree[child_link->name] = parent_link_name;
ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
}
}
return true;
};
/// in initXml(), onece tree is built,
/// it's time to find the root Link
bool initRoot(std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
// find the links that have no parent in the tree
for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++)
{
std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first);
if (parent == parent_link_tree.end())
{
// store root link
if (!this->root_link_)
{
getLink(l->first, this->root_link_);
}
// we already found a root link
else{
ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str());
return false;
}
}
}
if (!this->root_link_)
{
ROS_ERROR("No root link found. The robot xml is not a valid tree.");
return false;
}
ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
return true;
};
/// \brief complete list of Links
std::map<std::string, boost::shared_ptr<Link> > links_;
@ -112,6 +215,8 @@ public:
/// hmm...
boost::shared_ptr<Link> root_link_;
};
}

26
urdf_parser/include/urdf_parser/urdf_parser.h
View File

@ -41,36 +41,12 @@
#include <map>
#include <tinyxml/tinyxml.h>
#include <boost/function.hpp>
#include <urdf_interface/model.h>
namespace urdf{
class URDFParser : public ModelInterface
{
public:
URDFParser();
/// \brief Load Model from string
bool initURDF(const std::string &xml_string );
private:
/// non-const getLink()
void getURDFLink(const std::string& name,boost::shared_ptr<Link> &link) const;
/// non-const getURDFMaterial()
boost::shared_ptr<Material> getURDFMaterial(const std::string& name) const;
/// in initXml(), onece all links are loaded,
/// it's time to build a tree
bool initURDFTree(std::map<std::string, std::string> &parent_link_tree);
/// in initXml(), onece tree is built,
/// it's time to find the root Link
bool initURDFRoot(std::map<std::string, std::string> &parent_link_tree);
};
boost::shared_ptr<ModelInterface> parseURDF(const std::string &xml_string);
}

10
urdf_parser/src/check_urdf.cpp
View File

@ -70,8 +70,6 @@ int main(int argc, char** argv)
return -1;
}
URDFParser robot;
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
@ -82,17 +80,17 @@ int main(int argc, char** argv)
}
xml_file.close();
if (!robot.initURDF(xml_string)){
boost::shared_ptr<ModelInterface> robot = parseURDF(xml_string);
if (!robot){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}
std::cout << "robot name is: " << robot.getName() << std::endl;
std::cout << "robot name is: " << robot->getName() << std::endl;
// get info from parser
std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
// get root link
boost::shared_ptr<const Link> root_link=robot.getRoot();
boost::shared_ptr<const Link> root_link=robot->getRoot();
if (!root_link) return -1;
std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;

189
urdf_parser/src/urdf_parser.cpp
View File

@ -42,14 +42,11 @@
namespace urdf{
URDFParser::URDFParser()
{
this->clear();
}
bool URDFParser::initURDF(const std::string &xml_string)
boost::shared_ptr<ModelInterface> parseURDF(const std::string &xml_string)
{
this->clear();
boost::shared_ptr<ModelInterface> model(new ModelInterface);
model->clear();
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
@ -58,20 +55,19 @@ bool URDFParser::initURDF(const std::string &xml_string)
if (!robot_xml)
{
ROS_ERROR("Could not find the 'robot' element in the xml file");
return false;
model.reset();
return model;
}
ROS_DEBUG("Parsing robot xml");
if (!robot_xml) return false;
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
ROS_ERROR("No name given for the robot.");
return false;
model.reset();
return model;
}
this->name_ = std::string(name);
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"))
@ -81,15 +77,16 @@ bool URDFParser::initURDF(const std::string &xml_string)
if (material->initXml(material_xml))
{
if (this->getURDFMaterial(material->name))
if (model->getMaterial(material->name))
{
ROS_ERROR("material '%s' is not unique.", material->name.c_str());
material.reset();
return false;
model.reset();
return model;
}
else
{
this->materials_.insert(make_pair(material->name,material));
model->materials_.insert(make_pair(material->name,material));
ROS_DEBUG("successfully added a new material '%s'", material->name.c_str());
}
}
@ -97,7 +94,8 @@ bool URDFParser::initURDF(const std::string &xml_string)
{
ROS_ERROR("material xml is not initialized correctly");
material.reset();
return false;
model.reset();
return model;
}
}
@ -109,11 +107,11 @@ bool URDFParser::initURDF(const std::string &xml_string)
if (link->initXml(link_xml))
{
if (this->getLink(link->name))
if (model->getLink(link->name))
{
ROS_ERROR("link '%s' is not unique.", link->name.c_str());
link.reset();
return false;
model.reset();
return model;
}
else
{
@ -123,42 +121,43 @@ bool URDFParser::initURDF(const std::string &xml_string)
{
if (!link->visual->material_name.empty())
{
if (this->getURDFMaterial(link->visual->material_name))
if (model->getMaterial(link->visual->material_name))
{
ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str());
link->visual->material = this->getURDFMaterial( link->visual->material_name.c_str() );
link->visual->material = model->getMaterial( link->visual->material_name.c_str() );
}
else
{
if (link->visual->material)
{
ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str());
this->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
model->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
}
else
{
ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str());
link.reset();
return false;
model.reset();
return model;
}
}
}
}
this->links_.insert(make_pair(link->name,link));
model->links_.insert(make_pair(link->name,link));
ROS_DEBUG("successfully added a new link '%s'", link->name.c_str());
}
}
else
{
ROS_ERROR("link xml is not initialized correctly");
link.reset();
return false;
model.reset();
return model;
}
}
if (this->links_.empty()){
if (model->links_.empty()){
ROS_ERROR("No link elements found in urdf file");
return false;
model.reset();
return model;
}
// Get all Joint elements
@ -169,23 +168,23 @@ bool URDFParser::initURDF(const std::string &xml_string)
if (joint->initXml(joint_xml))
{
if (this->getJoint(joint->name))
if (model->getJoint(joint->name))
{
ROS_ERROR("joint '%s' is not unique.", joint->name.c_str());
joint.reset();
return false;
model.reset();
return model;
}
else
{
this->joints_.insert(make_pair(joint->name,joint));
model->joints_.insert(make_pair(joint->name,joint));
ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str());
}
}
else
{
ROS_ERROR("joint xml is not initialized correctly");
joint.reset();
return false;
model.reset();
return model;
}
}
@ -196,128 +195,22 @@ bool URDFParser::initURDF(const std::string &xml_string)
parent_link_tree.clear();
// building tree: name mapping
if (!this->initURDFTree(parent_link_tree))
if (!model->initTree(parent_link_tree))
{
ROS_ERROR("failed to build tree");
return false;
model.reset();
return model;
}
// find the root link
if (!this->initURDFRoot(parent_link_tree))
if (!model->initRoot(parent_link_tree))
{
ROS_ERROR("failed to find root link");
return false;
model.reset();
return model;
}
return true;
}
bool URDFParser::initURDFTree(std::map<std::string, std::string> &parent_link_tree)
{
// loop through all joints, for every link, assign children links and children joints
for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
{
std::string parent_link_name = joint->second->parent_link_name;
std::string child_link_name = joint->second->child_link_name;
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());
if (parent_link_name.empty() || child_link_name.empty())
{
ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str());
return false;
}
else
{
// find child and parent links
boost::shared_ptr<Link> child_link, parent_link;
this->getURDFLink(child_link_name, child_link);
if (!child_link)
{
ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() );
return false;
}
this->getURDFLink(parent_link_name, parent_link);
if (!parent_link)
{
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() );
return false;
}
//set parent link for child link
child_link->setParent(parent_link);
//set parent joint for child link
child_link->setParentJoint(joint->second);
//set child joint for parent link
parent_link->addChildJoint(joint->second);
//set child link for parent link
parent_link->addChild(child_link);
// fill in child/parent string map
parent_link_tree[child_link->name] = parent_link_name;
ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size());
}
}
return true;
}
bool URDFParser::initURDFRoot(std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
// find the links that have no parent in the tree
for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++)
{
std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first);
if (parent == parent_link_tree.end())
{
// store root link
if (!this->root_link_)
{
getURDFLink(l->first, this->root_link_);
}
// we already found a root link
else{
ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str());
return false;
}
}
}
if (!this->root_link_)
{
ROS_ERROR("No root link found. The robot xml is not a valid tree.");
return false;
}
ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str());
return true;
}
boost::shared_ptr<Material> URDFParser::getURDFMaterial(const std::string& name) const
{
boost::shared_ptr<Material> ptr;
if (this->materials_.find(name) == this->materials_.end())
ptr.reset();
else
ptr = this->materials_.find(name)->second;
return ptr;
}
void URDFParser::getURDFLink(const std::string& name,boost::shared_ptr<Link> &link) const
{
boost::shared_ptr<Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
link = ptr;
return model;
}
}

9
urdf_parser/src/urdf_to_graphiz.cpp
View File

@ -90,8 +90,6 @@ int main(int argc, char** argv)
return -1;
}
URDFParser robot;
// get the entire file
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
@ -103,14 +101,15 @@ int main(int argc, char** argv)
}
xml_file.close();
if (!robot.initURDF(xml_string)){
boost::shared_ptr<ModelInterface> robot = parseURDF(xml_string);
if (!robot){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}
string output = robot.getName();
string output = robot->getName();
// print entire tree to file
printTree(robot.getRoot(), output+".gv");
printTree(robot->getRoot(), output+".gv");
cout << "Created file " << output << ".gv" << endl;
string command = "dot -Tpdf "+output+".gv -o "+output+".pdf";

6
urdf_parser/test/memtest.cpp
View File

@ -1,4 +1,4 @@
#include <urdf_parser/urdf_parser.h>
#include "urdf_parser/urdf_parser.h"
#include <ros/ros.h>
#include <fstream>
#include <iostream>
@ -6,8 +6,6 @@
int main(int argc, char** argv){
ros::init(argc, argv, "memtest");
while (ros::ok()){
urdf::URDFParser urdf;
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
@ -19,6 +17,6 @@ int main(int argc, char** argv){
xml_file.close();
urdf.initURDF(xml_string);
urdf::parseURDF(xml_string);
}
}