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

attempt to refactor, now we have diamond inheritance, and won't compile. make init* static functions.

This commit is contained in:
John Hsu 2011-06-21 15:06:53 -07:00
parent 95b279f6df
commit 6e63977e35
24 changed files with 559 additions and 218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
assimp/Makefile
View File

@ -5,7 +5,7 @@ PACKAGE_NAME=`rospack find assimp`
TARBALL_NAME = assimp-svn-811
TARBALL = build/$(TARBALL_NAME).tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/$(TARBALL_NAME).tar.gz
TARBALL_PATCH = assimp.patch
TARBALL_PATCH = assimp.patch define_arch_arm.patch
UNPACK_CMD = tar xzf
SOURCE_DIR = build/$(TARBALL_NAME)
include $(shell rospack find mk)/download_unpack_build.mk

29
assimp/define_arch_arm.patch Normal file
View File

@ -0,0 +1,29 @@
--- code/Importer.cpp.orig 2011-06-20 11:57:37.234886987 +0200
+++ code/Importer.cpp 2011-06-20 11:58:26.891136987 +0200
@@ -904,6 +904,10 @@
<< " amd64"
#elif defined(ASSIMP_BUILD_IA_64BIT_ARCHITECTURE)
<< " itanium"
+#elif defined(ASSIMP_BUILD_PPC_32BIT_ARCHITECTURE)
+ << " ppc32"
+#elif defined(ASSIMP_BUILD_ARM_32BIT_ARCHITECTURE)
+ << " arm"
#else
# error unknown architecture
#endif
--- include/aiDefines.h.orig 2011-06-20 11:57:41.570824487 +0200
+++ include/aiDefines.h 2011-06-20 11:59:24.648949487 +0200
@@ -238,8 +238,12 @@
# define ASSIMP_BUILD_X86_32BIT_ARCHITECTURE
# elif defined(__x86_64__)
# define ASSIMP_BUILD_X86_64BIT_ARCHITECTURE
+# elif defined(__ppc__)
+# define ASSIMP_BUILD_PPC_32BIT_ARCHITECTURE
+# elif defined(__arm__)
+# define ASSIMP_BUILD_ARM_32BIT_ARCHITECTURE
# else
-# error unknown architecture
+# error "unknown architecture"
# endif
#else
# error unknown compiler

30
collada_parser/CMakeLists.txt Normal file
View File

@ -0,0 +1,30 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
# necessary for collada reader to create the temporary dae files due to limitations in the URDF geometry
check_function_exists(mkstemps HAVE_MKSTEMPS)
if( HAVE_MKSTEMPS )
add_definitions("-DHAVE_MKSTEMPS")
endif()
#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/collada_parser.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)

1
collada_parser/Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

76
collada_parser/include/collada_parser/collada_parser.h Normal file
View File

@ -0,0 +1,76 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef COLLADA_PARSER_COLLADA_PARSER_H
#define COLLADA_PARSER_COLLADA_PARSER_H
#include <string>
#include <map>
#include <boost/function.hpp>
#include <urdf_interface/model.h>
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;
};
}
#endif

26
collada_parser/mainpage.dox Normal file
View File

