pax_global_header00006660000000000000000000000064132614164600014515gustar00rootroot0000000000000052 comment=470729633ba0419735603a054fec0b5bbfe442f9 urdf-1.13.1/000077500000000000000000000000001326141646000125405ustar00rootroot00000000000000urdf-1.13.1/.gitignore000066400000000000000000000000151326141646000145240ustar00rootroot00000000000000# VIM .*.sw? urdf-1.13.1/README.md000066400000000000000000000006641326141646000140250ustar00rootroot00000000000000# URDF This contains packages for working with URDF files. See the ROS wiki for API documentation and tutorials. * [`urdf`](http://wiki.ros.org/urdf) * [`urdf_parser_plugin`](http://wiki.ros.org/urdf_parser_plugin) This was originally part of the [`ros/robot_model`](https://github.com/ros/robot_model) repository. It has been moved to this repo as described by [`ros/robot_model#195`](https://github.com/ros/robot_model/issues/195) urdf-1.13.1/urdf/000077500000000000000000000000001326141646000135005ustar00rootroot00000000000000urdf-1.13.1/urdf/CHANGELOG.rst000066400000000000000000000125001326141646000155170ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package urdf ^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.13.1 (2018-04-05) ------------------- * Eliminate a deprecation warning by renaming class_loader.h -> class_loader.hpp (`#16 `_) * Make the pointers const for the new tinyxml2 APIs (`#15 `_) * Contributors: Chris Lalancette, Shane Loretz 1.13.0 (2018-03-19) ------------------- * Add in TinyXML2 versions of the initXml API. Also add in some tests, and add in a deprecation warning for the TinyXML versions of the APIs. * Add some tests for the 'initXml' family of methods. * Small CMake fixups. In particular, make sure we define the correct define when building on WIN32. Also do whitespace cleanup. * Windows compatibility from ROS2. * Style fixes from ROS2 * update links now that this is in its own repo (`#7 `_) * Contributors: Chris Lalancette, Dmitry Rozhkov, Mikael Arguedas 1.12.12 (2017-11-08) -------------------- * Switched to package format 2 and made rostest a test_depend (`#221 `_) * Made rostest a test_depend (`#221 `_) * Added missing dependency on tinyxml (`#213 `_) * Contributors: Chris Lalancette, Mikael Arguedas 1.12.11 (2017-06-27) -------------------- * Shared ptr yakkety (`#207 `_) * Forward declare urdf::Model when urdfdom version is > 0.4 * Add test for upcasting from urdf::ModelSharedPtr to urdf::ModelInterfaceSharedPtr * Contributors: Shane Loretz 1.12.10 (2017-06-24) -------------------- * Change urdf::Model to use std::shared_ptrs in urdfdom > v0.4 (`#206 `_) * Contributors: Dave Coleman 1.12.9 (2017-04-26) ------------------- 1.12.8 (2017-03-27) ------------------- * Allow supplying NodeHandle for initParam (`#168 `_) * Allow supplying NodeHandle for initParam using new function. * fixed missing return statement in previous commit. * add Chris and Shane as maintainers (`#184 `_) * fix missed mandatory -std=c++11 flag (`#181 `_) collada_parser,kdl_parser,urdf: add c++11 flag, collada_parser: replace typeof with ansi __typeof\_\_ builded/tested on gentoo Thanks den4ix for the contribution! * Contributors: Denis Romanchuk, Piyush Khandelwal, William Woodall 1.12.7 (2017-01-26) ------------------- 1.12.6 (2017-01-04) ------------------- * Addressed gcc6 build error in the urdf package, forward port of `#156 `_ (`#173 `_) * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) * Contributors: Jochen Sprickerhof, William Woodall 1.12.5 (2016-10-27) ------------------- * Added urdf_compatibility.h header to define SharedPtr types (`#160 `_) This provides portability for downstream packages allowing them to use urdfdom 0.3 or 0.4. * urdf: Explicitly cast shared_ptr to bool in unit test. (`#158 `_) * Add smart ptr typedefs (`#153 `_) * Addressed gcc6 build error in urdf which was related to use of the isystem flag (`#157 `_) * Remove unneeded dependency on libpcrecpp (`#155 `_) * Contributors: Bence Magyar, Jochen Sprickerhof, Lukas Bulwahn, Maarten de Vries, Robert Haschke 1.12.4 (2016-08-23) ------------------- 1.12.3 (2016-06-10) ------------------- 1.12.2 (2016-04-12) ------------------- 1.12.1 (2016-04-10) ------------------- 1.11.8 (2015-09-11) ------------------- * Removed pcre hack for newer released collada-dom. * Fixed link order of libpcrecpp. * Contributors: Kei Okada 1.11.7 (2015-04-22) ------------------- * Removed the exporting of Boost and pcre as they are not used in the headers, and added TinyXML because it is. * Fixed a bug with pcrecpp on Ubuntu > 13.04. * Contributors: Kei Okada, William Woodall 1.11.6 (2014-11-30) ------------------- * Add install for static libs needed for Android cross-compilation * Contributors: Gary Servin 1.11.5 (2014-07-24) ------------------- 1.11.4 (2014-07-07) ------------------- * moving to new dependency for urdfdom and urdfdom_headers. https://github.com/ros/rosdistro/issues/4633 * Contributors: Tully Foote 1.11.3 (2014-06-24) ------------------- * fix urdfdom_headers find_package re `ros/rosdistro#4633 `_ * Contributors: Tully Foote 1.11.2 (2014-03-22) ------------------- 1.11.1 (2014-03-20) ------------------- 1.11.0 (2014-02-21) ------------------- * fix urdf files for test * fix test at urdf * Contributors: YoheiKakiuchi 1.10.18 (2013-12-04) -------------------- * add DEPENDS for kdl_parser * Contributors: Ioan Sucan 1.10.16 (2013-11-18) -------------------- * check for CATKIN_ENABLE_TESTING * fix for using collada_parser_plugin 1.10.15 (2013-08-17) -------------------- * fix `#30 `_ urdf-1.13.1/urdf/CMakeLists.txt000066400000000000000000000056251326141646000162500ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(urdf) find_package(Boost REQUIRED thread) find_package(urdfdom REQUIRED) find_package(urdfdom_headers REQUIRED) find_package(catkin REQUIRED COMPONENTS urdf_parser_plugin pluginlib rosconsole_bridge roscpp cmake_modules) find_package(TinyXML REQUIRED) find_package(TinyXML2 REQUIRED) # Find version components if(NOT urdfdom_headers_VERSION) set(urdfdom_headers_VERSION "0.0.0") endif() string(REGEX REPLACE "^([0-9]+).*" "\\1" URDFDOM_HEADERS_MAJOR_VERSION "${urdfdom_headers_VERSION}") string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" URDFDOM_HEADERS_MINOR_VERSION "${urdfdom_headers_VERSION}") string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" URDFDOM_HEADERS_REVISION_VERSION "${urdfdom_headers_VERSION}") set(generated_compat_header "${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/urdfdom_compatibility.h") include_directories("${CATKIN_DEVEL_PREFIX}/include") configure_file(urdfdom_compatibility.h.in "${generated_compat_header}" @ONLY) add_compile_options(-std=c++11) catkin_package( LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include ${CATKIN_DEVEL_PREFIX}/include CATKIN_DEPENDS rosconsole_bridge roscpp DEPENDS urdfdom_headers urdfdom Boost TinyXML TinyXML2 ) install(FILES ${generated_compat_header} DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) include_directories( include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${urdfdom_INCLUDE_DIRS} ${urdfdom_headers_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS} ${TinyXML2_INCLUDE_DIRS} ) link_directories(${Boost_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS}) add_library(${PROJECT_NAME} src/model.cpp src/rosconsole_bridge.cpp) target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${TinyXML2_LIBRARIES} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES}) if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "URDF_BUILDING_LIBRARY") endif() if(APPLE) set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") endif(APPLE) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest) add_rostest_gtest(test_urdf_parser test/test_robot_model_parser.launch test/test_robot_model_parser.cpp) target_link_libraries(test_urdf_parser ${PROJECT_NAME}) catkin_add_gtest(urdfdom_compatibility_test test/urdfdom_compatibility.cpp) target_link_libraries(urdfdom_compatibility_test ${PROJECT_NAME}) set_source_files_properties(test/test_model_parser_initxml.cpp PROPERTIES COMPILE_FLAGS -Wno-deprecated-declarations) catkin_add_gtest(test_model_parser_initxml test/test_model_parser_initxml.cpp) target_link_libraries(test_model_parser_initxml ${PROJECT_NAME}) endif() install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) urdf-1.13.1/urdf/include/000077500000000000000000000000001326141646000151235ustar00rootroot00000000000000urdf-1.13.1/urdf/include/urdf/000077500000000000000000000000001326141646000160635ustar00rootroot00000000000000urdf-1.13.1/urdf/include/urdf/model.h000066400000000000000000000065111326141646000173370ustar00rootroot00000000000000/********************************************************************* * 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__MODEL_H_ #define URDF__MODEL_H_ #include #include #include #include #include #include #include "urdf/visibility_control.hpp" namespace urdf { class Model : public ModelInterface { public: /// \brief Load Model from TiXMLElement URDF_EXPORT URDF_DEPRECATED("TinyXML API is deprecated, use the TinyXML2 version instead") bool initXml(TiXmlElement * xml); /// \brief Load Model from TiXMLDocument URDF_EXPORT URDF_DEPRECATED("TinyXML API is deprecated, use the TinyXML2 version instead") bool initXml(TiXmlDocument * xml); /// \brief Load Model from tinyxml2::XMLElement URDF_EXPORT bool initXml(const tinyxml2::XMLElement *xml); /// \brief Load Model from tinyxml2::XMLDocument URDF_EXPORT bool initXml(const tinyxml2::XMLDocument *xml); /// \brief Load Model given a filename URDF_EXPORT bool initFile(const std::string & filename); /// \brief Load Model given the name of a parameter on the parameter server URDF_EXPORT bool initParam(const std::string & param); /// \brief Load Model given the name of parameter on parameter server using provided nodehandle URDF_EXPORT bool initParamWithNodeHandle(const std::string & param, const ros::NodeHandle & nh = ros::NodeHandle()); /// \brief Load Model from a XML-string URDF_EXPORT bool initString(const std::string & xmlstring); }; // shared_ptr declarations moved to urdf/urdfdom_compatibility.h to allow for // std::shared_ptrs in latest version } // namespace urdf #endif // URDF__MODEL_H_ urdf-1.13.1/urdf/include/urdf/visibility_control.hpp000066400000000000000000000065001326141646000225240ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2017, Open Source Robotics Foundation, 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 copyright holder 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. *********************************************************************/ /* This header must be included by all urdf headers which declare symbols * which are defined in the urdf library. When not building the urdf * library, i.e. when using the headers in other package's code, the contents * of this header change the visibility of certain symbols which the urdf * library cannot have, but the consuming code must have inorder to link. */ #ifndef URDF__VISIBILITY_CONTROL_HPP_ #define URDF__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ #define URDF_EXPORT __attribute__ ((dllexport)) #define URDF_IMPORT __attribute__ ((dllimport)) #define URDF_DEPRECATED(msg) __attribute__((deprecated(msg))) #else #define URDF_EXPORT __declspec(dllexport) #define URDF_IMPORT __declspec(dllimport) #define URDF_DEPRECATED(msg) __attribute__((deprecated(msg))) #endif #ifdef URDF_BUILDING_LIBRARY #define URDF_PUBLIC URDF_EXPORT #else #define URDF_PUBLIC URDF_IMPORT #endif #define URDF_PUBLIC_TYPE URDF_PUBLIC #define URDF_LOCAL #else #define URDF_EXPORT __attribute__ ((visibility("default"))) #define URDF_IMPORT #define URDF_DEPRECATED(msg) __attribute__((deprecated(msg))) #if __GNUC__ >= 4 #define URDF_PUBLIC __attribute__ ((visibility("default"))) #define URDF_LOCAL __attribute__ ((visibility("hidden"))) #else #define URDF_PUBLIC #define URDF_LOCAL #endif #define URDF_PUBLIC_TYPE #endif #endif // URDF__VISIBILITY_CONTROL_HPP_ urdf-1.13.1/urdf/mainpage.dox000066400000000000000000000077741326141646000160140ustar00rootroot00000000000000/** \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 */ urdf-1.13.1/urdf/package.xml000066400000000000000000000034041326141646000156160ustar00rootroot00000000000000 urdf 1.13.1 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. Ioan Sucan Jackie Kay Chris Lalancette Shane Loretz BSD http://ros.org/wiki/urdf https://github.com/ros/urdf https://github.com/ros/urdf/issues catkin liburdfdom-dev liburdfdom-headers-dev rosconsole_bridge roscpp urdf_parser_plugin pluginlib cmake_modules tinyxml tinyxml2 liburdfdom-dev rosconsole_bridge roscpp pluginlib tinyxml tinyxml2 tinyxml tinyxml2 liburdfdom-headers-dev rostest urdf-1.13.1/urdf/src/000077500000000000000000000000001326141646000142675ustar00rootroot00000000000000urdf-1.13.1/urdf/src/model.cpp000066400000000000000000000145561326141646000161060ustar00rootroot00000000000000/********************************************************************* * 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 "urdf/model.h" /* we include the default parser for plain URDF files; other parsers are loaded via plugins (if available) */ #include #include #include #include #include #include #include #include namespace urdf { static bool IsColladaData(const std::string & data) { return data.find("Print(&printer); std::string str(printer.CStr()); return Model::initString(str); } bool Model::initXml(const tinyxml2::XMLElement *robot_xml) { if (!robot_xml) { ROS_ERROR("Could not parse the xml element"); return false; } std::stringstream ss; tinyxml2::XMLPrinter printer; robot_xml->Accept(&printer); ss << printer.CStr(); return Model::initString(ss.str()); } bool Model::initString(const std::string & xml_string) { urdf::ModelInterfaceSharedPtr model; // necessary for COLLADA compatibility if (IsColladaData(xml_string)) { ROS_DEBUG("Parsing robot collada xml string"); static boost::mutex PARSER_PLUGIN_LOCK; static boost::scoped_ptr > PARSER_PLUGIN_LOADER; boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK); try { if (!PARSER_PLUGIN_LOADER) { PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader("urdf_parser_plugin", "urdf::URDFParser")); } const std::vector &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses(); bool found = false; for (std::size_t i = 0 ; i < classes.size() ; ++i) { if (classes[i].find("urdf/ColladaURDFParser") != std::string::npos) { boost::shared_ptr instance = PARSER_PLUGIN_LOADER->createInstance(classes[i]); if (instance) { model = instance->parse(xml_string); } found = true; break; } } if (!found) { ROS_ERROR_STREAM("No URDF parser plugin found for Collada files. Did you install the corresponding package?"); } } catch(pluginlib::PluginlibException& ex) { ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what() << ". Will not parse Collada file."); } } else { ROS_DEBUG("Parsing robot urdf xml string"); model = parseURDF(xml_string); } // copy data from model into this object if (model) { this->links_ = model->links_; this->joints_ = model->joints_; this->materials_ = model->materials_; this->name_ = model->name_; this->root_link_ = model->root_link_; return true; } return false; } } // namespace urdf urdf-1.13.1/urdf/src/rosconsole_bridge.cpp000066400000000000000000000034561326141646000205050ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2011, 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. *********************************************************************/ #include REGISTER_ROSCONSOLE_BRIDGE; urdf-1.13.1/urdf/test/000077500000000000000000000000001326141646000144575ustar00rootroot00000000000000urdf-1.13.1/urdf/test/fail_pr2_desc_bracket.urdf000066400000000000000000000013741326141646000215350ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/fail_pr2_desc_double.urdf000066400000000000000000000027051326141646000213730ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/fail_pr2_desc_double_joint.urdf000066400000000000000000000027211326141646000225740ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/fail_pr2_desc_loop.urdf000066400000000000000000000026671326141646000211010ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/fail_pr2_desc_no_filename_in_mesh.urdf000066400000000000000000004250031326141646000240770ustar00rootroot00000000000000 true 1000.0 true 1000.0 true 1.0 5 -10.0 1.0 10.0 1200000.0 diagnostic battery_state self_test true 1000.0 -75.0676691729 fl_caster_l_wheel_link_geom 100.0 true 100.0 fl_caster_l_wheel_bumper 75.0676691729 fl_caster_r_wheel_link_geom 100.0 true 100.0 fl_caster_r_wheel_bumper -75.0676691729 -75.0676691729 fr_caster_l_wheel_link_geom 100.0 true 100.0 fr_caster_l_wheel_bumper 75.0676691729 fr_caster_r_wheel_link_geom 100.0 true 100.0 fr_caster_r_wheel_bumper -75.0676691729 -75.0676691729 bl_caster_l_wheel_link_geom 100.0 true 100.0 bl_caster_l_wheel_bumper 75.0676691729 bl_caster_r_wheel_link_geom 100.0 true 100.0 bl_caster_r_wheel_bumper -75.0676691729 -75.0676691729 br_caster_l_wheel_link_geom 100.0 true 100.0 br_caster_l_wheel_bumper 75.0676691729 br_caster_r_wheel_link_geom 100.0 true 100.0 br_caster_r_wheel_bumper -75.0676691729 base_link_geom 100.0 true 100.0 base_bumper base_link true 100.0 base_link base_pose_ground_truth 0.01 map 25.7 25.7 0 0 0 0 true 100.0 plug_holder plug_holder_pose_ground_truth 0.01 map 0 0 0 0 0 0 640 640 1 0.0 0.0 0.0 false -135 135 0.05 10.0 0.01 20.0 0.005 true 20.0 base_scan base_laser_link torso_lift_link_geom 100.0 true 100.0 torso_lift_bumper true 100.0 torso_lift_link imu_data 0.01 map 0 0 0 0 0 0 -52143.33 6.0 6.0 R8G8B8 2448 2050 45 0.1 100 20.0 true 20.0 prosilica/cam_info prosilica/image prosilica/image_rect prosilica/cam_info_service prosilica/poll hight_def_optical_frame 1224.5 1224.5 1025.5 2955 0 0 0 0 0 640 480 L8 90 0.1 100 20.0 true 20.0 wide_stereo/left_image wide_stereo_l_stereo_camera_frame 640 480 L8 90 0.1 100 20.0 true 20.0 wide_stereo/right_image wide_stereo_r_stereo_camera_frame true 20.0 wide_stereo_l_sensor wide_stereo_r_sensor wide_stereo/raw_stereo wide_stereo_optical_frame 320 320 240 320 0 0 0 0 0 -0.09 640 480 L8 45 0.1 100 20.0 true 20.0 narrow_stereo/left_image narrow_stereo_l_stereo_camera_frame 640 480 L8 45 0.1 100 20.0 true 20.0 narrow_stereo/right_image narrow_stereo_r_stereo_camera_frame true 20.0 narrow_stereo_l_sensor narrow_stereo_r_sensor narrow_stereo/raw_stereo narrow_stereo_optical_frame 320 320 240 772.55 0 0 0 0 0 -0.09 640 640 1 0.0 0.0 0.0 false -80 80 0.05 10.0 0.01 40.0 0.005 true 40.0 tilt_scan laser_tilt_link 6.0 r_shoulder_pan_link_geom 100.0 true 100.0 r_shoulder_pan_bumper true r_shoulder_lift_link_geom 100.0 true 100.0 r_r_shoulder_lift_bumper true true r_upper_arm_link_geom 100.0 true 100.0 r_upper_arm_bumper true r_elbow_flex_link_geom 100.0 true 100.0 r_elbow_flex_bumper true true true r_forearm_link_geom 100.0 true 100.0 r_forearm_bumper true r_wrist_flex_link_geom 100.0 true 100.0 r_wrist_flex_bumper true r_wrist_roll_link_geom 100.0 true 100.0 r_wrist_roll_bumper 63.16 61.89 32.65 -36.17 90.5142857143 true r_gripper_l_finger_link_geom 100.0 true 100.0 r_gripper_l_finger_bumper true r_gripper_r_finger_link_geom 100.0 true 100.0 r_gripper_r_finger_bumper true r_gripper_l_finger_tip_link_geom 100.0 true 100.0 r_gripper_l_finger_tip_bumper true r_gripper_r_finger_tip_link_geom 100.0 true 100.0 r_gripper_r_finger_tip_bumper true 100.0 r_gripper_l_finger_link r_gripper_l_finger_pose_ground_truth 0.0 map true 100.0 r_gripper_l_finger_link r_gripper_l_finger_force_ground_truth r_gripper_l_finger_link true r_gripper_palm_link_geom 100.0 true 100.0 r_gripper_palm_bumper true r_gripper_l_finger_tip_link r_gripper_float_link r_gripper_l_finger_tip_link 0 1 0 0 0 0 r_gripper_r_finger_tip_link r_gripper_float_link r_gripper_r_finger_tip_link 0 1 0 0 0 0 true 100.0 r_gripper_palm_link r_gripper_palm_pose_ground_truth 0 0 0 0 0 0 0.0 map true 100.0 r_gripper_tool_frame r_gripper_tool_frame_pose_ground_truth 0 0 0 0 0 0 0.0 /map true l_shoulder_pan_link_geom 100.0 true 100.0 l_shoulder_pan_bumper true l_shoulder_lift_link_geom 100.0 true 100.0 l_r_shoulder_lift_bumper true true l_upper_arm_link_geom 100.0 true 100.0 l_upper_arm_bumper true l_elbow_flex_link_geom 100.0 true 100.0 l_elbow_flex_bumper true true true l_forearm_link_geom 100.0 true 100.0 l_forearm_bumper true l_wrist_flex_link_geom 100.0 true 100.0 l_wrist_flex_bumper true l_wrist_roll_link_geom 100.0 true 100.0 l_wrist_roll_bumper 63.16 61.89 32.65 -36.17 90.5142857143 true l_gripper_l_finger_link_geom 100.0 true 100.0 l_gripper_l_finger_bumper true l_gripper_r_finger_link_geom 100.0 true 100.0 l_gripper_r_finger_bumper true l_gripper_l_finger_tip_link_geom 100.0 true 100.0 l_gripper_l_finger_tip_bumper true l_gripper_r_finger_tip_link_geom 100.0 true 100.0 l_gripper_r_finger_tip_bumper true 100.0 l_gripper_l_finger_link l_gripper_l_finger_pose_ground_truth 0.0 map true 100.0 l_gripper_l_finger_link l_gripper_l_finger_force_ground_truth l_gripper_l_finger_link true l_gripper_palm_link_geom 100.0 true 100.0 l_gripper_palm_bumper true l_gripper_l_finger_tip_link l_gripper_float_link l_gripper_l_finger_tip_link 0 1 0 0 0 0 l_gripper_r_finger_tip_link l_gripper_float_link l_gripper_r_finger_tip_link 0 1 0 0 0 0 true 100.0 l_gripper_palm_link l_gripper_palm_pose_ground_truth 0 0 0 0 0 0 0.0 map true 100.0 l_gripper_tool_frame l_gripper_tool_frame_pose_ground_truth 0 0 0 0 0 0 0.0 /map true 640 480 L8 90 0.1 100 20.0 true 20.0 l_forearm_cam/image l_forearm_cam_frame true PR2/Blue true 640 480 L8 90 0.1 100 20.0 true 20.0 r_forearm_cam/image r_forearm_cam_frame true PR2/Blue true urdf-1.13.1/urdf/test/fail_pr2_desc_no_joint2.urdf000066400000000000000000000013351326141646000220200ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/fail_pr2_desc_parent_itself.urdf000066400000000000000000000013361326141646000227570ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/fail_pr2_desc_two_trees.urdf000066400000000000000000000026771326141646000221440ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/fail_three_links_one_joint.urdf000066400000000000000000000011451326141646000227100ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/no_visual.urdf000066400000000000000000000004601326141646000173400ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/one_link.urdf000066400000000000000000000006111326141646000171350ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/pr2_desc.urdf000066400000000000000000003656721326141646000170650ustar00rootroot00000000000000 true 1000.0 true 1.0 5 power_state 10.0 87.78 -474 525 15.52 16.41 640 640 1 0.0 0.0 0.0 false -129.998394137 129.998394137 0.08 10.0 0.01 20 0.005 true 20 base_scan base_laser_link -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 -79.2380952381 79.2380952381 -79.2380952381 true base_link_geom 100.0 true 100.0 base_bumper true 100.0 base_link base_pose_ground_truth 0.01 map 25.7 25.7 0 0 0 0 base_footprint torso_lift_link_geom 100.0 true 100.0 torso_lift_bumper -52143.33 true 100.0 imu_link torso_lift_imu/data 2.89e-08 0 0 0 0 0 0 6.0 6.0 R8G8B8 2448 2050 45 0.1 100 20.0 true 20.0 /prosilica/image_raw /prosilica/camera_info /prosilica/request_image high_def_frame 1224.5 1224.5 1025.5 2955 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 640 480 BAYER_BGGR8 90 0.1 100 25.0 true 25.0 wide_stereo/left/image_raw wide_stereo/left/camera_info wide_stereo_optical_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 BAYER_BGGR8 90 0.1 100 25.0 true 25.0 wide_stereo/right/image_raw wide_stereo/right/camera_info wide_stereo_optical_frame 0.09 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 45 0.1 100 25.0 true 25.0 narrow_stereo/left/image_raw narrow_stereo/left/camera_info narrow_stereo_optical_frame 0 320.5 320.5 240.5 772.55 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 45 0.1 100 25.0 true 25.0 narrow_stereo/right/image_raw narrow_stereo/right/camera_info narrow_stereo_optical_frame 0.09 320.5 320.5 240.5 772.55 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue true 15.0 stereo_projection_pattern_high_res_red.png projector_wg6802418_child_frame stereo_projection_pattern_filter.png projector_wg6802418_controller/image projector_wg6802418_controller/projector 0.785398163397 0.4 10 640 640 1 0.0 0.0 0.0 false -79.9999999086 79.9999999086 0.08 10.0 0.01 40 0.005 true 40 tilt_scan laser_tilt_link -6.05 true 32.6525111499 true true 63.1552452977 61.8948225713 true true -90.5142857143 -1.0 true -36.167452007 true true true true r_gripper_l_finger_link_geom 100.0 true 100.0 r_gripper_l_finger_link r_gripper_l_finger_bumper true r_gripper_r_finger_link_geom 100.0 true r_gripper_r_finger_link 100.0 r_gripper_r_finger_bumper true false r_gripper_l_finger_tip_link_geom 100.0 true r_gripper_l_finger_tip_link 100.0 r_gripper_l_finger_tip_bumper true false r_gripper_r_finger_tip_link_geom 100.0 true r_gripper_r_finger_tip_link 100.0 r_gripper_r_finger_tip_bumper true 100.0 r_gripper_l_finger_link r_gripper_l_finger_pose_ground_truth 0.0 base_link true 100.0 r_gripper_l_finger_link r_gripper_l_finger_force_ground_truth r_gripper_l_finger_link true 0.17126 7.7562e-05 1.49095e-06 -9.83385e-06 0.000197083 -3.06125e-06 0.000181054 0.03598 0.0173 -0.00164 0.82991 -0.157 0.790675 0 -0 0 true false true 0.17389 7.73841e-05 -2.09309e-06 -8.36228e-06 0.000198474 2.4611e-06 0.00018107 0.03576 -0.01736 -0.00095 0.82991 -0.219 0.790675 0 -0 0 true false r_gripper_r_parallel_link r_gripper_palm_link r_gripper_palm_link 0 0 -1 0.2 0.05891 -0.031 0 r_gripper_l_parallel_link r_gripper_palm_link r_gripper_palm_link 0 0 1 0.2 0.05891 0.031 0 r_gripper_r_parallel_link r_gripper_r_finger_tip_link r_gripper_r_finger_tip_link 0 0 1 -0.018 -0.021 0 r_gripper_l_parallel_link r_gripper_l_finger_tip_link r_gripper_l_finger_tip_link 0 0 1 -0.018 0.021 0 r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link r_gripper_r_finger_tip_link 0 1 0 true true true r_gripper_palm_link_geom 100.0 true 100.0 r_gripper_palm_link r_gripper_palm_bumper true 100.0 r_gripper_palm_link r_gripper_palm_pose_ground_truth 0 0 0 0 0 0 0.0 map true 32.6525111499 true true 63.1552452977 61.8948225713 true true -90.5142857143 -1.0 true -36.167452007 true true true true l_gripper_l_finger_link_geom 100.0 true 100.0 l_gripper_l_finger_link l_gripper_l_finger_bumper true l_gripper_r_finger_link_geom 100.0 true l_gripper_r_finger_link 100.0 l_gripper_r_finger_bumper true false l_gripper_l_finger_tip_link_geom 100.0 true l_gripper_l_finger_tip_link 100.0 l_gripper_l_finger_tip_bumper true false l_gripper_r_finger_tip_link_geom 100.0 true l_gripper_r_finger_tip_link 100.0 l_gripper_r_finger_tip_bumper true 100.0 l_gripper_l_finger_link l_gripper_l_finger_pose_ground_truth 0.0 base_link true 100.0 l_gripper_l_finger_link l_gripper_l_finger_force_ground_truth l_gripper_l_finger_link true 0.17126 7.7562e-05 1.49095e-06 -9.83385e-06 0.000197083 -3.06125e-06 0.000181054 0.03598 0.0173 -0.00164 0.82991 0.219 0.790675 0 -0 0 true false true 0.17389 7.73841e-05 -2.09309e-06 -8.36228e-06 0.000198474 2.4611e-06 0.00018107 0.03576 -0.01736 -0.00095 0.82991 0.157 0.790675 0 -0 0 true false l_gripper_r_parallel_link l_gripper_palm_link l_gripper_palm_link 0 0 -1 0.2 0.05891 -0.031 0 l_gripper_l_parallel_link l_gripper_palm_link l_gripper_palm_link 0 0 1 0.2 0.05891 0.031 0 l_gripper_r_parallel_link l_gripper_r_finger_tip_link l_gripper_r_finger_tip_link 0 0 1 -0.018 -0.021 0 l_gripper_l_parallel_link l_gripper_l_finger_tip_link l_gripper_l_finger_tip_link 0 0 1 -0.018 0.021 0 l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link l_gripper_r_finger_tip_link 0 1 0 true true true l_gripper_palm_link_geom 100.0 true 100.0 l_gripper_palm_link l_gripper_palm_bumper true 100.0 l_gripper_palm_link l_gripper_palm_pose_ground_truth 0 0 0 0 0 0 0.0 map 640 480 L8 90 0.1 100 25.0 true 25.0 l_forearm_cam/image_raw l_forearm_cam/camera_info l_forearm_cam_optical_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue 640 480 L8 90 0.1 100 25.0 true 25.0 r_forearm_cam/image_raw r_forearm_cam/camera_info r_forearm_cam_optical_frame 0 320.5 320.5 240.5 320 0.00000001 0.00000001 0.00000001 0.00000001 0.00000001 true PR2/Blue urdf-1.13.1/urdf/test/pr2_desc_no_joint.urdf000066400000000000000000000005411326141646000207410ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/singularity.urdf000066400000000000000000000015571326141646000177230ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/test_model_parser_initxml.cpp000066400000000000000000000073651326141646000224550ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2018, Open Source Robotics Foundation * 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. *********************************************************************/ #include #include "urdf/model.h" static const std::string good_robot{"" " " " " ""}; TEST(model_parser_initxml, initstring_bad_xml) { const std::string robot{""}; urdf::Model model; ASSERT_FALSE(model.initString(robot)); } TEST(model_parser_initxml, initstring_good) { urdf::Model model; ASSERT_TRUE(model.initString(good_robot)); } TEST(model_parser_initxml, initxml_tinyxml_element_bad) { urdf::Model model; ASSERT_FALSE(model.initXml(reinterpret_cast(NULL))); } TEST(model_parser_initxml, initxml_tinyxml_element_good) { TiXmlDocument xml_doc; xml_doc.Parse(good_robot.c_str()); urdf::Model model; ASSERT_TRUE(model.initXml(xml_doc.RootElement())); } TEST(model_parser_initxml, initxml_tinyxml_document_bad) { urdf::Model model; ASSERT_FALSE(model.initXml(reinterpret_cast(NULL))); } TEST(model_parser_initxml, initxml_tinyxml_document_good) { TiXmlDocument xml_doc; xml_doc.Parse(good_robot.c_str()); urdf::Model model; ASSERT_TRUE(model.initXml(&xml_doc)); } TEST(model_parser_initxml, initxml_tinyxml2_element_bad) { urdf::Model model; ASSERT_FALSE(model.initXml(reinterpret_cast(NULL))); } TEST(model_parser_initxml, initxml_tinyxml2_element_good) { tinyxml2::XMLDocument xml_doc; xml_doc.Parse(good_robot.c_str()); urdf::Model model; ASSERT_TRUE(model.initXml(xml_doc.RootElement())); } TEST(model_parser_initxml, initxml_tinyxml2_document_bad) { urdf::Model model; ASSERT_FALSE(model.initXml(reinterpret_cast(NULL))); } TEST(model_parser_initxml, initxml_tinyxml2_document_good) { tinyxml2::XMLDocument xml_doc; xml_doc.Parse(good_robot.c_str()); urdf::Model model; ASSERT_TRUE(model.initXml(&xml_doc)); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } urdf-1.13.1/urdf/test/test_robot.urdf000066400000000000000000000256231326141646000175350ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/test_robot_model_parser.cpp000066400000000000000000000120401326141646000221000ustar00rootroot00000000000000/********************************************************************* * 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 "urdf/model.h" // Including ros, just to be able to call ros::init(), to remove unwanted // args from command-line. #include int g_argc; char ** g_argv; class TestParser : public testing::Test { public: bool checkModel(urdf::Model & robot) { // get root link urdf::LinkConstSharedPtr root_link = robot.getRoot(); if (!root_link) { ROS_ERROR("no root link %s", robot.getName().c_str()); return false; } // go through entire tree return this->traverse_tree(root_link); } protected: /// constructor // num_links starts at 1 because traverse_tree doesn't count the root node TestParser() : num_joints(0), num_links(1) { } /// Destructor ~TestParser() { } bool traverse_tree(urdf::LinkConstSharedPtr link, int level = 0) { ROS_INFO("Traversing tree at level %d, link size %lu", level, link->child_links.size()); level += 2; bool retval = true; for (const urdf::LinkSharedPtr & child : link->child_links) { ++num_links; if (child && child->parent_joint) { ++num_joints; // check rpy double roll, pitch, yaw; child->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll, pitch, yaw); if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) { ROS_ERROR("getRPY() returned nan!"); return false; } // recurse down the tree retval &= this->traverse_tree(child, level); } else { ROS_ERROR("root link: %s has a null child!", link->name.c_str()); return false; } } // no more children return retval; } size_t num_joints; size_t num_links; }; TEST_F(TestParser, test) { ASSERT_GE(g_argc, 3); std::string folder = std::string(g_argv[1]) + "/test/"; ROS_INFO("Folder %s", folder.c_str()); std::string file = std::string(g_argv[2]); bool expect_success = (file.substr(0, 5) != "fail_"); urdf::Model robot; ROS_INFO("Parsing file %s, expecting %d", (folder + file).c_str(), expect_success); if (!expect_success) { ASSERT_FALSE(robot.initFile(folder + file)); return; } ASSERT_EQ(g_argc, 7); std::string robot_name = std::string(g_argv[3]); std::string root_name = std::string(g_argv[4]); size_t expected_num_joints = atoi(g_argv[5]); size_t expected_num_links = atoi(g_argv[6]); ASSERT_TRUE(robot.initFile(folder + file)); EXPECT_EQ(robot.getName(), robot_name); urdf::LinkConstSharedPtr root = robot.getRoot(); ASSERT_TRUE(static_cast(root)); EXPECT_EQ(root->name, root_name); ASSERT_TRUE(checkModel(robot)); EXPECT_EQ(num_joints, expected_num_joints); EXPECT_EQ(num_links, expected_num_links); EXPECT_EQ(robot.joints_.size(), expected_num_joints); EXPECT_EQ(robot.links_.size(), expected_num_links); // test reading from parameter server ASSERT_TRUE(robot.initParam("robot_description")); ASSERT_FALSE(robot.initParam("robot_description_wim")); SUCCEED(); } int main(int argc, char ** argv) { // Calling ros::init(), just to remove unwanted args from command-line. ros::init(argc, argv, "test", ros::init_options::AnonymousName); testing::InitGoogleTest(&argc, argv); g_argc = argc; g_argv = argv; return RUN_ALL_TESTS(); } urdf-1.13.1/urdf/test/test_robot_model_parser.launch000066400000000000000000000035601326141646000225770ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/two_links_one_joint.urdf000066400000000000000000000007301326141646000214160ustar00rootroot00000000000000 urdf-1.13.1/urdf/test/urdfdom_compatibility.cpp000066400000000000000000000042341326141646000215570ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2017 Open Source Robotics Foundation * 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. *********************************************************************/ #include #include #include "urdf/model.h" TEST(UrdfCompatibility, UpcastSharedPtr) { urdf::ModelSharedPtr model(new urdf::Model); model->name_ = "UpcastSharedPtr"; urdf::ModelInterfaceSharedPtr interface = model; EXPECT_EQ(std::string("UpcastSharedPtr"), interface->getName()); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } urdf-1.13.1/urdf/urdfdom_compatibility.h.in000066400000000000000000000100071326141646000206450ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2016, CITEC, Bielefeld University * 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. *********************************************************************/ /* Robert Haschke */ #ifndef URDF__URDFDOM_COMPATIBILITY_H_ #define URDF__URDFDOM_COMPATIBILITY_H_ #define URDFDOM_HEADERS_MAJOR_VERSION @URDFDOM_HEADERS_MAJOR_VERSION@ #define URDFDOM_HEADERS_MINOR_VERSION @URDFDOM_HEADERS_MINOR_VERSION@ #define URDFDOM_HEADERS_REVISION_VERSION @URDFDOM_HEADERS_REVISION_VERSION@ // Define shared pointers for urdfdom versions less than 0.4 // Plus define ModelSharedPtr for 0.4 #if URDFDOM_HEADERS_MAJOR_VERSION == 0 && URDFDOM_HEADERS_MINOR_VERSION <= 4 #include #include #define URDF_TYPEDEF_CLASS_POINTER(Class) \ class Class; \ typedef boost::shared_ptr Class##SharedPtr; \ typedef boost::shared_ptr Class##ConstSharedPtr; \ typedef boost::weak_ptr Class##WeakPtr namespace urdf { // These shared_ptrs were added to urdfdom in 0.4.0, // so if urdfdom == 0.4 this is duplicate work URDF_TYPEDEF_CLASS_POINTER(Box); URDF_TYPEDEF_CLASS_POINTER(Collision); URDF_TYPEDEF_CLASS_POINTER(Cylinder); URDF_TYPEDEF_CLASS_POINTER(Geometry); URDF_TYPEDEF_CLASS_POINTER(Inertial); URDF_TYPEDEF_CLASS_POINTER(Joint); URDF_TYPEDEF_CLASS_POINTER(JointCalibration); URDF_TYPEDEF_CLASS_POINTER(JointDynamics); URDF_TYPEDEF_CLASS_POINTER(JointLimits); URDF_TYPEDEF_CLASS_POINTER(JointMimic); URDF_TYPEDEF_CLASS_POINTER(JointSafety); URDF_TYPEDEF_CLASS_POINTER(Link); URDF_TYPEDEF_CLASS_POINTER(Material); URDF_TYPEDEF_CLASS_POINTER(Mesh); URDF_TYPEDEF_CLASS_POINTER(Sphere); URDF_TYPEDEF_CLASS_POINTER(Visual); URDF_TYPEDEF_CLASS_POINTER(ModelInterface); // ModelSharedPtr is the only one that needs to be defined for urdfdom 0.4 URDF_TYPEDEF_CLASS_POINTER(Model); } // namespace urdf #undef URDF_TYPEDEF_CLASS_POINTER #else // urdfdom <= 0.4 #include #include #include namespace urdf { typedef std::shared_ptr ModelInterfaceSharedPtr; typedef std::shared_ptr ModelInterfaceConstSharedPtr; typedef std::weak_ptr ModelInterfaceWeakPtr; // Forward declaration class Model; typedef std::shared_ptr ModelSharedPtr; typedef std::shared_ptr ModelConstSharedPtr; typedef std::weak_ptr ModelWeakPtr; } // namespace urdf #endif // urdfdom > 0.4 #endif // URDF__URDFDOM_COMPATIBILITY_H_ urdf-1.13.1/urdf_parser_plugin/000077500000000000000000000000001326141646000164325ustar00rootroot00000000000000urdf-1.13.1/urdf_parser_plugin/CHANGELOG.rst000066400000000000000000000035361326141646000204620ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package urdf_parser_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.13.1 (2018-04-05) ------------------- 1.13.0 (2018-03-19) ------------------- * update links now that this is in its own repo (`#7 `_) * Contributors: Mikael Arguedas 1.12.12 (2017-11-08) -------------------- 1.12.11 (2017-06-27) -------------------- 1.12.10 (2017-06-24) -------------------- 1.12.9 (2017-04-26) ------------------- 1.12.8 (2017-03-27) ------------------- * add Chris and Shane as maintainers (`#184 `_) * Contributors: William Woodall 1.12.7 (2017-01-26) ------------------- 1.12.6 (2017-01-04) ------------------- * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) * Contributors: Jochen Sprickerhof 1.12.5 (2016-10-27) ------------------- 1.12.4 (2016-08-23) ------------------- 1.12.3 (2016-06-10) ------------------- 1.12.2 (2016-04-12) ------------------- 1.12.1 (2016-04-10) ------------------- 1.11.8 (2015-09-11) ------------------- 1.11.7 (2015-04-22) ------------------- 1.11.6 (2014-11-30) ------------------- 1.11.5 (2014-07-24) ------------------- 1.11.4 (2014-07-07) ------------------- * moving to new dependency for urdfdom and urdfdom_headers. https://github.com/ros/rosdistro/issues/4633 * Contributors: Tully Foote 1.11.3 (2014-06-24) ------------------- * update usage of urdfdom_headers for indigo/trusty * Contributors: William Woodall 1.11.2 (2014-03-22) ------------------- 1.11.1 (2014-03-20) ------------------- 1.11.0 (2014-02-21) ------------------- 1.10.18 (2013-12-04) -------------------- * add DEPENDS for kdl_parser * Contributors: Ioan Sucan 1.10.16 (2013-11-18) -------------------- 1.10.15 (2013-08-17) -------------------- urdf-1.13.1/urdf_parser_plugin/CMakeLists.txt000066400000000000000000000004561326141646000211770ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(urdf_parser_plugin) find_package(catkin REQUIRED) find_package(urdfdom_headers REQUIRED) catkin_package( INCLUDE_DIRS include DEPENDS urdfdom_headers ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) urdf-1.13.1/urdf_parser_plugin/include/000077500000000000000000000000001326141646000200555ustar00rootroot00000000000000urdf-1.13.1/urdf_parser_plugin/include/urdf_parser_plugin/000077500000000000000000000000001326141646000237475ustar00rootroot00000000000000urdf-1.13.1/urdf_parser_plugin/include/urdf_parser_plugin/parser.h000066400000000000000000000041771326141646000254250ustar00rootroot00000000000000/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2013, 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: Ioan Sucan */ #ifndef URDF_PARSER_PLUGIN_H #define URDF_PARSER_PLUGIN_H #include namespace urdf { /** \brief Base class for URDF parsers */ class URDFParser { public: URDFParser() { } virtual ~URDFParser() { } /// \brief Load Model from string virtual urdf::ModelInterfaceSharedPtr parse(const std::string &xml_string) = 0; }; } #endif urdf-1.13.1/urdf_parser_plugin/package.xml000066400000000000000000000014511326141646000205500ustar00rootroot00000000000000 urdf_parser_plugin 1.13.1 This package contains a C++ base class for URDF parsers. Ioan Sucan Jackie Kay Chris Lalancette Shane Loretz BSD http://ros.org/wiki/urdf https://github.com/ros/urdf https://github.com/ros/urdf/issues catkin liburdfdom-headers-dev liburdfdom-headers-dev