From bca3cb4ba93460e173d369c337605bbfe2220727 Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Mon, 4 Jun 2012 14:49:47 -0700 Subject: [PATCH] urdfdom as dep --- kdl_parser/src/kdl_parser.cpp | 2 +- urdf_interface/CMakeLists.txt | 19 - urdf_interface/Makefile | 1 - urdf_interface/include/urdf_interface/color.h | 104 --- urdf_interface/include/urdf_interface/joint.h | 235 ------- urdf_interface/include/urdf_interface/link.h | 260 ------- urdf_interface/include/urdf_interface/model.h | 224 ------ urdf_interface/include/urdf_interface/pose.h | 308 --------- urdf_interface/manifest.xml | 8 +- urdf_parser/CMakeLists.txt | 38 - urdf_parser/Makefile | 1 - urdf_parser/include/urdf_parser/urdf_parser.h | 53 -- urdf_parser/manifest.xml | 18 +- urdf_parser/src/check_urdf.cpp | 103 --- urdf_parser/src/joint.cpp | 566 --------------- urdf_parser/src/link.cpp | 648 ------------------ urdf_parser/src/urdf_parser.cpp | 217 ------ urdf_parser/src/urdf_to_graphiz.cpp | 120 ---- urdf_parser/test/memtest.cpp | 22 - 19 files changed, 12 insertions(+), 2935 deletions(-) delete mode 100644 urdf_interface/CMakeLists.txt delete mode 100644 urdf_interface/Makefile delete mode 100644 urdf_interface/include/urdf_interface/color.h delete mode 100644 urdf_interface/include/urdf_interface/joint.h delete mode 100644 urdf_interface/include/urdf_interface/link.h delete mode 100644 urdf_interface/include/urdf_interface/model.h delete mode 100644 urdf_interface/include/urdf_interface/pose.h delete mode 100644 urdf_parser/CMakeLists.txt delete mode 100644 urdf_parser/Makefile delete mode 100644 urdf_parser/include/urdf_parser/urdf_parser.h delete mode 100644 urdf_parser/src/check_urdf.cpp delete mode 100644 urdf_parser/src/joint.cpp delete mode 100644 urdf_parser/src/link.cpp delete mode 100644 urdf_parser/src/urdf_parser.cpp delete mode 100644 urdf_parser/src/urdf_to_graphiz.cpp delete mode 100644 urdf_parser/test/memtest.cpp diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp index dcb6418..2c4b546 100644 --- a/kdl_parser/src/kdl_parser.cpp +++ b/kdl_parser/src/kdl_parser.cpp @@ -36,7 +36,7 @@ #include "kdl_parser/kdl_parser.hpp" #include - +#include using namespace std; using namespace KDL; diff --git a/urdf_interface/CMakeLists.txt b/urdf_interface/CMakeLists.txt deleted file mode 100644 index f41e154..0000000 --- a/urdf_interface/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -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) -#rosbuild_add_boost_directories() - diff --git a/urdf_interface/Makefile b/urdf_interface/Makefile deleted file mode 100644 index b75b928..0000000 --- a/urdf_interface/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/urdf_interface/include/urdf_interface/color.h b/urdf_interface/include/urdf_interface/color.h deleted file mode 100644 index 64bcb81..0000000 --- a/urdf_interface/include/urdf_interface/color.h +++ /dev/null @@ -1,104 +0,0 @@ -/********************************************************************* -* 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: Josh Faust */ - -#ifndef URDF_INTERFACE_COLOR_H -#define URDF_INTERFACE_COLOR_H - -#include -#include -#include -#include -#include -#include - -namespace urdf -{ - -class Color -{ -public: - Color() {this->clear();}; - float r; - float g; - float b; - float a; - - void clear() - { - r = g = b = 0.0f; - a = 1.0f; - } - bool init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector rgba; - boost::split( pieces, vector_str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i) - { - if (!pieces[i].empty()) - { - try - { - rgba.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("color rgba element (%s) is not a valid float",pieces[i].c_str()); - return false; - } - } - } - - if (rgba.size() != 4) - { - ROS_ERROR("Color contains %i elements instead of 4 elements", (int)rgba.size()); - return false; - } - - this->r = rgba[0]; - this->g = rgba[1]; - this->b = rgba[2]; - this->a = rgba[3]; - - return true; - }; -}; - -} - -#endif - diff --git a/urdf_interface/include/urdf_interface/joint.h b/urdf_interface/include/urdf_interface/joint.h deleted file mode 100644 index 445c646..0000000 --- a/urdf_interface/include/urdf_interface/joint.h +++ /dev/null @@ -1,235 +0,0 @@ -/********************************************************************* -* 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_INTERFACE_JOINT_H -#define URDF_INTERFACE_JOINT_H - -#include -#include -#include -#include - -#include "pose.h" - -namespace urdf{ - -class Link; - -class JointDynamics -{ -public: - JointDynamics() { this->clear(); }; - double damping; - double friction; - - void clear() - { - damping = 0; - friction = 0; - }; - bool initXml(TiXmlElement* config); -}; - -class JointLimits -{ -public: - JointLimits() { this->clear(); }; - double lower; - double upper; - double effort; - double velocity; - - void clear() - { - lower = 0; - upper = 0; - effort = 0; - velocity = 0; - }; - bool initXml(TiXmlElement* config); -}; - -/// \brief Parameters for Joint Safety Controllers -class JointSafety -{ -public: - /// clear variables on construction - JointSafety() { this->clear(); }; - - /// - /// IMPORTANT: The safety controller support is very much PR2 specific, not intended for generic usage. - /// - /// Basic safety controller operation is as follows - /// - /// current safety controllers will take effect on joints outside the position range below: - /// - /// position range: [JointSafety::soft_lower_limit + JointLimits::velocity / JointSafety::k_position, - /// JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position] - /// - /// if (joint_position is outside of the position range above) - /// velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit) - /// velocity_limit_max = JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit) - /// else - /// velocity_limit_min = -JointLimits::velocity - /// velocity_limit_max = JointLimits::velocity - /// - /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity, - /// velocity_limit_max - JointLimits::effort / JointSafety::k_velocity] - /// - /// if (joint_velocity is outside of the velocity range above) - /// effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min) - /// effort_limit_max = JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max) - /// else - /// effort_limit_min = -JointLimits::effort - /// effort_limit_max = JointLimits::effort - /// - /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max] - /// - /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits - /// - double soft_upper_limit; - double soft_lower_limit; - double k_position; - double k_velocity; - - void clear() - { - soft_upper_limit = 0; - soft_lower_limit = 0; - k_position = 0; - k_velocity = 0; - }; - bool initXml(TiXmlElement* config); -}; - - -class JointCalibration -{ -public: - JointCalibration() { this->clear(); }; - double reference_position; - boost::shared_ptr rising, falling; - - void clear() - { - reference_position = 0; - }; - bool initXml(TiXmlElement* config); -}; - -class JointMimic -{ -public: - JointMimic() { this->clear(); }; - double offset; - double multiplier; - std::string joint_name; - - void clear() - { - offset = 0.0; - multiplier = 0.0; - joint_name.clear(); - }; - bool initXml(TiXmlElement* config); -}; - - -class Joint -{ -public: - - Joint() { this->clear(); }; - - std::string name; - enum - { - UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED - } type; - - /// \brief type_ meaning of axis_ - /// ------------------------------------------------------ - /// UNKNOWN unknown type - /// REVOLUTE rotation axis - /// PRISMATIC translation axis - /// FLOATING N/A - /// PLANAR plane normal axis - /// FIXED N/A - Vector3 axis; - - /// child Link element - /// child link frame is the same as the Joint frame - std::string child_link_name; - - /// parent Link element - /// origin specifies the transform from Parent Link to Joint Frame - std::string parent_link_name; - /// transform from Parent Link frame to Joint frame - Pose parent_to_joint_origin_transform; - - /// Joint Dynamics - boost::shared_ptr dynamics; - - /// Joint Limits - boost::shared_ptr limits; - - /// Unsupported Hidden Feature - boost::shared_ptr safety; - - /// Unsupported Hidden Feature - boost::shared_ptr calibration; - - /// Option to Mimic another Joint - boost::shared_ptr mimic; - - bool initXml(TiXmlElement* xml); - void clear() - { - this->axis.clear(); - this->child_link_name.clear(); - this->parent_link_name.clear(); - this->parent_to_joint_origin_transform.clear(); - this->dynamics.reset(); - this->limits.reset(); - this->safety.reset(); - this->calibration.reset(); - this->type = UNKNOWN; - }; -}; - -} - -#endif diff --git a/urdf_interface/include/urdf_interface/link.h b/urdf_interface/include/urdf_interface/link.h deleted file mode 100644 index 6f30254..0000000 --- a/urdf_interface/include/urdf_interface/link.h +++ /dev/null @@ -1,260 +0,0 @@ -/********************************************************************* -* 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_INTERFACE_LINK_H -#define URDF_INTERFACE_LINK_H - -#include -#include -#include -#include -#include - -#include "joint.h" -#include "color.h" - -namespace urdf{ - -class Geometry -{ -public: - enum {SPHERE, BOX, CYLINDER, MESH} type; - - virtual bool initXml(TiXmlElement *) = 0; - -}; - -class Sphere : public Geometry -{ -public: - Sphere() { this->clear(); }; - double radius; - - void clear() - { - radius = 0; - }; - bool initXml(TiXmlElement *); -}; - -class Box : public Geometry -{ -public: - Box() { this->clear(); }; - Vector3 dim; - - void clear() - { - dim.clear(); - }; - bool initXml(TiXmlElement *); -}; - -class Cylinder : public Geometry -{ -public: - Cylinder() { this->clear(); }; - double length; - double radius; - - void clear() - { - length = 0; - radius = 0; - }; - bool initXml(TiXmlElement *); -}; - -class Mesh : public Geometry -{ -public: - Mesh() { this->clear(); }; - std::string filename; - Vector3 scale; - - void clear() - { - filename.clear(); - // default scale - scale.x = 1; - scale.y = 1; - scale.z = 1; - }; - bool initXml(TiXmlElement *); - bool fileExists(std::string filename); -}; - -class Material -{ -public: - Material() { this->clear(); }; - std::string name; - std::string texture_filename; - Color color; - - void clear() - { - color.clear(); - texture_filename.clear(); - name.clear(); - }; - bool initXml(TiXmlElement* config); -}; - -class Inertial -{ -public: - Inertial() { this->clear(); }; - Pose origin; - double mass; - double ixx,ixy,ixz,iyy,iyz,izz; - - void clear() - { - origin.clear(); - mass = 0; - ixx = ixy = ixz = iyy = iyz = izz = 0; - }; - bool initXml(TiXmlElement* config); -}; - -class Visual -{ -public: - Visual() { this->clear(); }; - Pose origin; - boost::shared_ptr geometry; - - std::string material_name; - boost::shared_ptr material; - - void clear() - { - origin.clear(); - material_name.clear(); - material.reset(); - geometry.reset(); - this->group_name.clear(); - }; - bool initXml(TiXmlElement* config); - std::string group_name; -}; - -class Collision -{ -public: - Collision() { this->clear(); }; - Pose origin; - boost::shared_ptr geometry; - - void clear() - { - origin.clear(); - geometry.reset(); - this->group_name.clear(); - }; - bool initXml(TiXmlElement* config); - std::string group_name; -}; - - -class Link -{ -public: - Link() { this->clear(); }; - - std::string name; - - /// inertial element - boost::shared_ptr inertial; - - /// visual element - boost::shared_ptr visual; - - /// collision element - boost::shared_ptr collision; - - /// a collection of visual elements, keyed by a string tag called "group" - std::map > > > visual_groups; - - /// a collection of collision elements, keyed by a string tag called "group" - std::map > > > collision_groups; - - /// Parent Joint element - /// explicitly stating "parent" because we want directional-ness for tree structure - /// every link can have one parent - boost::shared_ptr parent_joint; - - std::vector > child_joints; - std::vector > child_links; - - bool initXml(TiXmlElement* config); - - boost::shared_ptr getParent() const - {return parent_link_.lock();}; - - void setParent(boost::shared_ptr parent); - - void clear() - { - this->name.clear(); - this->inertial.reset(); - this->visual.reset(); - this->collision.reset(); - this->parent_joint.reset(); - this->child_joints.clear(); - this->child_links.clear(); - this->collision_groups.clear(); - }; - void setParentJoint(boost::shared_ptr child); - void addChild(boost::shared_ptr child); - void addChildJoint(boost::shared_ptr child); - - void addVisual(std::string group_name, boost::shared_ptr visual); - boost::shared_ptr > > getVisuals(const std::string& group_name) const; - void addCollision(std::string group_name, boost::shared_ptr collision); - boost::shared_ptr > > getCollisions(const std::string& group_name) const; -private: - boost::weak_ptr parent_link_; - -}; - - - - -} - -#endif diff --git a/urdf_interface/include/urdf_interface/model.h b/urdf_interface/include/urdf_interface/model.h deleted file mode 100644 index a12f3dd..0000000 --- a/urdf_interface/include/urdf_interface/model.h +++ /dev/null @@ -1,224 +0,0 @@ -/********************************************************************* -* 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_INTERFACE_MODEL_H -#define URDF_INTERFACE_MODEL_H - -#include -#include -#include -#include - - -namespace urdf { - -class ModelInterface -{ -public: - boost::shared_ptr getRoot(void) const{return this->root_link_;}; - 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; - }; - - 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(); - }; - - /// non-const getLink() - void 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; - }; - - /// non-const getMaterial() - boost::shared_ptr 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; - }; - - /// in initXml(), onece all links are loaded, - /// it's time to build a tree - bool 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; - }; - - - /// in initXml(), onece tree is built, - /// it's time to find the root Link - bool 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; - }; - - /// \brief complete list of Links - std::map > links_; - /// \brief complete list of Joints - std::map > joints_; - /// \brief complete list of Materials - std::map > materials_; - - std::string name_; - - /// 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_; - - - -}; - -} - -#endif diff --git a/urdf_interface/include/urdf_interface/pose.h b/urdf_interface/include/urdf_interface/pose.h deleted file mode 100644 index 14f2792..0000000 --- a/urdf_interface/include/urdf_interface/pose.h +++ /dev/null @@ -1,308 +0,0 @@ -/********************************************************************* -* 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_INTERFACE_POSE_H -#define URDF_INTERFACE_POSE_H - -#include -#include -#include -#include -#include - -#include // FIXME: remove parser from here -#include - -namespace urdf{ - -class Vector3 -{ -public: - Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;}; - Vector3() {this->clear();}; - double x; - double y; - double z; - - void clear() {this->x=this->y=this->z=0.0;}; - bool init(const std::string &vector_str) - { - this->clear(); - std::vector pieces; - std::vector xyz; - boost::split( pieces, vector_str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i){ - if (pieces[i] != ""){ - try - { - xyz.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("Vector3 xyz element (%s) is not a valid float",pieces[i].c_str()); - return false; - } - } - } - - if (xyz.size() != 3) { - ROS_ERROR("Vector contains %i elements instead of 3 elements", (int)xyz.size()); - return false; - } - - this->x = xyz[0]; - this->y = xyz[1]; - this->z = xyz[2]; - - return true; - }; - Vector3 operator+(Vector3 vec) - { - return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z); - }; -}; - -class Rotation -{ -public: - Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;}; - Rotation() {this->clear();}; - void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const - { - quat_x = this->x; - quat_y = this->y; - quat_z = this->z; - quat_w = this->w; - }; - void getRPY(double &roll,double &pitch,double &yaw) const - { - double sqw; - double sqx; - double sqy; - double sqz; - - sqx = this->x * this->x; - sqy = this->y * this->y; - sqz = this->z * this->z; - sqw = this->w * this->w; - - roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz); - double sarg = -2 * (this->x*this->z - this->w*this->y); - pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg)); - yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz); - - }; - void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w) - { - this->x = quat_x; - this->y = quat_y; - this->z = quat_z; - this->w = quat_w; - this->normalize(); - }; - void setFromRPY(double roll, double pitch, double yaw) - { - double phi, the, psi; - - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - - this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); - this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); - this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); - this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); - - this->normalize(); - }; - - double x,y,z,w; - - bool init(const std::string &rotation_str) - { - this->clear(); - - Vector3 rpy; - - if (!rpy.init(rotation_str)) - return false; - else - { - this->setFromRPY(rpy.x,rpy.y,rpy.z); - return true; - } - - }; - - void clear() { this->x=this->y=this->z=0.0;this->w=1.0; } - - void normalize() - { - double s = sqrt(this->x * this->x + - this->y * this->y + - this->z * this->z + - this->w * this->w); - if (s == 0.0) - { - this->x = 0.0; - this->y = 0.0; - this->z = 0.0; - this->w = 1.0; - } - else - { - this->x /= s; - this->y /= s; - this->z /= s; - this->w /= s; - } - }; - - // Multiplication operator (copied from gazebo) - Rotation operator*( const Rotation &qt ) const - { - Rotation c; - - c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y; - c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x; - c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w; - c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z; - - return c; - }; - /// Rotate a vector using the quaternion - Vector3 operator*(Vector3 vec) const - { - Rotation tmp; - Vector3 result; - - tmp.w = 0.0; - tmp.x = vec.x; - tmp.y = vec.y; - tmp.z = vec.z; - - tmp = (*this) * (tmp * this->GetInverse()); - - result.x = tmp.x; - result.y = tmp.y; - result.z = tmp.z; - - return result; - }; - // Get the inverse of this quaternion - Rotation GetInverse() const - { - Rotation q; - - double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z; - - if (norm > 0.0) - { - q.w = this->w / norm; - q.x = -this->x / norm; - q.y = -this->y / norm; - q.z = -this->z / norm; - } - - return q; - }; - - -}; - -class Pose -{ -public: - Pose() { this->clear(); }; - - Vector3 position; - Rotation rotation; - - void clear() - { - this->position.clear(); - this->rotation.clear(); - }; - bool initXml(TiXmlElement* xml) - { - this->clear(); - if (!xml) - { - ROS_DEBUG("parsing pose: xml empty"); - return false; - } - else - { - const char* xyz_str = xml->Attribute("xyz"); - if (xyz_str == NULL) - { - ROS_DEBUG("parsing pose: no xyz, using default values."); - return true; - } - else - { - if (!this->position.init(xyz_str)) - { - ROS_ERROR("malformed xyz"); - this->position.clear(); - return false; - } - } - - const char* rpy_str = xml->Attribute("rpy"); - if (rpy_str == NULL) - { - ROS_DEBUG("parsing pose: no rpy, using default values."); - return true; - } - else - { - if (!this->rotation.init(rpy_str)) - { - ROS_ERROR("malformed rpy"); - return false; - this->rotation.clear(); - } - } - - return true; - } - }; -}; - -} - -#endif diff --git a/urdf_interface/manifest.xml b/urdf_interface/manifest.xml index 329e908..e8488ce 100644 --- a/urdf_interface/manifest.xml +++ b/urdf_interface/manifest.xml @@ -1,17 +1,19 @@ + This package is DEPRECATED. Please use the urdfdom dependency directly instead of this package. + This package contains URDF C++ interface headers that define the user API to the C++ URDF model. Wim Meeussen, John Hsu BSD - + http://ros.org/wiki/urdf_interface - + - + diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt deleted file mode 100644 index d9090ef..0000000 --- a/urdf_parser/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -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/urdf_parser.cpp) -target_link_libraries(${PROJECT_NAME} tinyxml) -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 deleted file mode 100644 index b75b928..0000000 --- a/urdf_parser/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h deleted file mode 100644 index 357c112..0000000 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************* -* 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{ - - boost::shared_ptr parseURDF(const std::string &xml_string); - -} - -#endif diff --git a/urdf_parser/manifest.xml b/urdf_parser/manifest.xml index a82e16f..fc0d779 100644 --- a/urdf_parser/manifest.xml +++ b/urdf_parser/manifest.xml @@ -1,5 +1,7 @@ + This package is DEPRECATED. Please use the urdfdom dependency directly instead of this package. + This package contains a C++ parser for the Unified Robot Description Format (URDF), which is an XML format for representing a robot model. The parser reads a URDF XML robot description, and @@ -9,21 +11,13 @@ Wim Meeussen, John Hsu, Rosen Diankov BSD - + http://ros.org/wiki/urdf_parser - + - - - - - + + - - - - - diff --git a/urdf_parser/src/check_urdf.cpp b/urdf_parser/src/check_urdf.cpp deleted file mode 100644 index 3402e49..0000000 --- a/urdf_parser/src/check_urdf.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/********************************************************************* -* 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 "urdf_parser/urdf_parser.h" -#include -#include - -using namespace urdf; - -void printTree(boost::shared_ptr link,int level = 0) -{ - level+=2; - int count = 0; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - for(int j=0;jname << std::endl; - // first grandchild - printTree(*child,level); - } - else - { - for(int j=0;jname << " has a null child!" << *child << std::endl; - } - } - -} - - -int main(int argc, char** argv) -{ - if (argc < 2){ - std::cerr << "Expect URDF xml file to parse" << std::endl; - return -1; - } - - 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(); - - boost::shared_ptr robot = parseURDF(xml_string); - if (!robot){ - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; - return -1; - } - std::cout << "robot name is: " << robot->getName() << std::endl; - - // get info from parser - std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; - // get root link - boost::shared_ptr root_link=robot->getRoot(); - if (!root_link) return -1; - - std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; - - - // print entire tree - printTree(root_link); - return 0; -} - diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp deleted file mode 100644 index 207fb6b..0000000 --- a/urdf_parser/src/joint.cpp +++ /dev/null @@ -1,566 +0,0 @@ -/********************************************************************* -* Software Ligcense 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: John Hsu */ - -#include -#include -#include - -namespace urdf{ - -bool JointDynamics::initXml(TiXmlElement* config) -{ - this->clear(); - - // Get joint damping - const char* damping_str = config->Attribute("damping"); - if (damping_str == NULL){ - ROS_DEBUG("joint dynamics: no damping, defaults to 0"); - this->damping = 0; - } - else - { - try - { - this->damping = boost::lexical_cast(damping_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("damping value (%s) is not a float",damping_str); - return false; - } - } - - // Get joint friction - const char* friction_str = config->Attribute("friction"); - if (friction_str == NULL){ - ROS_DEBUG("joint dynamics: no friction, defaults to 0"); - this->friction = 0; - } - else - { - try - { - this->friction = boost::lexical_cast(friction_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("friction value (%s) is not a float",friction_str); - return false; - } - } - - if (damping_str == NULL && friction_str == NULL) - { - ROS_ERROR("joint dynamics element specified with no damping and no friction"); - return false; - } - else{ - ROS_DEBUG("joint dynamics: damping %f and friction %f", damping, friction); - return true; - } -} - -bool JointLimits::initXml(TiXmlElement* config) -{ - this->clear(); - - // Get lower joint limit - const char* lower_str = config->Attribute("lower"); - if (lower_str == NULL){ - ROS_DEBUG("joint limit: no lower, defaults to 0"); - this->lower = 0; - } - else - { - try - { - this->lower = boost::lexical_cast(lower_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("lower value (%s) is not a float",lower_str); - return false; - } - } - - // Get upper joint limit - const char* upper_str = config->Attribute("upper"); - if (upper_str == NULL){ - ROS_DEBUG("joint limit: no upper, , defaults to 0"); - this->upper = 0; - } - else - { - try - { - this->upper = boost::lexical_cast(upper_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("upper value (%s) is not a float",upper_str); - return false; - } - } - - // Get joint effort limit - const char* effort_str = config->Attribute("effort"); - if (effort_str == NULL){ - ROS_ERROR("joint limit: no effort"); - return false; - } - else - { - try - { - this->effort = boost::lexical_cast(effort_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("effort value (%s) is not a float",effort_str); - return false; - } - } - - // Get joint velocity limit - const char* velocity_str = config->Attribute("velocity"); - if (velocity_str == NULL){ - ROS_ERROR("joint limit: no velocity"); - return false; - } - else - { - try - { - this->velocity = boost::lexical_cast(velocity_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("velocity value (%s) is not a float",velocity_str); - return false; - } - } - - return true; -} - -bool JointSafety::initXml(TiXmlElement* config) -{ - this->clear(); - - // Get soft_lower_limit joint limit - const char* soft_lower_limit_str = config->Attribute("soft_lower_limit"); - if (soft_lower_limit_str == NULL) - { - ROS_DEBUG("joint safety: no soft_lower_limit, using default value"); - this->soft_lower_limit = 0; - } - else - { - try - { - this->soft_lower_limit = boost::lexical_cast(soft_lower_limit_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("soft_lower_limit value (%s) is not a float",soft_lower_limit_str); - return false; - } - } - - // Get soft_upper_limit joint limit - const char* soft_upper_limit_str = config->Attribute("soft_upper_limit"); - if (soft_upper_limit_str == NULL) - { - ROS_DEBUG("joint safety: no soft_upper_limit, using default value"); - this->soft_upper_limit = 0; - } - else - { - try - { - this->soft_upper_limit = boost::lexical_cast(soft_upper_limit_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("soft_upper_limit value (%s) is not a float",soft_upper_limit_str); - return false; - } - } - - // Get k_position_ safety "position" gain - not exactly position gain - const char* k_position_str = config->Attribute("k_position"); - if (k_position_str == NULL) - { - ROS_DEBUG("joint safety: no k_position, using default value"); - this->k_position = 0; - } - else - { - try - { - this->k_position = boost::lexical_cast(k_position_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("k_position value (%s) is not a float",k_position_str); - return false; - } - } - // Get k_velocity_ safety velocity gain - const char* k_velocity_str = config->Attribute("k_velocity"); - if (k_velocity_str == NULL) - { - ROS_ERROR("joint safety: no k_velocity"); - return false; - } - else - { - try - { - this->k_velocity = boost::lexical_cast(k_velocity_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("k_velocity value (%s) is not a float",k_velocity_str); - return false; - } - } - - return true; -} - -bool JointCalibration::initXml(TiXmlElement* config) -{ - this->clear(); - - // Get rising edge position - const char* rising_position_str = config->Attribute("rising"); - if (rising_position_str == NULL) - { - ROS_DEBUG("joint calibration: no rising, using default value"); - this->rising.reset(); - } - else - { - try - { - this->rising.reset(new double(boost::lexical_cast(rising_position_str))); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("risingvalue (%s) is not a float",rising_position_str); - return false; - } - } - - // Get falling edge position - const char* falling_position_str = config->Attribute("falling"); - if (falling_position_str == NULL) - { - ROS_DEBUG("joint calibration: no falling, using default value"); - this->falling.reset(); - } - else - { - try - { - this->falling.reset(new double(boost::lexical_cast(falling_position_str))); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("fallingvalue (%s) is not a float",falling_position_str); - return false; - } - } - - return true; -} - -bool JointMimic::initXml(TiXmlElement* config) -{ - this->clear(); - - // Get name of joint to mimic - const char* joint_name_str = config->Attribute("joint"); - - if (joint_name_str == NULL) - { - ROS_ERROR("joint mimic: no mimic joint specified"); - //return false; - } - else - this->joint_name = joint_name_str; - - // Get mimic multiplier - const char* multiplier_str = config->Attribute("multiplier"); - - if (multiplier_str == NULL) - { - ROS_DEBUG("joint mimic: no multiplier, using default value of 1"); - this->multiplier = 1; - } - else - { - try - { - this->multiplier = boost::lexical_cast(multiplier_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("multiplier value (%s) is not a float",multiplier_str); - return false; - } - } - - - // Get mimic offset - const char* offset_str = config->Attribute("offset"); - if (offset_str == NULL) - { - ROS_DEBUG("joint mimic: no offset, using default value of 0"); - this->offset = 0; - } - else - { - try - { - this->offset = boost::lexical_cast(offset_str); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("offset value (%s) is not a float",offset_str); - return false; - } - } - - return true; -} - -bool Joint::initXml(TiXmlElement* config) -{ - this->clear(); - - // Get Joint Name - const char *name = config->Attribute("name"); - if (!name) - { - ROS_ERROR("unnamed joint found"); - return false; - } - this->name = name; - - // Get transform from Parent Link to Joint Frame - TiXmlElement *origin_xml = config->FirstChildElement("origin"); - if (!origin_xml) - { - ROS_DEBUG("Joint '%s' missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", this->name.c_str()); - this->parent_to_joint_origin_transform.clear(); - } - else - { - if (!this->parent_to_joint_origin_transform.initXml(origin_xml)) - { - ROS_ERROR("Malformed parent origin element for joint '%s'", this->name.c_str()); - this->parent_to_joint_origin_transform.clear(); - return false; - } - } - - // Get Parent Link - TiXmlElement *parent_xml = config->FirstChildElement("parent"); - if (parent_xml) - { - const char *pname = parent_xml->Attribute("link"); - if (!pname) - ROS_INFO("no parent link name specified for Joint link '%s'. this might be the root?", this->name.c_str()); - else - { - this->parent_link_name = std::string(pname); - - } - } - - // Get Child Link - TiXmlElement *child_xml = config->FirstChildElement("child"); - if (child_xml) - { - const char *pname = child_xml->Attribute("link"); - if (!pname) - ROS_INFO("no child link name specified for Joint link '%s'.", this->name.c_str()); - else - { - this->child_link_name = std::string(pname); - - } - } - - // Get Joint type - const char* type_char = config->Attribute("type"); - if (!type_char) - { - ROS_ERROR("joint '%s' has no type, check to see if it's a reference.", this->name.c_str()); - return false; - } - std::string type_str = type_char; - if (type_str == "planar") - { - type = PLANAR; - ROS_WARN("Planar joints are deprecated in the URDF!\n"); - } - else if (type_str == "floating") - { - type = FLOATING; - ROS_WARN("Floating joints are deprecated in the URDF!\n"); - } - else if (type_str == "revolute") - type = REVOLUTE; - else if (type_str == "continuous") - type = CONTINUOUS; - else if (type_str == "prismatic") - type = PRISMATIC; - else if (type_str == "fixed") - type = FIXED; - else - { - ROS_ERROR("Joint '%s' has no known type '%s'", this->name.c_str(), type_str.c_str()); - return false; - } - - // Get Joint Axis - if (this->type != FLOATING && this->type != FIXED) - { - // axis - TiXmlElement *axis_xml = config->FirstChildElement("axis"); - if (!axis_xml){ - ROS_DEBUG("no axis elemement for Joint link '%s', defaulting to (1,0,0) axis", this->name.c_str()); - this->axis = Vector3(1.0, 0.0, 0.0); - } - else{ - if (!axis_xml->Attribute("xyz")){ - ROS_ERROR("no xyz attribute for axis element for Joint link '%s'", this->name.c_str()); - } - else { - if (!this->axis.init(axis_xml->Attribute("xyz"))) - { - ROS_ERROR("Malformed axis element for joint '%s'", this->name.c_str()); - this->axis.clear(); - return false; - } - } - } - } - - // Get limit - TiXmlElement *limit_xml = config->FirstChildElement("limit"); - if (limit_xml) - { - limits.reset(new JointLimits); - if (!limits->initXml(limit_xml)) - { - ROS_ERROR("Could not parse limit element for joint '%s'", this->name.c_str()); - limits.reset(); - return false; - } - } - else if (this->type == REVOLUTE) - { - ROS_ERROR("Joint '%s' is of type REVOLUTE but it does not specify limits", this->name.c_str()); - return false; - } - else if (this->type == PRISMATIC) - { - ROS_INFO("Joint '%s' is of type PRISMATIC without limits", this->name.c_str()); - limits.reset(); - } - - // Get safety - TiXmlElement *safety_xml = config->FirstChildElement("safety_controller"); - if (safety_xml) - { - safety.reset(new JointSafety); - if (!safety->initXml(safety_xml)) - { - ROS_ERROR("Could not parse safety element for joint '%s'", this->name.c_str()); - safety.reset(); - return false; - } - } - - // Get calibration - TiXmlElement *calibration_xml = config->FirstChildElement("calibration"); - if (calibration_xml) - { - calibration.reset(new JointCalibration); - if (!calibration->initXml(calibration_xml)) - { - ROS_ERROR("Could not parse calibration element for joint '%s'", this->name.c_str()); - calibration.reset(); - return false; - } - } - - // Get Joint Mimic - TiXmlElement *mimic_xml = config->FirstChildElement("mimic"); - if (mimic_xml) - { - mimic.reset(new JointMimic); - if (!mimic->initXml(mimic_xml)) - { - ROS_ERROR("Could not parse mimic element for joint '%s'", this->name.c_str()); - mimic.reset(); - return false; - } - } - - // Get Dynamics - TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); - if (prop_xml) - { - dynamics.reset(new JointDynamics); - if (!dynamics->initXml(prop_xml)) - { - ROS_ERROR("Could not parse joint_dynamics element for joint '%s'", this->name.c_str()); - dynamics.reset(); - return false; - } - } - - return true; -} - - - -} diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp deleted file mode 100644 index 5e9861c..0000000 --- a/urdf_parser/src/link.cpp +++ /dev/null @@ -1,648 +0,0 @@ -/********************************************************************* -* 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 -#include -#include - -namespace urdf{ - -boost::shared_ptr parseGeometry(TiXmlElement *g) -{ - boost::shared_ptr geom; - if (!g) return geom; - - TiXmlElement *shape = g->FirstChildElement(); - if (!shape) - { - ROS_ERROR("Geometry tag contains no child element."); - return geom; - } - - std::string type_name = shape->ValueStr(); - if (type_name == "sphere") - geom.reset(new Sphere); - else if (type_name == "box") - geom.reset(new Box); - else if (type_name == "cylinder") - geom.reset(new Cylinder); - else if (type_name == "mesh") - geom.reset(new Mesh); - else - { - ROS_ERROR("Unknown geometry type '%s'", type_name.c_str()); - return geom; - } - - // clear geom object when fails to initialize - if (!geom->initXml(shape)){ - ROS_ERROR("Geometry failed to parse"); - geom.reset(); - } - - return geom; -} - -bool Material::initXml(TiXmlElement *config) -{ - bool has_rgb = false; - bool has_filename = false; - - this->clear(); - - if (!config->Attribute("name")) - { - ROS_ERROR("Material must contain a name attribute"); - return false; - } - - this->name = config->Attribute("name"); - - // texture - TiXmlElement *t = config->FirstChildElement("texture"); - if (t) - { - if (t->Attribute("filename")) - { - this->texture_filename = t->Attribute("filename"); - has_filename = true; - } - else - { - ROS_ERROR("texture has no filename for Material %s",this->name.c_str()); - } - } - - // color - TiXmlElement *c = config->FirstChildElement("color"); - if (c) - { - if (c->Attribute("rgba")) - { - if (!this->color.init(c->Attribute("rgba"))) - { - ROS_ERROR("Material %s has malformed color rgba values.",this->name.c_str()); - this->color.clear(); - return false; - } - else - has_rgb = true; - } - else - { - ROS_ERROR("Material %s color has no rgba",this->name.c_str()); - } - } - - return (has_rgb || has_filename); -} - -bool Inertial::initXml(TiXmlElement *config) -{ - this->clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (!o) - { - ROS_DEBUG("Origin tag not present for inertial element, using default (Identity)"); - this->origin.clear(); - } - else - { - if (!this->origin.initXml(o)) - { - ROS_ERROR("Inertial has a malformed origin tag"); - this->origin.clear(); - return false; - } - } - - TiXmlElement *mass_xml = config->FirstChildElement("mass"); - if (!mass_xml) - { - ROS_ERROR("Inertial element must have mass element"); - return false; - } - if (!mass_xml->Attribute("value")) - { - ROS_ERROR("Inertial: mass element must have value attributes"); - return false; - } - - try - { - mass = boost::lexical_cast(mass_xml->Attribute("value")); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("mass (%s) is not a float",mass_xml->Attribute("value")); - return false; - } - - TiXmlElement *inertia_xml = config->FirstChildElement("inertia"); - if (!inertia_xml) - { - ROS_ERROR("Inertial element must have inertia element"); - return false; - } - if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") && - inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && - inertia_xml->Attribute("izz"))) - { - ROS_ERROR("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); - return false; - } - try - { - ixx = boost::lexical_cast(inertia_xml->Attribute("ixx")); - ixy = boost::lexical_cast(inertia_xml->Attribute("ixy")); - ixz = boost::lexical_cast(inertia_xml->Attribute("ixz")); - iyy = boost::lexical_cast(inertia_xml->Attribute("iyy")); - iyz = boost::lexical_cast(inertia_xml->Attribute("iyz")); - izz = boost::lexical_cast(inertia_xml->Attribute("izz")); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("one of the inertia elements: ixx (%s) ixy (%s) ixz (%s) iyy (%s) iyz (%s) izz (%s) is not a valid double", - inertia_xml->Attribute("ixx"), - inertia_xml->Attribute("ixy"), - inertia_xml->Attribute("ixz"), - inertia_xml->Attribute("iyy"), - inertia_xml->Attribute("iyz"), - inertia_xml->Attribute("izz")); - return false; - } - - return true; -} - -bool Visual::initXml(TiXmlElement *config) -{ - this->clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (!o) - { - ROS_DEBUG("Origin tag not present for visual element, using default (Identity)"); - this->origin.clear(); - } - else if (!this->origin.initXml(o)) - { - ROS_ERROR("Visual has a malformed origin tag"); - this->origin.clear(); - return false; - } - - // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); - geometry = parseGeometry(geom); - if (!geometry) - { - ROS_ERROR("Malformed geometry for Visual element"); - return false; - } - - // Material - TiXmlElement *mat = config->FirstChildElement("material"); - if (!mat) - { - ROS_DEBUG("visual element has no material tag."); - } - else - { - // get material name - if (!mat->Attribute("name")) - { - ROS_ERROR("Visual material must contain a name attribute"); - return false; - } - this->material_name = mat->Attribute("name"); - - // try to parse material element in place - this->material.reset(new Material); - if (!this->material->initXml(mat)) - { - ROS_DEBUG("Could not parse material element in Visual block, maybe defined outside."); - this->material.reset(); - } - else - { - ROS_DEBUG("Parsed material element in Visual block."); - } - } - - // Group Tag (optional) - // collision blocks without a group tag are designated to the "default" group - const char *group_name_char = config->Attribute("group"); - if (!group_name_char) - group_name = std::string("default"); - else - group_name = std::string(group_name_char); - - return true; -} - -bool Collision::initXml(TiXmlElement* config) -{ - this->clear(); - - // Origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (!o) - { - ROS_DEBUG("Origin tag not present for collision element, using default (Identity)"); - this->origin.clear(); - } - else if (!this->origin.initXml(o)) - { - ROS_ERROR("Collision has a malformed origin tag"); - this->origin.clear(); - return false; - } - - // Geometry - TiXmlElement *geom = config->FirstChildElement("geometry"); - geometry = parseGeometry(geom); - if (!geometry) - { - ROS_ERROR("Malformed geometry for Collision element"); - return false; - } - - // Group Tag (optional) - // collision blocks without a group tag are designated to the "default" group - const char *group_name_char = config->Attribute("group"); - if (!group_name_char) - group_name = std::string("default"); - else - group_name = std::string(group_name_char); - - return true; -} - -bool Sphere::initXml(TiXmlElement *c) -{ - this->clear(); - - this->type = SPHERE; - if (!c->Attribute("radius")) - { - ROS_ERROR("Sphere shape must have a radius attribute"); - return false; - } - - try - { - radius = boost::lexical_cast(c->Attribute("radius")); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius")); - return false; - } - - return true; -} - -bool Box::initXml(TiXmlElement *c) -{ - this->clear(); - - this->type = BOX; - if (!c->Attribute("size")) - { - ROS_ERROR("Box shape has no size attribute"); - return false; - } - if (!dim.init(c->Attribute("size"))) - { - ROS_ERROR("Box shape has malformed size attribute"); - dim.clear(); - return false; - } - return true; -} - -bool Cylinder::initXml(TiXmlElement *c) -{ - this->clear(); - - this->type = CYLINDER; - if (!c->Attribute("length") || - !c->Attribute("radius")) - { - ROS_ERROR("Cylinder shape must have both length and radius attributes"); - return false; - } - - try - { - length = boost::lexical_cast(c->Attribute("length")); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("length (%s) is not a valid float",c->Attribute("length")); - return false; - } - - try - { - radius = boost::lexical_cast(c->Attribute("radius")); - } - catch (boost::bad_lexical_cast &e) - { - ROS_ERROR("radius (%s) is not a valid float",c->Attribute("radius")); - return false; - } - - return true; -} - - -bool Mesh::initXml(TiXmlElement *c) -{ - this->clear(); - - this->type = MESH; - if (!c->Attribute("filename")) - { - ROS_ERROR("Mesh must contain a filename attribute"); - return false; - } - - filename = c->Attribute("filename"); - - if (c->Attribute("scale")) - { - if (!this->scale.init(c->Attribute("scale"))) - { - ROS_ERROR("Mesh scale was specified, but could not be parsed"); - this->scale.clear(); - return false; - } - } - else - ROS_DEBUG("Mesh scale was not specified, default to (1,1,1)"); - - return true; -} - - -bool Link::initXml(TiXmlElement* config) -{ - this->clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - ROS_ERROR("No name given for the link."); - return false; - } - name = std::string(name_char); - - // Inertial (optional) - TiXmlElement *i = config->FirstChildElement("inertial"); - if (i) - { - inertial.reset(new Inertial); - if (!inertial->initXml(i)) - { - ROS_ERROR("Could not parse inertial element for Link '%s'", this->name.c_str()); - return false; - } - } - - // Multiple Visuals (optional) - for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual")) - { - boost::shared_ptr vis; - vis.reset(new Visual); - - if (vis->initXml(vis_xml)) - { - boost::shared_ptr > > viss = this->getVisuals(vis->group_name); - if (!viss) - { - // group does not exist, create one and add to map - viss.reset(new std::vector >); - // new group name, create vector, add vector to map and add Visual to the vector - this->visual_groups.insert(make_pair(vis->group_name,viss)); - ROS_DEBUG("successfully added a new visual group name '%s'",vis->group_name.c_str()); - } - - // group exists, add Visual to the vector in the map - viss->push_back(vis); - ROS_DEBUG("successfully added a new visual under group name '%s'",vis->group_name.c_str()); - } - else - { - ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str()); - vis.reset(); - return false; - } - } - // Visual (optional) - // Assign one single default visual pointer from the visual_groups map - this->visual.reset(); - boost::shared_ptr > > default_visual = this->getVisuals("default"); - if (!default_visual) - { - ROS_DEBUG("No 'default' visual group for Link '%s'", this->name.c_str()); - } - else if (default_visual->empty()) - { - ROS_DEBUG("'default' visual group is empty for Link '%s'", this->name.c_str()); - } - else - { - if (default_visual->size() > 1) - ROS_WARN("'default' visual group has %d visuals for Link '%s', taking the first one as default",(int)default_visual->size(), this->name.c_str()); - this->visual = (*default_visual->begin()); - } - - - - // Multiple Collisions (optional) - for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision")) - { - boost::shared_ptr col; - col.reset(new Collision); - - if (col->initXml(col_xml)) - { - boost::shared_ptr > > cols = this->getCollisions(col->group_name); - if (!cols) - { - // group does not exist, create one and add to map - cols.reset(new std::vector >); - // new group name, create vector, add vector to map and add Collision to the vector - this->collision_groups.insert(make_pair(col->group_name,cols)); - ROS_DEBUG("successfully added a new collision group name '%s'",col->group_name.c_str()); - } - - // group exists, add Collision to the vector in the map - cols->push_back(col); - ROS_DEBUG("successfully added a new collision under group name '%s'",col->group_name.c_str()); - } - else - { - ROS_ERROR("Could not parse collision element for Link '%s'", this->name.c_str()); - col.reset(); - return false; - } - } - - // Collision (optional) - // Assign one single default collision pointer from the collision_groups map - this->collision.reset(); - boost::shared_ptr > > default_collision = this->getCollisions("default"); - if (!default_collision) - { - ROS_DEBUG("No 'default' collision group for Link '%s'", this->name.c_str()); - } - else if (default_collision->empty()) - { - ROS_DEBUG("'default' collision group is empty for Link '%s'", this->name.c_str()); - } - else - { - if (default_collision->size() > 1) - ROS_WARN("'default' collision group has %d collisions for Link '%s', taking the first one as default",(int)default_collision->size(), this->name.c_str()); - this->collision = (*default_collision->begin()); - } - - return true; -} - -void Link::addVisual(std::string group_name, boost::shared_ptr visual) -{ - boost::shared_ptr > > viss = this->getVisuals(group_name); - if (!viss) - { - // group does not exist, create one and add to map - viss.reset(new std::vector >); - // new group name, create vector, add vector to map and add Visual to the vector - this->visual_groups.insert(make_pair(group_name,viss)); - ROS_DEBUG("successfully added a new visual group name '%s'",group_name.c_str()); - } - - // group exists, add Visual to the vector in the map - std::vector >::iterator vis_it = find(viss->begin(),viss->end(),visual); - if (vis_it != viss->end()) - ROS_WARN("attempted to add a visual that already exists under group name '%s', skipping.",group_name.c_str()); - else - viss->push_back(visual); - ROS_DEBUG("successfully added a new visual under group name '%s'",group_name.c_str()); - -} - -boost::shared_ptr > > Link::getVisuals(const std::string& group_name) const -{ - boost::shared_ptr > > ptr; - if (this->visual_groups.find(group_name) == this->visual_groups.end()) - ptr.reset(); - else - ptr = this->visual_groups.find(group_name)->second; - return ptr; -} - - -void Link::addCollision(std::string group_name, boost::shared_ptr collision) -{ - boost::shared_ptr > > viss = this->getCollisions(group_name); - if (!viss) - { - // group does not exist, create one and add to map - viss.reset(new std::vector >); - // new group name, create vector, add vector to map and add Collision to the vector - this->collision_groups.insert(make_pair(group_name,viss)); - ROS_DEBUG("successfully added a new collision group name '%s'",group_name.c_str()); - } - - // group exists, add Collision to the vector in the map - std::vector >::iterator vis_it = find(viss->begin(),viss->end(),collision); - if (vis_it != viss->end()) - ROS_WARN("attempted to add a collision that already exists under group name '%s', skipping.",group_name.c_str()); - else - viss->push_back(collision); - ROS_DEBUG("successfully added a new collision under group name '%s'",group_name.c_str()); - -} - -boost::shared_ptr > > Link::getCollisions(const std::string& group_name) const -{ - boost::shared_ptr > > ptr; - if (this->collision_groups.find(group_name) == this->collision_groups.end()) - ptr.reset(); - else - ptr = this->collision_groups.find(group_name)->second; - return ptr; -} - -void Link::setParent(boost::shared_ptr parent) -{ - this->parent_link_ = parent; - ROS_DEBUG("set parent Link '%s' for Link '%s'", parent->name.c_str(), this->name.c_str()); -} - -void Link::setParentJoint(boost::shared_ptr parent) -{ - this->parent_joint = parent; - ROS_DEBUG("set parent joint '%s' to Link '%s'", parent->name.c_str(), this->name.c_str()); -} - -void Link::addChild(boost::shared_ptr child) -{ - this->child_links.push_back(child); - ROS_DEBUG("added child Link '%s' to Link '%s'", child->name.c_str(), this->name.c_str()); -} - -void Link::addChildJoint(boost::shared_ptr child) -{ - this->child_joints.push_back(child); - ROS_DEBUG("added child Joint '%s' to Link '%s'", child->name.c_str(), this->name.c_str()); -} - - - -} - diff --git a/urdf_parser/src/urdf_parser.cpp b/urdf_parser/src/urdf_parser.cpp deleted file mode 100644 index 0234d87..0000000 --- a/urdf_parser/src/urdf_parser.cpp +++ /dev/null @@ -1,217 +0,0 @@ -/********************************************************************* -* 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/urdf_parser.h" - -namespace urdf{ - - - -boost::shared_ptr parseURDF(const std::string &xml_string) -{ - boost::shared_ptr model(new ModelInterface); - model->clear(); - - TiXmlDocument xml_doc; - xml_doc.Parse(xml_string.c_str()); - - TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); - if (!robot_xml) - { - ROS_ERROR("Could not find the 'robot' element in the xml file"); - model.reset(); - return model; - } - - // Get robot name - const char *name = robot_xml->Attribute("name"); - if (!name) - { - ROS_ERROR("No name given for the robot."); - model.reset(); - return model; - } - model->name_ = std::string(name); - - // Get all Material elements - for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) - { - boost::shared_ptr material; - material.reset(new Material); - - if (material->initXml(material_xml)) - { - if (model->getMaterial(material->name)) - { - ROS_ERROR("material '%s' is not unique.", material->name.c_str()); - material.reset(); - model.reset(); - return model; - } - else - { - model->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(); - model.reset(); - return model; - } - } - - // 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 (model->getLink(link->name)) - { - ROS_ERROR("link '%s' is not unique.", link->name.c_str()); - model.reset(); - return model; - } - 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 (model->getMaterial(link->visual->material_name)) - { - ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); - link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); - } - else - { - if (link->visual->material) - { - ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); - model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); - } - else - { - ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); - model.reset(); - return model; - } - } - } - } - - model->links_.insert(make_pair(link->name,link)); - ROS_DEBUG("successfully added a new link '%s'", link->name.c_str()); - } - } - else - { - ROS_ERROR("link xml is not initialized correctly"); - model.reset(); - return model; - } - } - if (model->links_.empty()){ - ROS_ERROR("No link elements found in urdf file"); - model.reset(); - return model; - } - - // 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 (model->getJoint(joint->name)) - { - ROS_ERROR("joint '%s' is not unique.", joint->name.c_str()); - model.reset(); - return model; - } - else - { - model->joints_.insert(make_pair(joint->name,joint)); - ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str()); - } - } - else - { - ROS_ERROR("joint xml is not initialized correctly"); - model.reset(); - return model; - } - } - - - // 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 (!model->initTree(parent_link_tree)) - { - ROS_ERROR("failed to build tree"); - model.reset(); - return model; - } - - // find the root link - if (!model->initRoot(parent_link_tree)) - { - ROS_ERROR("failed to find root link"); - model.reset(); - return model; - } - - return model; -} - -} - diff --git a/urdf_parser/src/urdf_to_graphiz.cpp b/urdf_parser/src/urdf_to_graphiz.cpp deleted file mode 100644 index 55c0f4d..0000000 --- a/urdf_parser/src/urdf_to_graphiz.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/********************************************************************* -* 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: -* -* * Redstributions 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 "urdf_parser/urdf_parser.h" -#include -#include - -using namespace urdf; -using namespace std; - -void addChildLinkNames(boost::shared_ptr link, ofstream& os) -{ - os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - addChildLinkNames(*child, os); -} - -void addChildJointNames(boost::shared_ptr link, ofstream& os) -{ - double r, p, y; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){ - (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y); - os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name - << "\" [label=\"xyz: " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " " - << "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl; - os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl; - addChildJointNames(*child, os); - } -} - - -void printTree(boost::shared_ptr link, string file) -{ - std::ofstream os; - os.open(file.c_str()); - os << "digraph G {" << endl; - - os << "node [shape=box];" << endl; - addChildLinkNames(link, os); - - os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl; - addChildJointNames(link, os); - - os << "}" << endl; - os.close(); -} - - - -int main(int argc, char** argv) -{ - if (argc != 2){ - std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl; - return -1; - } - - // 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(); - - boost::shared_ptr robot = parseURDF(xml_string); - if (!robot){ - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; - return -1; - } - string output = robot->getName(); - - // print entire tree to file - printTree(robot->getRoot(), output+".gv"); - cout << "Created file " << output << ".gv" << endl; - - string command = "dot -Tpdf "+output+".gv -o "+output+".pdf"; - system(command.c_str()); - cout << "Created file " << output << ".pdf" << endl; - return 0; -} - diff --git a/urdf_parser/test/memtest.cpp b/urdf_parser/test/memtest.cpp deleted file mode 100644 index c16871b..0000000 --- a/urdf_parser/test/memtest.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "urdf_parser/urdf_parser.h" -#include -#include -#include - -int main(int argc, char** argv){ - ros::init(argc, argv, "memtest"); - while (ros::ok()){ - 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::parseURDF(xml_string); - } -}