@ -0,0 +1,26 @@
/**
\mainpage
\htmlinclude manifest.html
\b collada_parser is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/

21
collada_parser/manifest.xml Normal file
View File

@ -0,0 +1,21 @@
<package>
<description brief="collada_parser">
collada_parser
</description>
<author>John Hsu</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/collada_parser</url>
<depend package="roscpp"/>
<depend package="urdf_interface"/>
<depend package="colladadom"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollada_parser"/>
</export>
</package>

138
urdf/src/collada_model_reader.cpp → collada_parser/src/collada_parser.cpp
View File

@ -33,8 +33,6 @@
*********************************************************************/
/* Author: Rosen Diankov, used OpenRAVE files for reference */
#include "urdf/model.h"
#include <vector>
#include <list>
#include <map>
@ -56,6 +54,14 @@
#include <boost/format.hpp>
#include <boost/shared_ptr.hpp>
#include <ros/ros.h>
#include <collada_parser/collada_parser.h>
#include <urdf_interface/model.h>
#ifndef HAVE_MKSTEMPS
#include <fstream>
#include <fcntl.h>
#endif
#ifndef HAVE_MKSTEMPS
#include <fstream>
#include <fcntl.h>
@ -66,6 +72,7 @@
namespace urdf{
class UnlinkFilename
{
public:
@ -382,7 +389,7 @@ class ColladaModelReader : public daeErrorHandler
};
public:
ColladaModelReader(Model& model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
ColladaModelReader(ColladaParser& model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) {
daeErrorHandler::setErrorHandler(this);
_resourcedir = ".";
}
@ -475,12 +482,12 @@ protected:
{
std::map<std::string, std::string> parent_link_tree;
// building tree: name mapping
if (!_model.initTree(parent_link_tree)) {
if (!_model.initColladaTree(parent_link_tree)) {
ROS_ERROR("failed to build tree");
}
// find the root link
if (!_model.initRoot(parent_link_tree)) {
if (!_model.initColladaRoot(parent_link_tree)) {
ROS_ERROR("failed to find root link");
}
}
@ -757,7 +764,7 @@ protected:
}
boost::shared_ptr<Link> plink;
_model.getLink(linkname,plink);
_model.getColladaLink(linkname,plink);
if( !plink ) {
plink.reset(new Link());
plink->name = linkname;
@ -2507,30 +2514,22 @@ protected:
int _nGlobalSensorId, _nGlobalManipulatorId;
std::string _filename;
std::string _resourcedir;
Model& _model;
ColladaParser& _model;
};
bool urdfFromColladaFile(std::string const& daefilename, Model& model)
bool urdfFromColladaFile(std::string const& daefilename, ColladaParser& model)
{
ColladaModelReader reader(model);
return reader.InitFromFile(daefilename);
}
bool urdfFromColladaData(std::string const& data, Model& model)
bool urdfFromColladaData(std::string const& data, ColladaParser& model)
{
ColladaModelReader reader(model);
return reader.InitFromData(data);
}
bool urdfFromTiXML(TiXmlElement *robot_xml, Model& model)
{
ColladaModelReader reader(model);
// have to convert all xml back to string (sigh..)
std::stringstream ss;
ss << *robot_xml;
return reader.InitFromData(ss.str());
}
bool IsColladaFile(const std::string& filename)
{
size_t len = filename.size();
@ -2544,4 +2543,107 @@ bool IsColladaData(const std::string& data)
return data.find("<COLLADA") != std::string::npos;
}
bool ColladaParser::initCollada(const std::string &xml_str)
{
ColladaModelReader reader(*this);
return reader.InitFromData(xml_str);
}
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;
}
}

12
urdf/CMakeLists.txt
View File

@ -21,18 +21,6 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
rosbuild_gensrv()
# necessary for collada reader to create the temporary dae files due to limitations in the URDF geometry
check_function_exists(mkstemps HAVE_MKSTEMPS)
if( HAVE_MKSTEMPS )
add_definitions("-DHAVE_MKSTEMPS")
endif()
#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/model.cpp src/collada_model_reader.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
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})

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

@ -34,17 +34,18 @@
/* Author: Wim Meeussen */
#ifndef ROBOT_MODEL_URDF_H
#define ROBOT_MODEL_URDF_H
#ifndef URDF_MODEL_H
#define URDF_MODEL_H
#include <string>
#include <map>
#include <urdf_parser/parser.h>
#include <urdf_parser/urdf_parser.h>
#include <collada_parser/collada_parser.h>
namespace urdf{
class Model: public urdf::Parser
class Model: public URDFParser, ColladaParser
{
public:
/// \brief Load Model from TiXMLElement
@ -57,8 +58,6 @@ public:
bool initParam(const std::string& param);
/// \brief Load Model from a XML-string
bool initString(const std::string& xmlstring);
friend class ColladaModelReader;
};
}

1
urdf/manifest.xml
View File

