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 );
}

4049
collada_parser/src/collada_parser.cpp
View File

File diff suppressed because it is too large Load Diff

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);
}
}