From 906f51104819b06a1358bbb1eb67b074237f2496 Mon Sep 17 00:00:00 2001 From: Wim Meeussen Date: Tue, 1 Mar 2011 20:28:59 -0800 Subject: [PATCH] split off urdf parser into new package urdf_parser --- urdf/CMakeLists.txt | 12 +- urdf/include/urdf/model.h | 61 +-- urdf/manifest.xml | 1 + urdf/src/model.cpp | 314 +-------------- urdf/test/memtest.cpp | 10 - urdf_parser/CMakeLists.txt | 38 ++ urdf_parser/Makefile | 1 + .../include/urdf_parser}/color.h | 0 .../include/urdf_parser}/joint.h | 0 .../include/urdf_parser}/link.h | 0 urdf_parser/include/urdf_parser/parser.h | 110 ++++++ .../include/urdf_parser}/pose.h | 0 urdf_parser/mainpage.dox | 159 ++++++++ urdf_parser/manifest.xml | 25 ++ {urdf => urdf_parser}/src/check_urdf.cpp | 6 +- {urdf => urdf_parser}/src/joint.cpp | 2 +- {urdf => urdf_parser}/src/link.cpp | 2 +- urdf_parser/src/parser.cpp | 371 ++++++++++++++++++ {urdf => urdf_parser}/src/urdf_to_graphiz.cpp | 6 +- urdf_parser/test/memtest.cpp | 13 + 20 files changed, 736 insertions(+), 395 deletions(-) delete mode 100644 urdf/test/memtest.cpp create mode 100644 urdf_parser/CMakeLists.txt create mode 100644 urdf_parser/Makefile rename {urdf/include/urdf => urdf_parser/include/urdf_parser}/color.h (100%) rename {urdf/include/urdf => urdf_parser/include/urdf_parser}/joint.h (100%) rename {urdf/include/urdf => urdf_parser/include/urdf_parser}/link.h (100%) create mode 100644 urdf_parser/include/urdf_parser/parser.h rename {urdf/include/urdf => urdf_parser/include/urdf_parser}/pose.h (100%) create mode 100644 urdf_parser/mainpage.dox create mode 100644 urdf_parser/manifest.xml rename {urdf => urdf_parser}/src/check_urdf.cpp (97%) rename {urdf => urdf_parser}/src/joint.cpp (99%) rename {urdf => urdf_parser}/src/link.cpp (99%) create mode 100644 urdf_parser/src/parser.cpp rename {urdf => urdf_parser}/src/urdf_to_graphiz.cpp (98%) create mode 100644 urdf_parser/test/memtest.cpp diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 96355f2..94f624b 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -28,21 +28,11 @@ if( HAVE_MKSTEMPS ) endif() #common commands for building c++ executables and libraries -rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/model.cpp src/collada_model_reader.cpp) +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(check_urdf src/check_urdf.cpp) -target_link_libraries(check_urdf ${PROJECT_NAME}) - -rosbuild_add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp) -target_link_libraries(urdf_to_graphiz ${PROJECT_NAME}) - -rosbuild_add_executable(mem_test test/memtest.cpp) -target_link_libraries(mem_test ${PROJECT_NAME}) - - 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 0d52895..51ff9be 100644 --- a/urdf/include/urdf/model.h +++ b/urdf/include/urdf/model.h @@ -34,23 +34,19 @@ /* Author: Wim Meeussen */ -#ifndef ROBOT_MODEL_PARSER_H -#define ROBOT_MODEL_PARSER_H +#ifndef ROBOT_MODEL_URDF_H +#define ROBOT_MODEL_URDF_H #include #include -#include -#include -#include "link.h" +#include namespace urdf{ -class Model +class Model: public urdf::Parser { public: - Model(); - /// \brief Load Model from TiXMLElement bool initXml(TiXmlElement *xml); /// \brief Load Model from TiXMLDocument @@ -62,55 +58,6 @@ public: /// \brief Load Model from a XML-string bool initString(const std::string& xmlstring); - 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_;}; - - void getLinks(std::vector >& links) const; - - /// \brief get parent Link of a Link given name - boost::shared_ptr getParentLink(const std::string& name) const; - /// \brief get parent Joint of a Link given name - boost::shared_ptr getParentJoint(const std::string& name) const; - /// \brief get child Link of a Link given name - boost::shared_ptr getChildLink(const std::string& name) const; - /// \brief get child Joint of a Link given name - boost::shared_ptr getChildJoint(const std::string& name) const; - - /// \brief complete list of Links - std::map > links_; - /// \brief complete list of Joints - std::map > joints_; - /// \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); - -private: - /// Model 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_; - friend class ColladaModelReader; }; diff --git a/urdf/manifest.xml b/urdf/manifest.xml index c3987b1..5ec4210 100644 --- a/urdf/manifest.xml +++ b/urdf/manifest.xml @@ -13,6 +13,7 @@ + diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index e244af3..3377d80 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -47,20 +47,6 @@ bool urdfFromTiXML(TiXmlElement *robot_xml, Model& model); bool IsColladaFile(const std::string& filename); bool IsColladaData(const std::string& data); -Model::Model() -{ - this->clear(); -} - -void Model::clear() -{ - name_.clear(); - this->links_.clear(); - this->joints_.clear(); - this->materials_.clear(); - this->root_link_.reset(); -} - bool Model::initFile(const std::string& filename) { @@ -71,7 +57,7 @@ bool Model::initFile(const std::string& filename) TiXmlDocument xml_doc; xml_doc.LoadFile(filename); - return initXml(&xml_doc); + return init(&xml_doc); } @@ -106,7 +92,7 @@ bool Model::initString(const std::string& xml_string) TiXmlDocument xml_doc; xml_doc.Parse(xml_string.c_str()); - return initXml(&xml_doc); + return init(&xml_doc); } @@ -125,19 +111,11 @@ bool Model::initXml(TiXmlDocument *xml_doc) } } - 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 initXml(robot_xml); + return init(xml_doc); } bool Model::initXml(TiXmlElement *robot_xml) { - this->clear(); - ROS_DEBUG("Parsing robot xml"); if (!robot_xml) return false; @@ -146,289 +124,7 @@ bool Model::initXml(TiXmlElement *robot_xml) return urdfFromTiXML(robot_xml,*this); } - // Get robot name - const char *name = robot_xml->Attribute("name"); - if (!name) - { - ROS_ERROR("No name given for the robot."); - return false; - } - this->name_ = std::string(name); - - // Get all Material elements - for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) - { - boost::shared_ptr material; - material.reset(new Material); - - if (material->initXml(material_xml)) - { - if (this->getMaterial(material->name)) - { - ROS_ERROR("material '%s' is not unique.", material->name.c_str()); - material.reset(); - return false; - } - else - { - this->materials_.insert(make_pair(material->name,material)); - ROS_DEBUG("successfully added a new material '%s'", material->name.c_str()); - } - } - else - { - ROS_ERROR("material xml is not initialized correctly"); - material.reset(); - return false; - } - } - - // Get all Link elements - for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) - { - boost::shared_ptr link; - link.reset(new Link); - - if (link->initXml(link_xml)) - { - if (this->getLink(link->name)) - { - ROS_ERROR("link '%s' is not unique.", link->name.c_str()); - link.reset(); - return false; - } - else - { - // set link visual material - ROS_DEBUG("setting link '%s' material", link->name.c_str()); - if (link->visual) - { - if (!link->visual->material_name.empty()) - { - if (this->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->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)); - } - else - { - ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); - link.reset(); - return false; - } - } - } - } - - this->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; - } - } - if (this->links_.empty()){ - ROS_ERROR("No link elements found in urdf file"); - return false; - } - - // Get all Joint elements - for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) - { - boost::shared_ptr joint; - joint.reset(new Joint); - - if (joint->initXml(joint_xml)) - { - if (this->getJoint(joint->name)) - { - ROS_ERROR("joint '%s' is not unique.", joint->name.c_str()); - joint.reset(); - return false; - } - else - { - this->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; - } - } - - - // every link has children links and joints, but no parents, so we create a - // local convenience data structure for keeping child->parent relations - std::map parent_link_tree; - parent_link_tree.clear(); - - // building tree: name mapping - if (!this->initTree(parent_link_tree)) - { - ROS_ERROR("failed to build tree"); - return false; - } - - // find the root link - if (!this->initRoot(parent_link_tree)) - { - ROS_ERROR("failed to find root link"); - return false; - } - - return true; -} - -bool Model::initTree(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++) - { - 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->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 \"\" 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 Model::initRoot(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_) - { - 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; -} - -boost::shared_ptr Model::getMaterial(const std::string& name) const -{ - boost::shared_ptr ptr; - if (this->materials_.find(name) == this->materials_.end()) - ptr.reset(); - else - ptr = this->materials_.find(name)->second; - return ptr; -} - -boost::shared_ptr Model::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 Model::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 Model::getLink(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; -} - -boost::shared_ptr Model::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; -} - + return init(robot_xml); } +}// namespace diff --git a/urdf/test/memtest.cpp b/urdf/test/memtest.cpp deleted file mode 100644 index e699d54..0000000 --- a/urdf/test/memtest.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include -#include - -int main(int argc, char** argv){ - ros::init(argc, argv, "memtest"); - while (ros::ok()){ - urdf::Model urdf; - urdf.initFile(std::string(argv[1])); - } -} diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt new file mode 100644 index 0000000..a5a72b7 --- /dev/null +++ b/urdf_parser/CMakeLists.txt @@ -0,0 +1,38 @@ +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 Debug) + +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) + +#uncomment if you have defined messages +#rosbuild_genmsg() + +rosbuild_gensrv() + +#common commands for building c++ executables and libraries +rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/parser.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) + +rosbuild_add_executable(check_urdf src/check_urdf.cpp) +target_link_libraries(check_urdf ${PROJECT_NAME}) + +rosbuild_add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp) +target_link_libraries(urdf_to_graphiz ${PROJECT_NAME}) + +rosbuild_add_executable(mem_test test/memtest.cpp) +target_link_libraries(mem_test ${PROJECT_NAME}) + diff --git a/urdf_parser/Makefile b/urdf_parser/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/urdf_parser/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/urdf/include/urdf/color.h b/urdf_parser/include/urdf_parser/color.h similarity index 100% rename from urdf/include/urdf/color.h rename to urdf_parser/include/urdf_parser/color.h diff --git a/urdf/include/urdf/joint.h b/urdf_parser/include/urdf_parser/joint.h similarity index 100% rename from urdf/include/urdf/joint.h rename to urdf_parser/include/urdf_parser/joint.h diff --git a/urdf/include/urdf/link.h b/urdf_parser/include/urdf_parser/link.h similarity index 100% rename from urdf/include/urdf/link.h rename to urdf_parser/include/urdf_parser/link.h diff --git a/urdf_parser/include/urdf_parser/parser.h b/urdf_parser/include/urdf_parser/parser.h new file mode 100644 index 0000000..7ce0e84 --- /dev/null +++ b/urdf_parser/include/urdf_parser/parser.h @@ -0,0 +1,110 @@ +/********************************************************************* +* 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_H +#define URDF_PARSER_H + +#include +#include +#include +#include +#include "urdf_parser/link.h" + + +namespace urdf{ + +class Parser +{ +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_;}; + + void getLinks(std::vector >& links) const; + + /// \brief get parent Link of a Link given name + boost::shared_ptr getParentLink(const std::string& name) const; + /// \brief get parent Joint of a Link given name + boost::shared_ptr getParentJoint(const std::string& name) const; + /// \brief get child Link of a Link given name + boost::shared_ptr getChildLink(const std::string& name) const; + /// \brief get child Joint of a Link given name + boost::shared_ptr getChildJoint(const std::string& name) const; + + /// \brief complete list of Links + std::map > links_; + /// \brief complete list of Joints + std::map > joints_; + /// \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 + /// 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_; +}; + +} + +#endif diff --git a/urdf/include/urdf/pose.h b/urdf_parser/include/urdf_parser/pose.h similarity index 100% rename from urdf/include/urdf/pose.h rename to urdf_parser/include/urdf_parser/pose.h diff --git a/urdf_parser/mainpage.dox b/urdf_parser/mainpage.dox new file mode 100644 index 0000000..9287a45 --- /dev/null +++ b/urdf_parser/mainpage.dox @@ -0,0 +1,159 @@ +/** +\mainpage +\htmlinclude manifest.html + +urdf::Model is a class containing robot model data structure. +Every Robot Description File (URDF) can be described as a list of Links (urdf::Model::links_) and Joints (urdf::Model::joints_). +The connection between links(nodes) and joints(edges) should define a tree (i.e. 1 parent link, 0+ children links). +\li Here is an example Robot Description Describing a Parent Link 'P', a Child Link 'C', and a Joint 'J' + @verbatim + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @endverbatim + + + +\section codeapi Code API + +The URDF parser API contains the following methods: + \li Parse and build tree from XML: urdf::Model::initXml + \li Parse and build tree from File: urdf::Model::initFile + \li Parse and build tree from String: urdf::Model::initString + \li Get Root Link: urdf::Model::getRoot + \li Get Link by name urdf::Model::getLink + \li Get all Link's urdf::Model::getLinks + \li Get Joint by name urdf::Model::getJoint + + + + + + + + +*/ diff --git a/urdf_parser/manifest.xml b/urdf_parser/manifest.xml new file mode 100644 index 0000000..6f67aaa --- /dev/null +++ b/urdf_parser/manifest.xml @@ -0,0 +1,25 @@ + + + This package contains a C++ parser for the Unified Robot Description + Format (URDF), which is an XML format for representing a robot model. + The code API of the parser has been through our review process and will remain + backwards compatible in future releases. + + Wim Meeussen, John Hsu, Rosen Diankov + BSD + + http://ros.org/wiki/urdf_parser + + + + + + + + + + + + + + diff --git a/urdf/src/check_urdf.cpp b/urdf_parser/src/check_urdf.cpp similarity index 97% rename from urdf/src/check_urdf.cpp rename to urdf_parser/src/check_urdf.cpp index 6f37716..8fe931c 100644 --- a/urdf/src/check_urdf.cpp +++ b/urdf_parser/src/check_urdf.cpp @@ -34,7 +34,7 @@ /* Author: Wim Meeussen */ -#include "urdf/model.h" +#include "urdf_parser/parser.h" #include using namespace urdf; @@ -76,8 +76,8 @@ int main(int argc, char** argv) return -1; } - Model robot; - if (!robot.initXml(robot_xml)){ + Parser robot; + if (!robot.init(robot_xml)){ std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; return -1; } diff --git a/urdf/src/joint.cpp b/urdf_parser/src/joint.cpp similarity index 99% rename from urdf/src/joint.cpp rename to urdf_parser/src/joint.cpp index 89b59b9..e00d1e4 100644 --- a/urdf/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -34,7 +34,7 @@ /* Author: John Hsu */ -#include +#include #include #include diff --git a/urdf/src/link.cpp b/urdf_parser/src/link.cpp similarity index 99% rename from urdf/src/link.cpp rename to urdf_parser/src/link.cpp index e5b86f8..084b971 100644 --- a/urdf/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -35,7 +35,7 @@ /* Author: Wim Meeussen */ -#include "urdf/link.h" +#include "urdf_parser/link.h" #include #include diff --git a/urdf_parser/src/parser.cpp b/urdf_parser/src/parser.cpp new file mode 100644 index 0000000..c8e45fa --- /dev/null +++ b/urdf_parser/src/parser.cpp @@ -0,0 +1,371 @@ +/********************************************************************* +* 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 */ + +#include +#include +#include +#include "urdf_parser/parser.h" + +namespace urdf{ + + +Parser::Parser() +{ + this->clear(); +} + +void Parser::clear() +{ + name_.clear(); + this->links_.clear(); + this->joints_.clear(); + this->materials_.clear(); + this->root_link_.reset(); +} + + + +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"); + 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; + + // Get robot name + const char *name = robot_xml->Attribute("name"); + if (!name) + { + ROS_ERROR("No name given for the robot."); + return false; + } + this->name_ = std::string(name); + + // Get all Material elements + for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) + { + boost::shared_ptr material; + material.reset(new Material); + + if (material->initXml(material_xml)) + { + if (this->getMaterial(material->name)) + { + ROS_ERROR("material '%s' is not unique.", material->name.c_str()); + material.reset(); + return false; + } + else + { + this->materials_.insert(make_pair(material->name,material)); + ROS_DEBUG("successfully added a new material '%s'", material->name.c_str()); + } + } + else + { + ROS_ERROR("material xml is not initialized correctly"); + material.reset(); + return false; + } + } + + // Get all Link elements + for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) + { + boost::shared_ptr link; + link.reset(new Link); + + if (link->initXml(link_xml)) + { + if (this->getLink(link->name)) + { + ROS_ERROR("link '%s' is not unique.", link->name.c_str()); + link.reset(); + return false; + } + else + { + // set link visual material + ROS_DEBUG("setting link '%s' material", link->name.c_str()); + if (link->visual) + { + if (!link->visual->material_name.empty()) + { + if (this->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->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)); + } + else + { + ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); + link.reset(); + return false; + } + } + } + } + + this->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; + } + } + if (this->links_.empty()){ + ROS_ERROR("No link elements found in urdf file"); + return false; + } + + // Get all Joint elements + for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + { + boost::shared_ptr joint; + joint.reset(new Joint); + + if (joint->initXml(joint_xml)) + { + if (this->getJoint(joint->name)) + { + ROS_ERROR("joint '%s' is not unique.", joint->name.c_str()); + joint.reset(); + return false; + } + else + { + this->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; + } + } + + + // every link has children links and joints, but no parents, so we create a + // local convenience data structure for keeping child->parent relations + std::map parent_link_tree; + parent_link_tree.clear(); + + // building tree: name mapping + if (!this->initTree(parent_link_tree)) + { + ROS_ERROR("failed to build tree"); + return false; + } + + // find the root link + if (!this->initRoot(parent_link_tree)) + { + ROS_ERROR("failed to find root link"); + return false; + } + + return true; +} + +bool Parser::initTree(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++) + { + 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->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 \"\" 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 Parser::initRoot(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_) + { + 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; +} + +boost::shared_ptr Parser::getMaterial(const std::string& name) const +{ + boost::shared_ptr ptr; + if (this->materials_.find(name) == this->materials_.end()) + ptr.reset(); + else + ptr = this->materials_.find(name)->second; + 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 +{ + boost::shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(); + else + ptr = this->links_.find(name)->second; + 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/src/urdf_to_graphiz.cpp b/urdf_parser/src/urdf_to_graphiz.cpp similarity index 98% rename from urdf/src/urdf_to_graphiz.cpp rename to urdf_parser/src/urdf_to_graphiz.cpp index 3541128..a92bfe9 100644 --- a/urdf/src/urdf_to_graphiz.cpp +++ b/urdf_parser/src/urdf_to_graphiz.cpp @@ -34,7 +34,7 @@ /* Author: Wim Meeussen */ -#include "urdf/model.h" +#include "urdf_parser/parser.h" #include #include @@ -98,8 +98,8 @@ int main(int argc, char** argv) return -1; } - Model robot; - if (!robot.initXml(robot_xml)){ + Parser robot; + if (!robot.init(robot_xml)){ 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 new file mode 100644 index 0000000..3316c16 --- /dev/null +++ b/urdf_parser/test/memtest.cpp @@ -0,0 +1,13 @@ +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "memtest"); + while (ros::ok()){ + urdf::Parser urdf; + + TiXmlDocument xml_doc; + xml_doc.LoadFile(std::string(argv[1])); + urdf.init(&xml_doc); + } +}