@ -14,6 +14,7 @@
<depend package="roscpp" />
<depend package="colladadom" />
<depend package="urdf_parser" />
<depend package="collada_parser" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/>

88
urdf/src/model.cpp
View File

@ -41,23 +41,32 @@
namespace urdf{
bool urdfFromColladaFile(std::string const& daefilename, Model& model);
bool urdfFromColladaData(std::string const& data, Model& model);
bool urdfFromTiXML(TiXmlElement *robot_xml, Model& model);
bool IsColladaFile(const std::string& filename);
bool IsColladaData(const std::string& data);
bool Model::initFile(const std::string& filename)
{
// necessary for COLLADA compatibility
if( IsColladaFile(filename) ) {
return urdfFromColladaFile(filename,*this);
}
TiXmlDocument xml_doc;
xml_doc.LoadFile(filename);
return init(&xml_doc);
// get the entire file
std::string xml_string;
std::fstream xml_file(filename, std::fstream::in);
if (xml_file.is_open())
{
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
return Model::initString(xml_string);
}
else
{
ROS_ERROR("Could not open file [%s] for parsing.",filename.c_str());
return false;
}
}
@ -78,53 +87,50 @@ bool Model::initParam(const std::string& param)
ROS_ERROR("Could not read parameter %s on parameter server", full_param.c_str());
return false;
}
return initString(xml_string);
return Model::initString(xml_string);
}
bool Model::initString(const std::string& xml_string)
{
// necessary for COLLADA compatibility
if( IsColladaData(xml_string) ) {
return urdfFromColladaData(xml_string,*this);
}
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
return init(&xml_doc);
}
bool Model::initXml(TiXmlDocument *xml_doc)
{
if (!xml_doc)
{
ROS_ERROR("Could not parse the xml");
ROS_ERROR("Could not parse the xml document");
return false;
}
// necessary for COLLADA compatibility
if( !!xml_doc->RootElement() ) {
if( std::string("COLLADA") == xml_doc->RootElement()->ValueStr() ) {
return urdfFromTiXML(xml_doc->RootElement(),*this);
}
}
std::stringstream ss;
ss << *xml_doc;
return init(xml_doc);
return Model::initString(ss.str());
}
bool Model::initXml(TiXmlElement *robot_xml)
{
ROS_DEBUG("Parsing robot xml");
if (!robot_xml) return false;
if (!robot_xml)
{
ROS_ERROR("Could not parse the xml element");
return false;
}
std::stringstream ss;
ss << *xml_doc;
return Model::initString(ss.str());
}
bool Model::initString(const std::string& xml_string)
{
// necessary for COLLADA compatibility
if( std::string("COLLADA") == robot_xml->ValueStr() ) {
return urdfFromTiXML(robot_xml,*this);
if( IsColladaData(xml_string) ) {
ROS_DEBUG("Parsing robot collada xml string");
return this->initCollada(xml_string);
}
else {
ROS_DEBUG("Parsing robot urdf xml string");
return this->initURDF(xml_string);
}
}
return init(robot_xml);
}
}// namespace

4
urdf_interface/include/urdf_interface/color.h
View File

@ -34,8 +34,8 @@
/* Author: Josh Faust */
#ifndef URDF_COLOR_H
#define URDF_COLOR_H
#ifndef URDF_INTERFACE_COLOR_H
#define URDF_INTERFACE_COLOR_H
#include <string>
#include <vector>

4
urdf_interface/include/urdf_interface/joint.h
View File

@ -34,8 +34,8 @@
/* Author: Wim Meeussen */
#ifndef URDF_JOINT_H
#define URDF_JOINT_H
#ifndef URDF_INTERFACE_JOINT_H
#define URDF_INTERFACE_JOINT_H
#include <string>
#include <vector>

4
urdf_interface/include/urdf_interface/link.h
View File

@ -34,8 +34,8 @@
/* Author: Wim Meeussen */
#ifndef URDF_LINK_H
#define URDF_LINK_H
#ifndef URDF_INTERFACE_LINK_H
#define URDF_INTERFACE_LINK_H
#include <string>
#include <vector>

