diff --git a/CMakeLists.txt b/CMakeLists.txt index 28105dd..5ae2089 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.4.6) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +set(ROSPACK_MAKEDIST true) + # Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of # directories (or patterns, but directories should suffice) that should # be excluded from the distro. This is not the place to put things that @@ -14,4 +16,4 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # variables. #list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) -rosbuild_make_distribution(0.1.0) +rosbuild_make_distribution(1.3.2) diff --git a/arm_kinematics/arm_kinematics.cpp b/arm_kinematics/arm_kinematics.cpp deleted file mode 100644 index 375f99e..0000000 --- a/arm_kinematics/arm_kinematics.cpp +++ /dev/null @@ -1,362 +0,0 @@ -// bsd license blah blah -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -using std::string; - -static const std::string IK_SERVICE = "get_ik"; -static const std::string FK_SERVICE = "get_fk"; -static const std::string IK_INFO_SERVICE = "get_ik_solver_info"; -static const std::string FK_INFO_SERVICE = "get_fk_solver_info"; - -class Kinematics { - public: - Kinematics(); - bool init(); - - private: - ros::NodeHandle nh, nh_private; - std::string root_name, tip_name; - KDL::JntArray joint_min, joint_max; - KDL::Chain chain; - unsigned int num_joints; - - KDL::ChainFkSolverPos_recursive* fk_solver; - KDL::ChainIkSolverPos_NR_JL *ik_solver_pos; - KDL::ChainIkSolverVel_pinv* ik_solver_vel; - - ros::ServiceServer ik_service,ik_solver_info_service; - ros::ServiceServer fk_service,fk_solver_info_service; - - tf::TransformListener tf_listener; - - kinematics_msgs::KinematicSolverInfo info; - - bool loadModel(const std::string xml); - bool readJoints(urdf::Model &robot_model); - int getJointIndex(const std::string &name); - int getKDLSegmentIndex(const std::string &name); - - /** - * @brief This is the basic IK service method that will compute and return an IK solution. - * @param A request message. See service definition for GetPositionIK for more information on this message. - * @param The response message. See service definition for GetPositionIK for more information on this message. - */ - bool getPositionIK(kinematics_msgs::GetPositionIK::Request &request, - kinematics_msgs::GetPositionIK::Response &response); - - /** - * @brief This is the basic kinematics info service that will return information about the kinematics node. - * @param A request message. See service definition for GetKinematicSolverInfo for more information on this message. - * @param The response message. See service definition for GetKinematicSolverInfo for more information on this message. - */ - bool getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, - kinematics_msgs::GetKinematicSolverInfo::Response &response); - - /** - * @brief This is the basic kinematics info service that will return information about the kinematics node. - * @param A request message. See service definition for GetKinematicSolverInfo for more information on this message. - * @param The response message. See service definition for GetKinematicSolverInfo for more information on this message. - */ - bool getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, - kinematics_msgs::GetKinematicSolverInfo::Response &response); - - /** - * @brief This is the basic forward kinematics service that will return information about the kinematics node. - * @param A request message. See service definition for GetPositionFK for more information on this message. - * @param The response message. See service definition for GetPositionFK for more information on this message. - */ - bool getPositionFK(kinematics_msgs::GetPositionFK::Request &request, - kinematics_msgs::GetPositionFK::Response &response); -}; - - - -Kinematics::Kinematics(): nh_private ("~") { -} - -bool Kinematics::init() { - // Get URDF XML - std::string urdf_xml, full_urdf_xml; - nh.param("urdf_xml",urdf_xml,std::string("robot_description")); - nh.searchParam(urdf_xml,full_urdf_xml); - ROS_DEBUG("Reading xml file from parameter server"); - std::string result; - if (!nh.getParam(full_urdf_xml, result)) { - ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str()); - return false; - } - - // Get Root and Tip From Parameter Service - if (!nh_private.getParam("root_name", root_name)) { - ROS_FATAL("GenericIK: No root name found on parameter server"); - return false; - } - if (!nh_private.getParam("tip_name", tip_name)) { - ROS_FATAL("GenericIK: No tip name found on parameter server"); - return false; - } - - // Load and Read Models - if (!loadModel(result)) { - ROS_FATAL("Could not load models!"); - return false; - } - - // Get Solver Parameters - int maxIterations; - double epsilon; - - nh_private.param("maxIterations", maxIterations, 1000); - nh_private.param("epsilon", epsilon, 1e-2); - - // Build Solvers - fk_solver = new KDL::ChainFkSolverPos_recursive(chain); - ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain); - ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max, - *fk_solver, *ik_solver_vel, maxIterations, epsilon); - - ROS_INFO("Advertising services"); - fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this); - ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this); - ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this); - fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this); - - return true; -} - -bool Kinematics::loadModel(const std::string xml) { - urdf::Model robot_model; - KDL::Tree tree; - - if (!robot_model.initString(xml)) { - ROS_FATAL("Could not initialize robot model"); - return -1; - } - if (!kdl_parser::treeFromString(xml, tree)) { - ROS_ERROR("Could not initialize tree object"); - return false; - } - if (!tree.getChain(root_name, tip_name, chain)) { - ROS_ERROR("Could not initialize chain object"); - return false; - } - - if (!readJoints(robot_model)) { - ROS_FATAL("Could not read information about the joints"); - return false; - } - - return true; -} - -bool Kinematics::readJoints(urdf::Model &robot_model) { - num_joints = 0; - // get joint maxs and mins - boost::shared_ptr link = robot_model.getLink(tip_name); - boost::shared_ptr joint; - - while (link && link->name != root_name) { - joint = robot_model.getJoint(link->parent_joint->name); - if (!joint) { - ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str()); - return false; - } - if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { - ROS_INFO( "adding joint: [%s]", joint->name.c_str() ); - num_joints++; - } - link = robot_model.getLink(link->getParent()->name); - } - - joint_min.resize(num_joints); - joint_max.resize(num_joints); - info.joint_names.resize(num_joints); - info.limits.resize(num_joints); - - link = robot_model.getLink(tip_name); - unsigned int i = 0; - while (link && i < num_joints) { - joint = robot_model.getJoint(link->parent_joint->name); - if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { - ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() ); - - float lower, upper; - int hasLimits; - if ( joint->type != urdf::Joint::CONTINUOUS ) { - lower = joint->limits->lower; - upper = joint->limits->upper; - hasLimits = 1; - } else { - lower = -M_PI; - upper = M_PI; - hasLimits = 0; - } - int index = num_joints - i -1; - - joint_min.data[index] = lower; - joint_max.data[index] = upper; - info.joint_names[index] = joint->name; - info.limits[index].joint_name = joint->name; - info.limits[index].has_position_limits = hasLimits; - info.limits[index].min_position = lower; - info.limits[index].max_position = upper; - i++; - } - link = robot_model.getLink(link->getParent()->name); - } - return true; -} - - -int Kinematics::getJointIndex(const std::string &name) { - for (unsigned int i=0; i < info.joint_names.size(); i++) { - if (info.joint_names[i] == name) - return i; - } - return -1; -} - -int Kinematics::getKDLSegmentIndex(const std::string &name) { - int i=0; - while (i < (int)chain.getNrOfSegments()) { - if (chain.getSegment(i).getName() == name) { - return i+1; - } - i++; - } - return -1; -} - -bool Kinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request, - kinematics_msgs::GetPositionIK::Response &response) { - - geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped; - tf::Stamped transform; - tf::Stamped transform_root; - tf::poseStampedMsgToTF( pose_msg_in, transform ); - - //Do the IK - KDL::JntArray jnt_pos_in; - KDL::JntArray jnt_pos_out; - jnt_pos_in.resize(num_joints); - for (unsigned int i=0; i < num_joints; i++) { - int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i]); - if (tmp_index >=0) { - jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i]; - } else { - ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str()); - } - } - - //Convert F to our root_frame - try { - tf_listener.transformPose(root_name, transform, transform_root); - } catch (...) { - ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str()); - response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; - return false; - } - - KDL::Frame F_dest; - tf::TransformTFToKDL(transform_root, F_dest); - - int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out); - - if (ik_valid >= 0) { - response.solution.joint_state.name = info.joint_names; - response.solution.joint_state.position.resize(num_joints); - for (unsigned int i=0; i < num_joints; i++) { - response.solution.joint_state.position[i] = jnt_pos_out(i); - ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i)); - } - response.error_code.val = response.error_code.SUCCESS; - return true; - } else { - ROS_DEBUG("An IK solution could not be found"); - response.error_code.val = response.error_code.NO_IK_SOLUTION; - return true; - } -} - -bool Kinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, - kinematics_msgs::GetKinematicSolverInfo::Response &response) { - response.kinematic_solver_info = info; - return true; -} - -bool Kinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, - kinematics_msgs::GetKinematicSolverInfo::Response &response) { - response.kinematic_solver_info = info; - return true; -} - -bool Kinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request, - kinematics_msgs::GetPositionFK::Response &response) { - KDL::Frame p_out; - KDL::JntArray jnt_pos_in; - geometry_msgs::PoseStamped pose; - tf::Stamped tf_pose; - - jnt_pos_in.resize(num_joints); - for (unsigned int i=0; i < num_joints; i++) { - int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]); - if (tmp_index >=0) - jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i]; - } - - response.pose_stamped.resize(request.fk_link_names.size()); - response.fk_link_names.resize(request.fk_link_names.size()); - - bool valid = true; - for (unsigned int i=0; i < request.fk_link_names.size(); i++) { - int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]); - ROS_DEBUG("End effector index: %d",segmentIndex); - ROS_DEBUG("Chain indices: %d",chain.getNrOfSegments()); - if (fk_solver->JntToCart(jnt_pos_in,p_out,segmentIndex) >=0) { - tf_pose.frame_id_ = root_name; - tf_pose.stamp_ = ros::Time(); - tf::PoseKDLToTF(p_out,tf_pose); - try { - tf_listener.transformPose(request.header.frame_id,tf_pose,tf_pose); - } catch (...) { - ROS_ERROR("Could not transform FK pose to frame: %s",request.header.frame_id.c_str()); - response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; - return false; - } - tf::poseStampedTFToMsg(tf_pose,pose); - response.pose_stamped[i] = pose; - response.fk_link_names[i] = request.fk_link_names[i]; - response.error_code.val = response.error_code.SUCCESS; - } else { - ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str()); - response.error_code.val = response.error_code.NO_FK_SOLUTION; - valid = false; - } - } - return true; -} - -int main(int argc, char **argv) { - ros::init(argc, argv, "arm_kinematics"); - Kinematics k; - if (k.init()<0) { - ROS_ERROR("Could not initialize kinematics node"); - return -1; - } - - ros::spin(); - return 0; -} - diff --git a/arm_kinematics/mainpage.dox b/arm_kinematics/mainpage.dox deleted file mode 100644 index 6f761b9..0000000 --- a/arm_kinematics/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b arm_kinematics is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/arm_kinematics/manifest.xml b/arm_kinematics/manifest.xml deleted file mode 100644 index d1f5b45..0000000 --- a/arm_kinematics/manifest.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - A generic package for computing both forward and backward kinematics for arms. - Developed as an alternative to pr2_arm_kinematics for people not using the PR2. - - David Lu!! - BSD - - http://ros.org/wiki/arm_kinematics - - - - - - - - - - diff --git a/assimp/Makefile b/assimp/Makefile new file mode 100644 index 0000000..02abba6 --- /dev/null +++ b/assimp/Makefile @@ -0,0 +1,36 @@ +all: installed + +PACKAGE_NAME=`rospack find assimp` +# Regular download +TARBALL_NAME = assimp-svn-811 +TARBALL = build/$(TARBALL_NAME).tar.gz +TARBALL_URL = http://pr.willowgarage.com/downloads/$(TARBALL_NAME).tar.gz +TARBALL_PATCH = assimp.patch +UNPACK_CMD = tar xzf +SOURCE_DIR = build/$(TARBALL_NAME) +include $(shell rospack find mk)/download_unpack_build.mk + +ASSIMP_DIR = $(shell rospack find assimp) +BOOST_INCLUDE_DIRS = $(shell rosboost-cfg --include_dirs) +BOOST_LIBRARY_DIRS = $(shell rosboost-cfg --lib_dirs signals,thread) +BOOST_LIBRARIES = $(shell rosboost-cfg --lflags signals,thread) +CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PACKAGE_NAME) \ + -D CMAKE_BUILD_TYPE=RelWithDebInfo \ + -D boost_include_dirs=$(BOOST_INCLUDE_DIRS) \ + -D boost_library_dirs=$(BOOST_LIBRARY_DIRS) \ + -D boost_libraries="$(BOOST_LIBRARIES)" + +installed: $(SOURCE_DIR)/unpacked + cd $(SOURCE_DIR) && mkdir -p build && cd build && cmake $(CMAKE_ARGS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) && make install + mkdir -p $(PACKAGE_NAME)/include/assimp + cp -r $(SOURCE_DIR)/include/* $(PACKAGE_NAME)/include/assimp/ + touch installed + +clean: + -cd $(SVN_DIR)/build && make clean + -rm -f installed + -rm -rf include lib + +wipe: clean + rm -rf build + diff --git a/assimp/assimp.patch b/assimp/assimp.patch new file mode 100644 index 0000000..2e2866d --- /dev/null +++ b/assimp/assimp.patch @@ -0,0 +1,56 @@ +Index: code/CMakeLists.txt +=================================================================== +--- code/CMakeLists.txt (revision 811) ++++ code/CMakeLists.txt (working copy) +@@ -742,6 +742,8 @@ + SOVERSION ${LIBRARY_SOVERSION} + ) + ++target_link_libraries(assimp ${boost_libraries}) ++ + INSTALL( TARGETS assimp DESTINATION ${LIB_INSTALL_DIR} ) + INSTALL( FILES ${PUBLIC_HEADERS} DESTINATION ${INCLUDE_INSTALL_DIR}/assimp ) + INSTALL( FILES ${COMPILER_HEADERS} DESTINATION ${INCLUDE_INSTALL_DIR}/assimp/Compiler ) +Index: CMakeLists.txt +=================================================================== +--- CMakeLists.txt (revision 811) ++++ CMakeLists.txt (working copy) +@@ -25,26 +25,21 @@ + CONFIGURE_FILE( "${CMAKE_SOURCE_DIR}/assimp.pc.in" "${CMAKE_BINARY_DIR}/assimp.pc" @ONLY ) + INSTALL( FILES "${CMAKE_BINARY_DIR}/assimp.pc" DESTINATION ${LIB_INSTALL_DIR}/pkgconfig/ ) + +-# Globally enbale Boost resp. the Boost workaround – it is also needed by the +-# tools which include the Assimp headers. +-IF ( ENABLE_BOOST_WORKAROUND ) +- INCLUDE_DIRECTORIES( BoostWorkaround ) +- ADD_DEFINITIONS( -DASSIMP_BUILD_BOOST_WORKAROUND ) +- MESSAGE( STATUS "Building a non-boost version of Assimp." ) +-ELSE ( ENABLE_BOOST_WORKAROUND ) +- FIND_PACKAGE( Boost 1.35 ) ++SET (boost_include_dirs "" CACHE STRING "Boost include paths. Use this to override automatic detection.") ++SET (boost_library_dirs "" CACHE STRING "Boost library paths. Use this to override automatic detection.") ++SET (boost_libraries "" CACHE STRING "Boost libraries. Use this to override automatic detection.") + +- IF ( NOT Boost_FOUND ) +- MESSAGE( FATAL_ERROR +- "Boost libraries (http://www.boost.org/) not found. " +- "You can build a non-boost version of Assimp with slightly reduced " +- "functionality by specifying -DENABLE_BOOST_WORKAROUND=ON." +- ) +- ENDIF ( NOT Boost_FOUND ) + +- INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIRS} ) +-ENDIF ( ENABLE_BOOST_WORKAROUND ) ++message(status " debug: ${boost_include_dirs} ${boost_libraries}" ) ++INCLUDE_DIRECTORIES( ++ include ++ ${boost_include_dirs} ++) + ++LINK_DIRECTORIES( ++ ${boost_library_dirs} ++) ++ + ADD_SUBDIRECTORY( code/ ) + IF ( WIN32 ) + ADD_SUBDIRECTORY( test/ ) diff --git a/assimp/manifest.xml b/assimp/manifest.xml new file mode 100644 index 0000000..5e88929 --- /dev/null +++ b/assimp/manifest.xml @@ -0,0 +1,25 @@ + + + + Open Asset Import Library (Short name: Assimp) is a free library to import various well-known 3D model formats + into applications. The library has been designed for maximum stability and efficiency. Written in C++, it is + licensed under the BSD license. The API is provided for both C and C++. Wrappers for Python and D are available, + more bindings are in development. + + The library loads models in a straightforward in-memory format that can be easily read and processed by applications. + Various post processing steps can be executed on the imported data to optimize it for a particular purpose. + + + + ASSIMP Development Team + BSD + + http://assimp.sourceforge.net/ + + + + + + + + diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt new file mode 100644 index 0000000..c7daaff --- /dev/null +++ b/collada_urdf/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +set(ROS_BUILD_TYPE Debug) +rosbuild_init() +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + +rosbuild_add_library(collada_urdf src/collada_urdf.cpp) +rosbuild_link_boost(collada_urdf system) + +rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp) +target_link_libraries(urdf_to_collada collada_urdf) + +rosbuild_add_gtest(test_collada_writer test/test_collada_urdf.cpp) +target_link_libraries(test_collada_writer collada_urdf) diff --git a/arm_kinematics/Makefile b/collada_urdf/Makefile similarity index 100% rename from arm_kinematics/Makefile rename to collada_urdf/Makefile diff --git a/collada_urdf/include/collada_urdf/collada_urdf.h b/collada_urdf/include/collada_urdf/collada_urdf.h new file mode 100644 index 0000000..ae7ed8f --- /dev/null +++ b/collada_urdf/include/collada_urdf/collada_urdf.h @@ -0,0 +1,90 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010, 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. +*********************************************************************/ + +/* Authors: Tim Field */ + +#ifndef COLLADA_URDF_COLLADA_URDF_H +#define COLLADA_URDF_COLLADA_URDF_H + +#include +#include +#include +#include "urdf/model.h" + +namespace collada_urdf { + +class ColladaUrdfException : public std::runtime_error +{ +public: + ColladaUrdfException(std::string const& what); +}; + +/** Construct a COLLADA DOM from an URDF file + * \param file The filename from where to read the URDF + * \param dom The resulting COLLADA DOM + * \return true on success, false on failure + */ +bool colladaFromUrdfFile(std::string const& file, boost::shared_ptr& dom); + +/** Construct a COLLADA DOM from a string containing URDF + * \param xml A string containing the XML description of the robot + * \param dom The resulting COLLADA DOM + * \return true on success, false on failure + */ +bool colladaFromUrdfString(std::string const& xml, boost::shared_ptr& dom); + +/** Construct a COLLADA DOM from a TiXmlDocument containing URDF + * \param xml_doc The TiXmlDocument containing URDF + * \param dom The resulting COLLADA DOM + * \return true on success, false on failure + */ +bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr& dom); + +/** Construct a COLLADA DOM from a URDF robot model + * \param robot_model The URDF robot model + * \param dom The resulting COLLADA DOM + * \return true on success, false on failure + */ +bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr& dom); + +/** Write a COLLADA DOM to a file + * \param dom COLLADA DOM to write + * \param file The filename to write the document to + * \return true on success, false on failure + */ +bool colladaToFile(boost::shared_ptr dom, std::string const& file); + +} + +#endif diff --git a/collada_urdf/manifest.xml b/collada_urdf/manifest.xml new file mode 100644 index 0000000..502f9d7 --- /dev/null +++ b/collada_urdf/manifest.xml @@ -0,0 +1,22 @@ + + + + This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents. + + Implements robot-specific COLLADA extensions as defined by + http://openrave.programmingvision.com/index.php/Started:COLLADA + + Tim Field and Rosen Diankov + BSD + + http://ros.org/wiki/collada_urdf + + + + + + + + + + diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp new file mode 100644 index 0000000..208ab9c --- /dev/null +++ b/collada_urdf/src/collada_urdf.cpp @@ -0,0 +1,1332 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010, Willow Garage, Inc., University of Tokyo +* 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. +*********************************************************************/ + +/* Authors: Rosen Diankov, Tim Field */ + +#include "collada_urdf/collada_urdf.h" +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++) +#define FOREACHC FOREACH + +using namespace std; + +namespace collada_urdf { + +/// ResourceIOStream is copied from rviz (BSD, Willow Garage) +class ResourceIOStream : public Assimp::IOStream +{ +public: + ResourceIOStream(const resource_retriever::MemoryResource& res) + : res_(res) + , pos_(res.data.get()) + {} + + ~ResourceIOStream() + {} + + size_t Read(void* buffer, size_t size, size_t count) + { + size_t to_read = size * count; + if (pos_ + to_read > res_.data.get() + res_.size) + { + to_read = res_.size - (pos_ - res_.data.get()); + } + + memcpy(buffer, pos_, to_read); + pos_ += to_read; + + return to_read; + } + + size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; } + + aiReturn Seek( size_t offset, aiOrigin origin) + { + uint8_t* new_pos = 0; + switch (origin) + { + case aiOrigin_SET: + new_pos = res_.data.get() + offset; + break; + case aiOrigin_CUR: + new_pos = pos_ + offset; // TODO is this right? can offset really not be negative + break; + case aiOrigin_END: + new_pos = res_.data.get() + res_.size - offset; // TODO is this right? + break; + default: + ROS_BREAK(); + } + + if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size) + { + return aiReturn_FAILURE; + } + + pos_ = new_pos; + return aiReturn_SUCCESS; + } + + size_t Tell() const + { + return pos_ - res_.data.get(); + } + + size_t FileSize() const + { + return res_.size; + } + + void Flush() {} + +private: + resource_retriever::MemoryResource res_; + uint8_t* pos_; +}; + +/// ResourceIOSystem is copied from rviz (BSD, Willow Garage) +class ResourceIOSystem : public Assimp::IOSystem +{ +public: + ResourceIOSystem() + { + } + + ~ResourceIOSystem() + { + } + + // Check whether a specific file exists + bool Exists(const char* file) const + { + // Ugly -- two retrievals where there should be one (Exists + Open) + // resource_retriever needs a way of checking for existence + // TODO: cache this + resource_retriever::MemoryResource res; + try { + res = retriever_.get(file); + } + catch (resource_retriever::Exception& e) { + return false; + } + + return true; + } + + // Get the path delimiter character we'd like to see + char getOsSeparator() const + { + return '/'; + } + + // ... and finally a method to open a custom stream + Assimp::IOStream* Open(const char* file, const char* mode) + { + ROS_ASSERT(mode == std::string("r") || mode == std::string("rb")); + + // Ugly -- two retrievals where there should be one (Exists + Open) + // resource_retriever needs a way of checking for existence + resource_retriever::MemoryResource res; + try { + res = retriever_.get(file); + } + catch (resource_retriever::Exception& e) { + return 0; + } + + return new ResourceIOStream(res); + } + + void Close(Assimp::IOStream* stream) { delete stream; } + +private: + mutable resource_retriever::Retriever retriever_; +}; + +/** \brief Implements writing urdf::Model objects to a COLLADA DOM. +*/ +class ColladaWriter : public daeErrorHandler +{ +private: + struct SCENE + { + domVisual_sceneRef vscene; + domKinematics_sceneRef kscene; + domPhysics_sceneRef pscene; + domInstance_with_extraRef viscene; + domInstance_kinematics_sceneRef kiscene; + domInstance_with_extraRef piscene; + }; + + struct LINKOUTPUT + { + list > listusedlinks; + list > listprocesseddofs; + daeElementRef plink; + domNodeRef pnode; + }; + + struct kinematics_model_output + { + struct axis_output + { + //axis_output(const string& sid, KinBody::JointConstPtr pjoint, int iaxis) : sid(sid), pjoint(pjoint), iaxis(iaxis) {} + axis_output() : iaxis(0) {} + string sid, nodesid; + boost::shared_ptr pjoint; + int iaxis; + string jointnodesid; + }; + domKinematics_modelRef kmodel; + std::vector vaxissids; + std::vector vlinksids; + }; + + struct axis_sids + { + axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {} + string axissid, valuesid, jointnodesid; + }; + + struct instance_kinematics_model_output + { + domInstance_kinematics_modelRef ikm; + std::vector vaxissids; + boost::shared_ptr kmout; + std::vector > vkinematicsbindings; + }; + + struct instance_articulated_system_output + { + domInstance_articulated_systemRef ias; + std::vector vaxissids; + std::vector vlinksids; + std::vector > vkinematicsbindings; + }; + +public: + ColladaWriter(const urdf::Model& robot, int writeoptions) : _writeoptions(writeoptions), _robot(robot), _dom(NULL) { + daeErrorHandler::setErrorHandler(this); + _collada.reset(new DAE); + _collada->setIOPlugin(NULL); + _collada->setDatabase(NULL); + _importer.SetIOHandler(new ResourceIOSystem()); + } + virtual ~ColladaWriter() {} + + boost::shared_ptr convert() + { + try { + const char* documentName = "urdf_snapshot"; + daeDocument *doc = NULL; + daeInt error = _collada->getDatabase()->insertDocument(documentName, &doc ); // also creates a collada root + if (error != DAE_OK || doc == NULL) { + throw ColladaUrdfException("Failed to create document"); + } + _dom = daeSafeCast(doc->getDomRoot()); + _dom->setAttribute("xmlns:math","http://www.w3.org/1998/Math/MathML"); + + //create the required asset tag + domAssetRef asset = daeSafeCast( _dom->add( COLLADA_ELEMENT_ASSET ) ); + { + // facet becomes owned by locale, so no need to explicitly delete + boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s"); + std::stringstream ss; + ss.imbue(std::locale(ss.getloc(), facet)); + ss << boost::posix_time::second_clock::local_time(); + + domAsset::domCreatedRef created = daeSafeCast( asset->add( COLLADA_ELEMENT_CREATED ) ); + created->setValue(ss.str().c_str()); + domAsset::domModifiedRef modified = daeSafeCast( asset->add( COLLADA_ELEMENT_MODIFIED ) ); + modified->setValue(ss.str().c_str()); + + domAsset::domContributorRef contrib = daeSafeCast( asset->add( COLLADA_TYPE_CONTRIBUTOR ) ); + domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast( contrib->add( COLLADA_ELEMENT_AUTHORING_TOOL ) ); + authoringtool->setValue("URDF Collada Writer"); + + domAsset::domUnitRef units = daeSafeCast( asset->add( COLLADA_ELEMENT_UNIT ) ); + units->setMeter(1); + units->setName("meter"); + + domAsset::domUp_axisRef zup = daeSafeCast( asset->add( COLLADA_ELEMENT_UP_AXIS ) ); + zup->setValue(UP_AXIS_Z_UP); + } + + _globalscene = _dom->getScene(); + if( !_globalscene ) { + _globalscene = daeSafeCast( _dom->add( COLLADA_ELEMENT_SCENE ) ); + } + + _visualScenesLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); + _visualScenesLib->setId("vscenes"); + _geometriesLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); + _geometriesLib->setId("geometries"); + _effectsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_EFFECTS)); + _effectsLib->setId("effects"); + _materialsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_MATERIALS)); + _materialsLib->setId("materials"); + _kinematicsModelsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); + _kinematicsModelsLib->setId("kmodels"); + _articulatedSystemsLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_ARTICULATED_SYSTEMS)); + _articulatedSystemsLib->setId("asystems"); + _kinematicsScenesLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); + _kinematicsScenesLib->setId("kscenes"); + _physicsScenesLib = daeSafeCast(_dom->add(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); + _physicsScenesLib->setId("pscenes"); + domExtraRef pextra_library_sensors = daeSafeCast(_dom->add(COLLADA_ELEMENT_EXTRA)); + pextra_library_sensors->setId("sensors"); + pextra_library_sensors->setType("library_sensors"); + _sensorsLib = daeSafeCast(pextra_library_sensors->add(COLLADA_ELEMENT_TECHNIQUE)); + _sensorsLib->setProfile("OpenRAVE"); ///< documented profile on robot extensions + + _CreateScene(); + _WritePhysics(); + _WriteRobot(); + _WriteBindingsInstance_kinematics_scene(); + return _collada; + } + catch (ColladaUrdfException ex) { + ROS_ERROR("Error converting: %s", ex.what()); + return boost::shared_ptr(); + } + } + +protected: + virtual void handleError(daeString msg) { throw ColladaUrdfException(msg); } + virtual void handleWarning(daeString msg) { std::cerr << "COLLADA DOM warning: " << msg << std::endl; } + + void _CreateScene() + { + // Create visual scene + _scene.vscene = daeSafeCast(_visualScenesLib->add(COLLADA_ELEMENT_VISUAL_SCENE)); + _scene.vscene->setId("vscene"); + _scene.vscene->setName("URDF Visual Scene"); + + // Create kinematics scene + _scene.kscene = daeSafeCast(_kinematicsScenesLib->add(COLLADA_ELEMENT_KINEMATICS_SCENE)); + _scene.kscene->setId("kscene"); + _scene.kscene->setName("URDF Kinematics Scene"); + + // Create physic scene + _scene.pscene = daeSafeCast(_physicsScenesLib->add(COLLADA_ELEMENT_PHYSICS_SCENE)); + _scene.pscene->setId("pscene"); + _scene.pscene->setName("URDF Physics Scene"); + + // Create instance visual scene + _scene.viscene = daeSafeCast(_globalscene->add( COLLADA_ELEMENT_INSTANCE_VISUAL_SCENE )); + _scene.viscene->setUrl( (string("#") + string(_scene.vscene->getID())).c_str() ); + + // Create instance kinematics scene + _scene.kiscene = daeSafeCast(_globalscene->add( COLLADA_ELEMENT_INSTANCE_KINEMATICS_SCENE )); + _scene.kiscene->setUrl( (string("#") + string(_scene.kscene->getID())).c_str() ); + + // Create instance physics scene + _scene.piscene = daeSafeCast(_globalscene->add( COLLADA_ELEMENT_INSTANCE_PHYSICS_SCENE )); + _scene.piscene->setUrl( (string("#") + string(_scene.pscene->getID())).c_str() ); + } + + void _WritePhysics() { + domPhysics_scene::domTechnique_commonRef common = daeSafeCast(_scene.pscene->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + // Create gravity + domTargetable_float3Ref g = daeSafeCast(common->add(COLLADA_ELEMENT_GRAVITY)); + g->getValue().set3 (0,0,0); + } + + /// \brief Write kinematic body in a given scene + void _WriteRobot(int id = 0) + { + ROS_DEBUG_STREAM(str(boost::format("writing robot as instance_articulated_system (%d) %s\n")%id%_robot.getName())); + string asid = _ComputeId(str(boost::format("robot%d")%id)); + string askid = _ComputeId(str(boost::format("%s_kinematics")%asid)); + string asmid = _ComputeId(str(boost::format("%s_motion")%asid)); + string iassid = _ComputeId(str(boost::format("%s_inst")%asmid)); + + domInstance_articulated_systemRef ias = daeSafeCast(_scene.kscene->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM)); + ias->setSid(iassid.c_str()); + ias->setUrl((string("#")+asmid).c_str()); + ias->setName(_robot.getName().c_str()); + + _iasout.reset(new instance_articulated_system_output()); + _iasout->ias = ias; + + // motion info + domArticulated_systemRef articulated_system_motion = daeSafeCast(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM)); + articulated_system_motion->setId(asmid.c_str()); + domMotionRef motion = daeSafeCast(articulated_system_motion->add(COLLADA_ELEMENT_MOTION)); + domMotion_techniqueRef mt = daeSafeCast(motion->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + domInstance_articulated_systemRef ias_motion = daeSafeCast(motion->add(COLLADA_ELEMENT_INSTANCE_ARTICULATED_SYSTEM)); + ias_motion->setUrl(str(boost::format("#%s")%askid).c_str()); + + // kinematics info + domArticulated_systemRef articulated_system_kinematics = daeSafeCast(_articulatedSystemsLib->add(COLLADA_ELEMENT_ARTICULATED_SYSTEM)); + articulated_system_kinematics->setId(askid.c_str()); + domKinematicsRef kinematics = daeSafeCast(articulated_system_kinematics->add(COLLADA_ELEMENT_KINEMATICS)); + domKinematics_techniqueRef kt = daeSafeCast(kinematics->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + + _WriteInstance_kinematics_model(kinematics,askid,id); + + for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) { + string axis_infosid = _ComputeId(str(boost::format("axis_info_inst%d")%idof)); + boost::shared_ptr pjoint = _ikmout->kmout->vaxissids.at(idof).pjoint; + BOOST_ASSERT(_mapjointindices[pjoint] == (int)idof); + //int iaxis = _ikmout->kmout->vaxissids.at(idof).iaxis; + + // Kinematics axis info + domKinematics_axis_infoRef kai = daeSafeCast(kt->add(COLLADA_ELEMENT_AXIS_INFO)); + kai->setAxis(str(boost::format("%s/%s")%_ikmout->kmout->kmodel->getID()%_ikmout->kmout->vaxissids.at(idof).sid).c_str()); + kai->setSid(axis_infosid.c_str()); + bool bactive = !pjoint->mimic; + double flower=0, fupper=0; + if( pjoint->type != urdf::Joint::CONTINUOUS ) { + if( !!pjoint->limits ) { + flower = pjoint->limits->lower; + fupper = pjoint->limits->upper; + } + if( flower == fupper ) { + bactive = false; + } + double fmult = 1.0; + if( pjoint->type != urdf::Joint::PRISMATIC ) { + fmult = 180.0/M_PI; + } + domKinematics_limitsRef plimits = daeSafeCast(kai->add(COLLADA_ELEMENT_LIMITS)); + daeSafeCast(plimits->add(COLLADA_ELEMENT_MIN)->add(COLLADA_ELEMENT_FLOAT))->setValue(flower*fmult); + daeSafeCast(plimits->add(COLLADA_ELEMENT_MAX)->add(COLLADA_ELEMENT_FLOAT))->setValue(fupper*fmult); + } + + domCommon_bool_or_paramRef active = daeSafeCast(kai->add(COLLADA_ELEMENT_ACTIVE)); + daeSafeCast(active->add(COLLADA_ELEMENT_BOOL))->setValue(bactive); + domCommon_bool_or_paramRef locked = daeSafeCast(kai->add(COLLADA_ELEMENT_LOCKED)); + daeSafeCast(locked->add(COLLADA_ELEMENT_BOOL))->setValue(false); + + // Motion axis info + domMotion_axis_infoRef mai = daeSafeCast(mt->add(COLLADA_ELEMENT_AXIS_INFO)); + mai->setAxis(str(boost::format("%s/%s")%askid%axis_infosid).c_str()); + if( !!pjoint->limits ) { + domCommon_float_or_paramRef speed = daeSafeCast(mai->add(COLLADA_ELEMENT_SPEED)); + daeSafeCast(speed->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->velocity); + domCommon_float_or_paramRef accel = daeSafeCast(mai->add(COLLADA_ELEMENT_ACCELERATION)); + daeSafeCast(accel->add(COLLADA_ELEMENT_FLOAT))->setValue(pjoint->limits->effort); + } + } + + // write the bindings + string asmsym = _ComputeId(str(boost::format("%s_%s")%asmid%_ikmout->ikm->getSid())); + string assym = _ComputeId(str(boost::format("%s_%s")%_scene.kscene->getID()%_ikmout->ikm->getSid())); + FOREACH(it, _ikmout->vkinematicsbindings) { + domKinematics_newparamRef abm = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); + abm->setSid(asmsym.c_str()); + daeSafeCast(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%it->first).c_str()); + domKinematics_bindRef ab = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); + ab->setSymbol(assym.c_str()); + daeSafeCast(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s")%asmid%asmsym).c_str()); + _iasout->vkinematicsbindings.push_back(make_pair(string(ab->getSymbol()), it->second)); + } + for(size_t idof = 0; idof < _ikmout->vaxissids.size(); ++idof) { + const axis_sids& kas = _ikmout->vaxissids.at(idof); + domKinematics_newparamRef abm = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); + abm->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.axissid)).c_str()); + daeSafeCast(abm->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.axissid).c_str()); + domKinematics_bindRef ab = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); + ab->setSymbol(str(boost::format("%s_%s")%assym%kas.axissid).c_str()); + daeSafeCast(ab->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.axissid).c_str()); + string valuesid; + if( kas.valuesid.size() > 0 ) { + domKinematics_newparamRef abmvalue = daeSafeCast(ias_motion->add(COLLADA_ELEMENT_NEWPARAM)); + abmvalue->setSid(_ComputeId(str(boost::format("%s_%s")%asmid%kas.valuesid)).c_str()); + daeSafeCast(abmvalue->add(COLLADA_ELEMENT_SIDREF))->setValue(str(boost::format("%s/%s")%askid%kas.valuesid).c_str()); + domKinematics_bindRef abvalue = daeSafeCast(ias->add(COLLADA_ELEMENT_BIND)); + valuesid = _ComputeId(str(boost::format("%s_%s")%assym%kas.valuesid)); + abvalue->setSymbol(valuesid.c_str()); + daeSafeCast(abvalue->add(COLLADA_ELEMENT_PARAM))->setRef(str(boost::format("%s/%s_%s")%asmid%asmid%kas.valuesid).c_str()); + } + _iasout->vaxissids.push_back(axis_sids(ab->getSymbol(),valuesid,kas.jointnodesid)); + } + } + + /// \brief Write kinematic body in a given scene + virtual void _WriteInstance_kinematics_model(daeElementRef parent, const string& sidscope, int id) + { + ROS_DEBUG_STREAM(str(boost::format("writing instance_kinematics_model %s\n")%_robot.getName())); + boost::shared_ptr kmout = WriteKinematics_model(id); + + _ikmout.reset(new instance_kinematics_model_output()); + _ikmout->kmout = kmout; + _ikmout->ikm = daeSafeCast(parent->add(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL)); + + string symscope, refscope; + if( sidscope.size() > 0 ) { + symscope = sidscope+string("_"); + refscope = sidscope+string("/"); + } + string ikmsid = _ComputeId(str(boost::format("%s_inst")%kmout->kmodel->getID())); + _ikmout->ikm->setUrl(str(boost::format("#%s")%kmout->kmodel->getID()).c_str()); + _ikmout->ikm->setSid(ikmsid.c_str()); + + domKinematics_newparamRef kbind = daeSafeCast(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM)); + kbind->setSid(_ComputeId(symscope+ikmsid).c_str()); + daeSafeCast(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid).c_str()); + _ikmout->vkinematicsbindings.push_back(make_pair(string(kbind->getSid()), str(boost::format("visual%d/node0")%id))); + + _ikmout->vaxissids.reserve(kmout->vaxissids.size()); + int i = 0; + FOREACH(it,kmout->vaxissids) { + domKinematics_newparamRef kbind = daeSafeCast(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM)); + string ref = it->sid; + size_t index = ref.find("/"); + while(index != string::npos) { + ref[index] = '.'; + index = ref.find("/",index+1); + } + string sid = _ComputeId(symscope+ikmsid+"_"+ref); + kbind->setSid(sid.c_str()); + daeSafeCast(kbind->add(COLLADA_ELEMENT_SIDREF))->setValue((refscope+ikmsid+"/"+it->sid).c_str()); + double value=0; + double flower=0, fupper=0; + if( !!it->pjoint->limits ) { + flower = it->pjoint->limits->lower; + fupper = it->pjoint->limits->upper; + if( flower > 0 || fupper < 0 ) { + value = 0.5*(flower+fupper); + } + } + + domKinematics_newparamRef pvalueparam = daeSafeCast(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM)); + pvalueparam->setSid((sid+string("_value")).c_str()); + daeSafeCast(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value); + _ikmout->vaxissids.push_back(axis_sids(sid,pvalueparam->getSid(),kmout->vaxissids.at(i).jointnodesid)); + ++i; + } + } + + virtual boost::shared_ptr WriteKinematics_model(int id) + { + domKinematics_modelRef kmodel = daeSafeCast(_kinematicsModelsLib->add(COLLADA_ELEMENT_KINEMATICS_MODEL)); + string kmodelid = _ComputeId(str(boost::format("kmodel%d")%id)); + kmodel->setId(kmodelid.c_str()); + kmodel->setName(_robot.getName().c_str()); + + domKinematics_model_techniqueRef ktec = daeSafeCast(kmodel->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + + // Create root node for the visual scene + domNodeRef pnoderoot = daeSafeCast(_scene.vscene->add(COLLADA_ELEMENT_NODE)); + string bodyid = _ComputeId(str(boost::format("visual%d")%id)); + pnoderoot->setId(bodyid.c_str()); + pnoderoot->setSid(bodyid.c_str()); + pnoderoot->setName(_robot.getName().c_str()); + + // Declare all the joints + _mapjointindices.clear(); + int index=0; + FOREACHC(itj, _robot.joints_) { + _mapjointindices[itj->second] = index++; + } + _maplinkindices.clear(); + index=0; + FOREACHC(itj, _robot.links_) { + _maplinkindices[itj->second] = index++; + } + _mapmaterialindices.clear(); + index=0; + FOREACHC(itj, _robot.materials_) { + _mapmaterialindices[itj->second] = index++; + } + + double lmin, lmax; + vector vdomjoints(_robot.joints_.size()); + boost::shared_ptr kmout(new kinematics_model_output()); + kmout->kmodel = kmodel; + kmout->vaxissids.resize(_robot.joints_.size()); + kmout->vlinksids.resize(_robot.links_.size()); + + FOREACHC(itjoint, _robot.joints_) { + boost::shared_ptr pjoint = itjoint->second; + int index = _mapjointindices[itjoint->second]; + domJointRef pdomjoint = daeSafeCast(ktec->add(COLLADA_ELEMENT_JOINT)); + string jointid = _ComputeId(pjoint->name);//str(boost::format("joint%d")%index); + pdomjoint->setSid(jointid.c_str() ); + pdomjoint->setName(pjoint->name.c_str()); + domAxis_constraintRef axis; + if( !!pjoint->limits ) { + lmin=pjoint->limits->lower; + lmax=pjoint->limits->upper; + } + else { + lmin = lmax = 0; + } + + double fmult = 1.0; + switch(pjoint->type) { + case urdf::Joint::REVOLUTE: + case urdf::Joint::CONTINUOUS: + axis = daeSafeCast(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE)); + fmult = 180.0f/M_PI; + lmin*=fmult; + lmax*=fmult; + break; + case urdf::Joint::PRISMATIC: + axis = daeSafeCast(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC)); + break; + case urdf::Joint::FIXED: + axis = daeSafeCast(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE)); + lmin = 0; + lmax = 0; + fmult = 0; + break; + default: + ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type)); + break; + } + + if( !axis ) { + continue; + } + + int ia = 0; + string axisid = _ComputeId(str(boost::format("axis%d")%ia)); + axis->setSid(axisid.c_str()); + kmout->vaxissids.at(index).pjoint = pjoint; + kmout->vaxissids.at(index).sid = jointid+string("/")+axisid; + kmout->vaxissids.at(index).iaxis = ia; + domAxisRef paxis = daeSafeCast(axis->add(COLLADA_ELEMENT_AXIS)); + paxis->getValue().setCount(3); + paxis->getValue()[0] = pjoint->axis.x; + paxis->getValue()[1] = pjoint->axis.y; + paxis->getValue()[2] = pjoint->axis.z; + if( pjoint->type != urdf::Joint::CONTINUOUS ) { + domJoint_limitsRef plimits = daeSafeCast(axis->add(COLLADA_TYPE_LIMITS)); + daeSafeCast(plimits->add(COLLADA_ELEMENT_MIN))->getValue() = lmin; + daeSafeCast(plimits->add(COLLADA_ELEMENT_MAX))->getValue() = lmax; + } + vdomjoints.at(index) = pdomjoint; + } + + LINKOUTPUT childinfo = _WriteLink(_robot.getRoot(), ktec, pnoderoot, kmodel->getID()); + FOREACHC(itused, childinfo.listusedlinks) { + kmout->vlinksids.at(itused->first) = itused->second; + } + FOREACH(itprocessed,childinfo.listprocesseddofs) { + kmout->vaxissids.at(itprocessed->first).jointnodesid = itprocessed->second; + } + + // create the formulas for all mimic joints + FOREACHC(itjoint, _robot.joints_) { + string jointsid = _ComputeId(itjoint->second->name); + boost::shared_ptr pjoint = itjoint->second; + if( !pjoint->mimic ) { + continue; + } + + domFormulaRef pf = daeSafeCast(ktec->add(COLLADA_ELEMENT_FORMULA)); + string formulaid = _ComputeId(str(boost::format("%s_formula")%jointsid)); + pf->setSid(formulaid.c_str()); + domCommon_float_or_paramRef ptarget = daeSafeCast(pf->add(COLLADA_ELEMENT_TARGET)); + string targetjointid = str(boost::format("%s/%s")%kmodel->getID()%jointsid); + daeSafeCast(ptarget->add(COLLADA_TYPE_PARAM))->setValue(targetjointid.c_str()); + + domTechniqueRef pftec = daeSafeCast(pf->add(COLLADA_ELEMENT_TECHNIQUE)); + pftec->setProfile("OpenRAVE"); + // save position equation + { + daeElementRef poselt = pftec->add("equation"); + poselt->setAttribute("type","position"); + // create a const0*joint+const1 formula + // a x b + daeElementRef pmath_math = poselt->add("math"); + daeElementRef pmath_apply = pmath_math->add("apply"); + { + daeElementRef pmath_plus = pmath_apply->add("plus"); + daeElementRef pmath_apply1 = pmath_apply->add("apply"); + { + daeElementRef pmath_times = pmath_apply1->add("times"); + daeElementRef pmath_const0 = pmath_apply1->add("cn"); + pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); + daeElementRef pmath_symb = pmath_apply1->add("csymbol"); + pmath_symb->setAttribute("encoding","COLLADA"); + pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name))); + } + daeElementRef pmath_const1 = pmath_apply->add("cn"); + pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset)); + } + } + // save first partial derivative + { + daeElementRef derivelt = pftec->add("equation"); + derivelt->setAttribute("type","first_partial"); + derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str()); + daeElementRef pmath_const0 = derivelt->add("cn"); + pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); + } + // save second partial derivative + { + daeElementRef derivelt = pftec->add("equation"); + derivelt->setAttribute("type","second_partial"); + derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str()); + daeElementRef pmath_const0 = derivelt->add("cn"); + pmath_const0->setCharData("0"); + } + + { + domFormula_techniqueRef pfcommontec = daeSafeCast(pf->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + // create a const0*joint+const1 formula + // a x b + daeElementRef pmath_math = pfcommontec->add("math"); + daeElementRef pmath_apply = pmath_math->add("apply"); + { + daeElementRef pmath_plus = pmath_apply->add("plus"); + daeElementRef pmath_apply1 = pmath_apply->add("apply"); + { + daeElementRef pmath_times = pmath_apply1->add("times"); + daeElementRef pmath_const0 = pmath_apply1->add("cn"); + pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); + daeElementRef pmath_symb = pmath_apply1->add("csymbol"); + pmath_symb->setAttribute("encoding","COLLADA"); + pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name))); + } + daeElementRef pmath_const1 = pmath_apply->add("cn"); + pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset)); + } + } + } + + return kmout; + } + + /// \brief Write link of a kinematic body + /// + /// \param link Link to write + /// \param pkinparent Kinbody parent + /// \param pnodeparent Node parent + /// \param strModelUri + virtual LINKOUTPUT _WriteLink(boost::shared_ptr plink, daeElementRef pkinparent, domNodeRef pnodeparent, const string& strModelUri) + { + LINKOUTPUT out; + int linkindex = _maplinkindices[plink]; + string linksid = _ComputeId(plink->name); + domLinkRef pdomlink = daeSafeCast(pkinparent->add(COLLADA_ELEMENT_LINK)); + pdomlink->setName(plink->name.c_str()); + pdomlink->setSid(linksid.c_str()); + + domNodeRef pnode = daeSafeCast(pnodeparent->add(COLLADA_ELEMENT_NODE)); + string nodeid = _ComputeId(str(boost::format("v%s_node%d")%strModelUri%linkindex)); + pnode->setId( nodeid.c_str() ); + string nodesid = _ComputeId(str(boost::format("node%d")%linkindex)); + pnode->setSid(nodesid.c_str()); + pnode->setName(plink->name.c_str()); + + boost::shared_ptr geometry; + boost::shared_ptr material; + urdf::Pose geometry_origin; + if( !!plink->visual ) { + geometry = plink->visual->geometry; + material = plink->visual->material; + geometry_origin = plink->visual->origin; + } + else if( !!plink->collision ) { + geometry = plink->collision->geometry; + geometry_origin = plink->collision->origin; + } + + if( !!geometry ) { + int igeom = 0; + string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); + domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid); + domInstance_geometryRef pinstgeom = daeSafeCast(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); + pinstgeom->setUrl((string("#")+geomid).c_str()); + + // material + _WriteMaterial(pdomgeom->getID(), material); + domBind_materialRef pmat = daeSafeCast(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); + domBind_material::domTechnique_commonRef pmattec = daeSafeCast(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + domInstance_materialRef pinstmat = daeSafeCast(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); + pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat"))); + pinstmat->setSymbol("mat0"); + } + + _WriteTransformation(pnode, geometry_origin); + urdf::Pose geometry_origin_inv = _poseInverse(geometry_origin); + + // process all children + FOREACHC(itjoint, plink->child_joints) { + boost::shared_ptr pjoint = *itjoint; + int index = _mapjointindices[pjoint]; + + // + domLink::domAttachment_fullRef attachment_full = daeSafeCast(pdomlink->add(COLLADA_ELEMENT_ATTACHMENT_FULL)); + string jointid = str(boost::format("%s/%s")%strModelUri%_ComputeId(pjoint->name)); + attachment_full->setJoint(jointid.c_str()); + + LINKOUTPUT childinfo = _WriteLink(_robot.getLink(pjoint->child_link_name), attachment_full, pnode, strModelUri); + FOREACH(itused,childinfo.listusedlinks) { + out.listusedlinks.push_back(make_pair(itused->first,linksid+string("/")+itused->second)); + } + FOREACH(itprocessed,childinfo.listprocesseddofs) { + out.listprocesseddofs.push_back(make_pair(itprocessed->first,nodesid+string("/")+itprocessed->second)); + } + + // rotate/translate elements + string jointnodesid = _ComputeId(str(boost::format("node_%s_axis0")%pjoint->name)); + switch(pjoint->type) { + case urdf::Joint::REVOLUTE: + case urdf::Joint::CONTINUOUS: + case urdf::Joint::FIXED: { + domRotateRef protate = daeSafeCast(childinfo.pnode->add(COLLADA_ELEMENT_ROTATE,0)); + protate->setSid(jointnodesid.c_str()); + protate->getValue().setCount(4); + protate->getValue()[0] = pjoint->axis.x; + protate->getValue()[1] = pjoint->axis.y; + protate->getValue()[2] = pjoint->axis.z; + protate->getValue()[3] = 0; + break; + } + case urdf::Joint::PRISMATIC: { + domTranslateRef ptrans = daeSafeCast(childinfo.pnode->add(COLLADA_ELEMENT_TRANSLATE,0)); + ptrans->setSid(jointnodesid.c_str()); + ptrans->getValue().setCount(3); + ptrans->getValue()[0] = 0; + ptrans->getValue()[1] = 0; + ptrans->getValue()[2] = 0; + break; + } + default: + ROS_WARN_STREAM(str(boost::format("unsupported joint type specified %d")%(int)pjoint->type)); + break; + } + + _WriteTransformation(attachment_full, pjoint->parent_to_joint_origin_transform); + _WriteTransformation(childinfo.pnode, pjoint->parent_to_joint_origin_transform); + _WriteTransformation(childinfo.pnode, geometry_origin_inv); // have to do multiply by inverse since geometry transformation is not part of hierarchy + out.listprocesseddofs.push_back(make_pair(index,string(childinfo.pnode->getSid())+string("/")+jointnodesid)); + // + } + + out.listusedlinks.push_back(make_pair(linkindex,linksid)); + out.plink = pdomlink; + out.pnode = pnode; + return out; + } + + domGeometryRef _WriteGeometry(boost::shared_ptr geometry, const std::string& geometry_id) + { + domGeometryRef cgeometry = daeSafeCast(_geometriesLib->add(COLLADA_ELEMENT_GEOMETRY)); + cgeometry->setId(geometry_id.c_str()); + switch (geometry->type) { + case urdf::Geometry::MESH: { + urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get(); + _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale); + break; + } + case urdf::Geometry::SPHERE: { + ROS_WARN_STREAM(str(boost::format("cannot export sphere geometries yet! %s")%geometry_id)); + break; + } + case urdf::Geometry::BOX: { + ROS_WARN_STREAM(str(boost::format("cannot export box geometries yet! %s")%geometry_id)); + break; + } + case urdf::Geometry::CYLINDER: { + ROS_WARN_STREAM(str(boost::format("cannot export cylinder geometries yet! %s")%geometry_id)); + break; + } + default: { + throw ColladaUrdfException(str(boost::format("undefined geometry type %d, name %s")%(int)geometry->type%geometry_id)); + } + } + return cgeometry; + } + + void _WriteMaterial(const string& geometry_id, boost::shared_ptr material) + { + string effid = geometry_id+string("_eff"); + string matid = geometry_id+string("_mat"); + domMaterialRef pdommat = daeSafeCast(_materialsLib->add(COLLADA_ELEMENT_MATERIAL)); + pdommat->setId(matid.c_str()); + domInstance_effectRef pdominsteff = daeSafeCast(pdommat->add(COLLADA_ELEMENT_INSTANCE_EFFECT)); + pdominsteff->setUrl((string("#")+effid).c_str()); + + urdf::Color ambient, diffuse; + ambient.init("0.1 0.1 0.1 0"); + diffuse.init("1 1 1 0"); + + if( !!material ) { + ambient.r = diffuse.r = material->color.r; + ambient.g = diffuse.g = material->color.g; + ambient.b = diffuse.b = material->color.b; + ambient.a = diffuse.a = material->color.a; + } + + domEffectRef effect = _WriteEffect(geometry_id, ambient, diffuse); + + // + domMaterialRef dommaterial = daeSafeCast(_materialsLib->add(COLLADA_ELEMENT_MATERIAL)); + string material_id = geometry_id + string("_mat"); + dommaterial->setId(material_id.c_str()); + { + // + domInstance_effectRef instance_effect = daeSafeCast(dommaterial->add(COLLADA_ELEMENT_INSTANCE_EFFECT)); + string effect_id(effect->getId()); + instance_effect->setUrl((string("#") + effect_id).c_str()); + } + // + + domEffectRef pdomeff = _WriteEffect(effid, ambient, diffuse); + } + + void _loadMesh(std::string const& filename, domGeometryRef pdomgeom, const urdf::Vector3& scale) + { + const aiScene* scene = _importer.ReadFile(filename, aiProcess_SortByPType|aiProcess_Triangulate);//|aiProcess_GenNormals|aiProcess_GenUVCoords|aiProcess_FlipUVs); + if( !scene ) { + ROS_WARN("failed to load resource %s",filename.c_str()); + return; + } + if( !scene->mRootNode ) { + ROS_WARN("resource %s has no data",filename.c_str()); + return; + } + if (!scene->HasMeshes()) { + ROS_WARN_STREAM(str(boost::format("No meshes found in file %s")%filename)); + return; + } + domMeshRef pdommesh = daeSafeCast(pdomgeom->add(COLLADA_ELEMENT_MESH)); + domSourceRef pvertsource = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_SOURCE)); + domAccessorRef pacc; + domFloat_arrayRef parray; + { + pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str()); + + parray = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY)); + parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str()); + parray->setDigits(6); // 6 decimal places + + domSource::domTechnique_commonRef psourcetec = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + pacc = daeSafeCast(psourcetec->add(COLLADA_ELEMENT_ACCESSOR)); + pacc->setSource(xsAnyURI(*parray, string("#")+string(parray->getID()))); + pacc->setStride(3); + + domParamRef px = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + px->setName("X"); px->setType("float"); + domParamRef py = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + py->setName("Y"); py->setType("float"); + domParamRef pz = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + pz->setName("Z"); pz->setType("float"); + } + domVerticesRef pverts = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_VERTICES)); + { + pverts->setId("vertices"); + domInput_localRef pvertinput = daeSafeCast(pverts->add(COLLADA_ELEMENT_INPUT)); + pvertinput->setSemantic("POSITION"); + pvertinput->setSource(domUrifragment(*pvertsource, string("#")+string(pvertsource->getID()))); + } + _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(),scale); + pacc->setCount(parray->getCount()); + } + + void _buildAiMesh(const aiScene* scene, aiNode* node, domMeshRef pdommesh, domFloat_arrayRef parray, const string& geomid, const urdf::Vector3& scale) + { + if( !node ) { + return; + } + aiMatrix4x4 transform = node->mTransformation; + aiNode *pnode = node->mParent; + while (pnode) { + // Don't convert to y-up orientation, which is what the root node in + // Assimp does + if (pnode->mParent != NULL) { + transform = pnode->mTransformation * transform; + } + pnode = pnode->mParent; + } + + { + uint32_t vertexOffset = parray->getCount(); + uint32_t nTotalVertices=0; + for (uint32_t i = 0; i < node->mNumMeshes; i++) { + aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; + nTotalVertices += input_mesh->mNumVertices; + } + + parray->getValue().grow(parray->getCount()+nTotalVertices*3); + parray->setCount(parray->getCount()+nTotalVertices); + + for (uint32_t i = 0; i < node->mNumMeshes; i++) { + aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; + for (uint32_t j = 0; j < input_mesh->mNumVertices; j++) { + aiVector3D p = input_mesh->mVertices[j]; + p *= transform; + parray->getValue().append(p.x*scale.x); + parray->getValue().append(p.y*scale.y); + parray->getValue().append(p.z*scale.z); + } + } + + // in order to save space, separate triangles from poly lists + + vector triangleindices, otherindices; + int nNumOtherPrimitives = 0; + for (uint32_t i = 0; i < node->mNumMeshes; i++) { + aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; + uint32_t indexCount = 0, otherIndexCount = 0; + for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) { + aiFace& face = input_mesh->mFaces[j]; + if( face.mNumIndices == 3 ) { + indexCount += face.mNumIndices; + } + else { + otherIndexCount += face.mNumIndices; + } + } + triangleindices.reserve(triangleindices.size()+indexCount); + otherindices.reserve(otherindices.size()+otherIndexCount); + for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) { + aiFace& face = input_mesh->mFaces[j]; + if( face.mNumIndices == 3 ) { + triangleindices.push_back(vertexOffset+face.mIndices[0]); + triangleindices.push_back(vertexOffset+face.mIndices[1]); + triangleindices.push_back(vertexOffset+face.mIndices[2]); + } + else { + for (uint32_t k = 0; k < face.mNumIndices; ++k) { + otherindices.push_back(face.mIndices[k]+vertexOffset); + } + nNumOtherPrimitives++; + } + } + vertexOffset += input_mesh->mNumVertices; + } + + if( triangleindices.size() > 0 ) { + domTrianglesRef ptris = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_TRIANGLES)); + ptris->setCount(triangleindices.size()/3); + ptris->setMaterial("mat0"); + domInput_local_offsetRef pvertoffset = daeSafeCast(ptris->add(COLLADA_ELEMENT_INPUT)); + pvertoffset->setSemantic("VERTEX"); + pvertoffset->setOffset(0); + pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid))); + domPRef pindices = daeSafeCast(ptris->add(COLLADA_ELEMENT_P)); + pindices->getValue().setCount(triangleindices.size()); + for(size_t ind = 0; ind < triangleindices.size(); ++ind) { + pindices->getValue()[ind] = triangleindices[ind]; + } + } + + if( nNumOtherPrimitives > 0 ) { + domPolylistRef ptris = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_POLYLIST)); + ptris->setCount(nNumOtherPrimitives); + ptris->setMaterial("mat0"); + domInput_local_offsetRef pvertoffset = daeSafeCast(ptris->add(COLLADA_ELEMENT_INPUT)); + pvertoffset->setSemantic("VERTEX"); + pvertoffset->setSource(domUrifragment(*pdommesh->getVertices(), str(boost::format("#%s/vertices")%geomid))); + domPRef pindices = daeSafeCast(ptris->add(COLLADA_ELEMENT_P)); + pindices->getValue().setCount(otherindices.size()); + for(size_t ind = 0; ind < otherindices.size(); ++ind) { + pindices->getValue()[ind] = otherindices[ind]; + } + + domPolylist::domVcountRef pcount = daeSafeCast(ptris->add(COLLADA_ELEMENT_VCOUNT)); + pcount->getValue().setCount(nNumOtherPrimitives); + uint32_t offset = 0; + for (uint32_t i = 0; i < node->mNumMeshes; i++) { + aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]]; + for (uint32_t j = 0; j < input_mesh->mNumFaces; j++) { + aiFace& face = input_mesh->mFaces[j]; + if( face.mNumIndices > 3 ) { + pcount->getValue()[offset++] = face.mNumIndices; + } + } + } + } + } + + for (uint32_t i=0; i < node->mNumChildren; ++i) { + _buildAiMesh(scene, node->mChildren[i], pdommesh,parray,geomid,scale); + } + } + + + domEffectRef _WriteEffect(std::string const& effect_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse) + { + // + domEffectRef effect = daeSafeCast(_effectsLib->add(COLLADA_ELEMENT_EFFECT)); + effect->setId(effect_id.c_str()); + { + // + domProfile_commonRef profile = daeSafeCast(effect->add(COLLADA_ELEMENT_PROFILE_COMMON)); + { + // + domProfile_common::domTechniqueRef technique = daeSafeCast(profile->add(COLLADA_ELEMENT_TECHNIQUE)); + { + // + domProfile_common::domTechnique::domPhongRef phong = daeSafeCast(technique->add(COLLADA_ELEMENT_PHONG)); + { + // + domFx_common_color_or_textureRef ambient = daeSafeCast(phong->add(COLLADA_ELEMENT_AMBIENT)); + { + // r g b a + domFx_common_color_or_texture::domColorRef ambient_color = daeSafeCast(ambient->add(COLLADA_ELEMENT_COLOR)); + ambient_color->getValue().setCount(4); + ambient_color->getValue()[0] = color_ambient.r; + ambient_color->getValue()[1] = color_ambient.g; + ambient_color->getValue()[2] = color_ambient.b; + ambient_color->getValue()[3] = color_ambient.a; + // + } + // + + // + domFx_common_color_or_textureRef diffuse = daeSafeCast(phong->add(COLLADA_ELEMENT_DIFFUSE)); + { + // r g b a + domFx_common_color_or_texture::domColorRef diffuse_color = daeSafeCast(diffuse->add(COLLADA_ELEMENT_COLOR)); + diffuse_color->getValue().setCount(4); + diffuse_color->getValue()[0] = color_diffuse.r; + diffuse_color->getValue()[1] = color_diffuse.g; + diffuse_color->getValue()[2] = color_diffuse.b; + diffuse_color->getValue()[3] = color_diffuse.a; + // + } + // + } + // + } + // + } + // + } + // + + return effect; + } + + /// \brief Write transformation + /// \param pelt Element to transform + /// \param t Transform to write + void _WriteTransformation(daeElementRef pelt, const urdf::Pose& t) + { + domRotateRef prot = daeSafeCast(pelt->add(COLLADA_ELEMENT_ROTATE,0)); + domTranslateRef ptrans = daeSafeCast(pelt->add(COLLADA_ELEMENT_TRANSLATE,0)); + ptrans->getValue().setCount(3); + ptrans->getValue()[0] = t.position.x; + ptrans->getValue()[1] = t.position.y; + ptrans->getValue()[2] = t.position.z; + + prot->getValue().setCount(4); + // extract axis from quaternion + double sinang = t.rotation.x*t.rotation.x+t.rotation.y*t.rotation.y+t.rotation.z*t.rotation.z; + if( std::fabs(sinang) < 1e-10 ) { + prot->getValue()[0] = 1; + prot->getValue()[1] = 0; + prot->getValue()[2] = 0; + prot->getValue()[3] = 0; + } + else { + urdf::Rotation quat; + if( t.rotation.w < 0 ) { + quat.x = -t.rotation.x; + quat.y = -t.rotation.y; + quat.z = -t.rotation.z; + quat.w = -t.rotation.w; + } + else { + quat = t.rotation; + } + sinang = std::sqrt(sinang); + prot->getValue()[0] = quat.x/sinang; + prot->getValue()[1] = quat.y/sinang; + prot->getValue()[2] = quat.z/sinang; + prot->getValue()[3] = angles::to_degrees(2.0*std::atan2(sinang,quat.w)); + } + } + + // binding in instance_kinematics_scene + void _WriteBindingsInstance_kinematics_scene() + { + FOREACHC(it, _iasout->vkinematicsbindings) { + domBind_kinematics_modelRef pmodelbind = daeSafeCast(_scene.kiscene->add(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL)); + pmodelbind->setNode(it->second.c_str()); + daeSafeCast(pmodelbind->add(COLLADA_ELEMENT_PARAM))->setValue(it->first.c_str()); + } + FOREACHC(it, _iasout->vaxissids) { + domBind_joint_axisRef pjointbind = daeSafeCast(_scene.kiscene->add(COLLADA_ELEMENT_BIND_JOINT_AXIS)); + pjointbind->setTarget(it->jointnodesid.c_str()); + daeSafeCast(pjointbind->add(COLLADA_ELEMENT_AXIS)->add(COLLADA_TYPE_PARAM))->setValue(it->axissid.c_str()); + daeSafeCast(pjointbind->add(COLLADA_ELEMENT_VALUE)->add(COLLADA_TYPE_PARAM))->setValue(it->valuesid.c_str()); + } + } + +private: + static urdf::Vector3 _poseMult(const urdf::Pose& p, const urdf::Vector3& v) + { + double ww = 2 * p.rotation.x * p.rotation.x; + double wx = 2 * p.rotation.x * p.rotation.y; + double wy = 2 * p.rotation.x * p.rotation.z; + double wz = 2 * p.rotation.x * p.rotation.w; + double xx = 2 * p.rotation.y * p.rotation.y; + double xy = 2 * p.rotation.y * p.rotation.z; + double xz = 2 * p.rotation.y * p.rotation.w; + double yy = 2 * p.rotation.z * p.rotation.z; + double yz = 2 * p.rotation.z * p.rotation.w; + urdf::Vector3 vnew; + vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x; + vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y; + vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z; + return vnew; + } + + static urdf::Pose _poseInverse(const urdf::Pose& p) + { + urdf::Pose pinv; + pinv.rotation.x = -p.rotation.x; + pinv.rotation.y = -p.rotation.y; + pinv.rotation.z = -p.rotation.z; + pinv.rotation.w = p.rotation.w; + urdf::Vector3 t = _poseMult(pinv,p.position); + pinv.position.x = -t.x; + pinv.position.y = -t.y; + pinv.position.z = -t.z; + return pinv; + } + + /// \brief computes a collada-compliant sid from the urdf name + static std::string _ComputeId(const std::string& name) + { + std::string newname = name; + for(size_t i = 0; i < newname.size(); ++i) { + if( newname[i] == '/' || newname[i] == ' ' || newname[i] == '.' ) { + newname[i] = '_'; + } + } + return newname; + } + + int _writeoptions; + + const urdf::Model& _robot; + boost::shared_ptr _collada; + domCOLLADA* _dom; + domCOLLADA::domSceneRef _globalscene; + + domLibrary_visual_scenesRef _visualScenesLib; + domLibrary_kinematics_scenesRef _kinematicsScenesLib; + domLibrary_kinematics_modelsRef _kinematicsModelsLib; + domLibrary_articulated_systemsRef _articulatedSystemsLib; + domLibrary_physics_scenesRef _physicsScenesLib; + domLibrary_materialsRef _materialsLib; + domLibrary_effectsRef _effectsLib; + domLibrary_geometriesRef _geometriesLib; + domTechniqueRef _sensorsLib;///< custom sensors library + SCENE _scene; + + boost::shared_ptr _ikmout; + boost::shared_ptr _iasout; + std::map< boost::shared_ptr, int > _mapjointindices; + std::map< boost::shared_ptr, int > _maplinkindices; + std::map< boost::shared_ptr, int > _mapmaterialindices; + Assimp::Importer _importer; +}; + + +ColladaUrdfException::ColladaUrdfException(std::string const& what) + : std::runtime_error(what) +{ +} + +bool colladaFromUrdfFile(string const& file, boost::shared_ptr& dom) { + TiXmlDocument urdf_xml; + if (!urdf_xml.LoadFile(file)) { + ROS_ERROR("Could not load XML file"); + return false; + } + + return colladaFromUrdfXml(&urdf_xml, dom); +} + +bool colladaFromUrdfString(string const& xml, boost::shared_ptr& dom) { + TiXmlDocument urdf_xml; + if (urdf_xml.Parse(xml.c_str()) == 0) { + ROS_ERROR("Could not parse XML document"); + return false; + } + + return colladaFromUrdfXml(&urdf_xml, dom); +} + +bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr& dom) { + urdf::Model robot_model; + if (!robot_model.initXml(xml_doc)) { + ROS_ERROR("Could not generate robot model"); + return false; + } + + return colladaFromUrdfModel(robot_model, dom); +} + +bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr& dom) { + ColladaWriter writer(robot_model,0); + dom = writer.convert(); + return dom != boost::shared_ptr(); +} + +bool colladaToFile(boost::shared_ptr dom, string const& file) { + daeString uri = dom->getDoc(0)->getDocumentURI()->getURI(); + return dom->writeTo(uri, file); +} + +} diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp new file mode 100644 index 0000000..163ec9d --- /dev/null +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -0,0 +1,66 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010, 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: Tim Field */ + +#include "collada_urdf/collada_urdf.h" + +int main(int argc, char** argv) +{ + if (argc != 3) { + std::cerr << "Usage: urdf_to_collada input.urdf output.dae" << std::endl; + return -1; + } + + ros::init(argc, argv, "urdf_to_collada"); + + std::string input_filename(argv[1]); + std::string output_filename(argv[2]); + + urdf::Model robot_model; + if( !robot_model.initFile(input_filename) ) { + ROS_ERROR("failed to open urdf file %s",input_filename.c_str()); + } + + boost::shared_ptr dom; + if (!collada_urdf::colladaFromUrdfModel(robot_model, dom)) { + std::cerr << std::endl << "Error converting document" << std::endl; + return -1; + } + + collada_urdf::colladaToFile(dom, output_filename); + std::cout << std::endl << "Document successfully written to " << output_filename << std::endl; + + return 0; +} diff --git a/collada_urdf/test/pr2.urdf b/collada_urdf/test/pr2.urdf new file mode 100644 index 0000000..17d8633 --- /dev/null +++ b/collada_urdf/test/pr2.urdf @@ -0,0 +1,3389 @@ + + + + + + + + + + + + + + + + + + + + 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.05 + 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 + map + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + -79.9999999086 + 79.9999999086 + 0.05 + 10.0 + 0.01 + 20 + + 0.005 + true + 20 + tilt_scan + laser_tilt_link + + + + + + + + + + + -6.05 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + 32.6525111499 + + + + 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 + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + r_upper_arm_link_geom + + 100.0 + + true + 100.0 + r_upper_arm_bumper + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + + r_elbow_flex_link_geom + 100.0 + + true + 100.0 + r_elbow_flex_bumper + + + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + false + + r_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + + true + false + + 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 + + + + + + true + 0.01 + 0.0001 + 0 + 0 + 0.0001 + 0 + 0.0001 + 0 + 0 + 0 + 0.82025 0.188 0.790675 + 0 -0 0 + + 0 0 0 + 0 -0 0 + 0.0 0.0 0.0 + + 0 0 0 + 0 -0 0 + 0.0 0.0 0.0 + unit_box + PR2/White + + + true + false + + + r_gripper_float_link + r_gripper_palm_link + r_gripper_float_link + 1 0 0 + -0.05 + 0.001 + + + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + 32.6525111499 + + + + 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 + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + l_upper_arm_link_geom + + 100.0 + + true + 100.0 + l_upper_arm_bumper + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + + l_elbow_flex_link_geom + 100.0 + + true + 100.0 + l_elbow_flex_bumper + + + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + false + + l_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + + true + false + + 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 + + + + + + true + 0.01 + 0.0001 + 0 + 0 + 0.0001 + 0 + 0.0001 + 0 + 0 + 0 + 0.82025 0.188 0.790675 + 0 -0 0 + + 0 0 0 + 0 -0 0 + 0.0 0.0 0.0 + + 0 0 0 + 0 -0 0 + 0.0 0.0 0.0 + unit_box + PR2/White + + + true + false + + + l_gripper_float_link + l_gripper_palm_link + l_gripper_float_link + 1 0 0 + -0.05 + 0.001 + + + 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 + 25.0 + + true + 25.0 + l_forearm_cam/image_raw + l_forearm_cam/camera_info + l_forearm_cam_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_frame + 0 + 320.5 + 320.5 + 240.5 + + + 320 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + PR2/Blue + + + diff --git a/collada_urdf/test/test_collada_urdf.cpp b/collada_urdf/test/test_collada_urdf.cpp new file mode 100644 index 0000000..677d029 --- /dev/null +++ b/collada_urdf/test/test_collada_urdf.cpp @@ -0,0 +1,86 @@ +// Copyright (c) 2010, 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 Willow Garage, Inc. 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: Tim Field */ + +#include "collada_urdf/collada_urdf.h" + +#include + +#include +#include +#include + +std::string readTestUrdfString() { + std::ifstream file("test/pr2.urdf"); + std::stringstream ss; + ss << file.rdbuf(); + return ss.str(); +} + +TEST(collada_urdf, collada_from_urdf_file_works) +{ + boost::shared_ptr dom; + ASSERT_TRUE(collada_urdf::colladaFromUrdfFile("test/pr2.urdf", dom)); + ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); +} + +TEST(collada_urdf, collada_from_urdf_string_works) +{ + std::string urdf_str = readTestUrdfString(); + + boost::shared_ptr dom; + ASSERT_TRUE(collada_urdf::colladaFromUrdfString(urdf_str, dom)); + ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); +} + +TEST(collada_urdf, collada_from_urdf_xml_works) +{ + TiXmlDocument urdf_xml; + ASSERT_TRUE(urdf_xml.Parse(readTestUrdfString().c_str()) > 0); + + boost::shared_ptr dom; + ASSERT_TRUE(collada_urdf::colladaFromUrdfXml(&urdf_xml, dom)); + ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); +} + +TEST(collada_urdf, collada_from_urdf_model_works) +{ + urdf::Model robot_model; + TiXmlDocument urdf_xml; + ASSERT_TRUE(urdf_xml.Parse(readTestUrdfString().c_str()) > 0); + ASSERT_TRUE(robot_model.initXml(&urdf_xml)); + + boost::shared_ptr dom; + ASSERT_TRUE(collada_urdf::colladaFromUrdfModel(robot_model, dom)); + ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/colladadom/Makefile b/colladadom/Makefile new file mode 100644 index 0000000..44e95ec --- /dev/null +++ b/colladadom/Makefile @@ -0,0 +1,46 @@ +all: installed +TARBALL = build/collada-dom-2.2.zip +TARBALL_URL = http://pr.willowgarage.com/downloads/collada-dom-2.2.zip +SOURCE_DIR = build/colladadom +INITIAL_DIR = build/collada-dom +UNPACK_CMD = unzip +TARBALL_PATCH = collada-dom.patch +MD5SUM_FILE = collada-dom-2.2.zip.md5sum + +include $(shell rospack find mk)/download_unpack_build.mk + +ROOT = $(shell rospack find colladadom) + +# Copied from build/colladadom/dom/Makefile: +# os: 'linux', 'mac', 'windows', or 'ps3'. Use the 'uname' command to decide the +# default value. To detect when we're on Windows we'll check to see if we're +# running on Cygwin or MinGW. +OS := linux +ifneq ($(shell uname | grep -i darwin),) +OS := mac +else ifneq ($(or $(shell uname | grep -i cygwin),$(shell uname | grep -i mingw)),) +OS := windows +endif + +installed: wiped $(SOURCE_DIR)/unpacked + @echo "ROOT is: $(ROOT)" + -mkdir -p $(ROOT)/include + -mkdir -p $(ROOT)/lib + @echo "making it" + cd $(SOURCE_DIR)/dom && make $(ROS_PARALLEL_JOBS) + cp -rf $(SOURCE_DIR)/dom/include/* $(ROOT)/include/ + cp -rf $(SOURCE_DIR)/dom/build/$(OS)-1.5/*.* $(ROOT)/lib/ + touch installed + +wiped: Makefile + make wipe + touch wiped + +clean: + -cd $(SOURCE_DIR) && make clean + rm -rf $(ROOT)/include $(ROOT)/lib installed + +wipe: clean + rm -rf build + +.PHONY : clean download wipe diff --git a/colladadom/collada-dom-2.2.zip.md5sum b/colladadom/collada-dom-2.2.zip.md5sum new file mode 100644 index 0000000..2ba20f4 --- /dev/null +++ b/colladadom/collada-dom-2.2.zip.md5sum @@ -0,0 +1 @@ +bbb76ef2a8037c945c5cdf26829dcb7d build/collada-dom-2.2.zip diff --git a/colladadom/collada-dom.patch b/colladadom/collada-dom.patch new file mode 100644 index 0000000..468977a --- /dev/null +++ b/colladadom/collada-dom.patch @@ -0,0 +1,100 @@ +Index: dom/Makefile +=================================================================== +--- dom/Makefile 2008-10-02 17:48:30.000000000 -0700 ++++ dom/Makefile 2010-03-03 14:44:09.162398249 -0800 +@@ -55,7 +55,7 @@ + conf := release + + # Collada version: '1.4', '1.5', or 'all' +-colladaVersion := 1.4 ++colladaVersion := 1.5 + + # parser: 'libxml', 'tinyxml', or 'all'. + parser := libxml +@@ -188,7 +188,7 @@ + ifneq ($(filter install uninstall installTest,$(MAKECMDGOALS)),) + # You can only install on Mac or Linux. Check for that. + ifeq ($(oss),linux) +-prefix := /usr/local ++prefix := ../../../ + else ifeq ($(oss),mac) + prefix := /Library/Frameworks + else +Index: dom/make/common.mk +=================================================================== +--- dom/make/common.mk 2008-05-14 17:30:42.000000000 -0700 ++++ dom/make/common.mk 2010-04-07 17:16:11.000000000 -0700 +@@ -17,6 +17,8 @@ + debugSuffix := + endif + ++ccFlags += $(shell rosboost-cfg --cflags) ++ + ifeq ($(os),mac) + # Add the -arch flags to specify what architectures we're building for. + ccFlags += $(addprefix -arch ,$(subst x86,i386,$(archs))) +Index: dom/make/domTest.mk +=================================================================== +--- dom/make/domTest.mk 2008-09-03 08:51:18.000000000 -0700 ++++ dom/make/domTest.mk 2010-04-07 17:17:18.000000000 -0700 +@@ -55,13 +55,7 @@ + endif + + # Boost defs +-ifeq ($(os),linux) +-libOpts += -lboost_filesystem +-else +-includeOpts += -Iexternal-libs/boost +-libOpts += external-libs/boost/lib/$(buildID)/libboost_system.a +-libOpts += external-libs/boost/lib/$(buildID)/libboost_filesystem.a +-endif ++libOpts += $(shell rosboost-cfg --lflags system,filesystem) + ifeq ($(os),ps3) + # PS3 doesn't support C++ locales, so tell boost not to use them + ccFlags += -DBOOST_NO_STD_LOCALE + +--- dom/src/dae/daeSIDResolver.cpp ++++ dom/src/dae/daeSIDResolver.cpp +@@ -152,6 +152,18 @@ + list& remainingPart) { + remainingPart.clear(); + ++ // custom change for following instance urls (Rosen Diankov) ++ if ( strncmp( container->getElementName(), "instance_", 9 ) == 0 ) { ++ daeURI *uri = (daeURI*)container->getAttributeValue("url"); ++ if ( uri != NULL && uri->getElement() != NULL ) { ++ daeElement *e = findWithDots( uri->getElement(), s, profile, finder, remainingPart ); ++ if ( e != NULL ) { ++ //found it ++ return e; ++ } ++ } ++ } ++ + // First see if the whole thing resolves correctly + if (daeElement* result = finder(container, s, profile)) + return result; +@@ -355,6 +367,23 @@ + if ((!member.empty() || haveArrayIndex1) && result.scalar == NULL) + return daeSidRef::resolveData(); + ++ if( !!result.elt && !result.array && !result.scalar ) { ++ // if newparam, follow its SIDREF (Rosen Diankov) ++ if( strcmp(result.elt->getElementName(),"newparam") == 0) { ++ daeElement* psidref = result.elt->getChild("SIDREF"); ++ if( !!psidref ) { ++ daeSidRef::resolveData newresult; ++ newresult = resolveImpl(daeSidRef(string("./") + psidref->getCharData(),result.elt->getParent(),sidRef.profile)); ++ if( !newresult.elt ) { ++ newresult = resolveImpl(daeSidRef(psidref->getCharData(),result.elt->getParent(),sidRef.profile)); ++ } ++ if( !!newresult.elt ) { ++ return newresult; ++ } ++ } ++ } ++ } ++ + // SID resolution was successful. + return result; + } diff --git a/urdf_tutorial/mainpage.dox b/colladadom/mainpage.dox similarity index 95% rename from urdf_tutorial/mainpage.dox rename to colladadom/mainpage.dox index a6d61dc..386e4a1 100644 --- a/urdf_tutorial/mainpage.dox +++ b/colladadom/mainpage.dox @@ -2,7 +2,7 @@ \mainpage \htmlinclude manifest.html -\b urdf_tutorial is ... +\b colladadom is ... + + + + + + + + + + + + + + + + + + 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.05 + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + -79.9999999086 + 79.9999999086 + 0.05 + 10.0 + 0.01 + 40 + + 0.005 + true + 40 + tilt_scan + laser_tilt_link + + + + + + + + + + + -6.05 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + 32.6525111499 + + + + 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 + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + r_upper_arm_link_geom + + 100.0 + + true + 100.0 + r_upper_arm_bumper + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + + r_elbow_flex_link_geom + 100.0 + + true + 100.0 + r_elbow_flex_bumper + + + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + false + + r_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + + true + false + + 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 + base_link + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_force_ground_truth + r_gripper_l_finger_link + + + + + + + + + + + + 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_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 + + + + 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 + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + l_upper_arm_link_geom + + 100.0 + + true + 100.0 + l_upper_arm_bumper + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + + l_elbow_flex_link_geom + 100.0 + + true + 100.0 + l_elbow_flex_bumper + + + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + false + + l_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + + true + false + + 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 + base_link + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_force_ground_truth + l_gripper_l_finger_link + + + + + + + + + + + + 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_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 + + diff --git a/kdl_parser/test/pr2_desc_bracket.xml b/kdl_parser/test/pr2_desc_bracket.xml new file mode 100644 index 0000000..990f441 --- /dev/null +++ b/kdl_parser/test/pr2_desc_bracket.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/kdl_parser/test/pr2_desc_bracket2.xml b/kdl_parser/test/pr2_desc_bracket2.xml new file mode 100644 index 0000000..70e513a --- /dev/null +++ b/kdl_parser/test/pr2_desc_bracket2.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/kdl_parser/test/pr2_desc_vector.xml b/kdl_parser/test/pr2_desc_vector.xml new file mode 100644 index 0000000..1dcc98f --- /dev/null +++ b/kdl_parser/test/pr2_desc_vector.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/kdl_parser/test/test_kdl_parser.cpp b/kdl_parser/test/test_kdl_parser.cpp new file mode 100644 index 0000000..c43d75f --- /dev/null +++ b/kdl_parser/test/test_kdl_parser.cpp @@ -0,0 +1,96 @@ +/********************************************************************* +* 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 "kdl_parser/kdl_parser.hpp" + +using namespace kdl_parser; + +int g_argc; +char** g_argv; + +class TestParser : public testing::Test +{ +public: + KDL::Tree my_tree; + +protected: + /// constructor + TestParser() + { + } + + + /// Destructor + ~TestParser() + { + } +}; + + + + +TEST_F(TestParser, test) +{ + for (int i=1; isecond.children.size(), (unsigned int)1); + ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment()); + ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0); + ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 15.6107, 0.001); + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_kdl_parser"); + g_argc = argc; + g_argv = argv; + return RUN_ALL_TESTS(); +} diff --git a/kdl_parser/test/test_kdl_parser.launch b/kdl_parser/test/test_kdl_parser.launch new file mode 100644 index 0000000..b97a9ba --- /dev/null +++ b/kdl_parser/test/test_kdl_parser.launch @@ -0,0 +1,6 @@ + + + diff --git a/urdf_tutorial/CMakeLists.txt b/resource_retriever/CMakeLists.txt similarity index 70% rename from urdf_tutorial/CMakeLists.txt rename to resource_retriever/CMakeLists.txt index f8f1c9c..86b5735 100644 --- a/urdf_tutorial/CMakeLists.txt +++ b/resource_retriever/CMakeLists.txt @@ -21,10 +21,15 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) #uncomment if you have defined services #rosbuild_gensrv() -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) +include(FindCURL) + +if(NOT CURL_FOUND) +message("CURL not found! Aborting...") +fail() +endif(NOT CURL_FOUND) + +include_directories(${CURL_INCLUDE_DIRS}) +rosbuild_add_library(${PROJECT_NAME} src/retriever.cpp) +target_link_libraries(${PROJECT_NAME} ${CURL_LIBRARIES}) + +add_subdirectory(test EXCLUDE_FROM_ALL) diff --git a/urdf_tutorial/Makefile b/resource_retriever/Makefile similarity index 100% rename from urdf_tutorial/Makefile rename to resource_retriever/Makefile diff --git a/resource_retriever/include/resource_retriever/retriever.h b/resource_retriever/include/resource_retriever/retriever.h new file mode 100644 index 0000000..c1fcf15 --- /dev/null +++ b/resource_retriever/include/resource_retriever/retriever.h @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2009, Willow Garage, Inc. + * + * 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 names of Stanford University or Willow Garage, Inc. 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. + */ + +#ifndef RESOURCE_RETRIEVER_RETRIEVER_H +#define RESOURCE_RETRIEVER_RETRIEVER_H + +#include +#include +#include +#include + +typedef void CURL; + +namespace resource_retriever +{ + +class Exception : public std::runtime_error +{ +public: + Exception(const std::string& file, const std::string& error_msg) + : std::runtime_error("Error retrieving file [" + file + "]: " + error_msg) + {} +}; + +/** + * \brief A combination of a pointer to data in memory along with the data's size. + */ +struct MemoryResource +{ + MemoryResource() + : size(0) + {} + + boost::shared_array data; + uint32_t size; +}; + +/** + * \brief Retrieves files from from a url. Caches a CURL handle so multiple accesses to a single url + * will keep connections open. + */ +class Retriever +{ +public: + Retriever(); + ~Retriever(); + + /** + * \brief Get a file and store it in memory + * \param url The url to retrieve. package://package/file will be turned into the correct file:// invocation + * \return The file, loaded into memory + * \throws resource_retriever::Exception if anything goes wrong. + */ + MemoryResource get(const std::string& url); + +private: + CURL* curl_handle_; +}; + +} // namespace resource_retriever + +#endif // RESOURCE_RETRIEVER_RETRIEVER_H diff --git a/resource_retriever/mainpage.dox b/resource_retriever/mainpage.dox new file mode 100644 index 0000000..a06a758 --- /dev/null +++ b/resource_retriever/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b resource_retriever is a package used for downloading files from a url. It also provides special handling of the package:// prefix, allowing things to be referenced +by path relative to a package. + + +\section codeapi Code API + + - resource_retriever::Retriever -- Use this class to download files. + + +*/ \ No newline at end of file diff --git a/resource_retriever/manifest.xml b/resource_retriever/manifest.xml new file mode 100644 index 0000000..5ab0755 --- /dev/null +++ b/resource_retriever/manifest.xml @@ -0,0 +1,29 @@ + + + + This package retrieves data from url-format files such as http://, + ftp://, package:// file://, etc., and loads the data into memory. + The package:// url for ros packages is translated into a local + file:// url. The resourse retriever was initially designed to load + mesh files into memory, but it can be used for any type of + data. The resource retriever is based on the the libcurl library. + + + Josh Faust (jfaust@willowgarage.com) + BSD + + http://ros.org/wiki/resource_retriever + + + + + + + + + + + + + + diff --git a/resource_retriever/src/retriever.cpp b/resource_retriever/src/retriever.cpp new file mode 100644 index 0000000..b09a981 --- /dev/null +++ b/resource_retriever/src/retriever.cpp @@ -0,0 +1,147 @@ +/* + * Copyright (C) 2009, Willow Garage, Inc. + * + * 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 names of Stanford University or Willow Garage, Inc. 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 "resource_retriever/retriever.h" + +#include + +#include +#include + +#include + +namespace resource_retriever +{ + +class CURLStaticInit +{ +public: + CURLStaticInit() + : initialized_(false) + { + CURLcode ret = curl_global_init(CURL_GLOBAL_ALL); + if (ret != 0) + { + ROS_ERROR("Error initializing libcurl! retcode = %d", ret); + } + else + { + initialized_ = true; + } + } + + ~CURLStaticInit() + { + if (initialized_) + { + curl_global_cleanup(); + } + } + + bool initialized_; +}; +static CURLStaticInit g_curl_init; + +Retriever::Retriever() +{ + curl_handle_ = curl_easy_init(); +} + +Retriever::~Retriever() +{ + if (curl_handle_) + { + curl_easy_cleanup(curl_handle_); + } +} + +struct MemoryBuffer +{ + std::vector v; +}; + +size_t curlWriteFunc(void* buffer, size_t size, size_t nmemb, void* userp) +{ + MemoryBuffer* membuf = (MemoryBuffer*)userp; + + size_t prev_size = membuf->v.size(); + membuf->v.resize(prev_size + size * nmemb); + memcpy(&membuf->v[prev_size], buffer, size * nmemb); + + return size * nmemb; +} + +MemoryResource Retriever::get(const std::string& url) +{ + std::string mod_url = url; + if (url.find("package://") == 0) + { + mod_url.erase(0, strlen("package://")); + size_t pos = mod_url.find("/"); + if (pos == std::string::npos) + { + throw Exception(url, "Could not parse package:// format into file:// format"); + } + + std::string package = mod_url.substr(0, pos); + mod_url.erase(0, pos); + std::string package_path = ros::package::getPath(package); + + if (package_path.empty()) + { + throw Exception(url, "Package [" + package + "] does not exist"); + } + + mod_url = "file://" + package_path + mod_url; + } + + curl_easy_setopt(curl_handle_, CURLOPT_URL, mod_url.c_str()); + curl_easy_setopt(curl_handle_, CURLOPT_WRITEFUNCTION, curlWriteFunc); + + char error_buffer[CURL_ERROR_SIZE]; + curl_easy_setopt(curl_handle_, CURLOPT_ERRORBUFFER , error_buffer); + + MemoryResource res; + MemoryBuffer buf; + curl_easy_setopt(curl_handle_, CURLOPT_WRITEDATA, &buf); + + CURLcode ret = curl_easy_perform(curl_handle_); + if (ret != 0) + { + throw Exception(mod_url, error_buffer); + } + else if (!buf.v.empty()) + { + res.size = buf.v.size(); + res.data.reset(new uint8_t[res.size]); + memcpy(res.data.get(), &buf.v[0], res.size); + } + + return res; +} + +} diff --git a/resource_retriever/test/CMakeLists.txt b/resource_retriever/test/CMakeLists.txt new file mode 100644 index 0000000..f1c0fa6 --- /dev/null +++ b/resource_retriever/test/CMakeLists.txt @@ -0,0 +1,4 @@ +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}) + +rosbuild_add_gtest(test/utest test.cpp) +target_link_libraries(test/utest ${PROJECT_NAME}) diff --git a/resource_retriever/test/test.cpp b/resource_retriever/test/test.cpp new file mode 100644 index 0000000..b29f40e --- /dev/null +++ b/resource_retriever/test/test.cpp @@ -0,0 +1,140 @@ +/********************************************************************* +* 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. +*********************************************************************/ + +#include + +#include +#include +#include + +using namespace resource_retriever; + +TEST(Retriever, getByPackage) +{ + try + { + Retriever r; + MemoryResource res = r.get("package://"ROS_PACKAGE_NAME"/test/test.txt"); + + ASSERT_EQ(res.size, 1); + ASSERT_EQ(res.data[0], 'A'); + } + catch (Exception& e) + { + FAIL(); + } +} + +TEST(Retriever, largeFile) +{ + try + { + std::string path = ros::package::getPath(ROS_PACKAGE_NAME) + "/test/large_file.dat"; + + FILE* f = fopen(path.c_str(), "w"); + + ASSERT_TRUE(f); + + for (int i = 0; i < 1024*1024*50; ++i) + { + fprintf(f, "A"); + } + fclose(f); + + Retriever r; + MemoryResource res = r.get("package://"ROS_PACKAGE_NAME"/test/large_file.dat"); + + ASSERT_EQ(res.size, 1024*1024*50); + } + catch (Exception& e) + { + FAIL(); + } +} + +TEST(Retriever, http) +{ + try + { + Retriever r; + MemoryResource res = r.get("http://pr.willowgarage.com/downloads/svnmerge.py"); + + ASSERT_GT(res.size, 0); + } + catch (Exception& e) + { + FAIL(); + } +} + +TEST(Retriever, invalidFiles) +{ + Retriever r; + + try + { + r.get("file://fail"); + FAIL(); + } + catch (Exception& e) + { + ROS_INFO("%s", e.what()); + } + + try + { + r.get("package://roscpp"); + FAIL(); + } + catch (Exception& e) + { + ROS_INFO("%s", e.what()); + } + + try + { + r.get("package://invalid_package_blah/test.xml"); + FAIL(); + } + catch (Exception& e) + { + ROS_INFO("%s", e.what()); + } +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/resource_retriever/test/test.txt b/resource_retriever/test/test.txt new file mode 100644 index 0000000..8c7e5a6 --- /dev/null +++ b/resource_retriever/test/test.txt @@ -0,0 +1 @@ +A \ No newline at end of file diff --git a/robot_state_publisher/CMakeLists.txt b/robot_state_publisher/CMakeLists.txt new file mode 100644 index 0000000..5c68421 --- /dev/null +++ b/robot_state_publisher/CMakeLists.txt @@ -0,0 +1,55 @@ +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() +rosbuild_add_boost_directories() + +#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() +#uncomment if you have defined services +#rosbuild_gensrv() + +rosbuild_add_library(${PROJECT_NAME} src/robot_state_publisher.cpp src/treefksolverposfull_recursive.cpp) + +rosbuild_add_executable(state_publisher src/joint_state_listener.cpp ) +target_link_libraries(state_publisher ${PROJECT_NAME}) + +rosbuild_add_executable(test_publisher test/test_publisher.cpp ) +target_link_libraries(test_publisher ${PROJECT_NAME}) +rosbuild_add_gtest_build_flags(test_publisher) +rosbuild_declare_test(test_publisher) +rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_publisher.launch) + +rosbuild_add_executable(test_one_link test/test_one_link.cpp ) +target_link_libraries(test_one_link ${PROJECT_NAME}) +rosbuild_add_gtest_build_flags(test_one_link) +rosbuild_declare_test(test_one_link) +rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_one_link.launch) + +rosbuild_add_executable(test_two_links_fixed_joint test/test_two_links_fixed_joint.cpp ) +target_link_libraries(test_two_links_fixed_joint ${PROJECT_NAME}) +rosbuild_add_gtest_build_flags(test_two_links_fixed_joint) +rosbuild_declare_test(test_two_links_fixed_joint) +rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_fixed_joint.launch) + +rosbuild_add_executable(test_two_links_moving_joint test/test_two_links_moving_joint.cpp ) +target_link_libraries(test_two_links_moving_joint ${PROJECT_NAME}) +rosbuild_add_gtest_build_flags(test_two_links_moving_joint) +rosbuild_declare_test(test_two_links_moving_joint) +rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_moving_joint.launch) + +# Download needed data file +rosbuild_download_test_data(http://pr.willowgarage.com/data/robot_state_publisher/joint_states_indexed.bag test/joint_states_indexed.bag 793e0b566ebe4698265a936b92fa2bba) diff --git a/robot_state_publisher/Makefile b/robot_state_publisher/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/robot_state_publisher/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/robot_state_publisher/doc.dox b/robot_state_publisher/doc.dox new file mode 100644 index 0000000..9267270 --- /dev/null +++ b/robot_state_publisher/doc.dox @@ -0,0 +1,11 @@ +/** +@mainpage + +@htmlinclude manifest.html + +This package contains the robot state publisher, which can be used as +a library or as a ROS node. The API documentation for the library can +be found here: robot_state_publisher::RobotStatePublisher + + +**/ diff --git a/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h b/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h new file mode 100644 index 0000000..2990c4c --- /dev/null +++ b/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h @@ -0,0 +1,74 @@ +/********************************************************************* +* 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 JOINT_STATE_LISTENER_H +#define JOINT_STATE_LISTENER_H + +#include +#include +#include +#include "robot_state_publisher/robot_state_publisher.h" + +using namespace std; +using namespace ros; +using namespace KDL; + +typedef boost::shared_ptr JointStateConstPtr; + +namespace robot_state_publisher{ + +class JointStateListener{ +public: + /** Constructor + * \param tree The kinematic model of a robot, represented by a KDL Tree + */ + JointStateListener(const KDL::Tree& tree); + + /// Destructor + ~JointStateListener(); + +private: + void callbackJointState(const JointStateConstPtr& state); + + NodeHandle n_, n_tilde_; + Rate publish_rate_; + robot_state_publisher::RobotStatePublisher state_publisher_; + Subscriber joint_state_sub_; +}; +} + + +#endif diff --git a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h b/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h new file mode 100644 index 0000000..5d0b7ac --- /dev/null +++ b/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h @@ -0,0 +1,86 @@ +/********************************************************************* +* 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 ROBOT_STATE_PUBLISHER_H +#define ROBOT_STATE_PUBLISHER_H + +#include +#include +#include +#include +#include "robot_state_publisher/treefksolverposfull_recursive.hpp" + +namespace robot_state_publisher{ + +class RobotStatePublisher +{ +public: + /** Constructor + * \param tree The kinematic model of a robot, represented by a KDL Tree + */ + RobotStatePublisher(const KDL::Tree& tree); + + /// Destructor + ~RobotStatePublisher(){}; + + /** Publish transforms to tf + * \param joint_positions A map of joint names and joint positions. + * \param time The time at which the joint positions were recorded + * returns true on success; return false when the robot model is empty or not all the joints in the robot model are specified in the joint map. + */ + bool publishTransforms(const std::map& joint_positions, const ros::Time& time); + +private: + ros::Publisher tf_publisher_; + bool flatten_tree_; + KDL::Tree tree_; + boost::scoped_ptr solver_; + std::string root_; + std::vector transforms_; + tf::TransformBroadcaster tf_broadcaster_; + + class empty_tree_exception: public std::exception{ + virtual const char* what() const throw(){ + return "Tree is empty";} + } empty_tree_ex; + +}; + + + +} + +#endif diff --git a/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp b/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp new file mode 100644 index 0000000..1d11b52 --- /dev/null +++ b/robot_state_publisher/include/robot_state_publisher/treefksolverposfull_recursive.hpp @@ -0,0 +1,53 @@ +// Copyright (C) 2009 Willow Garage Inc + +// Version: 1.0 +// Author: Wim Meeussen +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#ifndef KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP +#define KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP + +#include +#include + + +namespace KDL { + +class TreeFkSolverPosFull_recursive + +{ +public: + TreeFkSolverPosFull_recursive(const Tree& _tree); + ~TreeFkSolverPosFull_recursive(); + + int JntToCart(const std::map& q_in, std::map >& p_out, bool flatten_tree=true); + +private: + void addFrameToMap(const std::map& q_in, + std::map >& p_out, + const tf::Stamped& previous_frame, + const SegmentMap::const_iterator this_segment, + bool flatten_tree); + + Tree tree; + +}; +} + +#endif diff --git a/robot_state_publisher/manifest.xml b/robot_state_publisher/manifest.xml new file mode 100644 index 0000000..69aed7e --- /dev/null +++ b/robot_state_publisher/manifest.xml @@ -0,0 +1,34 @@ + + + + This package allows you to publish the state of a robot to + tf. Once the state gets published, it is + available to all components in the system that also use tf. + The package takes the joint angles of the robot as input + and publishes the 3D poses of the robot links, using a kinematic + tree model of the robot. The package can both be used as a library + and as a ROS node. This package has been well tested and the code + is stable. No major changes are planned in the near future + + + Wim Meeussen meeussen@willowgarage.com + BSD + + + http://ros.org/wiki/robot_state_publisher + + + + + + + + + + + + + + + + diff --git a/robot_state_publisher/src/joint_state_listener.cpp b/robot_state_publisher/src/joint_state_listener.cpp new file mode 100644 index 0000000..84f4875 --- /dev/null +++ b/robot_state_publisher/src/joint_state_listener.cpp @@ -0,0 +1,122 @@ +/********************************************************************* +* 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 "robot_state_publisher/robot_state_publisher.h" +#include "robot_state_publisher/joint_state_listener.h" +#include + + +using namespace std; +using namespace ros; +using namespace KDL; +using namespace robot_state_publisher; + + +JointStateListener::JointStateListener(const KDL::Tree& tree) + : n_tilde_("~"), publish_rate_(0.0), state_publisher_(tree) +{ + // set publish frequency + double publish_freq; + n_tilde_.param("publish_frequency", publish_freq, 50.0); + publish_rate_ = Rate(publish_freq); + + if (tree.getNrOfJoints() == 0){ + boost::shared_ptr empty_state(new sensor_msgs::JointState); + while (ros::ok()){ + empty_state->header.stamp = ros::Time::now(); + this->callbackJointState(empty_state); + publish_rate_.sleep(); + } + } + else{ + // subscribe to mechanism state + joint_state_sub_ = n_.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this); + } +}; + + +JointStateListener::~JointStateListener() +{}; + + +void JointStateListener::callbackJointState(const JointStateConstPtr& state) +{ + if (state->name.size() != state->position.size()){ + ROS_ERROR("Robot state publisher received an invalid joint state vector"); + return; + } + + // get joint positions from state message + map joint_positions; + for (unsigned int i=0; iname.size(); i++) + joint_positions.insert(make_pair(state->name[i], state->position[i])); + state_publisher_.publishTransforms(joint_positions, state->header.stamp); + publish_rate_.sleep(); +} + + + + + +// ---------------------------------- +// ----- MAIN ----------------------- +// ---------------------------------- +int main(int argc, char** argv) +{ + // Initialize ros + ros::init(argc, argv, "robot_state_publisher"); + NodeHandle node; + + // gets the location of the robot description on the parameter server + KDL::Tree tree; + if (!kdl_parser::treeFromParam("robot_description", tree)){ + ROS_ERROR("Failed to extract kdl tree from xml robot description"); + return -1; + } + + if (tree.getNrOfSegments() == 0){ + ROS_WARN("Robot state publisher got an empty tree and cannot publish any state to tf"); + ros::spin(); + } + else{ + JointStateListener state_publisher(tree); + ros::spin(); + } + + return 0; +} diff --git a/robot_state_publisher/src/robot_state_publisher.cpp b/robot_state_publisher/src/robot_state_publisher.cpp new file mode 100644 index 0000000..8faadd2 --- /dev/null +++ b/robot_state_publisher/src/robot_state_publisher.cpp @@ -0,0 +1,99 @@ +/********************************************************************* +* 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 "robot_state_publisher/robot_state_publisher.h" +#include +#include + +using namespace std; +using namespace ros; +using namespace KDL; + + + +namespace robot_state_publisher{ + +RobotStatePublisher::RobotStatePublisher(const Tree& tree) + :tree_(tree) +{ + // build tree solver + solver_.reset(new TreeFkSolverPosFull_recursive(tree_)); + + // get parameter to flatter tree or not + ros::NodeHandle n_private("~"); + n_private.param("flatten_tree", flatten_tree_, false); + + // advertise tf message + NodeHandle n; + tf_publisher_ = n.advertise("/tf", 5); + + // get the root segment of the tree + SegmentMap::const_iterator root = tree.getRootSegment(); + root_ = root->first; +} + + + +bool RobotStatePublisher::publishTransforms(const map& joint_positions, const Time& time) +{ + // calculate transforms + map > link_poses; + solver_->JntToCart(joint_positions, link_poses, flatten_tree_); + + if (link_poses.empty()){ + ROS_ERROR("Could not compute link poses. The tree or the state is invalid."); + return false; + } + transforms_.resize(link_poses.size()); + + unsigned int i = 0; + tf::Transform tf_frame; + for (map >::const_iterator f=link_poses.begin(); f!=link_poses.end(); f++){ + tf::TransformKDLToTF(f->second, transforms_[i]); + transforms_[i].stamp_ = time; + transforms_[i].frame_id_ = f->second.frame_id_; + transforms_[i].child_frame_id_ = f->first; + i++; + } + + tf_broadcaster_.sendTransform(transforms_); + + return true; +} +} + + + diff --git a/robot_state_publisher/src/treefksolverposfull_recursive.cpp b/robot_state_publisher/src/treefksolverposfull_recursive.cpp new file mode 100644 index 0000000..c4700cd --- /dev/null +++ b/robot_state_publisher/src/treefksolverposfull_recursive.cpp @@ -0,0 +1,85 @@ +// Copyright (C) 2009 Willow Garage Inc + +// Version: 1.0 +// Author: Wim Meeussen +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +#include "robot_state_publisher/treefksolverposfull_recursive.hpp" +#include +#include +#include + +using namespace std; + +namespace KDL { + +TreeFkSolverPosFull_recursive::TreeFkSolverPosFull_recursive(const Tree& _tree): + tree(_tree) +{ +} + +TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive() +{ +} + + +int TreeFkSolverPosFull_recursive::JntToCart(const map& q_in, map >& p_out, bool flatten_tree) +{ + // clear output + p_out.clear(); + + addFrameToMap(q_in, p_out, tf::Stamped(KDL::Frame::Identity(), ros::Time(), tree.getRootSegment()->second.segment.getName()), tree.getRootSegment(), flatten_tree); + + return 0; +} + + + +void TreeFkSolverPosFull_recursive::addFrameToMap(const map& q_in, + map >& p_out, + const tf::Stamped& previous_frame, + const SegmentMap::const_iterator this_segment, + bool flatten_tree) +{ + // get pose of this segment + tf::Stamped this_frame; + double jnt_p = 0; + if (this_segment->second.segment.getJoint().getType() != Joint::None){ + map::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName()); + if (jnt_pos == q_in.end()){ + ROS_DEBUG("Warning: TreeFKSolverPosFull Could not find value for joint '%s'. Skipping this tree branch", this_segment->first.c_str()); + return; + } + jnt_p = jnt_pos->second; + } + this_frame = tf::Stamped(previous_frame * this_segment->second.segment.pose(jnt_p), ros::Time(), previous_frame.frame_id_); + + if (this_segment->first != tree.getRootSegment()->first) + p_out.insert(make_pair(this_segment->first, tf::Stamped(this_frame, ros::Time(), previous_frame.frame_id_))); + + // get poses of child segments + for (vector::const_iterator child=this_segment->second.children.begin(); child !=this_segment->second.children.end(); child++){ + if (flatten_tree) + addFrameToMap(q_in, p_out, this_frame, *child, flatten_tree); + else + addFrameToMap(q_in, p_out, tf::Stamped(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree); + } +} + +} diff --git a/robot_state_publisher/test/one_link.urdf b/robot_state_publisher/test/one_link.urdf new file mode 100644 index 0000000..be21ee1 --- /dev/null +++ b/robot_state_publisher/test/one_link.urdf @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/robot_state_publisher/test/pr2.urdf b/robot_state_publisher/test/pr2.urdf new file mode 100644 index 0000000..07308a0 --- /dev/null +++ b/robot_state_publisher/test/pr2.urdf @@ -0,0 +1,3376 @@ + + + + + + + + + + + + + + + + + + + + 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.05 + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + 0.0 0.0 0.0 + false + -79.9999999086 + 79.9999999086 + 0.05 + 10.0 + 0.01 + 40 + + 0.005 + true + 40 + tilt_scan + laser_tilt_link + + + + + + + + + + + -6.05 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + 32.6525111499 + + + + 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 + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + r_upper_arm_link_geom + + 100.0 + + true + 100.0 + r_upper_arm_bumper + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + + r_elbow_flex_link_geom + 100.0 + + true + 100.0 + r_elbow_flex_bumper + + + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + false + + r_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + + true + false + + 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 + base_link + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_force_ground_truth + r_gripper_l_finger_link + + + + + + + + + + + + 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_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 + + + + 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 + + + + + + + + + 63.1552452977 + + + + + 61.8948225713 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + l_upper_arm_link_geom + + 100.0 + + true + 100.0 + l_upper_arm_bumper + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + -90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -1.0 + + + + l_elbow_flex_link_geom + 100.0 + + true + 100.0 + l_elbow_flex_bumper + + + + + true + + + + + + + + + -36.167452007 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + false + + l_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + + + true + false + + 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 + base_link + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_force_ground_truth + l_gripper_l_finger_link + + + + + + + + + + + + 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_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 + + diff --git a/robot_state_publisher/test/test_one_link.cpp b/robot_state_publisher/test/test_one_link.cpp new file mode 100644 index 0000000..3115042 --- /dev/null +++ b/robot_state_publisher/test/test_one_link.cpp @@ -0,0 +1,111 @@ +/********************************************************************* +* 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 +#include +#include "robot_state_publisher/joint_state_listener.h" + + +using namespace ros; +using namespace tf; +using namespace robot_state_publisher; + + +int g_argc; +char** g_argv; + +#define EPS 0.01 + +class TestPublisher : public testing::Test +{ +public: + JointStateListener* publisher; + +protected: + /// constructor + TestPublisher() + {} + + /// Destructor + ~TestPublisher() + {} +}; + + + + + +TEST_F(TestPublisher, test) +{ + ROS_INFO("Creating tf listener"); + TransformListener tf; + + ROS_INFO("Publishing joint state to robot state publisher"); + ros::NodeHandle n; + ros::Publisher js_pub = n.advertise("joint_states", 100); + sensor_msgs::JointState js_msg; + for (unsigned int i=0; i<100; i++){ + js_msg.header.stamp = ros::Time::now(); + js_pub.publish(js_msg); + ros::Duration(0.1).sleep(); + } + + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_robot_state_publisher"); + ros::NodeHandle node; + boost::thread ros_thread(boost::bind(&ros::spin)); + + g_argc = argc; + g_argv = argv; + int res = RUN_ALL_TESTS(); + ros_thread.interrupt(); + ros_thread.join(); + + return res; +} diff --git a/robot_state_publisher/test/test_one_link.launch b/robot_state_publisher/test/test_one_link.launch new file mode 100644 index 0000000..f46d8ee --- /dev/null +++ b/robot_state_publisher/test/test_one_link.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/robot_state_publisher/test/test_publisher.cpp b/robot_state_publisher/test/test_publisher.cpp new file mode 100644 index 0000000..530170e --- /dev/null +++ b/robot_state_publisher/test/test_publisher.cpp @@ -0,0 +1,122 @@ +/********************************************************************* +* 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 +#include +#include "robot_state_publisher/joint_state_listener.h" + + +using namespace ros; +using namespace tf; +using namespace robot_state_publisher; + + +int g_argc; +char** g_argv; + +#define EPS 0.01 + +class TestPublisher : public testing::Test +{ +public: + JointStateListener* publisher; + +protected: + /// constructor + TestPublisher() + {} + + /// Destructor + ~TestPublisher() + {} +}; + + + + + +TEST_F(TestPublisher, test) +{ + ROS_INFO("Creating tf listener"); + TransformListener tf; + + ROS_INFO("Waiting for bag to complete"); + Duration(15.0).sleep(); + + ASSERT_TRUE(tf.canTransform("base_link", "torso_lift_link", Time())); + ASSERT_TRUE(tf.canTransform("base_link", "r_gripper_palm_link", Time())); + ASSERT_TRUE(tf.canTransform("base_link", "r_gripper_palm_link", Time())); + ASSERT_TRUE(tf.canTransform("l_gripper_palm_link", "r_gripper_palm_link", Time())); + ASSERT_TRUE(tf.canTransform("l_gripper_palm_link", "fl_caster_r_wheel_link", Time())); + ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time())); + + tf::StampedTransform t; + tf.lookupTransform("base_link", "r_gripper_palm_link",Time(), t ); + EXPECT_NEAR(t.getOrigin().x(), 0.769198, EPS); + EXPECT_NEAR(t.getOrigin().y(), -0.188800, EPS); + EXPECT_NEAR(t.getOrigin().z(), 0.764914, EPS); + + tf.lookupTransform("l_gripper_palm_link", "r_gripper_palm_link",Time(), t ); + EXPECT_NEAR(t.getOrigin().x(), 0.000384222, EPS); + EXPECT_NEAR(t.getOrigin().y(), -0.376021, EPS); + EXPECT_NEAR(t.getOrigin().z(), -1.0858e-05, EPS); + + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_robot_state_publisher"); + ros::NodeHandle node; + boost::thread ros_thread(boost::bind(&ros::spin)); + + g_argc = argc; + g_argv = argv; + int res = RUN_ALL_TESTS(); + ros_thread.interrupt(); + ros_thread.join(); + + return res; +} diff --git a/robot_state_publisher/test/test_publisher.launch b/robot_state_publisher/test/test_publisher.launch new file mode 100644 index 0000000..29aa39d --- /dev/null +++ b/robot_state_publisher/test/test_publisher.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/robot_state_publisher/test/test_two_links_fixed_joint.cpp b/robot_state_publisher/test/test_two_links_fixed_joint.cpp new file mode 100644 index 0000000..530ffd1 --- /dev/null +++ b/robot_state_publisher/test/test_two_links_fixed_joint.cpp @@ -0,0 +1,114 @@ +/********************************************************************* +* 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 +#include +#include "robot_state_publisher/joint_state_listener.h" + + +using namespace ros; +using namespace tf; +using namespace robot_state_publisher; + + +int g_argc; +char** g_argv; + +#define EPS 0.01 + +class TestPublisher : public testing::Test +{ +public: + JointStateListener* publisher; + +protected: + /// constructor + TestPublisher() + {} + + /// Destructor + ~TestPublisher() + {} +}; + + + + + +TEST_F(TestPublisher, test) +{ + ROS_INFO("Creating tf listener"); + TransformListener tf; + + // don't need to publish joint state for tree with only fixed joints + // wait for tf data to come in + ros::Duration(10.0).sleep(); + + ASSERT_TRUE(tf.canTransform("link1", "link2", Time())); + ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time())); + + tf::StampedTransform t; + tf.lookupTransform("link1", "link2",Time(), t ); + EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS); + EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS); + EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS); + + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_robot_state_publisher"); + ros::NodeHandle node; + boost::thread ros_thread(boost::bind(&ros::spin)); + + g_argc = argc; + g_argv = argv; + int res = RUN_ALL_TESTS(); + ros_thread.interrupt(); + ros_thread.join(); + + return res; +} diff --git a/robot_state_publisher/test/test_two_links_fixed_joint.launch b/robot_state_publisher/test/test_two_links_fixed_joint.launch new file mode 100644 index 0000000..9ad67b0 --- /dev/null +++ b/robot_state_publisher/test/test_two_links_fixed_joint.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/robot_state_publisher/test/test_two_links_moving_joint.cpp b/robot_state_publisher/test/test_two_links_moving_joint.cpp new file mode 100644 index 0000000..2f05f79 --- /dev/null +++ b/robot_state_publisher/test/test_two_links_moving_joint.cpp @@ -0,0 +1,122 @@ +/********************************************************************* +* 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 +#include +#include "robot_state_publisher/joint_state_listener.h" + + +using namespace ros; +using namespace tf; +using namespace robot_state_publisher; + + +int g_argc; +char** g_argv; + +#define EPS 0.01 + +class TestPublisher : public testing::Test +{ +public: + JointStateListener* publisher; + +protected: + /// constructor + TestPublisher() + {} + + /// Destructor + ~TestPublisher() + {} +}; + + + + + +TEST_F(TestPublisher, test) +{ + ROS_INFO("Creating tf listener"); + TransformListener tf; + + ROS_INFO("Publishing joint state to robot state publisher"); + ros::NodeHandle n; + ros::Publisher js_pub = n.advertise("joint_states", 100); + sensor_msgs::JointState js_msg; + js_msg.name.push_back("joint1"); + js_msg.position.push_back(M_PI); + for (unsigned int i=0; i<100; i++){ + js_msg.header.stamp = ros::Time::now(); + js_pub.publish(js_msg); + ros::Duration(0.1).sleep(); + } + + ASSERT_TRUE(tf.canTransform("link1", "link2", Time())); + ASSERT_FALSE(tf.canTransform("base_link", "wim_link", Time())); + + tf::StampedTransform t; + tf.lookupTransform("link1", "link2",Time(), t ); + EXPECT_NEAR(t.getOrigin().x(), 5.0, EPS); + EXPECT_NEAR(t.getOrigin().y(), 0.0, EPS); + EXPECT_NEAR(t.getOrigin().z(), 0.0, EPS); + + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_robot_state_publisher"); + ros::NodeHandle node; + boost::thread ros_thread(boost::bind(&ros::spin)); + + g_argc = argc; + g_argv = argv; + int res = RUN_ALL_TESTS(); + ros_thread.interrupt(); + ros_thread.join(); + + return res; +} diff --git a/robot_state_publisher/test/test_two_links_moving_joint.launch b/robot_state_publisher/test/test_two_links_moving_joint.launch new file mode 100644 index 0000000..51a4b55 --- /dev/null +++ b/robot_state_publisher/test/test_two_links_moving_joint.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/robot_state_publisher/test/two_links_fixed_joint.urdf b/robot_state_publisher/test/two_links_fixed_joint.urdf new file mode 100644 index 0000000..8bd84ef --- /dev/null +++ b/robot_state_publisher/test/two_links_fixed_joint.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/robot_state_publisher/test/two_links_moving_joint.urdf b/robot_state_publisher/test/two_links_moving_joint.urdf new file mode 100644 index 0000000..eb50dd6 --- /dev/null +++ b/robot_state_publisher/test/two_links_moving_joint.urdf @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/rosdep.yaml b/rosdep.yaml new file mode 100644 index 0000000..2d78050 --- /dev/null +++ b/rosdep.yaml @@ -0,0 +1,4 @@ +pcre: + ubuntu: libpcre3-dev + fedora: pcre-devel + debian: libpcre3-dev diff --git a/stack.xml b/stack.xml index 7d7b363..6d3c88c 100644 --- a/stack.xml +++ b/stack.xml @@ -1,13 +1,19 @@ - This package contains a number of tools for making, viewing and using URDF models. - Maintained by David Lu!! - BSD - - http://ros.org/wiki/urdf_tools + + robot_model contains packages for modeling various + aspects of robot information, specified in the Xml Robot + Description Format (URDF). The core package of this stack + is urdf, which parses URDF files, and constructs an + object model (C++) of the robot. + + John Hsu johnhsu@willowgarage.com, Wim Meeussen + GPL,BSD,MIT + + http://ros.org/wiki/robot_model + - - - - + + + diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt new file mode 100644 index 0000000..96355f2 --- /dev/null +++ b/urdf/CMakeLists.txt @@ -0,0 +1,49 @@ +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() + +# necessary for collada reader to create the temporary dae files due to limitations in the URDF geometry +check_function_exists(mkstemps HAVE_MKSTEMPS) +if( HAVE_MKSTEMPS ) + add_definitions("-DHAVE_MKSTEMPS") +endif() + +#common commands for building c++ executables and libraries +rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/model.cpp src/collada_model_reader.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) + +rosbuild_add_executable(check_urdf src/check_urdf.cpp) +target_link_libraries(check_urdf ${PROJECT_NAME}) + +rosbuild_add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp) +target_link_libraries(urdf_to_graphiz ${PROJECT_NAME}) + +rosbuild_add_executable(mem_test test/memtest.cpp) +target_link_libraries(mem_test ${PROJECT_NAME}) + + +rosbuild_add_executable(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp) +rosbuild_add_gtest_build_flags(test_parser) +target_link_libraries(test_parser ${PROJECT_NAME}) +rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch) diff --git a/urdf/Makefile b/urdf/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/urdf/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/urdf/include/urdf/color.h b/urdf/include/urdf/color.h new file mode 100644 index 0000000..c132da4 --- /dev/null +++ b/urdf/include/urdf/color.h @@ -0,0 +1,104 @@ +/********************************************************************* +* 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_COLOR_H +#define URDF_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/include/urdf/joint.h b/urdf/include/urdf/joint.h new file mode 100644 index 0000000..551e2c6 --- /dev/null +++ b/urdf/include/urdf/joint.h @@ -0,0 +1,235 @@ +/********************************************************************* +* 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_JOINT_H +#define URDF_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; + multiplier = 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/include/urdf/link.h b/urdf/include/urdf/link.h new file mode 100644 index 0000000..af973e2 --- /dev/null +++ b/urdf/include/urdf/link.h @@ -0,0 +1,250 @@ +/********************************************************************* +* 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_LINK_H +#define URDF_LINK_H + +#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 *); +}; + +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(); + }; + bool initXml(TiXmlElement* config); +}; + +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 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); + + boost::shared_ptr > > getCollision(const std::string& grouip_name) const; +private: + boost::weak_ptr parent_link_; + +}; + + + + +} + +#endif diff --git a/urdf/include/urdf/model.h b/urdf/include/urdf/model.h new file mode 100644 index 0000000..0d52895 --- /dev/null +++ b/urdf/include/urdf/model.h @@ -0,0 +1,119 @@ +/********************************************************************* +* 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 ROBOT_MODEL_PARSER_H +#define ROBOT_MODEL_PARSER_H + +#include +#include +#include +#include +#include "link.h" + + +namespace urdf{ + +class Model +{ +public: + Model(); + + /// \brief Load Model from TiXMLElement + bool initXml(TiXmlElement *xml); + /// \brief Load Model from TiXMLDocument + bool initXml(TiXmlDocument *xml); + /// \brief Load Model given a filename + bool initFile(const std::string& filename); + /// \brief Load Model given the name of a parameter on the parameter server + bool initParam(const std::string& param); + /// \brief Load Model from a XML-string + bool initString(const std::string& xmlstring); + + boost::shared_ptr getRoot(void) const{return this->root_link_;}; + boost::shared_ptr getLink(const std::string& name) const; + boost::shared_ptr getJoint(const std::string& name) const; + const std::string& getName() const {return name_;}; + + void getLinks(std::vector >& links) const; + + /// \brief get parent Link of a Link given name + boost::shared_ptr getParentLink(const std::string& name) const; + /// \brief get parent Joint of a Link given name + boost::shared_ptr getParentJoint(const std::string& name) const; + /// \brief get child Link of a Link given name + boost::shared_ptr getChildLink(const std::string& name) const; + /// \brief get child Joint of a Link given name + boost::shared_ptr getChildJoint(const std::string& name) const; + + /// \brief complete list of Links + std::map > links_; + /// \brief complete list of Joints + std::map > joints_; + /// \brief complete list of Materials + std::map > materials_; + +protected: + void clear(); + + std::string name_; + + /// non-const getLink() + void getLink(const std::string& name,boost::shared_ptr &link) const; + + /// non-const getMaterial() + boost::shared_ptr getMaterial(const std::string& name) const; + + /// in initXml(), onece all links are loaded, + /// it's time to build a tree + bool initTree(std::map &parent_link_tree); + + /// in initXml(), onece tree is built, + /// it's time to find the root Link + bool initRoot(std::map &parent_link_tree); + +private: + /// Model is restricted to a tree for now, which means there exists one root link + /// typically, root link is the world(inertial). Where world is a special link + /// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint? + /// hmm... + boost::shared_ptr root_link_; + + friend class ColladaModelReader; +}; + +} + +#endif diff --git a/urdf/include/urdf/pose.h b/urdf/include/urdf/pose.h new file mode 100644 index 0000000..9013806 --- /dev/null +++ b/urdf/include/urdf/pose.h @@ -0,0 +1,251 @@ +/********************************************************************* +* 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_POSE_H +#define URDF_POSE_H + +#include +#include +#include +#include +#include +#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; + }; +}; + +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; + } + }; +}; + +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/mainpage.dox b/urdf/mainpage.dox new file mode 100644 index 0000000..9287a45 --- /dev/null +++ b/urdf/mainpage.dox @@ -0,0 +1,159 @@ +/** +\mainpage +\htmlinclude manifest.html + +urdf::Model is a class containing robot model data structure. +Every Robot Description File (URDF) can be described as a list of Links (urdf::Model::links_) and Joints (urdf::Model::joints_). +The connection between links(nodes) and joints(edges) should define a tree (i.e. 1 parent link, 0+ children links). +\li Here is an example Robot Description Describing a Parent Link 'P', a Child Link 'C', and a Joint 'J' + @verbatim + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @endverbatim + + + +\section codeapi Code API + +The URDF parser API contains the following methods: + \li Parse and build tree from XML: urdf::Model::initXml + \li Parse and build tree from File: urdf::Model::initFile + \li Parse and build tree from String: urdf::Model::initString + \li Get Root Link: urdf::Model::getRoot + \li Get Link by name urdf::Model::getLink + \li Get all Link's urdf::Model::getLinks + \li Get Joint by name urdf::Model::getJoint + + + + + + + + +*/ diff --git a/urdf/manifest.xml b/urdf/manifest.xml new file mode 100644 index 0000000..c3987b1 --- /dev/null +++ b/urdf/manifest.xml @@ -0,0 +1,26 @@ + + + This package contains a C++ parser for the Unified Robot Description + Format (URDF), which is an XML format for representing a robot model. + The code API of the parser has been through our review process and will remain + backwards compatible in future releases. + + Wim Meeussen, John Hsu, Rosen Diankov + BSD + + http://ros.org/wiki/urdf + + + + + + + + + + + + + + + diff --git a/urdf/src/check_urdf.cpp b/urdf/src/check_urdf.cpp new file mode 100644 index 0000000..6f37716 --- /dev/null +++ b/urdf/src/check_urdf.cpp @@ -0,0 +1,100 @@ +/********************************************************************* +* 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/model.h" +#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 xml file to parse" << std::endl; + return -1; + } + TiXmlDocument robot_model_xml; + robot_model_xml.LoadFile(argv[1]); + TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot"); + if (!robot_xml){ + std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl; + return -1; + } + + Model robot; + if (!robot.initXml(robot_xml)){ + 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/src/collada_model_reader.cpp b/urdf/src/collada_model_reader.cpp new file mode 100644 index 0000000..678aee8 --- /dev/null +++ b/urdf/src/collada_model_reader.cpp @@ -0,0 +1,2547 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010, University of Tokyo +* 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: Rosen Diankov, used OpenRAVE files for reference */ +#include "urdf/model.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#ifndef HAVE_MKSTEMPS +#include +#include +#endif + +#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++) +#define FOREACHC FOREACH + +namespace urdf{ + +class UnlinkFilename +{ +public: + UnlinkFilename(const std::string& filename) : _filename(filename) {} + virtual ~UnlinkFilename() { unlink(_filename.c_str()); } + std::string _filename; +}; +static std::list > _listTempFilenames; + +class ColladaModelReader : public daeErrorHandler +{ + + class JointAxisBinding + { + public: + JointAxisBinding(daeElementRef pvisualtrans, domAxis_constraintRef pkinematicaxis, domCommon_float_or_paramRef jointvalue, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) : pvisualtrans(pvisualtrans), pkinematicaxis(pkinematicaxis), jointvalue(jointvalue), kinematics_axis_info(kinematics_axis_info), motion_axis_info(motion_axis_info) { + BOOST_ASSERT( !!pkinematicaxis ); + daeElement* pae = pvisualtrans->getParentElement(); + while (!!pae) { + visualnode = daeSafeCast (pae); + if (!!visualnode) { + break; + } + pae = pae->getParentElement(); + } + + if (!visualnode) { + ROS_WARN_STREAM(str(boost::format("couldn't find parent node of element id %s, sid %s\n")%pkinematicaxis->getID()%pkinematicaxis->getSid())); + } + } + + daeElementRef pvisualtrans; + domAxis_constraintRef pkinematicaxis; + domCommon_float_or_paramRef jointvalue; + domNodeRef visualnode; + domKinematics_axis_infoRef kinematics_axis_info; + domMotion_axis_infoRef motion_axis_info; + }; + + /// \brief inter-collada bindings for a kinematics scene + class KinematicsSceneBindings + { + public: + std::list< std::pair > listKinematicsVisualBindings; + std::list listAxisBindings; + + bool AddAxisInfo(const domInstance_kinematics_model_Array& arr, domKinematics_axis_infoRef kinematics_axis_info, domMotion_axis_infoRef motion_axis_info) + { + if( !kinematics_axis_info ) { + return false; + } + for(size_t ik = 0; ik < arr.getCount(); ++ik) { + daeElement* pelt = daeSidRef(kinematics_axis_info->getAxis(), arr[ik]->getUrl().getElement()).resolve().elt; + if( !!pelt ) { + // look for the correct placement + bool bfound = false; + FOREACH(itbinding,listAxisBindings) { + if( itbinding->pkinematicaxis.cast() == pelt ) { + itbinding->kinematics_axis_info = kinematics_axis_info; + if( !!motion_axis_info ) { + itbinding->motion_axis_info = motion_axis_info; + } + bfound = true; + break; + } + } + if( !bfound ) { + ROS_WARN_STREAM(str(boost::format("could not find binding for axis: %s, %s\n")%kinematics_axis_info->getAxis()%pelt->getAttribute("sid"))); + return false; + } + return true; + } + } + ROS_WARN_STREAM(str(boost::format("could not find kinematics axis target: %s\n")%kinematics_axis_info->getAxis())); + return false; + } + }; + + struct USERDATA + { + USERDATA() {} + USERDATA(double scale) : scale(scale) {} + double scale; + boost::shared_ptr p; ///< custom managed data + }; + + enum GeomType { + GeomNone = 0, + GeomBox = 1, + GeomSphere = 2, + GeomCylinder = 3, + GeomTrimesh = 4, + }; + + struct GEOMPROPERTIES + { + Pose _t; ///< local transformation of the geom primitive with respect to the link's coordinate system + Vector3 vGeomData; ///< for boxes, first 3 values are extents + ///< for sphere it is radius + ///< for cylinder, first 2 values are radius and height + ///< for trimesh, none + Color diffuseColor, ambientColor; ///< hints for how to color the meshes + std::vector vertices; + std::vector indices; + + ///< discretization value is chosen. Should be transformed by _t before rendering + GeomType type; ///< the type of geometry primitive + + + // generate a sphere triangulation starting with an icosahedron + // all triangles are oriented counter clockwise + static void GenerateSphereTriangulation(std::vector realvertices, std::vector realindices, int levels) + { + const double GTS_M_ICOSAHEDRON_X = 0.850650808352039932181540497063011072240401406; + const double GTS_M_ICOSAHEDRON_Y = 0.525731112119133606025669084847876607285497935; + const double GTS_M_ICOSAHEDRON_Z = 0; + std::vector tempvertices[2]; + std::vector tempindices[2]; + + tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y)); + tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z)); + tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X)); + tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X)); + tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z)); + tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y)); + tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, +GTS_M_ICOSAHEDRON_X)); + tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y)); + tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z)); + tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X)); + tempvertices[0].push_back(Vector3(-GTS_M_ICOSAHEDRON_X, -GTS_M_ICOSAHEDRON_Y, +GTS_M_ICOSAHEDRON_Z)); + tempvertices[0].push_back(Vector3(+GTS_M_ICOSAHEDRON_Z, -GTS_M_ICOSAHEDRON_X, +GTS_M_ICOSAHEDRON_Y)); + + const int nindices=60; + int indices[nindices] = { + 0, 1, 2, + 1, 3, 4, + 3, 5, 6, + 2, 4, 7, + 5, 6, 8, + 2, 7, 9, + 0, 5, 8, + 7, 9, 10, + 0, 1, 5, + 7, 10, 11, + 1, 3, 5, + 6, 10, 11, + 3, 6, 11, + 9, 10, 8, + 3, 4, 11, + 6, 8, 10, + 4, 7, 11, + 1, 2, 4, + 0, 8, 9, + 0, 2, 9 + }; + + Vector3 v[3]; + // make sure oriented CCW + for(int i = 0; i < nindices; i += 3 ) { + v[0] = tempvertices[0][indices[i]]; + v[1] = tempvertices[0][indices[i+1]]; + v[2] = tempvertices[0][indices[i+2]]; + if( _dot3(v[0], _cross3(_sub3(v[1],v[0]),_sub3(v[2],v[0]))) < 0 ) { + std::swap(indices[i], indices[i+1]); + } + } + + tempindices[0].resize(nindices); + std::copy(&indices[0],&indices[nindices],tempindices[0].begin()); + std::vector* curvertices = &tempvertices[0], *newvertices = &tempvertices[1]; + std::vector *curindices = &tempindices[0], *newindices = &tempindices[1]; + while(levels-- > 0) { + + newvertices->resize(0); + newvertices->reserve(2*curvertices->size()); + newvertices->insert(newvertices->end(), curvertices->begin(), curvertices->end()); + newindices->resize(0); + newindices->reserve(4*curindices->size()); + + std::map< uint64_t, int > mapnewinds; + std::map< uint64_t, int >::iterator it; + + for(size_t i = 0; i < curindices->size(); i += 3) { + // for ever tri, create 3 new vertices and 4 new triangles. + v[0] = curvertices->at(curindices->at(i)); + v[1] = curvertices->at(curindices->at(i+1)); + v[2] = curvertices->at(curindices->at(i+2)); + + int inds[3]; + for(int j = 0; j < 3; ++j) { + uint64_t key = ((uint64_t)curindices->at(i+j)<<32)|(uint64_t)curindices->at(i + ((j+1)%3)); + it = mapnewinds.find(key); + + if( it == mapnewinds.end() ) { + inds[j] = mapnewinds[key] = mapnewinds[(key<<32)|(key>>32)] = (int)newvertices->size(); + newvertices->push_back(_normalize3(_add3(v[j],v[(j+1)%3 ]))); + } + else { + inds[j] = it->second; + } + } + + newindices->push_back(curindices->at(i)); newindices->push_back(inds[0]); newindices->push_back(inds[2]); + newindices->push_back(inds[0]); newindices->push_back(curindices->at(i+1)); newindices->push_back(inds[1]); + newindices->push_back(inds[2]); newindices->push_back(inds[0]); newindices->push_back(inds[1]); + newindices->push_back(inds[2]); newindices->push_back(inds[1]); newindices->push_back(curindices->at(i+2)); + } + + std::swap(newvertices,curvertices); + std::swap(newindices,curindices); + } + + realvertices = *curvertices; + realindices = *curindices; + } + + bool InitCollisionMesh(double fTessellation=1.0) + { + if( type == GeomTrimesh ) { + return true; + } + indices.clear(); + vertices.clear(); + + if( fTessellation < 0.01f ) { + fTessellation = 0.01f; + } + // start tesselating + switch(type) { + case GeomSphere: { + // log_2 (1+ tess) + GenerateSphereTriangulation(vertices,indices, 3 + (int)(logf(fTessellation) / logf(2.0f)) ); + double fRadius = vGeomData.x; + FOREACH(it, vertices) { + it->x *= fRadius; + it->y *= fRadius; + it->z *= fRadius; + } + break; + } + case GeomBox: { + // trivial + Vector3 ex = vGeomData; + Vector3 v[8] = { Vector3(ex.x, ex.y, ex.z), + Vector3(ex.x, ex.y, -ex.z), + Vector3(ex.x, -ex.y, ex.z), + Vector3(ex.x, -ex.y, -ex.z), + Vector3(-ex.x, ex.y, ex.z), + Vector3(-ex.x, ex.y, -ex.z), + Vector3(-ex.x, -ex.y, ex.z), + Vector3(-ex.x, -ex.y, -ex.z) }; + const int nindices = 36; + int startindices[] = { + 0, 1, 2, + 1, 2, 3, + 4, 5, 6, + 5, 6, 7, + 0, 1, 4, + 1, 4, 5, + 2, 3, 6, + 3, 6, 7, + 0, 2, 4, + 2, 4, 6, + 1, 3, 5, + 3, 5, 7 + }; + + for(int i = 0; i < nindices; i += 3 ) { + Vector3 v1 = v[startindices[i]]; + Vector3 v2 = v[startindices[i+1]]; + Vector3 v3 = v[startindices[i+2]]; + if( _dot3(v1, _sub3(v2,_cross3(v1, _sub3(v3,v1)))) < 0 ) { + std::swap(indices[i], indices[i+1]); + } + } + + vertices.resize(8); + std::copy(&v[0],&v[8],vertices.begin()); + indices.resize(nindices); + std::copy(&startindices[0],&startindices[nindices],indices.begin()); + break; + } + case GeomCylinder: { + // cylinder is on y axis + double rad = vGeomData.x, len = vGeomData.y*0.5f; + + int numverts = (int)(fTessellation*24.0f) + 3; + double dtheta = 2 * M_PI / (double)numverts; + vertices.push_back(Vector3(0,0,len)); + vertices.push_back(Vector3(0,0,-len)); + vertices.push_back(Vector3(rad,0,len)); + vertices.push_back(Vector3(rad,0,-len)); + + for(int i = 0; i < numverts+1; ++i) { + double s = rad * std::sin(dtheta * (double)i); + double c = rad * std::cos(dtheta * (double)i); + + int off = (int)vertices.size(); + vertices.push_back(Vector3(c, s, len)); + vertices.push_back(Vector3(c, s, -len)); + + indices.push_back(0); indices.push_back(off); indices.push_back(off-2); + indices.push_back(1); indices.push_back(off-1); indices.push_back(off+1); + indices.push_back(off-2); indices.push_back(off); indices.push_back(off-1); + indices.push_back(off); indices.push_back(off-1); indices.push_back(off+1); + } + break; + } + default: + BOOST_ASSERT(0); + } + return true; + } + }; + + public: + ColladaModelReader(Model& model) : _dom(NULL), _nGlobalSensorId(0), _nGlobalManipulatorId(0), _model(model) { + daeErrorHandler::setErrorHandler(this); + _resourcedir = "."; + } + virtual ~ColladaModelReader() { + _vuserdata.clear(); + _collada.reset(); + DAE::cleanup(); + } + + bool InitFromFile(const std::string& filename) { + ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s, filename: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE%filename)); + _collada.reset(new DAE); + _dom = _collada->open(filename); + if (!_dom) { + return false; + } + _filename=filename; + + size_t maxchildren = _countChildren(_dom); + _vuserdata.resize(0); + _vuserdata.reserve(maxchildren); + + double dScale = 1.0; + _processUserData(_dom, dScale); + ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren)); + return _Extract(); + } + + bool InitFromData(const std::string& pdata) { + ROS_DEBUG_STREAM(str(boost::format("init COLLADA reader version: %s, namespace: %s\n")%COLLADA_VERSION%COLLADA_NAMESPACE)); + _collada.reset(new DAE); + _dom = _collada->openFromMemory(".",pdata.c_str()); + if (!_dom) { + return false; + } + + size_t maxchildren = _countChildren(_dom); + _vuserdata.resize(0); + _vuserdata.reserve(maxchildren); + + double dScale = 1.0; + _processUserData(_dom, dScale); + ROS_DEBUG_STREAM(str(boost::format("processed children: %d/%d\n")%_vuserdata.size()%maxchildren)); + return _Extract(); + } + +protected: + + /// \extract the first possible robot in the scene + bool _Extract() + { + _model.clear(); + std::list< std::pair > > listPossibleBodies; + domCOLLADA::domSceneRef allscene = _dom->getScene(); + if( !allscene ) { + return false; + } + + // parse each instance kinematics scene, prioritize real robots + for (size_t iscene = 0; iscene < allscene->getInstance_kinematics_scene_array().getCount(); iscene++) { + domInstance_kinematics_sceneRef kiscene = allscene->getInstance_kinematics_scene_array()[iscene]; + domKinematics_sceneRef kscene = daeSafeCast (kiscene->getUrl().getElement().cast()); + if (!kscene) { + continue; + } + boost::shared_ptr bindings(new KinematicsSceneBindings()); + _ExtractKinematicsVisualBindings(allscene->getInstance_visual_scene(),kiscene,*bindings); + for(size_t ias = 0; ias < kscene->getInstance_articulated_system_array().getCount(); ++ias) { + if( _ExtractArticulatedSystem(kscene->getInstance_articulated_system_array()[ias], *bindings) ) { + _PostProcess(); + return true; + } + } + for(size_t ikmodel = 0; ikmodel < kscene->getInstance_kinematics_model_array().getCount(); ++ikmodel) { + listPossibleBodies.push_back(std::make_pair(kscene->getInstance_kinematics_model_array()[ikmodel], bindings)); + } + } + + FOREACH(it, listPossibleBodies) { + if( _ExtractKinematicsModel(it->first, *it->second) ) { + _PostProcess(); + return true; + } + } + + return false; + } + + void _PostProcess() + { + std::map parent_link_tree; + // building tree: name mapping + if (!_model.initTree(parent_link_tree)) { + ROS_ERROR("failed to build tree"); + } + + // find the root link + if (!_model.initRoot(parent_link_tree)) { + ROS_ERROR("failed to find root link"); + } + } + + /// \brief extracts an articulated system. Note that an articulated system can include other articulated systems + bool _ExtractArticulatedSystem(domInstance_articulated_systemRef ias, KinematicsSceneBindings& bindings) + { + if( !ias ) { + return false; + } + ROS_DEBUG_STREAM(str(boost::format("instance articulated system sid %s\n")%ias->getSid())); + domArticulated_systemRef articulated_system = daeSafeCast (ias->getUrl().getElement().cast()); + if( !articulated_system ) { + return false; + } + + boost::shared_ptr pinterface_type = _ExtractInterfaceType(ias->getExtra_array()); + if( !pinterface_type ) { + pinterface_type = _ExtractInterfaceType(articulated_system->getExtra_array()); + } + if( !!pinterface_type ) { + ROS_DEBUG_STREAM(str(boost::format("robot type: %s")%(*pinterface_type))); + } + + // set the name + if( _model.name_.size() == 0 && !!ias->getName() ) { + _model.name_ = ias->getName(); + } + if( _model.name_.size() == 0 && !!ias->getSid()) { + _model.name_ = ias->getSid(); + } + if( _model.name_.size() == 0 && !!articulated_system->getName() ) { + _model.name_ = articulated_system->getName(); + } + if( _model.name_.size() == 0 && !!articulated_system->getId()) { + _model.name_ = articulated_system->getId(); + } + + if( !!articulated_system->getMotion() ) { + domInstance_articulated_systemRef ias_new = articulated_system->getMotion()->getInstance_articulated_system(); + if( !!articulated_system->getMotion()->getTechnique_common() ) { + for(size_t i = 0; i < articulated_system->getMotion()->getTechnique_common()->getAxis_info_array().getCount(); ++i) { + domMotion_axis_infoRef motion_axis_info = articulated_system->getMotion()->getTechnique_common()->getAxis_info_array()[i]; + // this should point to a kinematics axis_info + domKinematics_axis_infoRef kinematics_axis_info = daeSafeCast(daeSidRef(motion_axis_info->getAxis(), ias_new->getUrl().getElement()).resolve().elt); + if( !!kinematics_axis_info ) { + // find the parent kinematics and go through all its instance kinematics models + daeElement* pparent = kinematics_axis_info->getParent(); + while(!!pparent && pparent->typeID() != domKinematics::ID()) { + pparent = pparent->getParent(); + } + BOOST_ASSERT(!!pparent); + bindings.AddAxisInfo(daeSafeCast(pparent)->getInstance_kinematics_model_array(), kinematics_axis_info, motion_axis_info); + } + else { + ROS_WARN_STREAM(str(boost::format("failed to find kinematics axis %s\n")%motion_axis_info->getAxis())); + } + } + } + if( !_ExtractArticulatedSystem(ias_new,bindings) ) { + return false; + } + } + else { + if( !articulated_system->getKinematics() ) { + ROS_WARN_STREAM(str(boost::format("collada tag empty? instance_articulated_system=%s\n")%ias->getID())); + return true; + } + + if( !!articulated_system->getKinematics()->getTechnique_common() ) { + for(size_t i = 0; i < articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array().getCount(); ++i) { + bindings.AddAxisInfo(articulated_system->getKinematics()->getInstance_kinematics_model_array(), articulated_system->getKinematics()->getTechnique_common()->getAxis_info_array()[i], NULL); + } + } + + for(size_t ik = 0; ik < articulated_system->getKinematics()->getInstance_kinematics_model_array().getCount(); ++ik) { + _ExtractKinematicsModel(articulated_system->getKinematics()->getInstance_kinematics_model_array()[ik],bindings); + } + } + + _ExtractRobotManipulators(articulated_system); + _ExtractRobotAttachedSensors(articulated_system); + return true; + } + + bool _ExtractKinematicsModel(domInstance_kinematics_modelRef ikm, KinematicsSceneBindings& bindings) + { + if( !ikm ) { + return false; + } + ROS_DEBUG_STREAM(str(boost::format("instance kinematics model sid %s\n")%ikm->getSid())); + domKinematics_modelRef kmodel = daeSafeCast (ikm->getUrl().getElement().cast()); + if (!kmodel) { + ROS_WARN_STREAM(str(boost::format("%s does not reference valid kinematics\n")%ikm->getSid())); + return false; + } + domPhysics_modelRef pmodel; + boost::shared_ptr pinterface_type = _ExtractInterfaceType(ikm->getExtra_array()); + if( !pinterface_type ) { + pinterface_type = _ExtractInterfaceType(kmodel->getExtra_array()); + } + if( !!pinterface_type ) { + ROS_DEBUG_STREAM(str(boost::format("kinbody interface type: %s")%(*pinterface_type))); + } + + // find matching visual node + domNodeRef pvisualnode; + FOREACH(it, bindings.listKinematicsVisualBindings) { + if( it->second == ikm ) { + pvisualnode = it->first; + break; + } + } + if( !pvisualnode ) { + ROS_WARN_STREAM(str(boost::format("failed to find visual node for instance kinematics model %s\n")%ikm->getSid())); + return false; + } + + if( _model.name_.size() == 0 && !!ikm->getName() ) { + _model.name_ = ikm->getName(); + } + if( _model.name_.size() == 0 && !!ikm->getID() ) { + _model.name_ = ikm->getID(); + } + + if (!_ExtractKinematicsModel(kmodel, pvisualnode, pmodel, bindings.listAxisBindings)) { + ROS_WARN_STREAM(str(boost::format("failed to load kinbody from kinematics model %s\n")%kmodel->getID())); + return false; + } + return true; + } + + /// \brief append the kinematics model to the openrave kinbody + bool _ExtractKinematicsModel(domKinematics_modelRef kmodel, domNodeRef pnode, domPhysics_modelRef pmodel, const std::list& listAxisBindings) + { + std::vector vdomjoints; + ROS_DEBUG_STREAM(str(boost::format("kinematics model: %s\n")%_model.name_)); + if( !!pnode ) { + ROS_DEBUG_STREAM(str(boost::format("node name: %s\n")%pnode->getId())); + } + + // Process joint of the kinbody + domKinematics_model_techniqueRef ktec = kmodel->getTechnique_common(); + + // Store joints + for (size_t ijoint = 0; ijoint < ktec->getJoint_array().getCount(); ++ijoint) { + vdomjoints.push_back(ktec->getJoint_array()[ijoint]); + } + + // Store instances of joints + for (size_t ijoint = 0; ijoint < ktec->getInstance_joint_array().getCount(); ++ijoint) { + domJointRef pelt = daeSafeCast (ktec->getInstance_joint_array()[ijoint]->getUrl().getElement()); + if (!pelt) { + ROS_WARN_STREAM("failed to get joint from instance\n"); + } + else { + vdomjoints.push_back(pelt); + } + } + + ROS_DEBUG_STREAM(str(boost::format("Number of root links in the kmodel %d\n")%ktec->getLink_array().getCount())); + for (size_t ilink = 0; ilink < ktec->getLink_array().getCount(); ++ilink) { + _ExtractLink(ktec->getLink_array()[ilink], ilink == 0 ? pnode : domNodeRef(), Pose(), Pose(), vdomjoints, listAxisBindings); + } + + // TODO: implement mathml + for (size_t iform = 0; iform < ktec->getFormula_array().getCount(); ++iform) { + domFormulaRef pf = ktec->getFormula_array()[iform]; + if (!pf->getTarget()) { + ROS_WARN_STREAM("formula target not valid\n"); + continue; + } + + // find the target joint + boost::shared_ptr pjoint = _getJointFromRef(pf->getTarget()->getParam()->getValue(),pf); + if (!pjoint) { + continue; + } + + if (!!pf->getTechnique_common()) { + daeElementRef peltmath; + daeTArray children; + pf->getTechnique_common()->getChildren(children); + for (size_t ichild = 0; ichild < children.getCount(); ++ichild) { + daeElementRef pelt = children[ichild]; + if (_checkMathML(pelt,std::string("math")) ) { + peltmath = pelt; + } + else { + ROS_WARN_STREAM(str(boost::format("unsupported formula element: %s\n")%pelt->getElementName())); + } + } + if (!!peltmath) { + // full math xml spec not supported, only looking for ax+b pattern: + // a x b + double a = 1, b = 0; + daeElementRef psymboljoint; + BOOST_ASSERT(peltmath->getChildren().getCount()>0); + daeElementRef papplyelt = peltmath->getChildren()[0]; + BOOST_ASSERT(_checkMathML(papplyelt,"apply")); + BOOST_ASSERT(papplyelt->getChildren().getCount()>0); + if( _checkMathML(papplyelt->getChildren()[0],"plus") ) { + BOOST_ASSERT(papplyelt->getChildren().getCount()==3); + daeElementRef pa = papplyelt->getChildren()[1]; + daeElementRef pb = papplyelt->getChildren()[2]; + if( !_checkMathML(papplyelt->getChildren()[1],"apply") ) { + std::swap(pa,pb); + } + if( !_checkMathML(pa,"csymbol") ) { + BOOST_ASSERT(_checkMathML(pa,"apply")); + BOOST_ASSERT(_checkMathML(pa->getChildren()[0],"times")); + if( _checkMathML(pa->getChildren()[1],"csymbol") ) { + psymboljoint = pa->getChildren()[1]; + BOOST_ASSERT(_checkMathML(pa->getChildren()[2],"cn")); + std::stringstream ss(pa->getChildren()[2]->getCharData()); + ss >> a; + } + else { + psymboljoint = pa->getChildren()[2]; + BOOST_ASSERT(_checkMathML(pa->getChildren()[1],"cn")); + std::stringstream ss(pa->getChildren()[1]->getCharData()); + ss >> a; + } + } + else { + psymboljoint = pa; + } + BOOST_ASSERT(_checkMathML(pb,"cn")); + { + std::stringstream ss(pb->getCharData()); + ss >> b; + } + } + else if( _checkMathML(papplyelt->getChildren()[0],"minus") ) { + BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[1],"csymbol")); + a = -1; + psymboljoint = papplyelt->getChildren()[1]; + } + else { + BOOST_ASSERT(_checkMathML(papplyelt->getChildren()[0],"csymbol")); + psymboljoint = papplyelt->getChildren()[0]; + } + BOOST_ASSERT(psymboljoint->hasAttribute("encoding")); + BOOST_ASSERT(psymboljoint->getAttribute("encoding")==std::string("COLLADA")); + boost::shared_ptr pbasejoint = _getJointFromRef(psymboljoint->getCharData().c_str(),pf); + if( !!pbasejoint ) { + // set the mimic properties + pjoint->mimic.reset(new JointMimic()); + pjoint->mimic->joint_name = pbasejoint->name; + pjoint->mimic->multiplier = a; + pjoint->mimic->offset = b; + ROS_DEBUG_STREAM(str(boost::format("assigning joint %s to mimic %s %f %f\n")%pjoint->name%pbasejoint->name%a%b)); + } + } + } + } + return true; + } + + /// \brief Extract Link info and add it to an existing body + boost::shared_ptr _ExtractLink(const domLinkRef pdomlink,const domNodeRef pdomnode, const Pose& tParentWorldLink, const Pose& tParentLink, const std::vector& vdomjoints, const std::list& listAxisBindings) { + // Set link name with the name of the COLLADA's Link + std::string linkname = _ExtractLinkName(pdomlink); + if( linkname.size() == 0 ) { + ROS_WARN_STREAM(" has no name or id, falling back to !\n"); + if( !!pdomnode ) { + if (!!pdomnode->getName()) { + linkname = pdomnode->getName(); + } + if( linkname.size() == 0 && !!pdomnode->getID()) { + linkname = pdomnode->getID(); + } + } + } + + boost::shared_ptr plink; + _model.getLink(linkname,plink); + if( !plink ) { + plink.reset(new Link()); + plink->name = linkname; + plink->visual.reset(new Visual()); + _model.links_.insert(std::make_pair(linkname,plink)); + } + + _getUserData(pdomlink)->p = plink; + if( !!pdomnode ) { + ROS_DEBUG_STREAM(str(boost::format("Node Id %s and Name %s\n")%pdomnode->getId()%pdomnode->getName())); + } + + std::list listGeomProperties; + if (!pdomlink) { + ROS_WARN_STREAM("Extract object NOT kinematics !!!\n"); + _ExtractGeometry(pdomnode,listGeomProperties,listAxisBindings,Pose()); + } + else { + ROS_DEBUG_STREAM(str(boost::format("Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount())); + Pose tlink = _poseFromMatrix(_ExtractFullTransform(pdomlink)); + plink->visual->origin = _poseMult(tParentLink, tlink); // use the kinematics coordinate system for each link +// ROS_INFO("link %s rot: %f %f %f %f",linkname.c_str(),plink->visual->origin.rotation.w, plink->visual->origin.rotation.x,plink->visual->origin.rotation.y,plink->visual->origin.rotation.z); +// ROS_INFO("link %s trans: %f %f %f",linkname.c_str(),plink->visual->origin.position.x,plink->visual->origin.position.y,plink->visual->origin.position.z); + + // Get the geometry + _ExtractGeometry(pdomnode,listGeomProperties,listAxisBindings,_poseMult(_poseMult(tParentWorldLink,tlink),plink->visual->origin)); + + ROS_DEBUG_STREAM(str(boost::format("After ExtractGeometry Attachment link elements: %d\n")%pdomlink->getAttachment_full_array().getCount())); + + // Process all atached links + for (size_t iatt = 0; iatt < pdomlink->getAttachment_full_array().getCount(); ++iatt) { + domLink::domAttachment_fullRef pattfull = pdomlink->getAttachment_full_array()[iatt]; + + // get link kinematics transformation + Pose tatt = _poseFromMatrix(_ExtractFullTransform(pattfull)); + + // process attached links + daeElement* peltjoint = daeSidRef(pattfull->getJoint(), pattfull).resolve().elt; + domJointRef pdomjoint = daeSafeCast (peltjoint); + + if (!pdomjoint) { + domInstance_jointRef pdomijoint = daeSafeCast (peltjoint); + if (!!pdomijoint) { + pdomjoint = daeSafeCast (pdomijoint->getUrl().getElement().cast()); + } + } + + if (!pdomjoint || pdomjoint->typeID() != domJoint::ID()) { + ROS_WARN_STREAM(str(boost::format("could not find attached joint %s!\n")%pattfull->getJoint())); + return boost::shared_ptr(); + } + + // get direct child link + if (!pattfull->getLink()) { + ROS_WARN_STREAM(str(boost::format("joint %s needs to be attached to a valid link\n")%pdomjoint->getID())); + continue; + } + + // find the correct joint in the bindings + daeTArray vdomaxes = pdomjoint->getChildrenByType(); + domNodeRef pchildnode; + + // see if joint has a binding to a visual node + FOREACHC(itaxisbinding,listAxisBindings) { + for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) { + // If the binding for the joint axis is found, retrieve the info + if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) { + pchildnode = itaxisbinding->visualnode; + break; + } + } + if( !!pchildnode ) { + break; + } + } + if (!pchildnode) { + ROS_DEBUG_STREAM(str(boost::format("joint %s has no visual binding\n")%pdomjoint->getID())); + } + + // create the joints before creating the child links + std::vector > vjoints(vdomaxes.getCount()); + for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) { + bool joint_active = true; // if not active, put into the passive list + FOREACHC(itaxisbinding,listAxisBindings) { + if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) { + if( !!itaxisbinding->kinematics_axis_info ) { + if( !!itaxisbinding->kinematics_axis_info->getActive() ) { + joint_active = resolveBool(itaxisbinding->kinematics_axis_info->getActive(),itaxisbinding->kinematics_axis_info); + } + } + break; + } + } + + boost::shared_ptr pjoint(new Joint()); + pjoint->limits.reset(new JointLimits()); + pjoint->parent_link_name = plink->name; + + if( !!pdomjoint->getName() ) { + pjoint->name = pdomjoint->getName(); + } + else { + pjoint->name = str(boost::format("dummy%d")%_model.joints_.size()); + } + + if( !joint_active ) { + ROS_INFO_STREAM(str(boost::format("joint %s is passive, but adding to hierarchy\n")%pjoint->name)); + } + + domAxis_constraintRef pdomaxis = vdomaxes[ic]; + if( strcmp(pdomaxis->getElementName(), "revolute") == 0 ) { + pjoint->type = Joint::REVOLUTE; + } + else if( strcmp(pdomaxis->getElementName(), "prismatic") == 0 ) { + pjoint->type = Joint::PRISMATIC; + } + else { + ROS_WARN_STREAM(str(boost::format("unsupported joint type: %s\n")%pdomaxis->getElementName())); + } + + _getUserData(pdomjoint)->p = pjoint; + _getUserData(pdomaxis)->p = boost::shared_ptr(new int(_model.joints_.size())); + _model.joints_[pjoint->name] = pjoint; + vjoints[ic] = pjoint; + } + + boost::shared_ptr pchildlink = _ExtractLink(pattfull->getLink(), pchildnode, _poseMult(_poseMult(tParentWorldLink,tlink), tatt), tatt, vdomjoints, listAxisBindings); + + if (!pchildlink) { + ROS_WARN_STREAM(str(boost::format("Link has no child: %s\n")%plink->name)); + continue; + } + + int numjoints = 0; + for (size_t ic = 0; ic < vdomaxes.getCount(); ++ic) { + domKinematics_axis_infoRef kinematics_axis_info; + domMotion_axis_infoRef motion_axis_info; + FOREACHC(itaxisbinding,listAxisBindings) { + bool bfound = false; + if (vdomaxes[ic] == itaxisbinding->pkinematicaxis) { + kinematics_axis_info = itaxisbinding->kinematics_axis_info; + motion_axis_info = itaxisbinding->motion_axis_info; + bfound = true; + break; + } + } + domAxis_constraintRef pdomaxis = vdomaxes[ic]; + if (!pchildlink) { + // create dummy child link + // multiple axes can be easily done with "empty links" + ROS_WARN_STREAM(str(boost::format("creating dummy link %s, num joints %d\n")%plink->name%numjoints)); + + std::stringstream ss; + ss << plink->name; + ss <<"_dummy" << numjoints; + pchildlink.reset(new Link()); + pchildlink->name = ss.str(); + _model.links_.insert(std::make_pair(pchildlink->name,pchildlink)); + } + + ROS_DEBUG_STREAM(str(boost::format("Joint %s assigned %d \n")%vjoints[ic]->name%ic)); + boost::shared_ptr pjoint = vjoints[ic]; + pjoint->child_link_name = pchildlink->name; + + // Axes and Anchor assignment. + pjoint->axis.x = pdomaxis->getAxis()->getValue()[0]; + pjoint->axis.y = pdomaxis->getAxis()->getValue()[1]; + pjoint->axis.z = pdomaxis->getAxis()->getValue()[2]; + + if (!motion_axis_info) { + ROS_WARN_STREAM(str(boost::format("No motion axis info for joint %s\n")%pjoint->name)); + } + + // Sets the Speed and the Acceleration of the joint + if (!!motion_axis_info) { + if (!!motion_axis_info->getSpeed()) { + pjoint->limits->velocity = resolveFloat(motion_axis_info->getSpeed(),motion_axis_info); + ROS_DEBUG("... Joint Speed: %f...\n",pjoint->limits->velocity); + } + if (!!motion_axis_info->getAcceleration()) { + pjoint->limits->effort = resolveFloat(motion_axis_info->getAcceleration(),motion_axis_info); + ROS_DEBUG("... Joint effort: %f...\n",pjoint->limits->effort); + } + } + + bool joint_locked = false; // if locked, joint angle is static + bool kinematics_limits = false; + + if (!!kinematics_axis_info) { + if (!!kinematics_axis_info->getLocked()) { + joint_locked = resolveBool(kinematics_axis_info->getLocked(),kinematics_axis_info); + } + + if (joint_locked) { // If joint is locked set limits to the static value. + if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) { + ROS_WARN_STREAM("lock joint!!\n"); + pjoint->limits->lower = 0; + pjoint->limits->upper = 0; + } + } + else if (kinematics_axis_info->getLimits()) { // If there are articulated system kinematics limits + kinematics_limits = true; + double fscale = (pjoint->type == Joint::REVOLUTE)?(M_PI/180.0f):_GetUnitScale(kinematics_axis_info); + if( pjoint->type == Joint::REVOLUTE || pjoint->type ==Joint::PRISMATIC) { + pjoint->limits->lower = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMin(),kinematics_axis_info)); + pjoint->limits->upper = fscale*(double)(resolveFloat(kinematics_axis_info->getLimits()->getMax(),kinematics_axis_info)); + } + } + } + + // Search limits in the joints section + if (!kinematics_axis_info || (!joint_locked && !kinematics_limits)) { + // If there are NO LIMITS + if( !pdomaxis->getLimits() ) { + ROS_DEBUG_STREAM(str(boost::format("There are NO LIMITS in joint %s:%d ...\n")%pjoint->name%kinematics_limits)); + if( pjoint->type == Joint::REVOLUTE ) { + pjoint->type = Joint::CONTINUOUS; // continuous means revolute? + pjoint->limits->lower = -M_PI; + pjoint->limits->upper = M_PI; + } + else { + pjoint->limits->lower = -100000; + pjoint->limits->upper = 100000; + } + } + else { + ROS_DEBUG_STREAM(str(boost::format("There are LIMITS in joint %s ...\n")%pjoint->name)); + double fscale = (pjoint->type == Joint::REVOLUTE)?(M_PI/180.0f):_GetUnitScale(pdomaxis); + pjoint->limits->lower = (double)pdomaxis->getLimits()->getMin()->getValue()*fscale; + pjoint->limits->upper = (double)pdomaxis->getLimits()->getMax()->getValue()*fscale; + } + } + + //ROS_INFO("joint %s axis: %f %f %f",pjoint->name.c_str(),pjoint->axis.x,pjoint->axis.y,pjoint->axis.z); + pjoint->parent_to_joint_origin_transform = tatt; + pjoint->limits->velocity = pjoint->type == Joint::PRISMATIC ? 0.01 : 0.5f; + pchildlink.reset(); + ++numjoints; + } + } + } + + if( pdomlink->getAttachment_start_array().getCount() > 0 ) { + ROS_WARN("urdf collada reader does not support attachment_start\n"); + } + if( pdomlink->getAttachment_end_array().getCount() > 0 ) { + ROS_WARN("urdf collada reader does not support attachment_end\n"); + } + + plink->visual->geometry = _CreateGeometry(plink->name, listGeomProperties); + return plink; + } + + boost::shared_ptr _CreateGeometry(const std::string& name, const std::list& listGeomProperties) + { + boost::shared_ptr geometry(new Mesh()); + geometry->type = Geometry::MESH; + geometry->scale.x = 1; + geometry->scale.y = 1; + geometry->scale.z = 1; + + std::vector vertices; + std::vector indices; + FOREACHC(it, listGeomProperties) { + int voffset = vertices.size(), ioffset = indices.size(); + vertices.resize(vertices.size()+it->vertices.size()); + for(size_t i = 0; i < it->vertices.size(); ++i) { + vertices[voffset+i] = _poseMult(it->_t, it->vertices[i]); + } + indices.resize(indices.size()+it->indices.size()); + for(size_t i = 0; i < it->indices.size(); ++i) { + indices[ioffset+i] = voffset+it->indices[i]; + } + } + + // have to save the geometry into individual collada 1.4 files since URDF does not allow triangle meshes to be specified + std::stringstream daedata; + daedata << str(boost::format("\n\ +\n\ + \n\ + \n\ + Rosen Diankov\n\ + \n\ + robot_model/urdf temporary collada geometry\n\ + \n\ + \n\ + \n\ + Z_UP\n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + 0.0 0.0 0.1 1\n\ + \n\ + \n\ + 0.8 0.8 0.8 1\n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + ")%(vertices.size()*3)); + // fill with vertices + FOREACH(it,vertices) { + daedata << it->x << " " << it->y << " " << it->z << " "; + } + daedata << str(boost::format("\n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ +

")%vertices.size()%(indices.size()/3)); + // fill with indices + FOREACH(it,indices) { + daedata << *it << " "; + } + daedata << str(boost::format("

\n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ + \n\ +")%name%name); + +#ifdef HAVE_MKSTEMPS + geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_XXXXXX.dae")%name); + int fd = mkstemps(&geometry->filename[0],4); +#else + int fd = -1; + for(int iter = 0; iter < 1000; ++iter) { + geometry->filename = str(boost::format("/tmp/collada_model_reader_%s_%d.dae")%name%rand()); + if( !!std::ifstream(geometry->filename.c_str())) { + fd = open(geometry->filename.c_str(),O_WRONLY|O_CREAT|O_EXCL); + if( fd != -1 ) { + break; + } + } + } + if( fd == -1 ) { + ROS_ERROR("failed to open geometry dae file %s",geometry->filename.c_str()); + return geometry; + } +#endif + //ROS_INFO("temp file: %s",geometry->filename.c_str()); + std::string daedatastr = daedata.str(); + if( (size_t)write(fd,daedatastr.c_str(),daedatastr.size()) != daedatastr.size() ) { + ROS_ERROR("failed to write to geometry dae file %s",geometry->filename.c_str()); + } + close(fd); + _listTempFilenames.push_back(boost::shared_ptr(new UnlinkFilename(geometry->filename))); + geometry->filename = std::string("file://") + geometry->filename; + return geometry; + } + + /// Extract Geometry and apply the transformations of the node + /// \param pdomnode Node to extract the goemetry + /// \param plink Link of the kinematics model + void _ExtractGeometry(const domNodeRef pdomnode,std::list& listGeomProperties, const std::list& listAxisBindings, const Pose& tlink) + { + if( !pdomnode ) { + return; + } + + ROS_DEBUG_STREAM(str(boost::format("ExtractGeometry(node,link) of %s\n")%pdomnode->getName())); + + // For all child nodes of pdomnode + for (size_t i = 0; i < pdomnode->getNode_array().getCount(); i++) { + // check if contains a joint + bool contains=false; + FOREACHC(it,listAxisBindings) { + // don't check ID's check if the reference is the same! + if ( (pdomnode->getNode_array()[i]) == (it->visualnode)){ + contains=true; + break; + } + } + if (contains) { + continue; + } + + _ExtractGeometry(pdomnode->getNode_array()[i],listGeomProperties, listAxisBindings,tlink); + // Plink stayes the same for all children + // replace pdomnode by child = pdomnode->getNode_array()[i] + // hope for the best! + // put everything in a subroutine in order to process pdomnode too! + } + + unsigned int nGeomBefore = listGeomProperties.size(); // #of Meshes already associated to this link + + // get the geometry + for (size_t igeom = 0; igeom < pdomnode->getInstance_geometry_array().getCount(); ++igeom) { + domInstance_geometryRef domigeom = pdomnode->getInstance_geometry_array()[igeom]; + domGeometryRef domgeom = daeSafeCast (domigeom->getUrl().getElement()); + if (!domgeom) { + continue; + } + + // Gets materials + std::map mapmaterials; + if (!!domigeom->getBind_material() && !!domigeom->getBind_material()->getTechnique_common()) { + const domInstance_material_Array& matarray = domigeom->getBind_material()->getTechnique_common()->getInstance_material_array(); + for (size_t imat = 0; imat < matarray.getCount(); ++imat) { + domMaterialRef pmat = daeSafeCast(matarray[imat]->getTarget().getElement()); + if (!!pmat) { + mapmaterials[matarray[imat]->getSymbol()] = pmat; + } + } + } + + // Gets the geometry + _ExtractGeometry(domgeom, mapmaterials, listGeomProperties); + } + + std::list::iterator itgeom= listGeomProperties.begin(); + for (unsigned int i=0; i< nGeomBefore; i++) { + itgeom++; // change only the transformations of the newly found geometries. + } + + boost::array tmnodegeom = _poseMult(_matrixFromPose(_poseInverse(tlink)), _poseMult(_getNodeParentTransform(pdomnode), _ExtractFullTransform(pdomnode))); + Pose tnodegeom; + Vector3 vscale(1,1,1); + _decompose(tmnodegeom, tnodegeom, vscale); + +// std::stringstream ss; ss << "geom: "; +// for(int i = 0; i < 4; ++i) { +// ss << tmnodegeom[4*0+i] << " " << tmnodegeom[4*1+i] << " " << tmnodegeom[4*2+i] << " "; +// } +// ROS_INFO(ss.str().c_str()); + + // Switch between different type of geometry PRIMITIVES + for (; itgeom != listGeomProperties.end(); itgeom++) { + itgeom->_t = tnodegeom; + switch (itgeom->type) { + case GeomBox: + itgeom->vGeomData.x *= vscale.x; + itgeom->vGeomData.y *= vscale.y; + itgeom->vGeomData.z *= vscale.z; + break; + case GeomSphere: { + itgeom->vGeomData.x *= std::max(vscale.z, std::max(vscale.x, vscale.y)); + break; + } + case GeomCylinder: + itgeom->vGeomData.x *= std::max(vscale.x, vscale.y); + itgeom->vGeomData.y *= vscale.z; + break; + case GeomTrimesh: + for(size_t i = 0; i < itgeom->vertices.size(); ++i ) { + itgeom->vertices[i] = _poseMult(tmnodegeom,itgeom->vertices[i]); + } + itgeom->_t = Pose(); // reset back to identity + break; + default: + ROS_WARN_STREAM(str(boost::format("unknown geometry type: %d\n")%itgeom->type)); + } + } + } + + /// Paint the Geometry with the color material + /// \param pmat Material info of the COLLADA's model + /// \param geom Geometry properties in OpenRAVE + void _FillGeometryColor(const domMaterialRef pmat, GEOMPROPERTIES& geom) + { + if( !!pmat && !!pmat->getInstance_effect() ) { + domEffectRef peffect = daeSafeCast(pmat->getInstance_effect()->getUrl().getElement().cast()); + if( !!peffect ) { + domProfile_common::domTechnique::domPhongRef pphong = daeSafeCast(peffect->getDescendant(daeElement::matchType(domProfile_common::domTechnique::domPhong::ID()))); + if( !!pphong ) { + if( !!pphong->getAmbient() && !!pphong->getAmbient()->getColor() ) { + domFx_color c = pphong->getAmbient()->getColor()->getValue(); + geom.ambientColor.r = c[0]; + geom.ambientColor.g = c[1]; + geom.ambientColor.b = c[2]; + geom.ambientColor.a = c[3]; + } + if( !!pphong->getDiffuse() && !!pphong->getDiffuse()->getColor() ) { + domFx_color c = pphong->getDiffuse()->getColor()->getValue(); + geom.diffuseColor.r = c[0]; + geom.diffuseColor.g = c[1]; + geom.diffuseColor.b = c[2]; + geom.diffuseColor.a = c[3]; + } + } + } + } + } + + /// Extract the Geometry in TRIANGLES and adds it to OpenRave + /// \param triRef Array of triangles of the COLLADA's model + /// \param vertsRef Array of vertices of the COLLADA's model + /// \param mapmaterials Materials applied to the geometry + /// \param plink Link of the kinematics model + bool _ExtractGeometry(const domTrianglesRef triRef, const domVerticesRef vertsRef, const std::map& mapmaterials, std::list& listGeomProperties) + { + if( !triRef ) { + return false; + } + listGeomProperties.push_back(GEOMPROPERTIES()); + GEOMPROPERTIES& geom = listGeomProperties.back(); + geom.type = GeomTrimesh; + + // resolve the material and assign correct colors to the geometry + if( !!triRef->getMaterial() ) { + std::map::const_iterator itmat = mapmaterials.find(triRef->getMaterial()); + if( itmat != mapmaterials.end() ) { + _FillGeometryColor(itmat->second,geom); + } + } + + size_t triangleIndexStride = 0, vertexoffset = -1; + domInput_local_offsetRef indexOffsetRef; + + for (unsigned int w=0;wgetInput_array().getCount();w++) { + size_t offset = triRef->getInput_array()[w]->getOffset(); + daeString str = triRef->getInput_array()[w]->getSemantic(); + if (!strcmp(str,"VERTEX")) { + indexOffsetRef = triRef->getInput_array()[w]; + vertexoffset = offset; + } + if (offset> triangleIndexStride) { + triangleIndexStride = offset; + } + } + triangleIndexStride++; + + const domList_of_uints& indexArray =triRef->getP()->getValue(); + geom.indices.reserve(triRef->getCount()*3); + geom.vertices.reserve(triRef->getCount()*3); + for (size_t i=0;igetInput_array().getCount();++i) { + domInput_localRef localRef = vertsRef->getInput_array()[i]; + daeString str = localRef->getSemantic(); + if ( strcmp(str,"POSITION") == 0 ) { + const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); + if( !node ) { + continue; + } + double fUnitScale = _GetUnitScale(node); + const domFloat_arrayRef flArray = node->getFloat_array(); + if (!!flArray) { + const domList_of_floats& listFloats = flArray->getValue(); + int k=vertexoffset; + int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' + for(size_t itri = 0; itri < triRef->getCount(); ++itri) { + if(k+2*triangleIndexStride < indexArray.getCount() ) { + for (int j=0;j<3;j++) { + int index0 = indexArray.get(k)*vertexStride; + domFloat fl0 = listFloats.get(index0); + domFloat fl1 = listFloats.get(index0+1); + domFloat fl2 = listFloats.get(index0+2); + k+=triangleIndexStride; + geom.indices.push_back(geom.vertices.size()); + geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); + } + } + } + } + else { + ROS_WARN_STREAM("float array not defined!\n"); + } + break; + } + } + if( geom.indices.size() != 3*triRef->getCount() ) { + ROS_WARN_STREAM("triangles declares wrong count!\n"); + } + geom.InitCollisionMesh(); + return true; + } + + /// Extract the Geometry in TRIGLE FANS and adds it to OpenRave + /// \param triRef Array of triangle fans of the COLLADA's model + /// \param vertsRef Array of vertices of the COLLADA's model + /// \param mapmaterials Materials applied to the geometry + bool _ExtractGeometry(const domTrifansRef triRef, const domVerticesRef vertsRef, const std::map& mapmaterials, std::list& listGeomProperties) + { + if( !triRef ) { + return false; + } + listGeomProperties.push_back(GEOMPROPERTIES()); + GEOMPROPERTIES& geom = listGeomProperties.back(); + geom.type = GeomTrimesh; + + // resolve the material and assign correct colors to the geometry + if( !!triRef->getMaterial() ) { + std::map::const_iterator itmat = mapmaterials.find(triRef->getMaterial()); + if( itmat != mapmaterials.end() ) { + _FillGeometryColor(itmat->second,geom); + } + } + + size_t triangleIndexStride = 0, vertexoffset = -1; + domInput_local_offsetRef indexOffsetRef; + + for (unsigned int w=0;wgetInput_array().getCount();w++) { + size_t offset = triRef->getInput_array()[w]->getOffset(); + daeString str = triRef->getInput_array()[w]->getSemantic(); + if (!strcmp(str,"VERTEX")) { + indexOffsetRef = triRef->getInput_array()[w]; + vertexoffset = offset; + } + if (offset> triangleIndexStride) { + triangleIndexStride = offset; + } + } + triangleIndexStride++; + size_t primitivecount = triRef->getCount(); + if( primitivecount > triRef->getP_array().getCount() ) { + ROS_WARN_STREAM("trifans has incorrect count\n"); + primitivecount = triRef->getP_array().getCount(); + } + for(size_t ip = 0; ip < primitivecount; ++ip) { + domList_of_uints indexArray =triRef->getP_array()[ip]->getValue(); + for (size_t i=0;igetInput_array().getCount();++i) { + domInput_localRef localRef = vertsRef->getInput_array()[i]; + daeString str = localRef->getSemantic(); + if ( strcmp(str,"POSITION") == 0 ) { + const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); + if( !node ) { + continue; + } + double fUnitScale = _GetUnitScale(node); + const domFloat_arrayRef flArray = node->getFloat_array(); + if (!!flArray) { + const domList_of_floats& listFloats = flArray->getValue(); + int k=vertexoffset; + int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' + size_t usedindices = 3*(indexArray.getCount()-2); + if( geom.indices.capacity() < geom.indices.size()+usedindices ) { + geom.indices.reserve(geom.indices.size()+usedindices); + } + if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) { + geom.vertices.reserve(geom.vertices.size()+indexArray.getCount()); + } + size_t startoffset = (int)geom.vertices.size(); + while(k < (int)indexArray.getCount() ) { + int index0 = indexArray.get(k)*vertexStride; + domFloat fl0 = listFloats.get(index0); + domFloat fl1 = listFloats.get(index0+1); + domFloat fl2 = listFloats.get(index0+2); + k+=triangleIndexStride; + geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); + } + for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) { + geom.indices.push_back(startoffset); + geom.indices.push_back(ivert-1); + geom.indices.push_back(ivert); + } + } + else { + ROS_WARN_STREAM("float array not defined!\n"); + } + break; + } + } + } + + geom.InitCollisionMesh(); + return false; + } + + /// Extract the Geometry in TRIANGLE STRIPS and adds it to OpenRave + /// \param triRef Array of Triangle Strips of the COLLADA's model + /// \param vertsRef Array of vertices of the COLLADA's model + /// \param mapmaterials Materials applied to the geometry + bool _ExtractGeometry(const domTristripsRef triRef, const domVerticesRef vertsRef, const std::map& mapmaterials, std::list& listGeomProperties) + { + if( !triRef ) { + return false; + } + listGeomProperties.push_back(GEOMPROPERTIES()); + GEOMPROPERTIES& geom = listGeomProperties.back(); + geom.type = GeomTrimesh; + + // resolve the material and assign correct colors to the geometry + if( !!triRef->getMaterial() ) { + std::map::const_iterator itmat = mapmaterials.find(triRef->getMaterial()); + if( itmat != mapmaterials.end() ) { + _FillGeometryColor(itmat->second,geom); + } + } + size_t triangleIndexStride = 0, vertexoffset = -1; + domInput_local_offsetRef indexOffsetRef; + + for (unsigned int w=0;wgetInput_array().getCount();w++) { + size_t offset = triRef->getInput_array()[w]->getOffset(); + daeString str = triRef->getInput_array()[w]->getSemantic(); + if (!strcmp(str,"VERTEX")) { + indexOffsetRef = triRef->getInput_array()[w]; + vertexoffset = offset; + } + if (offset> triangleIndexStride) { + triangleIndexStride = offset; + } + } + triangleIndexStride++; + size_t primitivecount = triRef->getCount(); + if( primitivecount > triRef->getP_array().getCount() ) { + ROS_WARN_STREAM("tristrips has incorrect count\n"); + primitivecount = triRef->getP_array().getCount(); + } + for(size_t ip = 0; ip < primitivecount; ++ip) { + domList_of_uints indexArray = triRef->getP_array()[ip]->getValue(); + for (size_t i=0;igetInput_array().getCount();++i) { + domInput_localRef localRef = vertsRef->getInput_array()[i]; + daeString str = localRef->getSemantic(); + if ( strcmp(str,"POSITION") == 0 ) { + const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); + if( !node ) { + continue; + } + double fUnitScale = _GetUnitScale(node); + const domFloat_arrayRef flArray = node->getFloat_array(); + if (!!flArray) { + const domList_of_floats& listFloats = flArray->getValue(); + int k=vertexoffset; + int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' + size_t usedindices = indexArray.getCount()-2; + if( geom.indices.capacity() < geom.indices.size()+usedindices ) { + geom.indices.reserve(geom.indices.size()+usedindices); + } + if( geom.vertices.capacity() < geom.vertices.size()+indexArray.getCount() ) { + geom.vertices.reserve(geom.vertices.size()+indexArray.getCount()); + } + + size_t startoffset = (int)geom.vertices.size(); + while(k < (int)indexArray.getCount() ) { + int index0 = indexArray.get(k)*vertexStride; + domFloat fl0 = listFloats.get(index0); + domFloat fl1 = listFloats.get(index0+1); + domFloat fl2 = listFloats.get(index0+2); + k+=triangleIndexStride; + geom.vertices.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); + } + + bool bFlip = false; + for(size_t ivert = startoffset+2; ivert < geom.vertices.size(); ++ivert) { + geom.indices.push_back(ivert-2); + geom.indices.push_back(bFlip ? ivert : ivert-1); + geom.indices.push_back(bFlip ? ivert-1 : ivert); + bFlip = !bFlip; + } + } + else { + ROS_WARN_STREAM("float array not defined!\n"); + } + break; + } + } + } + + geom.InitCollisionMesh(); + return false; + } + + /// Extract the Geometry in TRIANGLE STRIPS and adds it to OpenRave + /// \param triRef Array of Triangle Strips of the COLLADA's model + /// \param vertsRef Array of vertices of the COLLADA's model + /// \param mapmaterials Materials applied to the geometry + bool _ExtractGeometry(const domPolylistRef triRef, const domVerticesRef vertsRef, const std::map& mapmaterials, std::list& listGeomProperties) + { + if( !triRef ) { + return false; + } + listGeomProperties.push_back(GEOMPROPERTIES()); + GEOMPROPERTIES& geom = listGeomProperties.back(); + geom.type = GeomTrimesh; + + // resolve the material and assign correct colors to the geometry + if( !!triRef->getMaterial() ) { + std::map::const_iterator itmat = mapmaterials.find(triRef->getMaterial()); + if( itmat != mapmaterials.end() ) { + _FillGeometryColor(itmat->second,geom); + } + } + + size_t triangleIndexStride = 0,vertexoffset = -1; + domInput_local_offsetRef indexOffsetRef; + for (unsigned int w=0;wgetInput_array().getCount();w++) { + size_t offset = triRef->getInput_array()[w]->getOffset(); + daeString str = triRef->getInput_array()[w]->getSemantic(); + if (!strcmp(str,"VERTEX")) { + indexOffsetRef = triRef->getInput_array()[w]; + vertexoffset = offset; + } + if (offset> triangleIndexStride) { + triangleIndexStride = offset; + } + } + triangleIndexStride++; + const domList_of_uints& indexArray =triRef->getP()->getValue(); + for (size_t i=0;igetInput_array().getCount();++i) { + domInput_localRef localRef = vertsRef->getInput_array()[i]; + daeString str = localRef->getSemantic(); + if ( strcmp(str,"POSITION") == 0 ) { + const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); + if( !node ) { + continue; + } + double fUnitScale = _GetUnitScale(node); + const domFloat_arrayRef flArray = node->getFloat_array(); + if (!!flArray) { + const domList_of_floats& listFloats = flArray->getValue(); + size_t k=vertexoffset; + int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' + for(size_t ipoly = 0; ipoly < triRef->getVcount()->getValue().getCount(); ++ipoly) { + size_t numverts = triRef->getVcount()->getValue()[ipoly]; + if(numverts > 0 && k+(numverts-1)*triangleIndexStride < indexArray.getCount() ) { + size_t startoffset = geom.vertices.size(); + for (size_t j=0;j& mapmaterials, std::list& listGeomProperties) + { + if( !geom ) { + return false; + } + std::vector vconvexhull; + if (geom->getMesh()) { + const domMeshRef meshRef = geom->getMesh(); + for (size_t tg = 0;tggetTriangles_array().getCount();tg++) { + _ExtractGeometry(meshRef->getTriangles_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); + } + for (size_t tg = 0;tggetTrifans_array().getCount();tg++) { + _ExtractGeometry(meshRef->getTrifans_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); + } + for (size_t tg = 0;tggetTristrips_array().getCount();tg++) { + _ExtractGeometry(meshRef->getTristrips_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); + } + for (size_t tg = 0;tggetPolylist_array().getCount();tg++) { + _ExtractGeometry(meshRef->getPolylist_array()[tg], meshRef->getVertices(), mapmaterials, listGeomProperties); + } + if( meshRef->getPolygons_array().getCount()> 0 ) { + ROS_WARN_STREAM("openrave does not support collada polygons\n"); + } + + // if( alltrimesh.vertices.size() == 0 ) { + // const domVerticesRef vertsRef = meshRef->getVertices(); + // for (size_t i=0;igetInput_array().getCount();i++) { + // domInput_localRef localRef = vertsRef->getInput_array()[i]; + // daeString str = localRef->getSemantic(); + // if ( strcmp(str,"POSITION") == 0 ) { + // const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); + // if( !node ) + // continue; + // double fUnitScale = _GetUnitScale(node); + // const domFloat_arrayRef flArray = node->getFloat_array(); + // if (!!flArray) { + // const domList_of_floats& listFloats = flArray->getValue(); + // int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor' + // vconvexhull.reserve(vconvexhull.size()+listFloats.getCount()); + // for (size_t vertIndex = 0;vertIndex < listFloats.getCount();vertIndex+=vertexStride) { + // //btVector3 verts[3]; + // domFloat fl0 = listFloats.get(vertIndex); + // domFloat fl1 = listFloats.get(vertIndex+1); + // domFloat fl2 = listFloats.get(vertIndex+2); + // vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); + // } + // } + // } + // } + // + // _computeConvexHull(vconvexhull,alltrimesh); + // } + + return true; + } + else if (geom->getConvex_mesh()) { + { + const domConvex_meshRef convexRef = geom->getConvex_mesh(); + daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement(); + if ( !!otherElemRef ) { + domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef; + ROS_WARN_STREAM( "otherLinked\n"); + } + else { + ROS_WARN("convexMesh polyCount = %d\n",(int)convexRef->getPolygons_array().getCount()); + ROS_WARN("convexMesh triCount = %d\n",(int)convexRef->getTriangles_array().getCount()); + } + } + + const domConvex_meshRef convexRef = geom->getConvex_mesh(); + //daeString urlref = convexRef->getConvex_hull_of().getURI(); + daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI(); + if (urlref2) { + daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement(); + + // Load all the geometry libraries + for ( size_t i = 0; i < _dom->getLibrary_geometries_array().getCount(); i++) { + domLibrary_geometriesRef libgeom = _dom->getLibrary_geometries_array()[i]; + for (size_t i = 0; i < libgeom->getGeometry_array().getCount(); i++) { + domGeometryRef lib = libgeom->getGeometry_array()[i]; + if (!strcmp(lib->getId(),urlref2+1)) { // skip the # at the front of urlref2 + //found convex_hull geometry + domMesh *meshElement = lib->getMesh();//linkedGeom->getMesh(); + if (meshElement) { + const domVerticesRef vertsRef = meshElement->getVertices(); + for (size_t i=0;igetInput_array().getCount();i++) { + domInput_localRef localRef = vertsRef->getInput_array()[i]; + daeString str = localRef->getSemantic(); + if ( strcmp(str,"POSITION") == 0) { + const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); + if( !node ) { + continue; + } + double fUnitScale = _GetUnitScale(node); + const domFloat_arrayRef flArray = node->getFloat_array(); + if (!!flArray) { + vconvexhull.reserve(vconvexhull.size()+flArray->getCount()); + const domList_of_floats& listFloats = flArray->getValue(); + for (size_t k=0;k+2getCount();k+=3) { + domFloat fl0 = listFloats.get(k); + domFloat fl1 = listFloats.get(k+1); + domFloat fl2 = listFloats.get(k+2); + vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); + } + } + } + } + } + } + } + } + } + else { + //no getConvex_hull_of but direct vertices + const domVerticesRef vertsRef = convexRef->getVertices(); + for (size_t i=0;igetInput_array().getCount();i++) { + domInput_localRef localRef = vertsRef->getInput_array()[i]; + daeString str = localRef->getSemantic(); + if ( strcmp(str,"POSITION") == 0 ) { + const domSourceRef node = daeSafeCast(localRef->getSource().getElement()); + if( !node ) { + continue; + } + double fUnitScale = _GetUnitScale(node); + const domFloat_arrayRef flArray = node->getFloat_array(); + if (!!flArray) { + const domList_of_floats& listFloats = flArray->getValue(); + vconvexhull.reserve(vconvexhull.size()+flArray->getCount()); + for (size_t k=0;k+2getCount();k+=3) { + domFloat fl0 = listFloats.get(k); + domFloat fl1 = listFloats.get(k+1); + domFloat fl2 = listFloats.get(k+2); + vconvexhull.push_back(Vector3(fl0*fUnitScale,fl1*fUnitScale,fl2*fUnitScale)); + } + } + } + } + } + + if( vconvexhull.size()> 0 ) { + listGeomProperties.push_back(GEOMPROPERTIES()); + GEOMPROPERTIES& geom = listGeomProperties.back(); + geom.type = GeomTrimesh; + + //_computeConvexHull(vconvexhull,trimesh); + geom.InitCollisionMesh(); + } + return true; + } + + return false; + } + + /// \brief extract the robot manipulators + void _ExtractRobotManipulators(const domArticulated_systemRef as) + { + ROS_DEBUG("collada manipulators not supported yet"); + } + + /// \brief Extract Sensors attached to a Robot + void _ExtractRobotAttachedSensors(const domArticulated_systemRef as) + { + ROS_DEBUG("collada sensors not supported yet"); + } + + inline daeElementRef _getElementFromUrl(const daeURI &uri) + { + return daeStandardURIResolver(*_collada).resolveElement(uri); + } + + static daeElement* searchBinding(domCommon_sidref_or_paramRef paddr, daeElementRef parent) + { + if( !!paddr->getSIDREF() ) { + return daeSidRef(paddr->getSIDREF()->getValue(),parent).resolve().elt; + } + if (!!paddr->getParam()) { + return searchBinding(paddr->getParam()->getValue(),parent); + } + return NULL; + } + + /// Search a given parameter reference and stores the new reference to search. + /// \param ref the reference name to search + /// \param parent The array of parameter where the method searchs. + static daeElement* searchBinding(daeString ref, daeElementRef parent) + { + if( !parent ) { + return NULL; + } + daeElement* pelt = NULL; + domKinematics_sceneRef kscene = daeSafeCast(parent.cast()); + if( !!kscene ) { + pelt = searchBindingArray(ref,kscene->getInstance_articulated_system_array()); + if( !!pelt ) { + return pelt; + } + return searchBindingArray(ref,kscene->getInstance_kinematics_model_array()); + } + domArticulated_systemRef articulated_system = daeSafeCast(parent.cast()); + if( !!articulated_system ) { + if( !!articulated_system->getKinematics() ) { + pelt = searchBindingArray(ref,articulated_system->getKinematics()->getInstance_kinematics_model_array()); + if( !!pelt ) { + return pelt; + } + } + if( !!articulated_system->getMotion() ) { + return searchBinding(ref,articulated_system->getMotion()->getInstance_articulated_system()); + } + return NULL; + } + // try to get a bind array + daeElementRef pbindelt; + const domKinematics_bind_Array* pbindarray = NULL; + const domKinematics_newparam_Array* pnewparamarray = NULL; + domInstance_articulated_systemRef ias = daeSafeCast(parent.cast()); + if( !!ias ) { + pbindarray = &ias->getBind_array(); + pbindelt = ias->getUrl().getElement(); + pnewparamarray = &ias->getNewparam_array(); + } + if( !pbindarray || !pbindelt ) { + domInstance_kinematics_modelRef ikm = daeSafeCast(parent.cast()); + if( !!ikm ) { + pbindarray = &ikm->getBind_array(); + pbindelt = ikm->getUrl().getElement(); + pnewparamarray = &ikm->getNewparam_array(); + } + } + if( !!pbindarray && !!pbindelt ) { + for (size_t ibind = 0; ibind < pbindarray->getCount(); ++ibind) { + domKinematics_bindRef pbind = (*pbindarray)[ibind]; + if( !!pbind->getSymbol() && strcmp(pbind->getSymbol(), ref) == 0 ) { + // found a match + if( !!pbind->getParam() ) { + return daeSidRef(pbind->getParam()->getRef(), pbindelt).resolve().elt; + } + else if( !!pbind->getSIDREF() ) { + return daeSidRef(pbind->getSIDREF()->getValue(), pbindelt).resolve().elt; + } + } + } + for(size_t inewparam = 0; inewparam < pnewparamarray->getCount(); ++inewparam) { + domKinematics_newparamRef newparam = (*pnewparamarray)[inewparam]; + if( !!newparam->getSid() && strcmp(newparam->getSid(), ref) == 0 ) { + if( !!newparam->getSIDREF() ) { // can only bind with SIDREF + return daeSidRef(newparam->getSIDREF()->getValue(),pbindelt).resolve().elt; + } + ROS_WARN_STREAM(str(boost::format("newparam sid=%s does not have SIDREF\n")%newparam->getSid())); + } + } + } + ROS_WARN_STREAM(str(boost::format("failed to get binding '%s' for element: %s\n")%ref%parent->getElementName())); + return NULL; + } + + static daeElement* searchBindingArray(daeString ref, const domInstance_articulated_system_Array& paramArray) + { + for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) { + daeElement* pelt = searchBinding(ref,paramArray[iikm].cast()); + if( !!pelt ) { + return pelt; + } + } + return NULL; + } + + static daeElement* searchBindingArray(daeString ref, const domInstance_kinematics_model_Array& paramArray) + { + for(size_t iikm = 0; iikm < paramArray.getCount(); ++iikm) { + daeElement* pelt = searchBinding(ref,paramArray[iikm].cast()); + if( !!pelt ) { + return pelt; + } + } + return NULL; + } + + template static xsBoolean resolveBool(domCommon_bool_or_paramRef paddr, const U& parent) { + if( !!paddr->getBool() ) { + return paddr->getBool()->getValue(); + } + if( !paddr->getParam() ) { + ROS_WARN_STREAM("param not specified, setting to 0\n"); + return false; + } + for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) { + domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam]; + if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) { + if( !!pnewparam->getBool() ) { + return pnewparam->getBool()->getValue(); + } + else if( !!pnewparam->getSIDREF() ) { + domKinematics_newparam::domBoolRef ptarget = daeSafeCast(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt); + if( !ptarget ) { + ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID()); + continue; + } + return ptarget->getValue(); + } + } + } + ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue())); + return false; + } + template static domFloat resolveFloat(domCommon_float_or_paramRef paddr, const U& parent) { + if( !!paddr->getFloat() ) { + return paddr->getFloat()->getValue(); + } + if( !paddr->getParam() ) { + ROS_WARN_STREAM("param not specified, setting to 0\n"); + return 0; + } + for(size_t iparam = 0; iparam < parent->getNewparam_array().getCount(); ++iparam) { + domKinematics_newparamRef pnewparam = parent->getNewparam_array()[iparam]; + if( !!pnewparam->getSid() && strcmp(pnewparam->getSid(), paddr->getParam()->getValue()) == 0 ) { + if( !!pnewparam->getFloat() ) { + return pnewparam->getFloat()->getValue(); + } + else if( !!pnewparam->getSIDREF() ) { + domKinematics_newparam::domFloatRef ptarget = daeSafeCast(daeSidRef(pnewparam->getSIDREF()->getValue(), pnewparam).resolve().elt); + if( !ptarget ) { + ROS_WARN("failed to resolve %s from %s\n", pnewparam->getSIDREF()->getValue(), paddr->getID()); + continue; + } + return ptarget->getValue(); + } + } + } + ROS_WARN_STREAM(str(boost::format("failed to resolve %s\n")%paddr->getParam()->getValue())); + return 0; + } + + static bool resolveCommon_float_or_param(daeElementRef pcommon, daeElementRef parent, float& f) + { + daeElement* pfloat = pcommon->getChild("float"); + if( !!pfloat ) { + std::stringstream sfloat(pfloat->getCharData()); + sfloat >> f; + return !!sfloat; + } + daeElement* pparam = pcommon->getChild("param"); + if( !!pparam ) { + if( pparam->hasAttribute("ref") ) { + ROS_WARN_STREAM("cannot process param ref\n"); + } + else { + daeElement* pelt = daeSidRef(pparam->getCharData(),parent).resolve().elt; + if( !!pelt ) { + ROS_WARN_STREAM(str(boost::format("found param ref: %s from %s\n")%pelt->getCharData()%pparam->getCharData())); + } + } + } + return false; + } + + static boost::array _matrixIdentity() + { + boost::array m = {{1,0,0,0,0,1,0,0,0,0,1,0}}; + return m; + }; + + /// Gets all transformations applied to the node + static boost::array _getTransform(daeElementRef pelt) + { + boost::array m = _matrixIdentity(); + domRotateRef protate = daeSafeCast(pelt); + if( !!protate ) { + m = _matrixFromAxisAngle(Vector3(protate->getValue()[0],protate->getValue()[1],protate->getValue()[2]), (double)(protate->getValue()[3]*(M_PI/180.0))); + return m; + } + + domTranslateRef ptrans = daeSafeCast(pelt); + if( !!ptrans ) { + double scale = _GetUnitScale(pelt); + m[3] = ptrans->getValue()[0]*scale; + m[7] = ptrans->getValue()[1]*scale; + m[11] = ptrans->getValue()[2]*scale; + return m; + } + + domMatrixRef pmat = daeSafeCast(pelt); + if( !!pmat ) { + double scale = _GetUnitScale(pelt); + for(int i = 0; i < 3; ++i) { + m[4*i+0] = pmat->getValue()[4*i+0]; + m[4*i+1] = pmat->getValue()[4*i+1]; + m[4*i+2] = pmat->getValue()[4*i+2]; + m[4*i+3] = pmat->getValue()[4*i+3]*scale; + } + return m; + } + + domScaleRef pscale = daeSafeCast(pelt); + if( !!pscale ) { + m[0] = pscale->getValue()[0]; + m[4*1+1] = pscale->getValue()[1]; + m[4*2+2] = pscale->getValue()[2]; + return m; + } + + domLookatRef pcamera = daeSafeCast(pelt); + if( pelt->typeID() == domLookat::ID() ) { + ROS_ERROR_STREAM("look at transform not implemented\n"); + return m; + } + + domSkewRef pskew = daeSafeCast(pelt); + if( !!pskew ) { + ROS_ERROR_STREAM("skew transform not implemented\n"); + } + + return m; + } + + /// Travels recursively the node parents of the given one + /// to extract the Transform arrays that affects the node given + template static boost::array _getNodeParentTransform(const T pelt) { + domNodeRef pnode = daeSafeCast(pelt->getParent()); + if( !pnode ) { + return _matrixIdentity(); + } + return _poseMult(_getNodeParentTransform(pnode), _ExtractFullTransform(pnode)); + } + + /// \brief Travel by the transformation array and calls the _getTransform method + template static boost::array _ExtractFullTransform(const T pelt) { + boost::array t = _matrixIdentity(); + for(size_t i = 0; i < pelt->getContents().getCount(); ++i) { + t = _poseMult(t, _getTransform(pelt->getContents()[i])); + } + return t; + } + + /// \brief Travel by the transformation array and calls the _getTransform method + template static boost::array _ExtractFullTransformFromChildren(const T pelt) { + boost::array t = _matrixIdentity(); + daeTArray children; pelt->getChildren(children); + for(size_t i = 0; i < children.getCount(); ++i) { + t = _poseMult(t, _getTransform(children[i])); + } + return t; + } + + // decompose a matrix into a scale and rigid transform (necessary for model scales) + void _decompose(const boost::array& tm, Pose& tout, Vector3& vscale) + { + vscale.x = 1; vscale.y = 1; vscale.z = 1; + tout = _poseFromMatrix(tm); + } + + virtual void handleError( daeString msg ) + { + ROS_ERROR("COLLADA error: %s\n", msg); + } + + virtual void handleWarning( daeString msg ) + { + ROS_WARN("COLLADA warning: %s\n", msg); + } + + inline static double _GetUnitScale(daeElement* pelt) + { + return ((USERDATA*)pelt->getUserData())->scale; + } + + domTechniqueRef _ExtractOpenRAVEProfile(const domTechnique_Array& arr) + { + for(size_t i = 0; i < arr.getCount(); ++i) { + if( strcmp(arr[i]->getProfile(), "OpenRAVE") == 0 ) { + return arr[i]; + } + } + return domTechniqueRef(); + } + + /// \brief returns an openrave interface type from the extra array + boost::shared_ptr _ExtractInterfaceType(const domExtra_Array& arr) { + for(size_t i = 0; i < arr.getCount(); ++i) { + if( strcmp(arr[i]->getType(),"interface_type") == 0 ) { + domTechniqueRef tec = _ExtractOpenRAVEProfile(arr[i]->getTechnique_array()); + if( !!tec ) { + daeElement* ptype = tec->getChild("interface"); + if( !!ptype ) { + return boost::shared_ptr(new std::string(ptype->getCharData())); + } + } + } + } + return boost::shared_ptr(); + } + + std::string _ExtractLinkName(domLinkRef pdomlink) { + std::string linkname; + if( !!pdomlink ) { + if( !!pdomlink->getName() ) { + linkname = pdomlink->getName(); + } + if( linkname.size() == 0 && !!pdomlink->getID() ) { + linkname = pdomlink->getID(); + } + } + return linkname; + } + + bool _checkMathML(daeElementRef pelt,const std::string& type) + { + if( pelt->getElementName()==type ) { + return true; + } + // check the substring after ':' + std::string name = pelt->getElementName(); + std::size_t pos = name.find_last_of(':'); + if( pos == std::string::npos ) { + return false; + } + return name.substr(pos+1)==type; + } + + boost::shared_ptr _getJointFromRef(xsToken targetref, daeElementRef peltref) { + daeElement* peltjoint = daeSidRef(targetref, peltref).resolve().elt; + domJointRef pdomjoint = daeSafeCast (peltjoint); + + if (!pdomjoint) { + domInstance_jointRef pdomijoint = daeSafeCast (peltjoint); + if (!!pdomijoint) { + pdomjoint = daeSafeCast (pdomijoint->getUrl().getElement().cast()); + } + } + + if (!pdomjoint || pdomjoint->typeID() != domJoint::ID() || !pdomjoint->getName()) { + ROS_WARN_STREAM(str(boost::format("could not find collada joint %s!\n")%targetref)); + return boost::shared_ptr(); + } + + boost::shared_ptr pjoint = _model.joints_[std::string(pdomjoint->getName())]; + if(!pjoint) { + ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName())); + } + return pjoint; + } + + /// \brief go through all kinematics binds to get a kinematics/visual pair + /// \param kiscene instance of one kinematics scene, binds the kinematic and visual models + /// \param bindings the extracted bindings + static void _ExtractKinematicsVisualBindings(domInstance_with_extraRef viscene, domInstance_kinematics_sceneRef kiscene, KinematicsSceneBindings& bindings) + { + domKinematics_sceneRef kscene = daeSafeCast (kiscene->getUrl().getElement().cast()); + if (!kscene) { + return; + } + for (size_t imodel = 0; imodel < kiscene->getBind_kinematics_model_array().getCount(); imodel++) { + domArticulated_systemRef articulated_system; // if filled, contains robot-specific information, so create a robot + domBind_kinematics_modelRef kbindmodel = kiscene->getBind_kinematics_model_array()[imodel]; + if (!kbindmodel->getNode()) { + ROS_WARN_STREAM("do not support kinematics models without references to nodes\n"); + continue; + } + + // visual information + domNodeRef node = daeSafeCast(daeSidRef(kbindmodel->getNode(), viscene->getUrl().getElement()).resolve().elt); + if (!node) { + ROS_WARN_STREAM(str(boost::format("bind_kinematics_model does not reference valid node %s\n")%kbindmodel->getNode())); + continue; + } + + // kinematics information + daeElement* pelt = searchBinding(kbindmodel,kscene); + domInstance_kinematics_modelRef kimodel = daeSafeCast(pelt); + if (!kimodel) { + if( !pelt ) { + ROS_WARN_STREAM("bind_kinematics_model does not reference element\n"); + } + else { + ROS_WARN_STREAM(str(boost::format("bind_kinematics_model cannot find reference to %s:\n")%pelt->getElementName())); + } + continue; + } + bindings.listKinematicsVisualBindings.push_back(std::make_pair(node,kimodel)); + } + // axis info + for (size_t ijoint = 0; ijoint < kiscene->getBind_joint_axis_array().getCount(); ++ijoint) { + domBind_joint_axisRef bindjoint = kiscene->getBind_joint_axis_array()[ijoint]; + daeElementRef pjtarget = daeSidRef(bindjoint->getTarget(), viscene->getUrl().getElement()).resolve().elt; + if (!pjtarget) { + ROS_ERROR_STREAM(str(boost::format("Target Node %s NOT found!!!\n")%bindjoint->getTarget())); + continue; + } + daeElement* pelt = searchBinding(bindjoint->getAxis(),kscene); + domAxis_constraintRef pjointaxis = daeSafeCast(pelt); + if (!pjointaxis) { + continue; + } + bindings.listAxisBindings.push_back(JointAxisBinding(pjtarget, pjointaxis, bindjoint->getValue(), NULL, NULL)); + } + } + + size_t _countChildren(daeElement* pelt) { + size_t c = 1; + daeTArray children; + pelt->getChildren(children); + for (size_t i = 0; i < children.getCount(); ++i) { + c += _countChildren(children[i]); + } + return c; + } + + void _processUserData(daeElement* pelt, double scale) + { + // getChild could be optimized since asset tag is supposed to appear as the first element + domAssetRef passet = daeSafeCast (pelt->getChild("asset")); + if (!!passet && !!passet->getUnit()) { + scale = passet->getUnit()->getMeter(); + } + + _vuserdata.push_back(USERDATA(scale)); + pelt->setUserData(&_vuserdata.back()); + daeTArray children; + pelt->getChildren(children); + for (size_t i = 0; i < children.getCount(); ++i) { + if (children[i] != passet) { + _processUserData(children[i], scale); + } + } + } + + USERDATA* _getUserData(daeElement* pelt) + { + BOOST_ASSERT(!!pelt); + void* p = pelt->getUserData(); + BOOST_ASSERT(!!p); + return (USERDATA*)p; + } + + // + // openrave math functions (from geometry.h) + // + + static Vector3 _poseMult(const Pose& p, const Vector3& v) + { + double ww = 2 * p.rotation.x * p.rotation.x; + double wx = 2 * p.rotation.x * p.rotation.y; + double wy = 2 * p.rotation.x * p.rotation.z; + double wz = 2 * p.rotation.x * p.rotation.w; + double xx = 2 * p.rotation.y * p.rotation.y; + double xy = 2 * p.rotation.y * p.rotation.z; + double xz = 2 * p.rotation.y * p.rotation.w; + double yy = 2 * p.rotation.z * p.rotation.z; + double yz = 2 * p.rotation.z * p.rotation.w; + Vector3 vnew; + vnew.x = (1-xx-yy) * v.x + (wx-yz) * v.y + (wy+xz)*v.z + p.position.x; + vnew.y = (wx+yz) * v.x + (1-ww-yy) * v.y + (xy-wz)*v.z + p.position.y; + vnew.z = (wy-xz) * v.x + (xy+wz) * v.y + (1-ww-xx)*v.z + p.position.z; + return vnew; + } + + static Vector3 _poseMult(const boost::array& m, const Vector3& v) + { + Vector3 vnew; + vnew.x = m[4*0+0] * v.x + m[4*0+1] * v.y + m[4*0+2] * v.z + m[4*0+3]; + vnew.y = m[4*1+0] * v.x + m[4*1+1] * v.y + m[4*1+2] * v.z + m[4*1+3]; + vnew.z = m[4*2+0] * v.x + m[4*2+1] * v.y + m[4*2+2] * v.z + m[4*2+3]; + return vnew; + } + + static boost::array _poseMult(const boost::array& m0, const boost::array& m1) + { + boost::array mres; + mres[0*4+0] = m0[0*4+0]*m1[0*4+0]+m0[0*4+1]*m1[1*4+0]+m0[0*4+2]*m1[2*4+0]; + mres[0*4+1] = m0[0*4+0]*m1[0*4+1]+m0[0*4+1]*m1[1*4+1]+m0[0*4+2]*m1[2*4+1]; + mres[0*4+2] = m0[0*4+0]*m1[0*4+2]+m0[0*4+1]*m1[1*4+2]+m0[0*4+2]*m1[2*4+2]; + mres[1*4+0] = m0[1*4+0]*m1[0*4+0]+m0[1*4+1]*m1[1*4+0]+m0[1*4+2]*m1[2*4+0]; + mres[1*4+1] = m0[1*4+0]*m1[0*4+1]+m0[1*4+1]*m1[1*4+1]+m0[1*4+2]*m1[2*4+1]; + mres[1*4+2] = m0[1*4+0]*m1[0*4+2]+m0[1*4+1]*m1[1*4+2]+m0[1*4+2]*m1[2*4+2]; + mres[2*4+0] = m0[2*4+0]*m1[0*4+0]+m0[2*4+1]*m1[1*4+0]+m0[2*4+2]*m1[2*4+0]; + mres[2*4+1] = m0[2*4+0]*m1[0*4+1]+m0[2*4+1]*m1[1*4+1]+m0[2*4+2]*m1[2*4+1]; + mres[2*4+2] = m0[2*4+0]*m1[0*4+2]+m0[2*4+1]*m1[1*4+2]+m0[2*4+2]*m1[2*4+2]; + mres[3] = m1[3] * m0[0] + m1[7] * m0[1] + m1[11] * m0[2] + m0[3]; + mres[7] = m1[3] * m0[4] + m1[7] * m0[5] + m1[11] * m0[6] + m0[7]; + mres[11] = m1[3] * m0[8] + m1[7] * m0[9] + m1[11] * m0[10] + m0[11]; + return mres; + } + + static Pose _poseMult(const Pose& p0, const Pose& p1) + { + Pose p; + p.position = _poseMult(p0,p1.position); + p.rotation = _quatMult(p0.rotation,p1.rotation); + return p; + } + + static Pose _poseInverse(const Pose& p) + { + Pose pinv; + pinv.rotation.x = -p.rotation.x; + pinv.rotation.y = -p.rotation.y; + pinv.rotation.z = -p.rotation.z; + pinv.rotation.w = p.rotation.w; + Vector3 t = _poseMult(pinv,p.position); + pinv.position.x = -t.x; + pinv.position.y = -t.y; + pinv.position.z = -t.z; + return pinv; + } + + static Rotation _quatMult(const Rotation& quat0, const Rotation& quat1) + { + Rotation q; + q.x = quat0.w*quat1.x + quat0.x*quat1.w + quat0.y*quat1.z - quat0.z*quat1.y; + q.y = quat0.w*quat1.y + quat0.y*quat1.w + quat0.z*quat1.x - quat0.x*quat1.z; + q.z = quat0.w*quat1.z + quat0.z*quat1.w + quat0.x*quat1.y - quat0.y*quat1.x; + q.w = quat0.w*quat1.w - quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z; + double fnorm = std::sqrt(q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w); + // don't touch the divides + q.x /= fnorm; + q.y /= fnorm; + q.z /= fnorm; + q.w /= fnorm; + return q; + } + + static boost::array _matrixFromAxisAngle(const Vector3& axis, double angle) + { + return _matrixFromQuat(_quatFromAxisAngle(axis.x,axis.y,axis.z,angle)); + } + + static boost::array _matrixFromQuat(const Rotation& quat) + { + boost::array m; + double qq1 = 2*quat.x*quat.x; + double qq2 = 2*quat.y*quat.y; + double qq3 = 2*quat.z*quat.z; + m[4*0+0] = 1 - qq2 - qq3; + m[4*0+1] = 2*(quat.x*quat.y - quat.w*quat.z); + m[4*0+2] = 2*(quat.x*quat.z + quat.w*quat.y); + m[4*0+3] = 0; + m[4*1+0] = 2*(quat.x*quat.y + quat.w*quat.z); + m[4*1+1] = 1 - qq1 - qq3; + m[4*1+2] = 2*(quat.y*quat.z - quat.w*quat.x); + m[4*1+3] = 0; + m[4*2+0] = 2*(quat.x*quat.z - quat.w*quat.y); + m[4*2+1] = 2*(quat.y*quat.z + quat.w*quat.x); + m[4*2+2] = 1 - qq1 - qq2; + m[4*2+3] = 0; + return m; + } + + static Pose _poseFromMatrix(const boost::array& m) + { + Pose t; + t.rotation = _quatFromMatrix(m); + t.position.x = m[3]; + t.position.y = m[7]; + t.position.z = m[11]; + return t; + } + + static boost::array _matrixFromPose(const Pose& t) + { + boost::array m = _matrixFromQuat(t.rotation); + m[3] = t.position.x; + m[7] = t.position.y; + m[11] = t.position.z; + return m; + } + + static Rotation _quatFromAxisAngle(double x, double y, double z, double angle) + { + Rotation q; + double axislen = std::sqrt(x*x+y*y+z*z); + if( axislen == 0 ) { + return q; + } + angle *= 0.5; + double sang = std::sin(angle)/axislen; + q.w = std::cos(angle); + q.x = x*sang; + q.y = y*sang; + q.z = z*sang; + return q; + } + + static Rotation _quatFromMatrix(const boost::array& mat) + { + Rotation rot; + double tr = mat[4*0+0] + mat[4*1+1] + mat[4*2+2]; + if (tr >= 0) { + rot.w = tr + 1; + rot.x = (mat[4*2+1] - mat[4*1+2]); + rot.y = (mat[4*0+2] - mat[4*2+0]); + rot.z = (mat[4*1+0] - mat[4*0+1]); + } + else { + // find the largest diagonal element and jump to the appropriate case + if (mat[4*1+1] > mat[4*0+0]) { + if (mat[4*2+2] > mat[4*1+1]) { + rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1; + rot.x = (mat[4*2+0] + mat[4*0+2]); + rot.y = (mat[4*1+2] + mat[4*2+1]); + rot.w = (mat[4*1+0] - mat[4*0+1]); + } + else { + rot.y = (mat[4*1+1] - (mat[4*2+2] + mat[4*0+0])) + 1; + rot.z = (mat[4*1+2] + mat[4*2+1]); + rot.x = (mat[4*0+1] + mat[4*1+0]); + rot.w = (mat[4*0+2] - mat[4*2+0]); + } + } + else if (mat[4*2+2] > mat[4*0+0]) { + rot.z = (mat[4*2+2] - (mat[4*0+0] + mat[4*1+1])) + 1; + rot.x = (mat[4*2+0] + mat[4*0+2]); + rot.y = (mat[4*1+2] + mat[4*2+1]); + rot.w = (mat[4*1+0] - mat[4*0+1]); + } + else { + rot.x = (mat[4*0+0] - (mat[4*1+1] + mat[4*2+2])) + 1; + rot.y = (mat[4*0+1] + mat[4*1+0]); + rot.z = (mat[4*2+0] + mat[4*0+2]); + rot.w = (mat[4*2+1] - mat[4*1+2]); + } + } + double fnorm = std::sqrt(rot.x*rot.x+rot.y*rot.y+rot.z*rot.z+rot.w*rot.w); + // don't touch the divides + rot.x /= fnorm; + rot.y /= fnorm; + rot.z /= fnorm; + rot.w /= fnorm; + return rot; + } + + static double _dot3(const Vector3& v0, const Vector3& v1) + { + return v0.x*v1.x + v0.y*v1.y + v0.z*v1.z; + } + static Vector3 _cross3(const Vector3& v0, const Vector3& v1) + { + Vector3 v; + v.x = v0.y * v1.z - v0.z * v1.y; + v.y = v0.z * v1.x - v0.x * v1.z; + v.z = v0.x * v1.y - v0.y * v1.x; + return v; + } + static Vector3 _sub3(const Vector3& v0, const Vector3& v1) + { + Vector3 v; + v.x = v0.x-v1.x; + v.y = v0.y-v1.y; + v.z = v0.z-v1.z; + return v; + } + static Vector3 _add3(const Vector3& v0, const Vector3& v1) + { + Vector3 v; + v.x = v0.x+v1.x; + v.y = v0.y+v1.y; + v.z = v0.z+v1.z; + return v; + } + static Vector3 _normalize3(const Vector3& v0) + { + Vector3 v; + double norm = std::sqrt(v0.x*v0.x+v0.y*v0.y+v0.z*v0.z); + v.x = v0.x/norm; + v.y = v0.y/norm; + v.z = v0.z/norm; + return v; + } + + boost::shared_ptr _collada; + domCOLLADA* _dom; + std::vector _vuserdata; // all userdata + int _nGlobalSensorId, _nGlobalManipulatorId; + std::string _filename; + std::string _resourcedir; + Model& _model; +}; + +bool urdfFromColladaFile(std::string const& daefilename, Model& model) +{ + ColladaModelReader reader(model); + return reader.InitFromFile(daefilename); +} + +bool urdfFromColladaData(std::string const& data, Model& model) +{ + ColladaModelReader reader(model); + return reader.InitFromData(data); +} + +bool urdfFromTiXML(TiXmlElement *robot_xml, Model& model) +{ + ColladaModelReader reader(model); + // have to convert all xml back to string (sigh..) + std::stringstream ss; + ss << *robot_xml; + return reader.InitFromData(ss.str()); +} + +bool IsColladaFile(const std::string& filename) +{ + size_t len = filename.size(); + if( len < 4 ) + return false; + return filename[len-4] == '.' && ::tolower(filename[len-3]) == 'd' && ::tolower(filename[len-2]) == 'a' && ::tolower(filename[len-1]) == 'e'; +} + +bool IsColladaData(const std::string& data) +{ + return data.find(" +#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; + else if (type_str == "floating") + type = FLOATING; + 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/src/link.cpp b/urdf/src/link.cpp new file mode 100644 index 0000000..e5b86f8 --- /dev/null +++ b/urdf/src/link.cpp @@ -0,0 +1,540 @@ +/********************************************************************* +* 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/link.h" +#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."); + } + } + + 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; + } + } + + // Visual (optional) + TiXmlElement *v = config->FirstChildElement("visual"); + if (v) + { + visual.reset(new Visual); + if (!visual->initXml(v)) + { + ROS_ERROR("Could not parse visual element for Link '%s'", this->name.c_str()); + return false; + } + } + + // 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->getCollision(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->getCollision("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 + this->collision = (*default_collision->begin()); + + return true; +} + +boost::shared_ptr > > Link::getCollision(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/src/model.cpp b/urdf/src/model.cpp new file mode 100644 index 0000000..e244af3 --- /dev/null +++ b/urdf/src/model.cpp @@ -0,0 +1,434 @@ +/********************************************************************* +* 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/model.h" + +namespace urdf{ + +bool urdfFromColladaFile(std::string const& daefilename, Model& model); +bool urdfFromColladaData(std::string const& data, Model& model); +bool urdfFromTiXML(TiXmlElement *robot_xml, Model& model); +bool IsColladaFile(const std::string& filename); +bool IsColladaData(const std::string& data); + +Model::Model() +{ + this->clear(); +} + +void Model::clear() +{ + name_.clear(); + this->links_.clear(); + this->joints_.clear(); + this->materials_.clear(); + this->root_link_.reset(); +} + + +bool Model::initFile(const std::string& filename) +{ + // necessary for COLLADA compatibility + if( IsColladaFile(filename) ) { + return urdfFromColladaFile(filename,*this); + } + TiXmlDocument xml_doc; + xml_doc.LoadFile(filename); + + return initXml(&xml_doc); +} + + +bool Model::initParam(const std::string& param) +{ + ros::NodeHandle nh; + std::string xml_string; + + // gets the location of the robot description on the parameter server + std::string full_param; + if (!nh.searchParam(param, full_param)){ + ROS_ERROR("Could not find parameter %s on parameter server", param.c_str()); + return false; + } + + // read the robot description from the parameter server + if (!nh.getParam(full_param, xml_string)){ + ROS_ERROR("Could not read parameter %s on parameter server", full_param.c_str()); + return false; + } + return initString(xml_string); +} + + +bool Model::initString(const std::string& xml_string) +{ + // necessary for COLLADA compatibility + if( IsColladaData(xml_string) ) { + return urdfFromColladaData(xml_string,*this); + } + + TiXmlDocument xml_doc; + xml_doc.Parse(xml_string.c_str()); + + return initXml(&xml_doc); +} + + +bool Model::initXml(TiXmlDocument *xml_doc) +{ + if (!xml_doc) + { + ROS_ERROR("Could not parse the xml"); + return false; + } + + // necessary for COLLADA compatibility + if( !!xml_doc->RootElement() ) { + if( std::string("COLLADA") == xml_doc->RootElement()->ValueStr() ) { + return urdfFromTiXML(xml_doc->RootElement(),*this); + } + } + + TiXmlElement *robot_xml = xml_doc->FirstChildElement("robot"); + if (!robot_xml) + { + ROS_ERROR("Could not find the 'robot' element in the xml file"); + return false; + } + return initXml(robot_xml); +} + +bool Model::initXml(TiXmlElement *robot_xml) +{ + this->clear(); + + ROS_DEBUG("Parsing robot xml"); + if (!robot_xml) return false; + + // necessary for COLLADA compatibility + if( std::string("COLLADA") == robot_xml->ValueStr() ) { + return urdfFromTiXML(robot_xml,*this); + } + + // Get robot name + const char *name = robot_xml->Attribute("name"); + if (!name) + { + ROS_ERROR("No name given for the robot."); + return false; + } + this->name_ = std::string(name); + + // Get all Material elements + for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) + { + boost::shared_ptr material; + material.reset(new Material); + + if (material->initXml(material_xml)) + { + if (this->getMaterial(material->name)) + { + ROS_ERROR("material '%s' is not unique.", material->name.c_str()); + material.reset(); + return false; + } + else + { + this->materials_.insert(make_pair(material->name,material)); + ROS_DEBUG("successfully added a new material '%s'", material->name.c_str()); + } + } + else + { + ROS_ERROR("material xml is not initialized correctly"); + material.reset(); + return false; + } + } + + // Get all Link elements + for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) + { + boost::shared_ptr link; + link.reset(new Link); + + if (link->initXml(link_xml)) + { + if (this->getLink(link->name)) + { + ROS_ERROR("link '%s' is not unique.", link->name.c_str()); + link.reset(); + return false; + } + else + { + // set link visual material + ROS_DEBUG("setting link '%s' material", link->name.c_str()); + if (link->visual) + { + if (!link->visual->material_name.empty()) + { + if (this->getMaterial(link->visual->material_name)) + { + ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); + link->visual->material = this->getMaterial( link->visual->material_name.c_str() ); + } + else + { + if (link->visual->material) + { + ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); + this->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); + } + else + { + ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); + link.reset(); + return false; + } + } + } + } + + this->links_.insert(make_pair(link->name,link)); + ROS_DEBUG("successfully added a new link '%s'", link->name.c_str()); + } + } + else + { + ROS_ERROR("link xml is not initialized correctly"); + link.reset(); + return false; + } + } + if (this->links_.empty()){ + ROS_ERROR("No link elements found in urdf file"); + return false; + } + + // Get all Joint elements + for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) + { + boost::shared_ptr joint; + joint.reset(new Joint); + + if (joint->initXml(joint_xml)) + { + if (this->getJoint(joint->name)) + { + ROS_ERROR("joint '%s' is not unique.", joint->name.c_str()); + joint.reset(); + return false; + } + else + { + this->joints_.insert(make_pair(joint->name,joint)); + ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str()); + } + } + else + { + ROS_ERROR("joint xml is not initialized correctly"); + joint.reset(); + return false; + } + } + + + // every link has children links and joints, but no parents, so we create a + // local convenience data structure for keeping child->parent relations + std::map parent_link_tree; + parent_link_tree.clear(); + + // building tree: name mapping + if (!this->initTree(parent_link_tree)) + { + ROS_ERROR("failed to build tree"); + return false; + } + + // find the root link + if (!this->initRoot(parent_link_tree)) + { + ROS_ERROR("failed to find root link"); + return false; + } + + return true; +} + +bool Model::initTree(std::map &parent_link_tree) +{ + // loop through all joints, for every link, assign children links and children joints + for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) + { + std::string parent_link_name = joint->second->parent_link_name; + std::string child_link_name = joint->second->child_link_name; + + ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); + if (parent_link_name.empty() || child_link_name.empty()) + { + ROS_ERROR(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str()); + return false; + } + else + { + // find child and parent links + boost::shared_ptr child_link, parent_link; + this->getLink(child_link_name, child_link); + if (!child_link) + { + ROS_ERROR(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() ); + return false; + } + this->getLink(parent_link_name, parent_link); + if (!parent_link) + { + ROS_ERROR(" parent link '%s' of joint '%s' not found. The Boxturtle urdf parser used to automatically add this link for you, but this is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint from your urdf file, or add \"\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() ); + return false; + } + + //set parent link for child link + child_link->setParent(parent_link); + + //set parent joint for child link + child_link->setParentJoint(joint->second); + + //set child joint for parent link + parent_link->addChildJoint(joint->second); + + //set child link for parent link + parent_link->addChild(child_link); + + // fill in child/parent string map + parent_link_tree[child_link->name] = parent_link_name; + + ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size()); + } + } + + return true; +} + + + +bool Model::initRoot(std::map &parent_link_tree) +{ + + this->root_link_.reset(); + + // find the links that have no parent in the tree + for (std::map >::iterator l=this->links_.begin(); l!=this->links_.end(); l++) + { + std::map::iterator parent = parent_link_tree.find(l->first); + if (parent == parent_link_tree.end()) + { + // store root link + if (!this->root_link_) + { + getLink(l->first, this->root_link_); + } + // we already found a root link + else{ + ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str()); + return false; + } + } + } + if (!this->root_link_) + { + ROS_ERROR("No root link found. The robot xml is not a valid tree."); + return false; + } + ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str()); + + return true; +} + +boost::shared_ptr Model::getMaterial(const std::string& name) const +{ + boost::shared_ptr ptr; + if (this->materials_.find(name) == this->materials_.end()) + ptr.reset(); + else + ptr = this->materials_.find(name)->second; + return ptr; +} + +boost::shared_ptr Model::getLink(const std::string& name) const +{ + boost::shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(); + else + ptr = this->links_.find(name)->second; + return ptr; +} + +void Model::getLinks(std::vector >& links) const +{ + for (std::map >::const_iterator link = this->links_.begin();link != this->links_.end(); link++) + { + links.push_back(link->second); + } +} + +void Model::getLink(const std::string& name,boost::shared_ptr &link) const +{ + boost::shared_ptr ptr; + if (this->links_.find(name) == this->links_.end()) + ptr.reset(); + else + ptr = this->links_.find(name)->second; + link = ptr; +} + +boost::shared_ptr Model::getJoint(const std::string& name) const +{ + boost::shared_ptr ptr; + if (this->joints_.find(name) == this->joints_.end()) + ptr.reset(); + else + ptr = this->joints_.find(name)->second; + return ptr; +} + +} + diff --git a/urdf/src/urdf_to_graphiz.cpp b/urdf/src/urdf_to_graphiz.cpp new file mode 100644 index 0000000..3541128 --- /dev/null +++ b/urdf/src/urdf_to_graphiz.cpp @@ -0,0 +1,117 @@ +/********************************************************************* +* 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/model.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; + } + + TiXmlDocument robot_model_xml; + robot_model_xml.LoadFile(argv[1]); + TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot"); + if (!robot_xml){ + std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl; + return -1; + } + + Model robot; + if (!robot.initXml(robot_xml)){ + 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/test/fail_pr2_desc_bracket.urdf b/urdf/test/fail_pr2_desc_bracket.urdf new file mode 100644 index 0000000..d0caaa8 --- /dev/null +++ b/urdf/test/fail_pr2_desc_bracket.urdf @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/fail_pr2_desc_double.urdf b/urdf/test/fail_pr2_desc_double.urdf new file mode 100644 index 0000000..10bbd4c --- /dev/null +++ b/urdf/test/fail_pr2_desc_double.urdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/fail_pr2_desc_double_joint.urdf b/urdf/test/fail_pr2_desc_double_joint.urdf new file mode 100644 index 0000000..2974a7a --- /dev/null +++ b/urdf/test/fail_pr2_desc_double_joint.urdf @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/fail_pr2_desc_loop.urdf b/urdf/test/fail_pr2_desc_loop.urdf new file mode 100644 index 0000000..ab21024 --- /dev/null +++ b/urdf/test/fail_pr2_desc_loop.urdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/fail_pr2_desc_no_filename_in_mesh.urdf b/urdf/test/fail_pr2_desc_no_filename_in_mesh.urdf new file mode 100644 index 0000000..88d75b3 --- /dev/null +++ b/urdf/test/fail_pr2_desc_no_filename_in_mesh.urdf @@ -0,0 +1,3377 @@ + + + + + + + + + + + + + + + + + + + + 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 + + + + + + + diff --git a/urdf/test/fail_pr2_desc_no_joint2.urdf b/urdf/test/fail_pr2_desc_no_joint2.urdf new file mode 100644 index 0000000..be1fdbc --- /dev/null +++ b/urdf/test/fail_pr2_desc_no_joint2.urdf @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/fail_pr2_desc_parent_itself.urdf b/urdf/test/fail_pr2_desc_parent_itself.urdf new file mode 100644 index 0000000..5eda24d --- /dev/null +++ b/urdf/test/fail_pr2_desc_parent_itself.urdf @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/fail_pr2_desc_two_trees.urdf b/urdf/test/fail_pr2_desc_two_trees.urdf new file mode 100644 index 0000000..415e74c --- /dev/null +++ b/urdf/test/fail_pr2_desc_two_trees.urdf @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/fail_three_links_one_joint.urdf b/urdf/test/fail_three_links_one_joint.urdf new file mode 100644 index 0000000..a6766a9 --- /dev/null +++ b/urdf/test/fail_three_links_one_joint.urdf @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/memtest.cpp b/urdf/test/memtest.cpp new file mode 100644 index 0000000..e699d54 --- /dev/null +++ b/urdf/test/memtest.cpp @@ -0,0 +1,10 @@ +#include +#include + +int main(int argc, char** argv){ + ros::init(argc, argv, "memtest"); + while (ros::ok()){ + urdf::Model urdf; + urdf.initFile(std::string(argv[1])); + } +} diff --git a/urdf/test/no_visual.urdf b/urdf/test/no_visual.urdf new file mode 100644 index 0000000..0554bac --- /dev/null +++ b/urdf/test/no_visual.urdf @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/urdf/test/one_link.urdf b/urdf/test/one_link.urdf new file mode 100644 index 0000000..15e64d5 --- /dev/null +++ b/urdf/test/one_link.urdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/pr2_desc.urdf b/urdf/test/pr2_desc.urdf new file mode 100644 index 0000000..cbc5c77 --- /dev/null +++ b/urdf/test/pr2_desc.urdf @@ -0,0 +1,3238 @@ + + + + + + + + + + + + + + + + + + + + + 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 + + diff --git a/urdf/test/pr2_desc_no_joint.urdf b/urdf/test/pr2_desc_no_joint.urdf new file mode 100644 index 0000000..11fc4fd --- /dev/null +++ b/urdf/test/pr2_desc_no_joint.urdf @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/urdf/test/singularity.urdf b/urdf/test/singularity.urdf new file mode 100644 index 0000000..f86fa92 --- /dev/null +++ b/urdf/test/singularity.urdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/test/test_robot_model_parser.cpp b/urdf/test/test_robot_model_parser.cpp new file mode 100644 index 0000000..f238762 --- /dev/null +++ b/urdf/test/test_robot_model_parser.cpp @@ -0,0 +1,150 @@ +/********************************************************************* +* 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 "urdf/model.h" + +// Including ros, just to be able to call ros::init(), to remove unwanted +// args from command-line. +#include + +using namespace urdf; + +int g_argc; +char** g_argv; + +class TestParser : public testing::Test +{ +public: + Model robot; + + bool checkModel() + { + // get root link + boost::shared_ptr root_link=this->robot.getRoot(); + if (!root_link) + { + ROS_ERROR("no root link %s",this->robot.getName().c_str()); + return false; + } + + // go through entire tree + return this->traverse_tree(root_link); + + }; + +protected: + /// constructor + TestParser() + { + } + + + /// Destructor + ~TestParser() + { + } + + bool traverse_tree(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) + { + // check rpy + double roll,pitch,yaw; + (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw); + + if (isnan(roll) || isnan(pitch) || isnan(yaw)) + { + ROS_ERROR("getRPY() returned nan!"); + return false; + } + // recurse down the tree + return this->traverse_tree(*child,level); + } + else + { + ROS_ERROR("root link: %s has a null child!",link->name.c_str()); + return false; + } + } + // no children + return true; + }; +}; + + + + +TEST_F(TestParser, test) +{ + std::string folder = std::string(g_argv[1]) + "/test/"; + ROS_INFO("Folder %s",folder.c_str()); + for (int i=2; i + + + + diff --git a/urdf/test/two_links_one_joint.urdf b/urdf/test/two_links_one_joint.urdf new file mode 100644 index 0000000..e1e0062 --- /dev/null +++ b/urdf/test/two_links_one_joint.urdf @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/urdf_tutorial/01-myfirst.urdf b/urdf_tutorial/01-myfirst.urdf deleted file mode 100644 index 4180a5c..0000000 --- a/urdf_tutorial/01-myfirst.urdf +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/urdf_tutorial/02-multipleshapes.urdf b/urdf_tutorial/02-multipleshapes.urdf deleted file mode 100644 index 53deaad..0000000 --- a/urdf_tutorial/02-multipleshapes.urdf +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf_tutorial/03-origins.urdf b/urdf_tutorial/03-origins.urdf deleted file mode 100644 index 926e1c7..0000000 --- a/urdf_tutorial/03-origins.urdf +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf_tutorial/04-materials.urdf b/urdf_tutorial/04-materials.urdf deleted file mode 100644 index bb9c5ca..0000000 --- a/urdf_tutorial/04-materials.urdf +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf_tutorial/05-visual.urdf b/urdf_tutorial/05-visual.urdf deleted file mode 100644 index ff82229..0000000 --- a/urdf_tutorial/05-visual.urdf +++ /dev/null @@ -1,247 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf_tutorial/06-flexible.urdf b/urdf_tutorial/06-flexible.urdf deleted file mode 100644 index 0875c17..0000000 --- a/urdf_tutorial/06-flexible.urdf +++ /dev/null @@ -1,264 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf_tutorial/07-physics.urdf b/urdf_tutorial/07-physics.urdf deleted file mode 100644 index 9416037..0000000 --- a/urdf_tutorial/07-physics.urdf +++ /dev/null @@ -1,415 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf_tutorial/08-macroed.urdf.xacro b/urdf_tutorial/08-macroed.urdf.xacro deleted file mode 100644 index d51fe2b..0000000 --- a/urdf_tutorial/08-macroed.urdf.xacro +++ /dev/null @@ -1,236 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf_tutorial/display.launch b/urdf_tutorial/display.launch deleted file mode 100644 index 4edccd2..0000000 --- a/urdf_tutorial/display.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/urdf_tutorial/gazebo.launch b/urdf_tutorial/gazebo.launch deleted file mode 100644 index 74fd130..0000000 --- a/urdf_tutorial/gazebo.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf_tutorial/images/flexible.png b/urdf_tutorial/images/flexible.png deleted file mode 100644 index 575a498..0000000 Binary files a/urdf_tutorial/images/flexible.png and /dev/null differ diff --git a/urdf_tutorial/images/materials.png b/urdf_tutorial/images/materials.png deleted file mode 100644 index 0c38f6c..0000000 Binary files a/urdf_tutorial/images/materials.png and /dev/null differ diff --git a/urdf_tutorial/images/multipleshapes.png b/urdf_tutorial/images/multipleshapes.png deleted file mode 100644 index f29cde3..0000000 Binary files a/urdf_tutorial/images/multipleshapes.png and /dev/null differ diff --git a/urdf_tutorial/images/myfirst.png b/urdf_tutorial/images/myfirst.png deleted file mode 100644 index c6e7a9d..0000000 Binary files a/urdf_tutorial/images/myfirst.png and /dev/null differ diff --git a/urdf_tutorial/images/origins.png b/urdf_tutorial/images/origins.png deleted file mode 100644 index 72a268a..0000000 Binary files a/urdf_tutorial/images/origins.png and /dev/null differ diff --git a/urdf_tutorial/images/visual.png b/urdf_tutorial/images/visual.png deleted file mode 100644 index b41adca..0000000 Binary files a/urdf_tutorial/images/visual.png and /dev/null differ diff --git a/urdf_tutorial/manifest.xml b/urdf_tutorial/manifest.xml deleted file mode 100644 index d2f3d34..0000000 --- a/urdf_tutorial/manifest.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - Support code for the step by step URDF tutorials on ROS.org - - David Lu!! - BSD - - http://ros.org/wiki/urdf_tutorial - - - - diff --git a/urdf_tutorial/r2d2.xacro b/urdf_tutorial/r2d2.xacro deleted file mode 100644 index 85a021e..0000000 --- a/urdf_tutorial/r2d2.xacro +++ /dev/null @@ -1,277 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 50.0 - 50.0 - 100000000.0 - 1.0 - Gazebo/Fish - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 100 - left_back_wheel_joint - right_back_wheel_joint - ${width} - ${wheeldiam} - 5 - - - - - true - 100.0 - base_link - base_pose_ground_truth - 0.01 - map - 25.7 25.7 0 - 0 0 0 - - - - - - true - 1000.0 - - - - - diff --git a/urdf_tutorial/urdf.vcg b/urdf_tutorial/urdf.vcg deleted file mode 100644 index cd9e661..0000000 --- a/urdf_tutorial/urdf.vcg +++ /dev/null @@ -1,44 +0,0 @@ -Background\ ColorR=0.0941176 -Background\ ColorG=0 -Background\ ColorB=0.466667 -Fixed\ Frame=/base_link -Target\ Frame= -Grid.Alpha=0.5 -Grid.Cell\ Size=0.5 -Grid.ColorR=0.898039 -Grid.ColorG=0.898039 -Grid.ColorB=0.898039 -Grid.Enabled=1 -Grid.Line\ Style=0 -Grid.Line\ Width=0.03 -Grid.Normal\ Cell\ Count=0 -Grid.OffsetX=0 -Grid.OffsetY=0 -Grid.OffsetZ=0 -Grid.Plane=0 -Grid.Plane\ Cell\ Count=10 -Grid.Reference\ Frame= -Robot\ Model.Alpha=1 -Robot\ Model.Collision\ Enabled=0 -Robot\ Model.Enabled=1 -Robot\ Model.Robot\ Description=robot_description -Robot\ Model.TF\ Prefix= -Robot\ Model.Update\ Interval=0 -Robot\ Model.Visual\ Enabled=1 -Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ legShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ legShow\ Trail=0 -Tool\ 2D\ Nav\ GoalTopic=goal -Tool\ 2D\ Pose\ EstimateTopic=initialpose -Camera\ Type=rviz::OrbitViewController -Camera\ Config=1.15779 3.76081 2.16475 0 0 0 -Property\ Grid\ State=selection=.Global StatusTopStatus;expanded=.Global Options,Grid.Enabled,Robot Model.Enabled;scrollpos=0,0;splitterpos=150,285;ispageselected=1 -[Display0] -Name=Grid -Package=rviz -ClassName=rviz::GridDisplay -[Display1] -Name=Robot Model -Package=rviz -ClassName=rviz::RobotModelDisplay diff --git a/urdf_tutorial/xacrodisplay.launch b/urdf_tutorial/xacrodisplay.launch deleted file mode 100644 index c05d16b..0000000 --- a/urdf_tutorial/xacrodisplay.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - -