diff --git a/assimp/Makefile b/assimp/Makefile index 02abba6..da539e0 100644 --- a/assimp/Makefile +++ b/assimp/Makefile @@ -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 diff --git a/assimp/define_arch_arm.patch b/assimp/define_arch_arm.patch new file mode 100644 index 0000000..d4305b6 --- /dev/null +++ b/assimp/define_arch_arm.patch @@ -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 diff --git a/collada_parser/CMakeLists.txt b/collada_parser/CMakeLists.txt new file mode 100644 index 0000000..f4c85b4 --- /dev/null +++ b/collada_parser/CMakeLists.txt @@ -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) + diff --git a/collada_parser/Makefile b/collada_parser/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/collada_parser/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/collada_parser/include/collada_parser/collada_parser.h b/collada_parser/include/collada_parser/collada_parser.h new file mode 100644 index 0000000..e0c1145 --- /dev/null +++ b/collada_parser/include/collada_parser/collada_parser.h @@ -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 +#include +#include + +#include + + +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) const; + + /// non-const getColladaMaterial() + //boost::shared_ptr getColladaMaterial(const std::string& name) const; + + /// in initXml(), onece all links are loaded, + /// it's time to build a tree + bool initColladaTree(std::map &parent_link_tree); + + /// in initXml(), onece tree is built, + /// it's time to find the root Link + bool initColladaRoot(std::map &parent_link_tree); + + friend class ColladaModelReader; +}; + +} + +#endif diff --git a/collada_parser/mainpage.dox b/collada_parser/mainpage.dox new file mode 100644 index 0000000..643964d --- /dev/null +++ b/collada_parser/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b collada_parser is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/collada_parser/manifest.xml b/collada_parser/manifest.xml new file mode 100644 index 0000000..1c5e4d0 --- /dev/null +++ b/collada_parser/manifest.xml @@ -0,0 +1,21 @@ + + + + collada_parser + + + John Hsu + BSD + + http://ros.org/wiki/collada_parser + + + + + + + + + + + diff --git a/urdf/src/collada_model_reader.cpp b/collada_parser/src/collada_parser.cpp similarity index 96% rename from urdf/src/collada_model_reader.cpp rename to collada_parser/src/collada_parser.cpp index 678aee8..b4e2f81 100644 --- a/urdf/src/collada_model_reader.cpp +++ b/collada_parser/src/collada_parser.cpp @@ -33,8 +33,6 @@ *********************************************************************/ /* Author: Rosen Diankov, used OpenRAVE files for reference */ -#include "urdf/model.h" - #include #include #include @@ -56,6 +54,14 @@ #include #include +#include +#include +#include + +#ifndef HAVE_MKSTEMPS +#include +#include +#endif #ifndef HAVE_MKSTEMPS #include #include @@ -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 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 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(" &parent_link_tree) +{ + // loop through all joints, for every link, assign children links and children joints + for (std::map >::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 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 \"\" 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 &parent_link_tree) +{ + + this->root_link_.reset(); + + // find the links that have no parent in the tree + for (std::map >::iterator l=this->links_.begin(); l!=this->links_.end(); l++) + { + std::map::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) const +{ + boost::shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(); + else + ptr = this->links_.find(name)->second; + link = ptr; +} } diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 94f624b..7c39b54 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -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}) diff --git a/urdf/include/urdf/model.h b/urdf/include/urdf/model.h index 51ff9be..3fc4931 100644 --- a/urdf/include/urdf/model.h +++ b/urdf/include/urdf/model.h @@ -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 #include -#include +#include +#include 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; }; } diff --git a/urdf/manifest.xml b/urdf/manifest.xml index 5ec4210..18ef0a0 100644 --- a/urdf/manifest.xml +++ b/urdf/manifest.xml @@ -14,6 +14,7 @@ + diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index 3377d80..8b93437 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -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; - - // necessary for COLLADA compatibility - if( std::string("COLLADA") == robot_xml->ValueStr() ) { - return urdfFromTiXML(robot_xml,*this); + if (!robot_xml) + { + ROS_ERROR("Could not parse the xml element"); + return false; } - return init(robot_xml); + std::stringstream ss; + ss << *xml_doc; + + return Model::initString(ss.str()); } +bool Model::initString(const std::string& xml_string) +{ + // necessary for COLLADA compatibility + 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); + } +} + + + }// namespace diff --git a/urdf_interface/include/urdf_interface/color.h b/urdf_interface/include/urdf_interface/color.h index c132da4..f9bab0c 100644 --- a/urdf_interface/include/urdf_interface/color.h +++ b/urdf_interface/include/urdf_interface/color.h @@ -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 #include diff --git a/urdf_interface/include/urdf_interface/joint.h b/urdf_interface/include/urdf_interface/joint.h index 551e2c6..742b025 100644 --- a/urdf_interface/include/urdf_interface/joint.h +++ b/urdf_interface/include/urdf_interface/joint.h @@ -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 #include diff --git a/urdf_interface/include/urdf_interface/link.h b/urdf_interface/include/urdf_interface/link.h index 96f4528..8f5af44 100644 --- a/urdf_interface/include/urdf_interface/link.h +++ b/urdf_interface/include/urdf_interface/link.h @@ -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 #include diff --git a/urdf_parser/include/urdf_parser/parser.h b/urdf_interface/include/urdf_interface/model.h similarity index 69% rename from urdf_parser/include/urdf_parser/parser.h rename to urdf_interface/include/urdf_interface/model.h index 1293330..7416933 100644 --- a/urdf_parser/include/urdf_parser/parser.h +++ b/urdf_interface/include/urdf_interface/model.h @@ -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 #include -#include #include #include -namespace urdf{ +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 getRoot(void) const{return this->root_link_;}; - boost::shared_ptr getLink(const std::string& name) const; - boost::shared_ptr getJoint(const std::string& name) const; - const std::string& getName() const {return name_;}; + boost::shared_ptr getLink(const std::string& name) const + { + boost::shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(); + else + ptr = this->links_.find(name)->second; + return ptr; + }; - void getLinks(std::vector >& links) const; + boost::shared_ptr getJoint(const std::string& name) const + { + boost::shared_ptr 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 >& links) const + { + for (std::map >::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 getParentLink(const std::string& name) const; + //virtual boost::shared_ptr getParentLink(const std::string& name) const = 0; /// \brief get parent Joint of a Link given name - boost::shared_ptr getParentJoint(const std::string& name) const; + //virtual boost::shared_ptr getParentJoint(const std::string& name) const = 0; /// \brief get child Link of a Link given name - boost::shared_ptr getChildLink(const std::string& name) const; + //virtual boost::shared_ptr getChildLink(const std::string& name) const = 0; /// \brief get child Joint of a Link given name - boost::shared_ptr getChildJoint(const std::string& name) const; + //virtual boost::shared_ptr getChildJoint(const std::string& name) const = 0; /// \brief complete list of Links std::map > links_; @@ -79,30 +104,14 @@ public: /// \brief complete list of Materials std::map > materials_; -protected: - void clear(); - std::string name_; - /// non-const getLink() - void getLink(const std::string& name,boost::shared_ptr &link) const; - - /// non-const getMaterial() - boost::shared_ptr getMaterial(const std::string& name) const; - - /// in initXml(), onece all links are loaded, - /// it's time to build a tree - bool initTree(std::map &parent_link_tree); - - /// in initXml(), onece tree is built, - /// it's time to find the root Link - bool initRoot(std::map &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 root_link_; + }; } diff --git a/urdf_interface/include/urdf_interface/pose.h b/urdf_interface/include/urdf_interface/pose.h index 9d8bd79..c781da4 100644 --- a/urdf_interface/include/urdf_interface/pose.h +++ b/urdf_interface/include/urdf_interface/pose.h @@ -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 #include @@ -44,6 +44,9 @@ #include #include +#include // FIXME: remove parser from here +#include + namespace urdf{ class Vector3 diff --git a/urdf_interface/manifest.xml b/urdf_interface/manifest.xml index 2e71051..2cffad1 100644 --- a/urdf_interface/manifest.xml +++ b/urdf_interface/manifest.xml @@ -6,7 +6,7 @@ BSD http://ros.org/wiki/urdf_interface - + diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index a5a72b7..a5efa3e 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -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) diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h new file mode 100644 index 0000000..ed3a363 --- /dev/null +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -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 +#include +#include +#include + +#include + + +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) const; + + /// non-const getURDFMaterial() + boost::shared_ptr getURDFMaterial(const std::string& name) const; + + /// in initXml(), onece all links are loaded, + /// it's time to build a tree + bool initURDFTree(std::map &parent_link_tree); + + /// in initXml(), onece tree is built, + /// it's time to find the root Link + bool initURDFRoot(std::map &parent_link_tree); + +}; + +} + +#endif diff --git a/urdf_parser/src/check_urdf.cpp b/urdf_parser/src/check_urdf.cpp index 8fe931c..0d919c2 100644 --- a/urdf_parser/src/check_urdf.cpp +++ b/urdf_parser/src/check_urdf.cpp @@ -34,8 +34,9 @@ /* Author: Wim Meeussen */ -#include "urdf_parser/parser.h" +#include "urdf_parser/urdf_parser.h" #include +#include 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; } diff --git a/urdf_parser/src/parser.cpp b/urdf_parser/src/urdf_parser.cpp similarity index 82% rename from urdf_parser/src/parser.cpp rename to urdf_parser/src/urdf_parser.cpp index c8e45fa..51074fd 100644 --- a/urdf_parser/src/parser.cpp +++ b/urdf_parser/src/urdf_parser.cpp @@ -37,48 +37,29 @@ #include #include #include -#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 &parent_link_tree) +bool URDFParser::initURDFTree(std::map &parent_link_tree) { // loop through all joints, for every link, assign children links and children joints for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) @@ -249,13 +230,13 @@ bool Parser::initTree(std::map &parent_link_tree) { // find child and parent links boost::shared_ptr 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 \"\" 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 &parent_link_tree) -bool Parser::initRoot(std::map &parent_link_tree) +bool URDFParser::initURDFRoot(std::map &parent_link_tree) { this->root_link_.reset(); @@ -300,7 +281,7 @@ bool Parser::initRoot(std::map &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 &parent_link_tree) return true; } -boost::shared_ptr Parser::getMaterial(const std::string& name) const +boost::shared_ptr URDFParser::getURDFMaterial(const std::string& name) const { boost::shared_ptr ptr; if (this->materials_.find(name) == this->materials_.end()) @@ -329,25 +310,7 @@ boost::shared_ptr Parser::getMaterial(const std::string& name) const return ptr; } -boost::shared_ptr Parser::getLink(const std::string& name) const -{ - boost::shared_ptr 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 >& links) const -{ - for (std::map >::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) const +void URDFParser::getURDFLink(const std::string& name,boost::shared_ptr &link) const { boost::shared_ptr ptr; if (this->links_.find(name) == this->links_.end()) @@ -357,15 +320,5 @@ void Parser::getLink(const std::string& name,boost::shared_ptr &link) cons link = ptr; } -boost::shared_ptr Parser::getJoint(const std::string& name) const -{ - boost::shared_ptr ptr; - if (this->joints_.find(name) == this->joints_.end()) - ptr.reset(); - else - ptr = this->joints_.find(name)->second; - return ptr; -} - } diff --git a/urdf_parser/src/urdf_to_graphiz.cpp b/urdf_parser/src/urdf_to_graphiz.cpp index a92bfe9..9f4eee1 100644 --- a/urdf_parser/src/urdf_to_graphiz.cpp +++ b/urdf_parser/src/urdf_to_graphiz.cpp @@ -34,7 +34,7 @@ /* Author: Wim Meeussen */ -#include "urdf_parser/parser.h" +#include "urdf_parser/urdf_parser.h" #include #include @@ -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; } diff --git a/urdf_parser/test/memtest.cpp b/urdf_parser/test/memtest.cpp index 3316c16..bfde1c6 100644 --- a/urdf_parser/test/memtest.cpp +++ b/urdf_parser/test/memtest.cpp @@ -1,13 +1,24 @@ -#include +#include #include +#include +#include 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); } }