83
urdf_parser/include/urdf_parser/parser.h → urdf_interface/include/urdf_interface/model.h
View File

@ -34,43 +34,68 @@
/* Author: Wim Meeussen */
#ifndef URDF_PARSER_H
#define URDF_PARSER_H
#ifndef URDF_INTERFACE_MODEL_H
#define URDF_INTERFACE_MODEL_H
#include <string>
#include <map>
#include <tinyxml/tinyxml.h>
#include <boost/function.hpp>
#include <urdf_interface/link.h>
namespace urdf {
class Parser
class ModelInterface
{
public:
Parser();
/// \brief Load Model from TiXMLElement
bool init(TiXmlElement *xml);
/// \brief Load Model from TiXMLDocument
bool init(TiXmlDocument *xml);
boost::shared_ptr<const Link> getRoot(void) const{return this->root_link_;};
boost::shared_ptr<const Link> getLink(const std::string& name) const;
boost::shared_ptr<const Joint> getJoint(const std::string& name) const;
const std::string& getName() const {return name_;};
boost::shared_ptr<const Link> getLink(const std::string& name) const
{
boost::shared_ptr<const Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
return ptr;
};
void getLinks(std::vector<boost::shared_ptr<Link> >& links) const;
boost::shared_ptr<const Joint> getJoint(const std::string& name) const
{
boost::shared_ptr<const Joint> ptr;
if (this->joints_.find(name) == this->joints_.end())
ptr.reset();
else
ptr = this->joints_.find(name)->second;
return ptr;
};
const std::string& getName() const {return name_;};
void getLinks(std::vector<boost::shared_ptr<Link> >& links) const
{
for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
{
links.push_back(link->second);
}
};
void clear()
{
name_.clear();
this->links_.clear();
this->joints_.clear();
this->materials_.clear();
this->root_link_.reset();
};
/// \brief get parent Link of a Link given name
boost::shared_ptr<const Link> getParentLink(const std::string& name) const;
//virtual boost::shared_ptr<const Link> getParentLink(const std::string& name) const = 0;
/// \brief get parent Joint of a Link given name
boost::shared_ptr<const Joint> getParentJoint(const std::string& name) const;
//virtual boost::shared_ptr<const Joint> getParentJoint(const std::string& name) const = 0;
/// \brief get child Link of a Link given name
boost::shared_ptr<const Link> getChildLink(const std::string& name) const;
//virtual boost::shared_ptr<const Link> getChildLink(const std::string& name) const = 0;
/// \brief get child Joint of a Link given name
boost::shared_ptr<const Joint> getChildJoint(const std::string& name) const;
//virtual boost::shared_ptr<const Joint> getChildJoint(const std::string& name) const = 0;
/// \brief complete list of Links
std::map<std::string, boost::shared_ptr<Link> > links_;
@ -79,30 +104,14 @@ public:
/// \brief complete list of Materials
std::map<std::string, boost::shared_ptr<Material> > materials_;
protected:
void clear();
std::string name_;
/// non-const getLink()
void getLink(const std::string& name,boost::shared_ptr<Link> &link) const;
/// non-const getMaterial()
boost::shared_ptr<Material> getMaterial(const std::string& name) const;
/// 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);
/// 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);
/// Model is restricted to a tree for now, which means there exists one root link
/// ModelInterface is restricted to a tree for now, which means there exists one root link
/// typically, root link is the world(inertial). Where world is a special link
/// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint?
/// hmm...
boost::shared_ptr<Link> root_link_;
};
}

7
urdf_interface/include/urdf_interface/pose.h
View File

@ -34,8 +34,8 @@
/* Author: Wim Meeussen */
#ifndef URDF_POSE_H
#define URDF_POSE_H
#ifndef URDF_INTERFACE_POSE_H
#define URDF_INTERFACE_POSE_H
#include <string>
#include <vector>
@ -44,6 +44,9 @@
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <tinyxml/tinyxml.h> // FIXME: remove parser from here
#include <ros/ros.h>
namespace urdf{
class Vector3

2
urdf_interface/manifest.xml
View File

@ -6,7 +6,7 @@
<license>BSD</license>
<review status="Doc reviewed" notes=""/>
<url>http://ros.org/wiki/urdf_interface</url>
<depend package="tinyxml" />
<export>
<cpp cflags="-I${prefix}/include"/>
</export>

2
urdf_parser/CMakeLists.txt
View File

@ -22,7 +22,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
rosbuild_gensrv()
#common commands for building c++ executables and libraries
rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/parser.cpp)
rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/urdf_parser.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)

77
urdf_parser/include/urdf_parser/urdf_parser.h Normal file
View File

@ -0,0 +1,77 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Wim Meeussen */
#ifndef URDF_PARSER_URDF_PARSER_H
#define URDF_PARSER_URDF_PARSER_H
#include <string>
#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);
};
}
#endif

25
urdf_parser/src/check_urdf.cpp
View File

@ -34,8 +34,9 @@
/* Author: Wim Meeussen */
#include "urdf_parser/parser.h"
#include "urdf_parser/urdf_parser.h"
#include <iostream>
#include <fstream>
using namespace urdf;
@ -68,16 +69,20 @@ int main(int argc, char** argv)
std::cerr << "Expect xml file to parse" << std::endl;
return -1;
}
TiXmlDocument robot_model_xml;
robot_model_xml.LoadFile(argv[1]);
TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
if (!robot_xml){
std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
return -1;
}
Parser robot;
if (!robot.init(robot_xml)){
URDFParser robot;
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
if (!robot.initURDF(xml_string)){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}

85
urdf_parser/src/parser.cpp → urdf_parser/src/urdf_parser.cpp
View File

@ -37,48 +37,29 @@
#include <boost/algorithm/string.hpp>
#include <ros/ros.h>
#include <vector>
#include "urdf_parser/parser.h"
#include "urdf_parser/urdf_parser.h"
namespace urdf{
Parser::Parser()
URDFParser::URDFParser()
{
this->clear();
}
void Parser::clear()
bool URDFParser::initURDF(const std::string &xml_string)
{
name_.clear();
this->links_.clear();
this->joints_.clear();
this->materials_.clear();
this->root_link_.reset();
}
this->clear();
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
bool Parser::init(TiXmlDocument *xml_doc)
{
if (!xml_doc)
{
ROS_ERROR("Could not parse the xml");
return false;
}
TiXmlElement *robot_xml = xml_doc->FirstChildElement("robot");
TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
if (!robot_xml)
{
ROS_ERROR("Could not find the 'robot' element in the xml file");
return false;
}
return init(robot_xml);
}
bool Parser::init(TiXmlElement *robot_xml)
{
this->clear();
ROS_DEBUG("Parsing robot xml");
if (!robot_xml) return false;
@ -100,7 +81,7 @@ bool Parser::init(TiXmlElement *robot_xml)
if (material->initXml(material_xml))
{
if (this->getMaterial(material->name))
if (this->getURDFMaterial(material->name))
{
ROS_ERROR("material '%s' is not unique.", material->name.c_str());
material.reset();
@ -142,10 +123,10 @@ bool Parser::init(TiXmlElement *robot_xml)
{
if (!link->visual->material_name.empty())
{
if (this->getMaterial(link->visual->material_name))
if (this->getURDFMaterial(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->getMaterial( link->visual->material_name.c_str() );
link->visual->material = this->getURDFMaterial( link->visual->material_name.c_str() );
}
else
{
@ -215,14 +196,14 @@ bool Parser::init(TiXmlElement *robot_xml)
parent_link_tree.clear();
// building tree: name mapping
if (!this->initTree(parent_link_tree))
if (!this->initURDFTree(parent_link_tree))
{
ROS_ERROR("failed to build tree");
return false;
}
// find the root link
if (!this->initRoot(parent_link_tree))
if (!this->initURDFRoot(parent_link_tree))
{
ROS_ERROR("failed to find root link");
return false;
@ -231,7 +212,7 @@ bool Parser::init(TiXmlElement *robot_xml)
return true;
}
bool Parser::initTree(std::map<std::string, std::string> &parent_link_tree)
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++)
@ -249,13 +230,13 @@ bool Parser::initTree(std::map<std::string, std::string> &parent_link_tree)
{
// find child and parent links
boost::shared_ptr<Link> child_link, parent_link;
this->getLink(child_link_name, child_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->getLink(parent_link_name, parent_link);
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() );
@ -286,7 +267,7 @@ bool Parser::initTree(std::map<std::string, std::string> &parent_link_tree)
bool Parser::initRoot(std::map<std::string, std::string> &parent_link_tree)
bool URDFParser::initURDFRoot(std::map<std::string, std::string> &parent_link_tree)
{
this->root_link_.reset();
@ -300,7 +281,7 @@ bool Parser::initRoot(std::map<std::string, std::string> &parent_link_tree)
// store root link
if (!this->root_link_)
{
getLink(l->first, this->root_link_);
getURDFLink(l->first, this->root_link_);
}
// we already found a root link
else{
@ -319,7 +300,7 @@ bool Parser::initRoot(std::map<std::string, std::string> &parent_link_tree)
return true;
}
boost::shared_ptr<Material> Parser::getMaterial(const std::string& name) const
boost::shared_ptr<Material> URDFParser::getURDFMaterial(const std::string& name) const
{
boost::shared_ptr<Material> ptr;
if (this->materials_.find(name) == this->materials_.end())
@ -329,25 +310,7 @@ boost::shared_ptr<Material> Parser::getMaterial(const std::string& name) const
return ptr;
}
boost::shared_ptr<const Link> Parser::getLink(const std::string& name) const
{
boost::shared_ptr<const Link> ptr;
if (this->links_.find(name) == this->links_.end())
ptr.reset();
else
ptr = this->links_.find(name)->second;
return ptr;
}
void Parser::getLinks(std::vector<boost::shared_ptr<Link> >& links) const
{
for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
{
links.push_back(link->second);
}
}
void Parser::getLink(const std::string& name,boost::shared_ptr<Link> &link) const
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())
@ -357,15 +320,5 @@ void Parser::getLink(const std::string& name,boost::shared_ptr<Link> &link) cons
link = ptr;
}
boost::shared_ptr<const Joint> Parser::getJoint(const std::string& name) const
{
boost::shared_ptr<const Joint> ptr;
if (this->joints_.find(name) == this->joints_.end())
ptr.reset();
else
ptr = this->joints_.find(name)->second;
return ptr;
}
}

24
urdf_parser/src/urdf_to_graphiz.cpp
View File

@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
#include "urdf_parser/parser.h"
#include "urdf_parser/urdf_parser.h"
#include <iostream>
#include <fstream>
@ -90,16 +90,20 @@ int main(int argc, char** argv)
return -1;
}
TiXmlDocument robot_model_xml;
robot_model_xml.LoadFile(argv[1]);
TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
if (!robot_xml){
std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
return -1;
}
URDFParser robot;
Parser robot;
if (!robot.init(robot_xml)){
// get the entire file
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
if (!robot.initURDF(xml_string)){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}

21
urdf_parser/test/memtest.cpp
View File

@ -1,13 +1,24 @@
#include <urdf_parser/parser.h>
#include <urdf_parser/urdf_parser.h>
#include <ros/ros.h>
#include <fstream>
#include <iostream>
int main(int argc, char** argv){
ros::init(argc, argv, "memtest");
while (ros::ok()){
urdf::Parser urdf;
urdf::URDFParser urdf;
TiXmlDocument xml_doc;
xml_doc.LoadFile(std::string(argv[1]));
urdf.init(&xml_doc);
std::string xml_string;
std::fstream xml_file(argv[1], std::fstream::in);
while ( xml_file.good() )
{
std::string line;
std::getline( xml_file, line);
xml_string += (line + "\n");
}
xml_file.close();
urdf.initURDF(xml_string);
}
}