├── .gitignore ├── README.md ├── eigen_conversions ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── eigen_conversions │ │ ├── eigen_kdl.h │ │ └── eigen_msg.h ├── mainpage.dox ├── package.xml └── src │ ├── eigen_kdl.cpp │ └── eigen_msg.cpp ├── geometry ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── kdl_conversions ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── kdl_conversions │ │ └── kdl_msg.h ├── mainpage.dox ├── package.xml └── src │ └── kdl_msg.cpp ├── tf ├── CHANGELOG.rst ├── CMakeLists.txt ├── conf.py ├── doc │ ├── bifrucation.gv │ └── bifrucation.pdf ├── include │ └── tf │ │ ├── LinearMath │ │ ├── Matrix3x3.h │ │ ├── MinMax.h │ │ ├── QuadWord.h │ │ ├── Quaternion.h │ │ ├── Scalar.h │ │ ├── Transform.h │ │ └── Vector3.h │ │ ├── exceptions.h │ │ ├── message_filter.h │ │ ├── tf.h │ │ ├── time_cache.h │ │ ├── transform_broadcaster.h │ │ ├── transform_datatypes.h │ │ └── transform_listener.h ├── index.rst ├── mainpage.dox ├── msg │ └── tfMessage.msg ├── package.xml ├── remap_tf.launch ├── rosdoc.yaml ├── scripts │ ├── bullet_migration_sed.py │ ├── groovy_compatibility │ │ ├── tf_remap │ │ └── view_frames │ ├── python_benchmark.py │ ├── tf_remap │ └── view_frames ├── setup.py ├── src │ ├── cache.cpp │ ├── change_notifier.cpp │ ├── empty_listener.cpp │ ├── static_transform_publisher.cpp │ ├── tf.cpp │ ├── tf │ │ ├── __init__.py │ │ ├── broadcaster.py │ │ ├── listener.py │ │ ├── tfwtf.py │ │ └── transformations.py │ ├── tf_echo.cpp │ ├── tf_monitor.cpp │ ├── transform_broadcaster.cpp │ └── transform_listener.cpp ├── srv │ └── FrameGraph.srv ├── test │ ├── cache_unittest.cpp │ ├── method_test.py │ ├── operator_overload.cpp │ ├── python_debug_test.py │ ├── quaternion.cpp │ ├── speed_test.cpp │ ├── testBroadcaster.cpp │ ├── testListener.cpp │ ├── testPython.py │ ├── test_broadcaster.launch │ ├── test_datatype_conversion.py │ ├── test_message_filter.cpp │ ├── test_message_filter.xml │ ├── test_transform_datatypes.cpp │ ├── tf_benchmark.cpp │ ├── tf_unittest.cpp │ ├── tf_unittest_future.cpp │ ├── transform_listener_unittest.cpp │ ├── transform_listener_unittest.launch │ ├── transform_twist_test.cpp │ ├── transform_twist_test.launch │ └── velocity_test.cpp ├── tf_python.rst └── transformations.rst └── tf_conversions ├── CHANGELOG.rst ├── CMakeLists.txt ├── conf.py ├── include └── tf_conversions │ ├── mainpage.dox │ ├── tf_eigen.h │ └── tf_kdl.h ├── index.rst ├── package.xml ├── rosdoc.yaml ├── setup.py ├── src ├── tf_conversions │ ├── __init__.py │ └── posemath.py ├── tf_eigen.cpp └── tf_kdl.cpp └── test ├── posemath.py ├── test_eigen_tf.cpp └── test_kdl_tf.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Archived - ROS 1 End-of-life 2 | 3 | This repository contains ROS 1 packages. 4 | The last supported ROS 1 release, ROS Noetic, [officially reached end of life on May 31st, 2025](https://bit.ly/NoeticEOL). 5 | 6 | -------------------------------------------------------------------------------- /eigen_conversions/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package eigen_conversions 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.13.4 (2025-04-25) 6 | ------------------- 7 | 8 | 1.13.3 (2025-04-10) 9 | ------------------- 10 | * Updated library install for better portability (`#208 `_) (`#214 `_) 11 | * Contributors: Tully Foote 12 | 13 | 1.13.2 (2020-06-08) 14 | ------------------- 15 | 16 | 1.13.1 (2020-05-15) 17 | ------------------- 18 | * Format 2 and build_export_depend orocos kdl (`#210 `_) 19 | * Contributors: Shane Loretz 20 | 21 | 1.13.0 (2020-03-10) 22 | ------------------- 23 | * Replace kdl packages with rosdep keys (`#206 `_) 24 | * Contributors: Shane Loretz 25 | 26 | 1.12.1 (2020-03-10) 27 | ------------------- 28 | * Bump CMake version to avoid CMP0048 warning (`#204 `_) 29 | * Contributors: Shane Loretz 30 | 31 | 1.12.0 (2018-05-02) 32 | ------------------- 33 | * Remove dependency on cmake_modules (`#157 `_) 34 | This is not needed anymore with the move from Eigen to Eigen3 in 707eb41. 35 | * Contributors: Jochen Sprickerhof 36 | 37 | 1.11.9 (2017-07-14) 38 | ------------------- 39 | * Fix cmake dependency export usage 40 | * Contributors: Timo Röhling 41 | 42 | 1.11.8 (2016-03-04) 43 | ------------------- 44 | * eigen_conversions: Add conversions for Eigen::Isometry3d 45 | * Contributors: Maarten de Vries 46 | 47 | 1.11.7 (2015-04-21) 48 | ------------------- 49 | 50 | 1.11.6 (2015-03-25) 51 | ------------------- 52 | 53 | 1.11.5 (2015-03-17) 54 | ------------------- 55 | 56 | 1.11.4 (2014-12-23) 57 | ------------------- 58 | 59 | 1.11.3 (2014-05-07) 60 | ------------------- 61 | 62 | 1.11.2 (2014-02-25) 63 | ------------------- 64 | * alphabetization and updating cmake to find Eigen 65 | * Contributors: Tully Foote 66 | 67 | 1.11.1 (2014-02-23) 68 | ------------------- 69 | * add dep on cmake_modules for finding eigen 70 | * Contributors: Ioan Sucan 71 | 72 | 1.11.0 (2014-02-14) 73 | ------------------- 74 | * fixed Eigen-KDL 3D Vector conversion functions 75 | * Contributors: fevb 76 | 77 | 1.10.8 (2014-02-01) 78 | ------------------- 79 | 80 | 1.10.7 (2013-12-27) 81 | ------------------- 82 | 83 | 1.10.6 (2013-08-28) 84 | ------------------- 85 | 86 | 1.10.5 (2013-07-19) 87 | ------------------- 88 | 89 | 1.10.4 (2013-07-11) 90 | ------------------- 91 | 92 | 1.10.3 (2013-07-09) 93 | ------------------- 94 | 95 | 1.10.2 (2013-07-09) 96 | ------------------- 97 | 98 | 1.10.1 (2013-07-05) 99 | ------------------- 100 | 101 | 1.10.0 (2013-07-05) 102 | ------------------- 103 | 104 | 1.9.31 (2013-04-18 18:16) 105 | ------------------------- 106 | * add link directories 107 | 108 | 1.9.30 (2013-04-18 16:26) 109 | ------------------------- 110 | * Fixes `#11 `_: orocos_kdl now has cmake rules oroco-kdl 111 | update cmake to use orocos_kdl as plain cmake package instead of catkin package 112 | Signed-off-by: Ruben Smits 113 | 114 | 1.9.29 (2013-01-13) 115 | ------------------- 116 | 117 | 1.9.28 (2013-01-02) 118 | ------------------- 119 | 120 | 1.9.27 (2012-12-21) 121 | ------------------- 122 | 123 | 1.9.26 (2012-12-14) 124 | ------------------- 125 | * add missing dep to catkin 126 | 127 | 1.9.25 (2012-12-13) 128 | ------------------- 129 | 130 | 1.9.24 (2012-12-11) 131 | ------------------- 132 | * Version 1.9.24 133 | * Fixes to CMakeLists.txt's while building from source 134 | 135 | 1.9.23 (2012-11-22) 136 | ------------------- 137 | * Releaseing version 1.9.23 138 | 139 | 1.9.22 (2012-11-04 09:14) 140 | ------------------------- 141 | 142 | 1.9.21 (2012-11-04 01:19) 143 | ------------------------- 144 | 145 | 1.9.20 (2012-11-02) 146 | ------------------- 147 | 148 | 1.9.19 (2012-10-31) 149 | ------------------- 150 | * Removed deprecated 'brief' attribute from tags. 151 | 152 | 1.9.18 (2012-10-16) 153 | ------------------- 154 | 155 | 1.9.17 (2012-10-02) 156 | ------------------- 157 | * fix several dependency issues 158 | 159 | 1.9.16 (2012-09-29) 160 | ------------------- 161 | * adding geometry metapackage and updating to 1.9.16 162 | 163 | 1.9.15 (2012-09-30) 164 | ------------------- 165 | * fix a few dependency/catkin problems 166 | * remove old API files 167 | * comply to the new catkin API 168 | 169 | 1.9.14 (2012-09-18) 170 | ------------------- 171 | 172 | 1.9.13 (2012-09-17) 173 | ------------------- 174 | 175 | 1.9.12 (2012-09-16) 176 | ------------------- 177 | 178 | 1.9.11 (2012-09-14 22:49) 179 | ------------------------- 180 | 181 | 1.9.10 (2012-09-14 22:30) 182 | ------------------------- 183 | 184 | 1.9.9 (2012-09-11) 185 | ------------------ 186 | * update depends 187 | * minor patches for new build system 188 | 189 | 1.9.8 (2012-09-03) 190 | ------------------ 191 | 192 | 1.9.7 (2012-08-10 12:19) 193 | ------------------------ 194 | * minor build fixes 195 | * completed set of eigen conversions; added KDL conversions 196 | * adding additional conversion functions 197 | 198 | 1.9.6 (2012-08-02 19:59) 199 | ------------------------ 200 | 201 | 1.9.5 (2012-08-02 19:48) 202 | ------------------------ 203 | 204 | 1.9.4 (2012-08-02 18:29) 205 | ------------------------ 206 | 207 | 1.9.3 (2012-08-02 18:28) 208 | ------------------------ 209 | * forgot to install some things 210 | * also using DEPENDS 211 | 212 | 1.9.2 (2012-08-01 21:05) 213 | ------------------------ 214 | 215 | 1.9.1 (2012-08-01 19:16) 216 | ------------------------ 217 | * install manifest.xml 218 | 219 | 1.9.0 (2012-08-01 18:52) 220 | ------------------------ 221 | * catkin build system 222 | * added some additional conversion routines between eigen and messages 223 | * removing dependency on eigen package 224 | * compiling with eigen3 225 | * missed a rename 226 | * more extensive search 227 | * applying patch from sed script for eigen3 compatability 228 | * Moving eigen_conversions from sandbox. 229 | -------------------------------------------------------------------------------- /eigen_conversions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(eigen_conversions) 3 | 4 | find_package(orocos_kdl REQUIRED) 5 | find_package(catkin REQUIRED geometry_msgs std_msgs) 6 | find_package(Eigen3 REQUIRED) 7 | 8 | include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) 9 | include_directories(include ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) 10 | link_directories(${catkin_LIBRARY_DIRS}) 11 | link_directories(${orocos_kdl_LIBRARY_DIRS}) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} 15 | LIBRARIES ${PROJECT_NAME} 16 | CATKIN_DEPENDS geometry_msgs std_msgs 17 | DEPENDS orocos_kdl 18 | ) 19 | 20 | add_library(${PROJECT_NAME} 21 | src/eigen_msg.cpp 22 | src/eigen_kdl.cpp 23 | ) 24 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 25 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) 26 | 27 | install(DIRECTORY include/${PROJECT_NAME}/ 28 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 29 | 30 | install(TARGETS ${PROJECT_NAME} 31 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 34 | -------------------------------------------------------------------------------- /eigen_conversions/include/eigen_conversions/eigen_kdl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* 31 | * Author: Adam Leeper, Stuart Glaser 32 | */ 33 | 34 | #ifndef EIGEN_KDL_CONVERSIONS_H 35 | #define EIGEN_KDL_CONVERSIONS_H 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | namespace tf { 43 | 44 | /// Converts a KDL rotation into an Eigen quaternion 45 | void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e); 46 | 47 | /// Converts an Eigen quaternion into a KDL rotation 48 | void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k); 49 | 50 | /// Converts a KDL frame into an Eigen Affine3d 51 | void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e); 52 | 53 | /// Converts a KDL frame into an Eigen Isometry3d 54 | void transformKDLToEigen(const KDL::Frame &k, Eigen::Isometry3d &e); 55 | 56 | /// Converts an Eigen Affine3d into a KDL frame 57 | void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k); 58 | 59 | /// Converts an Eigen Isometry3d into a KDL frame 60 | void transformEigenToKDL(const Eigen::Isometry3d &e, KDL::Frame &k); 61 | 62 | /// Converts a KDL twist into an Eigen matrix 63 | void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix &e); 64 | 65 | /// Converts an Eigen matrix into a KDL Twist 66 | void twistEigenToKDL(const Eigen::Matrix &e, KDL::Twist &k); 67 | 68 | /// Converts a KDL vector into an Eigen matrix 69 | void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix &e); 70 | 71 | /// Converts an Eigen matrix into a KDL vector 72 | void vectorEigenToKDL(const Eigen::Matrix &e, KDL::Vector &k); 73 | 74 | /// Converts a KDL wrench into an Eigen matrix 75 | void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix &e); 76 | 77 | /// Converts an Eigen matrix into a KDL wrench 78 | void wrenchEigenToKDL(const Eigen::Matrix &e, KDL::Wrench &k); 79 | 80 | } // namespace 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /eigen_conversions/include/eigen_conversions/eigen_msg.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* 31 | * Author: Stuart Glaser 32 | */ 33 | 34 | #ifndef EIGEN_MSG_CONVERSIONS_H 35 | #define EIGEN_MSG_CONVERSIONS_H 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | namespace tf { 50 | 51 | /// Converts a Point message into an Eigen Vector 52 | void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e); 53 | 54 | /// Converts an Eigen Vector into a Point message 55 | void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m); 56 | 57 | /// Converts a Pose message into an Eigen Affine3d 58 | void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e); 59 | 60 | /// Converts a Pose message into an Eigen Isometry3d 61 | void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Isometry3d &e); 62 | 63 | /// Converts an Eigen Affine3d into a Pose message 64 | void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m); 65 | 66 | /// Converts an Eigen Isometry3d into a Pose message 67 | void poseEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Pose &m); 68 | 69 | /// Converts a Quaternion message into an Eigen Quaternion 70 | void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e); 71 | 72 | /// Converts an Eigen Quaternion into a Quaternion message 73 | void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m); 74 | 75 | /// Converts a Transform message into an Eigen Affine3d 76 | void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e); 77 | 78 | /// Converts a Transform message into an Eigen Isometry3d 79 | void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Isometry3d &e); 80 | 81 | /// Converts an Eigen Affine3d into a Transform message 82 | void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m); 83 | 84 | /// Converts an Eigen Isometry3d into a Transform message 85 | void transformEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Transform &m); 86 | 87 | /// Converts a Twist message into an Eigen matrix 88 | void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix &e); 89 | 90 | /// Converts an Eigen matrix into a Twist message 91 | void twistEigenToMsg(const Eigen::Matrix &e, geometry_msgs::Twist &m); 92 | 93 | /// Converts a Vector message into an Eigen Vector 94 | void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e); 95 | 96 | /// Converts an Eigen Vector into a Vector message 97 | void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m); 98 | 99 | /// Converts a Wrench message into an Eigen matrix 100 | void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix &e); 101 | 102 | /// Converts an Eigen matrix into a Wrench message 103 | void wrenchEigenToMsg(const Eigen::Matrix &e, geometry_msgs::Wrench &m); 104 | 105 | /// Converts an Eigen matrix into a Float64MultiArray message 106 | template 107 | void matrixEigenToMsg(const Eigen::MatrixBase &e, std_msgs::Float64MultiArray &m) 108 | { 109 | if (m.layout.dim.size() != 2) 110 | m.layout.dim.resize(2); 111 | m.layout.dim[0].stride = e.rows() * e.cols(); 112 | m.layout.dim[0].size = e.rows(); 113 | m.layout.dim[1].stride = e.cols(); 114 | m.layout.dim[1].size = e.cols(); 115 | if ((int)m.data.size() != e.size()) 116 | m.data.resize(e.size()); 117 | int ii = 0; 118 | for (int i = 0; i < e.rows(); ++i) 119 | for (int j = 0; j < e.cols(); ++j) 120 | m.data[ii++] = e.coeff(i, j); 121 | } 122 | 123 | } // namespace 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /eigen_conversions/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b eigen_conversions is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /eigen_conversions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | eigen_conversions 3 | 1.13.4 4 | 5 | 6 | Conversion functions between: 7 | - Eigen and KDL 8 | - Eigen and geometry_msgs. 9 | 10 | 11 | Stuart Glaser 12 | Adam Leeper 13 | Tully Foote 14 | 15 | BSD 16 | http://ros.org/wiki/eigen_conversions 17 | 18 | catkin 19 | 20 | geometry_msgs 21 | eigen 22 | liborocos-kdl-dev 23 | std_msgs 24 | 25 | liborocos-kdl-dev 26 | 27 | geometry_msgs 28 | eigen 29 | liborocos-kdl 30 | std_msgs 31 | 32 | -------------------------------------------------------------------------------- /eigen_conversions/src/eigen_kdl.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | namespace tf { 33 | 34 | void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e) 35 | { 36 | // // is it faster to do this? 37 | k.GetQuaternion(e.x(), e.y(), e.z(), e.w()); 38 | 39 | // or this? 40 | //double x, y, z, w; 41 | //k.GetQuaternion(x, y, z, w); 42 | //e = Eigen::Quaterniond(w, x, y, z); 43 | } 44 | 45 | void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k) 46 | { 47 | k = KDL::Rotation::Quaternion(e.x(), e.y(), e.z(), e.w()); 48 | } 49 | 50 | namespace { 51 | template 52 | void transformKDLToEigenImpl(const KDL::Frame &k, T &e) 53 | { 54 | // translation 55 | for (unsigned int i = 0; i < 3; ++i) 56 | e(i, 3) = k.p[i]; 57 | 58 | // rotation matrix 59 | for (unsigned int i = 0; i < 9; ++i) 60 | e(i/3, i%3) = k.M.data[i]; 61 | 62 | // "identity" row 63 | e(3,0) = 0.0; 64 | e(3,1) = 0.0; 65 | e(3,2) = 0.0; 66 | e(3,3) = 1.0; 67 | } 68 | 69 | template 70 | void transformEigenToKDLImpl(const T &e, KDL::Frame &k) 71 | { 72 | for (unsigned int i = 0; i < 3; ++i) 73 | k.p[i] = e(i, 3); 74 | for (unsigned int i = 0; i < 9; ++i) 75 | k.M.data[i] = e(i/3, i%3); 76 | } 77 | 78 | } 79 | 80 | void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e) 81 | { 82 | transformKDLToEigenImpl(k, e); 83 | } 84 | 85 | void transformKDLToEigen(const KDL::Frame &k, Eigen::Isometry3d &e) 86 | { 87 | transformKDLToEigenImpl(k, e); 88 | } 89 | 90 | void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k) 91 | { 92 | transformEigenToKDLImpl(e, k); 93 | } 94 | 95 | void transformEigenToKDL(const Eigen::Isometry3d &e, KDL::Frame &k) 96 | { 97 | transformEigenToKDLImpl(e, k); 98 | } 99 | 100 | void twistEigenToKDL(const Eigen::Matrix &e, KDL::Twist &k) 101 | { 102 | for(int i = 0; i < 6; ++i) 103 | k[i] = e[i]; 104 | } 105 | 106 | void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix &e) 107 | { 108 | for(int i = 0; i < 6; ++i) 109 | e[i] = k[i]; 110 | } 111 | 112 | void vectorEigenToKDL(const Eigen::Matrix &e, KDL::Vector &k) 113 | { 114 | for(int i = 0; i < 3; ++i) 115 | k[i] = e[i]; 116 | } 117 | void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix &e) 118 | { 119 | for(int i = 0; i < 3; ++i) 120 | e[i] = k[i]; 121 | } 122 | 123 | void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix &e) 124 | { 125 | for(int i = 0; i < 6; ++i) 126 | e[i] = k[i]; 127 | } 128 | 129 | void wrenchEigenToKDL(const Eigen::Matrix &e, KDL::Wrench &k) 130 | { 131 | for(int i = 0; i < 6; ++i) 132 | k[i] = e[i]; 133 | } 134 | 135 | 136 | } // namespace 137 | -------------------------------------------------------------------------------- /eigen_conversions/src/eigen_msg.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | #include 32 | 33 | namespace tf { 34 | 35 | void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e) 36 | { 37 | e(0) = m.x; 38 | e(1) = m.y; 39 | e(2) = m.z; 40 | } 41 | 42 | void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m) 43 | { 44 | m.x = e(0); 45 | m.y = e(1); 46 | m.z = e(2); 47 | } 48 | 49 | namespace { 50 | template 51 | void poseMsgToEigenImpl(const geometry_msgs::Pose &m, T &e) 52 | { 53 | e = Eigen::Translation3d(m.position.x, 54 | m.position.y, 55 | m.position.z) * 56 | Eigen::Quaterniond(m.orientation.w, 57 | m.orientation.x, 58 | m.orientation.y, 59 | m.orientation.z); 60 | } 61 | 62 | template 63 | void poseEigenToMsgImpl(const T &e, geometry_msgs::Pose &m) 64 | { 65 | m.position.x = e.translation()[0]; 66 | m.position.y = e.translation()[1]; 67 | m.position.z = e.translation()[2]; 68 | Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear(); 69 | m.orientation.x = q.x(); 70 | m.orientation.y = q.y(); 71 | m.orientation.z = q.z(); 72 | m.orientation.w = q.w(); 73 | if (m.orientation.w < 0) { 74 | m.orientation.x *= -1; 75 | m.orientation.y *= -1; 76 | m.orientation.z *= -1; 77 | m.orientation.w *= -1; 78 | } 79 | } 80 | 81 | template 82 | void transformMsgToEigenImpl(const geometry_msgs::Transform &m, T &e) 83 | { 84 | e = Eigen::Translation3d(m.translation.x, 85 | m.translation.y, 86 | m.translation.z) * 87 | Eigen::Quaterniond(m.rotation.w, 88 | m.rotation.x, 89 | m.rotation.y, 90 | m.rotation.z); 91 | } 92 | 93 | template 94 | void transformEigenToMsgImpl(const T &e, geometry_msgs::Transform &m) 95 | { 96 | m.translation.x = e.translation()[0]; 97 | m.translation.y = e.translation()[1]; 98 | m.translation.z = e.translation()[2]; 99 | Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear(); 100 | m.rotation.x = q.x(); 101 | m.rotation.y = q.y(); 102 | m.rotation.z = q.z(); 103 | m.rotation.w = q.w(); 104 | if (m.rotation.w < 0) { 105 | m.rotation.x *= -1; 106 | m.rotation.y *= -1; 107 | m.rotation.z *= -1; 108 | m.rotation.w *= -1; 109 | } 110 | } 111 | } 112 | 113 | void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e) 114 | { 115 | poseMsgToEigenImpl(m, e); 116 | } 117 | 118 | void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Isometry3d &e) 119 | { 120 | poseMsgToEigenImpl(m, e); 121 | } 122 | 123 | void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m) 124 | { 125 | poseEigenToMsgImpl(e, m); 126 | } 127 | 128 | void poseEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Pose &m) 129 | { 130 | poseEigenToMsgImpl(e, m); 131 | } 132 | 133 | void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e) 134 | { 135 | e = Eigen::Quaterniond(m.w, m.x, m.y, m.z); 136 | } 137 | 138 | void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m) 139 | { 140 | m.x = e.x(); 141 | m.y = e.y(); 142 | m.z = e.z(); 143 | m.w = e.w(); 144 | } 145 | 146 | void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e) 147 | { 148 | transformMsgToEigenImpl(m, e); 149 | } 150 | 151 | void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Isometry3d &e) 152 | { 153 | transformMsgToEigenImpl(m, e); 154 | } 155 | 156 | void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m) 157 | { 158 | transformEigenToMsgImpl(e, m); 159 | } 160 | 161 | void transformEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Transform &m) 162 | { 163 | transformEigenToMsgImpl(e, m); 164 | } 165 | 166 | void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e) 167 | { 168 | e(0) = m.x; 169 | e(1) = m.y; 170 | e(2) = m.z; 171 | } 172 | 173 | void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m) 174 | { 175 | m.x = e(0); 176 | m.y = e(1); 177 | m.z = e(2); 178 | } 179 | 180 | void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix &e) 181 | { 182 | e[0] = m.linear.x; 183 | e[1] = m.linear.y; 184 | e[2] = m.linear.z; 185 | e[3] = m.angular.x; 186 | e[4] = m.angular.y; 187 | e[5] = m.angular.z; 188 | } 189 | 190 | void twistEigenToMsg(const Eigen::Matrix &e, geometry_msgs::Twist &m) 191 | { 192 | m.linear.x = e[0]; 193 | m.linear.y = e[1]; 194 | m.linear.z = e[2]; 195 | m.angular.x = e[3]; 196 | m.angular.y = e[4]; 197 | m.angular.z = e[5]; 198 | } 199 | 200 | void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix &e) 201 | { 202 | e[0] = m.force.x; 203 | e[1] = m.force.y; 204 | e[2] = m.force.z; 205 | e[3] = m.torque.x; 206 | e[4] = m.torque.y; 207 | e[5] = m.torque.z; 208 | } 209 | 210 | void wrenchEigenToMsg(const Eigen::Matrix &e, geometry_msgs::Wrench &m) 211 | { 212 | m.force.x = e[0]; 213 | m.force.y = e[1]; 214 | m.force.z = e[2]; 215 | m.torque.x = e[3]; 216 | m.torque.y = e[4]; 217 | m.torque.z = e[5]; 218 | } 219 | 220 | } // namespace 221 | -------------------------------------------------------------------------------- /geometry/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package geometry 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.13.4 (2025-04-25) 6 | ------------------- 7 | 8 | 1.13.3 (2025-04-10) 9 | ------------------- 10 | 11 | 1.13.2 (2020-06-08) 12 | ------------------- 13 | 14 | 1.13.1 (2020-05-15) 15 | ------------------- 16 | 17 | 1.13.0 (2020-03-10) 18 | ------------------- 19 | 20 | 1.12.1 (2020-03-10) 21 | ------------------- 22 | * Bump CMake version to avoid CMP0048 warning (`#204 `_) 23 | * Contributors: Shane Loretz 24 | 25 | 1.12.0 (2018-05-02) 26 | ------------------- 27 | 28 | 1.11.9 (2017-07-14) 29 | ------------------- 30 | * [doc] Add migration notice in manifest. (`#129 `_) 31 | * Contributors: Isaac I.Y. Saito 32 | 33 | 1.11.8 (2016-03-04) 34 | ------------------- 35 | 36 | 1.11.7 (2015-04-21) 37 | ------------------- 38 | 39 | 1.11.6 (2015-03-25) 40 | ------------------- 41 | 42 | 1.11.5 (2015-03-17) 43 | ------------------- 44 | 45 | 1.11.4 (2014-12-23) 46 | ------------------- 47 | * Update package.xml 48 | * Contributors: David Lu!! 49 | 50 | 1.11.3 (2014-05-07) 51 | ------------------- 52 | 53 | 1.11.2 (2014-02-25) 54 | ------------------- 55 | 56 | 1.11.1 (2014-02-23) 57 | ------------------- 58 | 59 | 1.11.0 (2014-02-14) 60 | ------------------- 61 | 62 | 1.10.8 (2014-02-01) 63 | ------------------- 64 | 65 | 1.10.7 (2013-12-27) 66 | ------------------- 67 | 68 | 1.10.6 (2013-08-28) 69 | ------------------- 70 | 71 | 1.10.5 (2013-07-19) 72 | ------------------- 73 | 74 | 1.10.4 (2013-07-11) 75 | ------------------- 76 | 77 | 1.10.3 (2013-07-09) 78 | ------------------- 79 | 80 | 1.10.2 (2013-07-09) 81 | ------------------- 82 | 83 | 1.10.1 (2013-07-05) 84 | ------------------- 85 | 86 | 1.10.0 (2013-07-05) 87 | ------------------- 88 | 89 | 1.9.31 (2013-04-18 18:16) 90 | ------------------------- 91 | 92 | 1.9.30 (2013-04-18 16:26) 93 | ------------------------- 94 | * add buildtool_depend 95 | * add CMakeLists.txt for metapackage 96 | 97 | 1.9.29 (2013-01-13) 98 | ------------------- 99 | 100 | 1.9.28 (2013-01-02) 101 | ------------------- 102 | 103 | 1.9.27 (2012-12-21) 104 | ------------------- 105 | 106 | 1.9.26 (2012-12-14) 107 | ------------------- 108 | 109 | 1.9.25 (2012-12-13) 110 | ------------------- 111 | 112 | 1.9.24 (2012-12-11) 113 | ------------------- 114 | * Version 1.9.24 115 | 116 | 1.9.23 (2012-11-22) 117 | ------------------- 118 | * Releaseing version 1.9.23 119 | 120 | 1.9.22 (2012-11-04 09:14) 121 | ------------------------- 122 | 123 | 1.9.21 (2012-11-04 01:19) 124 | ------------------------- 125 | 126 | 1.9.20 (2012-11-02) 127 | ------------------- 128 | * depend on angles 129 | 130 | 1.9.19 (2012-10-31) 131 | ------------------- 132 | 133 | 1.9.18 (2012-10-16) 134 | ------------------- 135 | 136 | 1.9.17 (2012-10-02) 137 | ------------------- 138 | 139 | 1.9.16 (2012-09-29) 140 | ------------------- 141 | * fixing xml syntax 142 | * adding geometry metapackage and updating to 1.9.16 143 | 144 | 1.9.15 (2012-09-30) 145 | ------------------- 146 | 147 | 1.9.14 (2012-09-18) 148 | ------------------- 149 | 150 | 1.9.13 (2012-09-17) 151 | ------------------- 152 | 153 | 1.9.12 (2012-09-16) 154 | ------------------- 155 | 156 | 1.9.11 (2012-09-14 22:49) 157 | ------------------------- 158 | 159 | 1.9.10 (2012-09-14 22:30) 160 | ------------------------- 161 | 162 | 1.9.9 (2012-09-11) 163 | ------------------ 164 | 165 | 1.9.8 (2012-09-03) 166 | ------------------ 167 | 168 | 1.9.7 (2012-08-10 12:19) 169 | ------------------------ 170 | 171 | 1.9.6 (2012-08-02 19:59) 172 | ------------------------ 173 | 174 | 1.9.5 (2012-08-02 19:48) 175 | ------------------------ 176 | 177 | 1.9.4 (2012-08-02 18:29) 178 | ------------------------ 179 | 180 | 1.9.3 (2012-08-02 18:28) 181 | ------------------------ 182 | 183 | 1.9.2 (2012-08-01 21:05) 184 | ------------------------ 185 | 186 | 1.9.1 (2012-08-01 19:16) 187 | ------------------------ 188 | 189 | 1.9.0 (2012-08-01 18:52) 190 | ------------------------ 191 | -------------------------------------------------------------------------------- /geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(geometry) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /geometry/package.xml: -------------------------------------------------------------------------------- 1 | 2 | geometry 3 | 1.13.4 4 |

A metapackage for geometry library suite.

5 |

Migration: Since ROS Hydro, tf has been "deprecated" in favor of tf2. tf2 is an iteration on tf providing generally the same feature set more efficiently. As well as adding a few new features.
6 | As tf2 is a major change the tf API has been maintained in its current form. Since tf2 has a superset of the tf features with a subset of the dependencies the tf implementation has been removed and replaced with calls to tf2 under the hood. This will mean that all users will be compatible with tf2. It is recommended for new work to use tf2 directly as it has a cleaner interface. However tf will continue to be supported for through at least J Turtle. 7 |

8 | 9 | Tully Foote 10 | BSD 11 | 12 | http://www.ros.org/wiki/geometry 13 | https://code.ros.org/trac/ros-pkg/query?component=geometry&status=assigned&status=new&status=reopened 14 | https://kforge.ros.org/geometry/geometry 15 | 16 | Tully Foote 17 | 18 | catkin 19 | 20 | 21 | angles 22 | eigen_conversions 23 | kdl_conversions 24 | tf 25 | tf_conversions 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /kdl_conversions/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package kdl_conversions 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.13.4 (2025-04-25) 6 | ------------------- 7 | 8 | 1.13.3 (2025-04-10) 9 | ------------------- 10 | * Remove extra semicolon for function definitions (`#240 `_) 11 | * Update library install for better portability. (`#208 `_) (`#214 `_) 12 | * Contributors: Kenji Brameld (TRACLabs), Tully Foote 13 | 14 | 1.13.2 (2020-06-08) 15 | ------------------- 16 | 17 | 1.13.1 (2020-05-15) 18 | ------------------- 19 | * Format 2 and build_export_depend orocos kdl (`#210 `_) 20 | * Contributors: Shane Loretz 21 | 22 | 1.13.0 (2020-03-10) 23 | ------------------- 24 | * Replace kdl packages with rosdep keys (`#206 `_) 25 | * Contributors: Shane Loretz 26 | 27 | 1.12.1 (2020-03-10) 28 | ------------------- 29 | * Bump CMake version to avoid CMP0048 warning (`#204 `_) 30 | * windows bring up, use ROS_DEPRECATED 31 | * Contributors: James Xu, Shane Loretz 32 | 33 | 1.12.0 (2018-05-02) 34 | ------------------- 35 | 36 | 1.11.9 (2017-07-14) 37 | ------------------- 38 | * Fix cmake dependency export usage 39 | * Contributors: Timo Röhling 40 | 41 | 1.11.8 (2016-03-04) 42 | ------------------- 43 | 44 | 1.11.7 (2015-04-21) 45 | ------------------- 46 | 47 | 1.11.6 (2015-03-25) 48 | ------------------- 49 | 50 | 1.11.5 (2015-03-17) 51 | ------------------- 52 | 53 | 1.11.4 (2014-12-23) 54 | ------------------- 55 | * Update package.xml 56 | * Contributors: David Lu!! 57 | 58 | 1.11.3 (2014-05-07) 59 | ------------------- 60 | 61 | 1.11.2 (2014-02-25) 62 | ------------------- 63 | 64 | 1.11.1 (2014-02-23) 65 | ------------------- 66 | 67 | 1.11.0 (2014-02-14) 68 | ------------------- 69 | 70 | 1.10.8 (2014-02-01) 71 | ------------------- 72 | 73 | 1.10.7 (2013-12-27) 74 | ------------------- 75 | 76 | 1.10.6 (2013-08-28) 77 | ------------------- 78 | 79 | 1.10.5 (2013-07-19) 80 | ------------------- 81 | 82 | 1.10.4 (2013-07-11) 83 | ------------------- 84 | 85 | 1.10.3 (2013-07-09) 86 | ------------------- 87 | 88 | 1.10.2 (2013-07-09) 89 | ------------------- 90 | 91 | 1.10.1 (2013-07-05) 92 | ------------------- 93 | 94 | 1.10.0 (2013-07-05) 95 | ------------------- 96 | 97 | 1.9.31 (2013-04-18 18:16) 98 | ------------------------- 99 | * add link directories 100 | 101 | 1.9.30 (2013-04-18 16:26) 102 | ------------------------- 103 | * Fixes `#11 `_: orocos_kdl now has cmake rules oroco-kdl 104 | update cmake to use orocos_kdl as plain cmake package instead of catkin package 105 | Signed-off-by: Ruben Smits 106 | 107 | 1.9.29 (2013-01-13) 108 | ------------------- 109 | 110 | 1.9.28 (2013-01-02) 111 | ------------------- 112 | 113 | 1.9.27 (2012-12-21) 114 | ------------------- 115 | 116 | 1.9.26 (2012-12-14) 117 | ------------------- 118 | * add missing dep to catkin 119 | 120 | 1.9.25 (2012-12-13) 121 | ------------------- 122 | 123 | 1.9.24 (2012-12-11) 124 | ------------------- 125 | * Version 1.9.24 126 | * Fixes to CMakeLists.txt's while building from source 127 | 128 | 1.9.23 (2012-11-22) 129 | ------------------- 130 | * Releaseing version 1.9.23 131 | 132 | 1.9.22 (2012-11-04 09:14) 133 | ------------------------- 134 | * more backwards compatible conversions and an include to make sure it gets into the old header location 135 | 136 | 1.9.21 (2012-11-04 01:19) 137 | ------------------------- 138 | 139 | 1.9.20 (2012-11-02) 140 | ------------------- 141 | 142 | 1.9.19 (2012-10-31) 143 | ------------------- 144 | * Removed deprecated 'brief' attribute from tags. 145 | 146 | 1.9.18 (2012-10-16) 147 | ------------------- 148 | 149 | 1.9.17 (2012-10-02) 150 | ------------------- 151 | * fix several dependency issues 152 | 153 | 1.9.16 (2012-09-29) 154 | ------------------- 155 | * adding geometry metapackage and updating to 1.9.16 156 | 157 | 1.9.15 (2012-09-30) 158 | ------------------- 159 | * fix a few dependency/catkin problems 160 | * remove old API files 161 | * comply to the new catkin API 162 | 163 | 1.9.14 (2012-09-18) 164 | ------------------- 165 | 166 | 1.9.13 (2012-09-17) 167 | ------------------- 168 | * update manifests 169 | 170 | 1.9.12 (2012-09-16) 171 | ------------------- 172 | 173 | 1.9.11 (2012-09-14 22:49) 174 | ------------------------- 175 | 176 | 1.9.10 (2012-09-14 22:30) 177 | ------------------------- 178 | 179 | 1.9.9 (2012-09-11) 180 | ------------------ 181 | * minor patches for new build system 182 | 183 | 1.9.8 (2012-09-03) 184 | ------------------ 185 | 186 | 1.9.7 (2012-08-10 12:19) 187 | ------------------------ 188 | * minor build fixes 189 | * fixed some minor errors from last commit 190 | * completed set of eigen conversions; added KDL conversions 191 | 192 | 1.9.6 (2012-08-02 19:59) 193 | ------------------------ 194 | 195 | 1.9.5 (2012-08-02 19:48) 196 | ------------------------ 197 | 198 | 1.9.4 (2012-08-02 18:29) 199 | ------------------------ 200 | 201 | 1.9.3 (2012-08-02 18:28) 202 | ------------------------ 203 | 204 | 1.9.2 (2012-08-01 21:05) 205 | ------------------------ 206 | 207 | 1.9.1 (2012-08-01 19:16) 208 | ------------------------ 209 | 210 | 1.9.0 (2012-08-01 18:52) 211 | ------------------------ 212 | -------------------------------------------------------------------------------- /kdl_conversions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(kdl_conversions) 3 | 4 | find_package(catkin REQUIRED geometry_msgs) 5 | find_package(orocos_kdl REQUIRED) 6 | catkin_package( 7 | INCLUDE_DIRS include 8 | LIBRARIES ${PROJECT_NAME} 9 | CATKIN_DEPENDS geometry_msgs 10 | DEPENDS orocos_kdl 11 | ) 12 | 13 | include_directories(include ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) 14 | link_directories(${catkin_LIBRARY_DIRS}) 15 | link_directories(${orocos_kdl_LIBRARY_DIRS}) 16 | 17 | add_library(${PROJECT_NAME} 18 | src/kdl_msg.cpp 19 | ) 20 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 21 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) 22 | 23 | install(DIRECTORY include/${PROJECT_NAME}/ 24 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 25 | 26 | install(TARGETS ${PROJECT_NAME} 27 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 28 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 29 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 30 | -------------------------------------------------------------------------------- /kdl_conversions/include/kdl_conversions/kdl_msg.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Adam Leeper 30 | */ 31 | 32 | #ifndef CONVERSIONS_KDL_MSG_H 33 | #define CONVERSIONS_KDL_MSG_H 34 | 35 | #include "kdl/frames.hpp" 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | namespace tf { 47 | /// Conversion functions from/to the corresponding KDL and geometry_msgs types. 48 | 49 | /// Converts a geometry_msgs Point into a KDL Vector 50 | void pointMsgToKDL(const geometry_msgs::Point &m, KDL::Vector &k); 51 | 52 | /// Converts a KDL Vector into a geometry_msgs Vector3 53 | void pointKDLToMsg(const KDL::Vector &k, geometry_msgs::Point &m); 54 | 55 | /// Converts a Pose message into a KDL Frame 56 | void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k); 57 | 58 | /// Converts a KDL Frame into a Pose message 59 | void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m); 60 | 61 | /// Converts a quaternion message into a KDL Rotation 62 | void quaternionMsgToKDL(const geometry_msgs::Quaternion &m, KDL::Rotation &k); 63 | 64 | /// Converts a KDL Rotation into a message quaternion 65 | void quaternionKDLToMsg(const KDL::Rotation &k, geometry_msgs::Quaternion &m); 66 | 67 | /// Converts a Transform message into a KDL Frame 68 | void transformMsgToKDL(const geometry_msgs::Transform &m, KDL::Frame &k); 69 | 70 | /// Converts a KDL Frame into a Transform message 71 | void transformKDLToMsg(const KDL::Frame &k, geometry_msgs::Transform &m); 72 | 73 | /// Converts a Twist message into a KDL Twist 74 | void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k); 75 | 76 | /// Converts a KDL Twist into a Twist message 77 | void twistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m); 78 | 79 | /// Converts a Vector3 message into a KDL Vector 80 | void vectorMsgToKDL(const geometry_msgs::Vector3 &m, KDL::Vector &k); 81 | 82 | /// Converts a KDL Vector into a Vector3 message 83 | void vectorKDLToMsg(const KDL::Vector &k, geometry_msgs::Vector3 &m); 84 | 85 | /// Converts a Wrench message into a KDL Wrench 86 | void wrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &k); 87 | 88 | /// Converts a KDL Wrench into a Wrench message 89 | void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m); 90 | 91 | 92 | //Deprecated methods use above: 93 | /// Converts a Pose message into a KDL Frame 94 | ROS_DEPRECATED void PoseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k); 95 | 96 | /// Converts a KDL Frame into a Pose message 97 | ROS_DEPRECATED void PoseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m); 98 | 99 | 100 | 101 | /// Converts a Twist message into a KDL Twist 102 | ROS_DEPRECATED void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k); 103 | 104 | /// Converts a KDL Twist into a Twist message 105 | ROS_DEPRECATED void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m); 106 | 107 | 108 | 109 | } 110 | 111 | 112 | #endif 113 | 114 | 115 | 116 | -------------------------------------------------------------------------------- /kdl_conversions/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b kdl_conversions 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /kdl_conversions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | kdl_conversions 3 | 1.13.4 4 | 5 | 6 | Conversion functions between KDL and geometry_msgs types. 7 | 8 | 9 | Adam Leeper 10 | Tully Foote 11 | 12 | BSD 13 | http://ros.org/wiki/kdl_conversions 14 | 15 | catkin 16 | 17 | geometry_msgs 18 | liborocos-kdl-dev 19 | 20 | liborocos-kdl-dev 21 | 22 | geometry_msgs 23 | liborocos-kdl 24 | 25 | -------------------------------------------------------------------------------- /kdl_conversions/src/kdl_msg.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "kdl_conversions/kdl_msg.h" 31 | 32 | namespace tf { 33 | 34 | void pointMsgToKDL(const geometry_msgs::Point &m, KDL::Vector &k) 35 | { 36 | k[0] = m.x; 37 | k[1] = m.y; 38 | k[2] = m.z; 39 | } 40 | 41 | void pointKDLToMsg(const KDL::Vector &k, geometry_msgs::Point &m) 42 | { 43 | m.x = k[0]; 44 | m.y = k[1]; 45 | m.z = k[2]; 46 | } 47 | 48 | void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k) 49 | { 50 | k.p[0] = m.position.x; 51 | k.p[1] = m.position.y; 52 | k.p[2] = m.position.z; 53 | 54 | k.M = KDL::Rotation::Quaternion( m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w); 55 | } 56 | 57 | void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m) 58 | { 59 | m.position.x = k.p[0]; 60 | m.position.y = k.p[1]; 61 | m.position.z = k.p[2]; 62 | 63 | k.M.GetQuaternion(m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w); 64 | } 65 | 66 | void quaternionMsgToKDL(const geometry_msgs::Quaternion &m, KDL::Rotation &k) 67 | { 68 | k = KDL::Rotation::Quaternion(m.x, m.y, m.z, m.w); 69 | } 70 | 71 | void quaternionKDLToMsg(const KDL::Rotation &k, geometry_msgs::Quaternion &m) 72 | { 73 | k.GetQuaternion(m.x, m.y, m.z, m.w); 74 | } 75 | 76 | void transformMsgToKDL(const geometry_msgs::Transform &m, KDL::Frame &k) 77 | { 78 | k.p[0] = m.translation.x; 79 | k.p[1] = m.translation.y; 80 | k.p[2] = m.translation.z; 81 | 82 | k.M = KDL::Rotation::Quaternion( m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w); 83 | } 84 | 85 | void transformKDLToMsg(const KDL::Frame &k, geometry_msgs::Transform &m) 86 | { 87 | m.translation.x = k.p[0]; 88 | m.translation.y = k.p[1]; 89 | m.translation.z = k.p[2]; 90 | 91 | k.M.GetQuaternion(m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w); 92 | } 93 | 94 | void twistKDLToMsg(const KDL::Twist &t, geometry_msgs::Twist &m) 95 | { 96 | m.linear.x = t.vel[0]; 97 | m.linear.y = t.vel[1]; 98 | m.linear.z = t.vel[2]; 99 | m.angular.x = t.rot[0]; 100 | m.angular.y = t.rot[1]; 101 | m.angular.z = t.rot[2]; 102 | } 103 | 104 | void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &t) 105 | { 106 | t.vel[0] = m.linear.x; 107 | t.vel[1] = m.linear.y; 108 | t.vel[2] = m.linear.z; 109 | t.rot[0] = m.angular.x; 110 | t.rot[1] = m.angular.y; 111 | t.rot[2] = m.angular.z; 112 | } 113 | 114 | void vectorMsgToKDL(const geometry_msgs::Vector3 &m, KDL::Vector &k) 115 | { 116 | k[0] = m.x; 117 | k[1] = m.y; 118 | k[2] = m.z; 119 | } 120 | 121 | void vectorKDLToMsg(const KDL::Vector &k, geometry_msgs::Vector3 &m) 122 | { 123 | m.x = k[0]; 124 | m.y = k[1]; 125 | m.z = k[2]; 126 | } 127 | 128 | void wrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &k) 129 | { 130 | k[0] = m.force.x; 131 | k[1] = m.force.y; 132 | k[2] = m.force.z; 133 | k[3] = m.torque.x; 134 | k[4] = m.torque.y; 135 | k[5] = m.torque.z; 136 | } 137 | 138 | void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m) 139 | { 140 | m.force.x = k[0]; 141 | m.force.y = k[1]; 142 | m.force.z = k[2]; 143 | m.torque.x = k[3]; 144 | m.torque.y = k[4]; 145 | m.torque.z = k[5]; 146 | } 147 | 148 | 149 | ///// Deprecated methods for backwards compatability 150 | 151 | /// Converts a Pose message into a KDL Frame 152 | void PoseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k) { poseMsgToKDL(m, k);} 153 | 154 | /// Converts a KDL Frame into a Pose message 155 | void PoseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m) { poseKDLToMsg(k, m);} 156 | 157 | /// Converts a Twist message into a KDL Twist 158 | void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k) {twistMsgToKDL(m, k);} 159 | 160 | /// Converts a KDL Twist into a Twist message 161 | void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m){twistKDLToMsg(k, m);} 162 | 163 | 164 | } // namespace tf 165 | 166 | -------------------------------------------------------------------------------- /tf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(tf) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | angles 6 | geometry_msgs 7 | message_filters 8 | message_generation 9 | rosconsole 10 | roscpp 11 | rostime 12 | sensor_msgs 13 | std_msgs 14 | tf2_ros 15 | ) 16 | find_package(Boost REQUIRED COMPONENTS thread system) 17 | 18 | catkin_python_setup() 19 | 20 | include_directories( 21 | include 22 | ${Boost_INCLUDE_DIR} 23 | ${catkin_INCLUDE_DIRS} 24 | ) 25 | 26 | add_message_files(DIRECTORY msg FILES tfMessage.msg) 27 | add_service_files(DIRECTORY srv FILES FrameGraph.srv) 28 | 29 | generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs) 30 | 31 | catkin_package( 32 | INCLUDE_DIRS include 33 | LIBRARIES ${PROJECT_NAME} 34 | CATKIN_DEPENDS geometry_msgs message_filters message_runtime roscpp sensor_msgs std_msgs tf2_ros rosconsole 35 | ) 36 | 37 | add_library(${PROJECT_NAME} 38 | src/cache.cpp 39 | src/tf.cpp 40 | src/transform_broadcaster.cpp 41 | src/transform_listener.cpp 42 | ) 43 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 44 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 45 | 46 | # Debug 47 | add_executable(tf_empty_listener src/empty_listener.cpp) 48 | target_link_libraries(tf_empty_listener ${PROJECT_NAME}) 49 | 50 | add_executable(tf_echo src/tf_echo.cpp) 51 | target_link_libraries(tf_echo ${PROJECT_NAME}) 52 | 53 | add_executable(tf_change_notifier src/change_notifier.cpp) 54 | target_link_libraries(tf_change_notifier ${PROJECT_NAME}) 55 | 56 | add_executable(tf_monitor src/tf_monitor.cpp) 57 | target_link_libraries(tf_monitor ${PROJECT_NAME}) 58 | 59 | add_executable(static_transform_publisher src/static_transform_publisher.cpp) 60 | target_link_libraries(static_transform_publisher ${PROJECT_NAME}) 61 | 62 | # Tests 63 | if(CATKIN_ENABLE_TESTING AND NOT ANDROID) 64 | 65 | find_package(rostest REQUIRED) 66 | 67 | catkin_add_gtest(tf_unittest test/tf_unittest.cpp) 68 | target_include_directories(tf_unittest PRIVATE ${rostest_INCLUDE_DIRS}) 69 | target_link_libraries(tf_unittest ${PROJECT_NAME}) 70 | 71 | catkin_add_gtest(tf_quaternion_unittest test/quaternion.cpp) 72 | target_link_libraries(tf_quaternion_unittest ${PROJECT_NAME}) 73 | 74 | catkin_add_gtest(test_transform_datatypes test/test_transform_datatypes.cpp) 75 | target_link_libraries(test_transform_datatypes ${PROJECT_NAME} ${Boost_LIBRARIES}) 76 | 77 | add_executable(transform_listener_unittest test/transform_listener_unittest.cpp) 78 | target_link_libraries(transform_listener_unittest ${PROJECT_NAME} ${GTEST_LIBRARIES}) 79 | add_rostest(test/transform_listener_unittest.launch) 80 | 81 | # Disabled because of changes in TransformStorage 82 | #catkin_add_gtest_future(tf_unittest_future test/tf_unittest_future.cpp) 83 | #target_link_libraries(tf_unittest_future ${PROJECT_NAME}) 84 | 85 | catkin_add_gtest(test_velocity test/velocity_test.cpp) 86 | target_link_libraries(test_velocity ${PROJECT_NAME}) 87 | 88 | #add_executable(test_transform_twist test/transform_twist_test.cpp) 89 | #target_link_libraries(test_transform_twist ${PROJECT_NAME}) 90 | #catkin_add_gtest_build_flags(test_transform_twist) 91 | #add_rostest(test/transform_twist_test.launch) 92 | 93 | catkin_add_gtest(cache_unittest test/cache_unittest.cpp) 94 | target_link_libraries(cache_unittest ${PROJECT_NAME}) 95 | 96 | add_executable(test_message_filter EXCLUDE_FROM_ALL test/test_message_filter.cpp) 97 | target_link_libraries(test_message_filter tf ${Boost_LIBRARIES} ${GTEST_LIBRARIES}) 98 | add_rostest(test/test_message_filter.xml) 99 | 100 | 101 | ### Benchmarking 102 | #catkin_add_gtest_future(tf_benchmark test/tf_benchmark.cpp) 103 | #target_link_libraries(tf_benchmark ${PROJECT_NAME}) 104 | 105 | add_executable(testListener test/testListener.cpp) 106 | target_link_libraries(testListener ${PROJECT_NAME} ${GTEST_LIBRARIES}) 107 | add_rostest(test/test_broadcaster.launch) 108 | 109 | add_executable(testBroadcaster test/testBroadcaster.cpp) 110 | target_link_libraries(testBroadcaster ${PROJECT_NAME}) 111 | 112 | catkin_add_nosetests(test/testPython.py) 113 | add_executable(tf_speed_test EXCLUDE_FROM_ALL test/speed_test.cpp) 114 | target_link_libraries(tf_speed_test ${PROJECT_NAME}) 115 | 116 | if(TARGET tests) 117 | add_dependencies(tests testBroadcaster testListener transform_listener_unittest test_message_filter) 118 | endif() 119 | 120 | endif() # CATKIN_ENABLE_TESTING AND NOT ANDROID 121 | 122 | install(DIRECTORY include/${PROJECT_NAME}/ 123 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 124 | ) 125 | 126 | # Install library 127 | install(TARGETS ${PROJECT_NAME} 128 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 129 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 130 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 131 | ) 132 | 133 | # Install executable 134 | install(TARGETS tf_echo tf_empty_listener tf_change_notifier tf_monitor static_transform_publisher 135 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 136 | ) 137 | 138 | # Install rosrun-able scripts 139 | catkin_install_python(PROGRAMS 140 | scripts/bullet_migration_sed.py 141 | scripts/tf_remap 142 | scripts/view_frames 143 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) 144 | 145 | -------------------------------------------------------------------------------- /tf/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # tf documentation build configuration file, created by 4 | # sphinx-quickstart on Mon Jun 1 14:21:53 2009. 5 | # 6 | # This file is execfile()d with the current directory set to its containing dir. 7 | # 8 | # Note that not all possible configuration values are present in this 9 | # autogenerated file. 10 | # 11 | # All configuration values have a default; values that are commented out 12 | # serve to show the default. 13 | 14 | import sys, os 15 | 16 | # If extensions (or modules to document with autodoc) are in another directory, 17 | # add these directories to sys.path here. If the directory is relative to the 18 | # documentation root, use os.path.abspath to make it absolute, like shown here. 19 | #sys.path.append(os.path.abspath('.')) 20 | 21 | # -- General configuration ----------------------------------------------------- 22 | 23 | # Add any Sphinx extension module names here, as strings. They can be extensions 24 | # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. 25 | extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] 26 | 27 | # Add any paths that contain templates here, relative to this directory. 28 | templates_path = ['_templates'] 29 | 30 | # The suffix of source filenames. 31 | source_suffix = '.rst' 32 | 33 | # The encoding of source files. 34 | #source_encoding = 'utf-8' 35 | 36 | # The master toctree document. 37 | master_doc = 'index' 38 | 39 | # General information about the project. 40 | project = u'tf' 41 | copyright = u'2009, Willow Garage, Inc.' 42 | 43 | # The version info for the project you're documenting, acts as replacement for 44 | # |version| and |release|, also used in various other places throughout the 45 | # built documents. 46 | # 47 | # The short X.Y version. 48 | version = '0.1' 49 | # The full version, including alpha/beta/rc tags. 50 | release = '0.1.0' 51 | 52 | # The language for content autogenerated by Sphinx. Refer to documentation 53 | # for a list of supported languages. 54 | #language = None 55 | 56 | # There are two options for replacing |today|: either, you set today to some 57 | # non-false value, then it is used: 58 | #today = '' 59 | # Else, today_fmt is used as the format for a strftime call. 60 | #today_fmt = '%B %d, %Y' 61 | 62 | # List of documents that shouldn't be included in the build. 63 | #unused_docs = [] 64 | 65 | # List of directories, relative to source directory, that shouldn't be searched 66 | # for source files. 67 | exclude_trees = ['_build'] 68 | 69 | # The reST default role (used for this markup: `text`) to use for all documents. 70 | #default_role = None 71 | 72 | # If true, '()' will be appended to :func: etc. cross-reference text. 73 | #add_function_parentheses = True 74 | 75 | # If true, the current module name will be prepended to all description 76 | # unit titles (such as .. function::). 77 | #add_module_names = True 78 | 79 | # If true, sectionauthor and moduleauthor directives will be shown in the 80 | # output. They are ignored by default. 81 | #show_authors = False 82 | 83 | # The name of the Pygments (syntax highlighting) style to use. 84 | pygments_style = 'sphinx' 85 | 86 | # A list of ignored prefixes for module index sorting. 87 | #modindex_common_prefix = [] 88 | 89 | 90 | # -- Options for HTML output --------------------------------------------------- 91 | 92 | # The theme to use for HTML and HTML Help pages. Major themes that come with 93 | # Sphinx are currently 'default' and 'sphinxdoc'. 94 | html_theme = 'default' 95 | 96 | # Theme options are theme-specific and customize the look and feel of a theme 97 | # further. For a list of options available for each theme, see the 98 | # documentation. 99 | #html_theme_options = {} 100 | 101 | # Add any paths that contain custom themes here, relative to this directory. 102 | #html_theme_path = [] 103 | 104 | # The name for this set of Sphinx documents. If None, it defaults to 105 | # " v documentation". 106 | #html_title = None 107 | 108 | # A shorter title for the navigation bar. Default is the same as html_title. 109 | #html_short_title = None 110 | 111 | # The name of an image file (relative to this directory) to place at the top 112 | # of the sidebar. 113 | #html_logo = None 114 | 115 | # The name of an image file (within the static path) to use as favicon of the 116 | # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 117 | # pixels large. 118 | #html_favicon = None 119 | 120 | # Add any paths that contain custom static files (such as style sheets) here, 121 | # relative to this directory. They are copied after the builtin static files, 122 | # so a file named "default.css" will overwrite the builtin "default.css". 123 | html_static_path = ['_static'] 124 | 125 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 126 | # using the given strftime format. 127 | #html_last_updated_fmt = '%b %d, %Y' 128 | 129 | # If true, SmartyPants will be used to convert quotes and dashes to 130 | # typographically correct entities. 131 | #html_use_smartypants = True 132 | 133 | # Custom sidebar templates, maps document names to template names. 134 | #html_sidebars = {} 135 | 136 | # Additional templates that should be rendered to pages, maps page names to 137 | # template names. 138 | #html_additional_pages = {} 139 | 140 | # If false, no module index is generated. 141 | #html_use_modindex = True 142 | 143 | # If false, no index is generated. 144 | #html_use_index = True 145 | 146 | # If true, the index is split into individual pages for each letter. 147 | #html_split_index = False 148 | 149 | # If true, links to the reST sources are added to the pages. 150 | #html_show_sourcelink = True 151 | 152 | # If true, an OpenSearch description file will be output, and all pages will 153 | # contain a tag referring to it. The value of this option must be the 154 | # base URL from which the finished HTML is served. 155 | #html_use_opensearch = '' 156 | 157 | # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). 158 | #html_file_suffix = '' 159 | 160 | # Output file base name for HTML help builder. 161 | htmlhelp_basename = 'tfdoc' 162 | 163 | 164 | # -- Options for LaTeX output -------------------------------------------------- 165 | 166 | # The paper size ('letter' or 'a4'). 167 | #latex_paper_size = 'letter' 168 | 169 | # The font size ('10pt', '11pt' or '12pt'). 170 | #latex_font_size = '10pt' 171 | 172 | # Grouping the document tree into LaTeX files. List of tuples 173 | # (source start file, target name, title, author, documentclass [howto/manual]). 174 | latex_documents = [ 175 | ('index', 'tf.tex', u'stereo\\_utils Documentation', 176 | u'James Bowman', 'manual'), 177 | ] 178 | 179 | # The name of an image file (relative to this directory) to place at the top of 180 | # the title page. 181 | #latex_logo = None 182 | 183 | # For "manual" documents, if this is true, then toplevel headings are parts, 184 | # not chapters. 185 | #latex_use_parts = False 186 | 187 | # Additional stuff for the LaTeX preamble. 188 | #latex_preamble = '' 189 | 190 | # Documents to append as an appendix to all manuals. 191 | #latex_appendices = [] 192 | 193 | # If false, no module index is generated. 194 | #latex_use_modindex = True 195 | 196 | 197 | # Example configuration for intersphinx: refer to the Python standard library. 198 | intersphinx_mapping = { 199 | 'http://docs.python.org/': None, 200 | 'http://docs.opencv.org/3.0-last-rst/': None, 201 | 'http://docs.scipy.org/doc/numpy' : None 202 | } 203 | -------------------------------------------------------------------------------- /tf/doc/bifrucation.gv: -------------------------------------------------------------------------------- 1 | digraph G{ 2 | label = "Effect of publishing Inverted Parent Child Relationship on TF tree"; 3 | subgraph cluster0{ 4 | a [label="A"]; 5 | aa [label="AA"]; 6 | b [label="B"]; 7 | bb [label="BB"]; 8 | c [ label="C"]; 9 | d [ label="D"]; 10 | a -> b; 11 | aa -> b; 12 | bb -> c; 13 | b -> c; 14 | c -> d; 15 | label = "a) Example Tree"; 16 | color=black; 17 | } 18 | subgraph cluster1{ 19 | a1 [label="A"]; 20 | aa1 [label="AA"]; 21 | b1 [label="B"]; 22 | bb1 [label="BB"]; 23 | c1 [ label="C"]; 24 | d1 [ label="D"]; 25 | a1 -> b1; 26 | aa1 -> b1; 27 | bb1 -> c1; 28 | c1 -> b1; 29 | label = "b) C-B Inverted Case1 (Part of the time)"; 30 | color=black; 31 | } 32 | subgraph cluster2{ 33 | a2 [label="A"]; 34 | aa2 [label="AA"]; 35 | b2 [label="B"]; 36 | bb2 [label="BB"]; 37 | c2 [ label="C"]; 38 | d2 [ label="D"]; 39 | 40 | a2 -> b2; 41 | aa2 -> b2; 42 | bb2 -> c2; 43 | c2 -> d2; 44 | label = "c) C-B Inverted Case2 (part of the time)"; 45 | color=black; 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /tf/doc/bifrucation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros/geometry/f23e0aa39053a57263615ebb7d603fc97f09707c/tf/doc/bifrucation.pdf -------------------------------------------------------------------------------- /tf/include/tf/LinearMath/MinMax.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 3 | 4 | This software is provided 'as-is', without any express or implied warranty. 5 | In no event will the authors be held liable for any damages arising from the use of this software. 6 | Permission is granted to anyone to use this software for any purpose, 7 | including commercial applications, and to alter it and redistribute it freely, 8 | subject to the following restrictions: 9 | 10 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 | 3. This notice may not be removed or altered from any source distribution. 13 | */ 14 | 15 | 16 | 17 | #ifndef TF_MINMAX_H 18 | #define TF_MINMAX_H 19 | 20 | template 21 | TFSIMD_FORCE_INLINE const T& tfMin(const T& a, const T& b) 22 | { 23 | return a < b ? a : b ; 24 | } 25 | 26 | template 27 | TFSIMD_FORCE_INLINE const T& tfMax(const T& a, const T& b) 28 | { 29 | return a > b ? a : b; 30 | } 31 | 32 | 33 | template 34 | TFSIMD_FORCE_INLINE void tfSetMin(T& a, const T& b) 35 | { 36 | if (b < a) 37 | { 38 | a = b; 39 | } 40 | } 41 | 42 | template 43 | TFSIMD_FORCE_INLINE void tfSetMax(T& a, const T& b) 44 | { 45 | if (a < b) 46 | { 47 | a = b; 48 | } 49 | } 50 | 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /tf/include/tf/LinearMath/QuadWord.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 3 | 4 | This software is provided 'as-is', without any express or implied warranty. 5 | In no event will the authors be held liable for any damages arising from the use of this software. 6 | Permission is granted to anyone to use this software for any purpose, 7 | including commercial applications, and to alter it and redistribute it freely, 8 | subject to the following restrictions: 9 | 10 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 | 3. This notice may not be removed or altered from any source distribution. 13 | */ 14 | 15 | 16 | #ifndef TF_QUADWORD_H 17 | #define TF_QUADWORD_H 18 | 19 | #include "Scalar.h" 20 | #include "MinMax.h" 21 | 22 | 23 | #if defined (__CELLOS_LV2) && defined (__SPU__) 24 | #include 25 | #endif 26 | 27 | 28 | namespace tf 29 | { 30 | /**@brief The QuadWord class is base class for Vector3 and Quaternion. 31 | * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. 32 | */ 33 | #ifndef USE_LIBSPE2 34 | ATTRIBUTE_ALIGNED16(class) QuadWord 35 | #else 36 | class QuadWord 37 | #endif 38 | { 39 | protected: 40 | 41 | #if defined (__SPU__) && defined (__CELLOS_LV2__) 42 | union { 43 | vec_float4 mVec128; 44 | tfScalar m_floats[4]; 45 | }; 46 | public: 47 | vec_float4 get128() const 48 | { 49 | return mVec128; 50 | } 51 | protected: 52 | #else //__CELLOS_LV2__ __SPU__ 53 | tfScalar m_floats[4]; 54 | #endif //__CELLOS_LV2__ __SPU__ 55 | 56 | public: 57 | 58 | 59 | /**@brief Return the x value */ 60 | TFSIMD_FORCE_INLINE const tfScalar& getX() const { return m_floats[0]; } 61 | /**@brief Return the y value */ 62 | TFSIMD_FORCE_INLINE const tfScalar& getY() const { return m_floats[1]; } 63 | /**@brief Return the z value */ 64 | TFSIMD_FORCE_INLINE const tfScalar& getZ() const { return m_floats[2]; } 65 | /**@brief Set the x value */ 66 | TFSIMD_FORCE_INLINE void setX(tfScalar x) { m_floats[0] = x;} 67 | /**@brief Set the y value */ 68 | TFSIMD_FORCE_INLINE void setY(tfScalar y) { m_floats[1] = y;} 69 | /**@brief Set the z value */ 70 | TFSIMD_FORCE_INLINE void setZ(tfScalar z) { m_floats[2] = z;} 71 | /**@brief Set the w value */ 72 | TFSIMD_FORCE_INLINE void setW(tfScalar w) { m_floats[3] = w;} 73 | /**@brief Return the x value */ 74 | TFSIMD_FORCE_INLINE const tfScalar& x() const { return m_floats[0]; } 75 | /**@brief Return the y value */ 76 | TFSIMD_FORCE_INLINE const tfScalar& y() const { return m_floats[1]; } 77 | /**@brief Return the z value */ 78 | TFSIMD_FORCE_INLINE const tfScalar& z() const { return m_floats[2]; } 79 | /**@brief Return the w value */ 80 | TFSIMD_FORCE_INLINE const tfScalar& w() const { return m_floats[3]; } 81 | 82 | //TFSIMD_FORCE_INLINE tfScalar& operator[](int i) { return (&m_floats[0])[i]; } 83 | //TFSIMD_FORCE_INLINE const tfScalar& operator[](int i) const { return (&m_floats[0])[i]; } 84 | ///operator tfScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. 85 | TFSIMD_FORCE_INLINE operator tfScalar *() { return &m_floats[0]; } 86 | TFSIMD_FORCE_INLINE operator const tfScalar *() const { return &m_floats[0]; } 87 | 88 | TFSIMD_FORCE_INLINE bool operator==(const QuadWord& other) const 89 | { 90 | return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); 91 | } 92 | 93 | TFSIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const 94 | { 95 | return !(*this == other); 96 | } 97 | 98 | /**@brief Set x,y,z and zero w 99 | * @param x Value of x 100 | * @param y Value of y 101 | * @param z Value of z 102 | */ 103 | TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z) 104 | { 105 | m_floats[0]=x; 106 | m_floats[1]=y; 107 | m_floats[2]=z; 108 | m_floats[3] = 0.f; 109 | } 110 | 111 | /* void getValue(tfScalar *m) const 112 | { 113 | m[0] = m_floats[0]; 114 | m[1] = m_floats[1]; 115 | m[2] = m_floats[2]; 116 | } 117 | */ 118 | /**@brief Set the values 119 | * @param x Value of x 120 | * @param y Value of y 121 | * @param z Value of z 122 | * @param w Value of w 123 | */ 124 | TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) 125 | { 126 | m_floats[0]=x; 127 | m_floats[1]=y; 128 | m_floats[2]=z; 129 | m_floats[3]=w; 130 | } 131 | /**@brief No initialization constructor */ 132 | TFSIMD_FORCE_INLINE QuadWord() 133 | // :m_floats[0](tfScalar(0.)),m_floats[1](tfScalar(0.)),m_floats[2](tfScalar(0.)),m_floats[3](tfScalar(0.)) 134 | { 135 | } 136 | 137 | /**@brief Three argument constructor (zeros w) 138 | * @param x Value of x 139 | * @param y Value of y 140 | * @param z Value of z 141 | */ 142 | TFSIMD_FORCE_INLINE QuadWord(const tfScalar& x, const tfScalar& y, const tfScalar& z) 143 | { 144 | m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; 145 | } 146 | 147 | /**@brief Initializing constructor 148 | * @param x Value of x 149 | * @param y Value of y 150 | * @param z Value of z 151 | * @param w Value of w 152 | */ 153 | TFSIMD_FORCE_INLINE QuadWord(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) 154 | { 155 | m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; 156 | } 157 | 158 | /**@brief Set each element to the max of the current values and the values of another QuadWord 159 | * @param other The other QuadWord to compare with 160 | */ 161 | TFSIMD_FORCE_INLINE void setMax(const QuadWord& other) 162 | { 163 | tfSetMax(m_floats[0], other.m_floats[0]); 164 | tfSetMax(m_floats[1], other.m_floats[1]); 165 | tfSetMax(m_floats[2], other.m_floats[2]); 166 | tfSetMax(m_floats[3], other.m_floats[3]); 167 | } 168 | /**@brief Set each element to the min of the current values and the values of another QuadWord 169 | * @param other The other QuadWord to compare with 170 | */ 171 | TFSIMD_FORCE_INLINE void setMin(const QuadWord& other) 172 | { 173 | tfSetMin(m_floats[0], other.m_floats[0]); 174 | tfSetMin(m_floats[1], other.m_floats[1]); 175 | tfSetMin(m_floats[2], other.m_floats[2]); 176 | tfSetMin(m_floats[3], other.m_floats[3]); 177 | } 178 | 179 | 180 | 181 | }; 182 | 183 | } 184 | 185 | #endif //TFSIMD_QUADWORD_H 186 | -------------------------------------------------------------------------------- /tf/include/tf/exceptions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #ifndef TF_EXCEPTIONS_H 33 | #define TF_EXCEPTIONS_H 34 | 35 | #include 36 | #include 37 | 38 | namespace tf{ 39 | 40 | // Pass through exceptions from tf2 41 | typedef tf2::TransformException TransformException; 42 | typedef tf2::LookupException LookupException; 43 | typedef tf2::ConnectivityException ConnectivityException; 44 | typedef tf2::ExtrapolationException ExtrapolationException; 45 | typedef tf2::InvalidArgumentException InvalidArgument; 46 | 47 | 48 | } 49 | 50 | 51 | #endif //TF_EXCEPTIONS_H 52 | -------------------------------------------------------------------------------- /tf/include/tf/time_cache.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #ifndef TF_TIME_CACHE_H 33 | #define TF_TIME_CACHE_H 34 | 35 | #include 36 | #include 37 | 38 | #include "tf/transform_datatypes.h" 39 | #include "tf/exceptions.h" 40 | 41 | #include "tf/LinearMath/Transform.h" 42 | 43 | #include 44 | 45 | namespace tf 46 | { 47 | enum ExtrapolationMode { ONE_VALUE, INTERPOLATE, EXTRAPOLATE_BACK, EXTRAPOLATE_FORWARD }; 48 | 49 | 50 | typedef uint32_t CompactFrameID; 51 | typedef std::pair P_TimeAndFrameID; 52 | 53 | /** \brief Storage for transforms and their parent */ 54 | class TransformStorage 55 | { 56 | public: 57 | TransformStorage(); 58 | TransformStorage(const StampedTransform& data, CompactFrameID frame_id, CompactFrameID child_frame_id); 59 | 60 | TransformStorage(const TransformStorage& rhs) 61 | { 62 | *this = rhs; 63 | } 64 | 65 | TransformStorage& operator=(const TransformStorage& rhs) 66 | { 67 | #if 01 68 | rotation_ = rhs.rotation_; 69 | translation_ = rhs.translation_; 70 | stamp_ = rhs.stamp_; 71 | frame_id_ = rhs.frame_id_; 72 | child_frame_id_ = rhs.child_frame_id_; 73 | #endif 74 | return *this; 75 | } 76 | 77 | bool operator< (const TransformStorage &b) const 78 | { 79 | return this->stamp_ < b.stamp_; 80 | } 81 | 82 | 83 | tf::Quaternion rotation_; 84 | tf::Vector3 translation_; 85 | ros::Time stamp_; 86 | CompactFrameID frame_id_; 87 | CompactFrameID child_frame_id_; 88 | }; 89 | 90 | 91 | 92 | 93 | /** \brief A class to keep a sorted linked list in time 94 | * This builds and maintains a list of timestamped 95 | * data. And provides lookup functions to get 96 | * data out as a function of time. */ 97 | class TimeCache 98 | { 99 | public: 100 | static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below. 101 | static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; //!< Maximum length of linked list, to make sure not to be able to use unlimited memory. 102 | static const int64_t DEFAULT_MAX_STORAGE_TIME = 1ULL * 1000000000LL; //!< default value of 10 seconds storage 103 | 104 | TimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME)); 105 | 106 | bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); 107 | bool insertData(const TransformStorage& new_data); 108 | void clearList(); 109 | CompactFrameID getParent(ros::Time time, std::string* error_str); 110 | P_TimeAndFrameID getLatestTimeAndParent(); 111 | 112 | /// Debugging information methods 113 | unsigned int getListLength(); 114 | ros::Time getLatestTimestamp(); 115 | ros::Time getOldestTimestamp(); 116 | 117 | 118 | private: 119 | typedef std::set L_TransformStorage; 120 | L_TransformStorage storage_; 121 | 122 | ros::Duration max_storage_time_; 123 | 124 | 125 | /// A helper function for getData 126 | //Assumes storage is already locked for it 127 | inline uint8_t findClosest(const TransformStorage*& one, const TransformStorage*& two, ros::Time target_time, std::string* error_str); 128 | 129 | inline void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output); 130 | 131 | 132 | void pruneList(); 133 | 134 | 135 | 136 | }; 137 | 138 | 139 | } 140 | #endif // TF_TIME_CACHE_H 141 | -------------------------------------------------------------------------------- /tf/include/tf/transform_broadcaster.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | /** \author Tully Foote */ 32 | 33 | #ifndef TF_TRANSFORMBROADCASTER_H 34 | #define TF_TRANSFORMBROADCASTER_H 35 | 36 | #include "tf/tf.h" 37 | #include "tf/tfMessage.h" 38 | 39 | #include 40 | 41 | 42 | namespace tf 43 | { 44 | 45 | 46 | /** \brief This class provides an easy way to publish coordinate frame transform information. 47 | * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the 48 | * necessary data needed for each message. */ 49 | 50 | class TransformBroadcaster{ 51 | public: 52 | /** \brief Constructor (needs a ros::Node reference) */ 53 | TransformBroadcaster(); 54 | 55 | /** \brief Send a StampedTransform 56 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 57 | void sendTransform(const StampedTransform & transform); 58 | 59 | /** \brief Send a vector of StampedTransforms 60 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 61 | void sendTransform(const std::vector & transforms); 62 | 63 | /** \brief Send a TransformStamped message 64 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 65 | void sendTransform(const geometry_msgs::TransformStamped & transform); 66 | 67 | /** \brief Send a vector of TransformStamped messages 68 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 69 | void sendTransform(const std::vector & transforms); 70 | 71 | private: 72 | 73 | tf2_ros::TransformBroadcaster tf2_broadcaster_; 74 | 75 | }; 76 | 77 | } 78 | 79 | #endif //TF_TRANSFORMBROADCASTER_H 80 | -------------------------------------------------------------------------------- /tf/index.rst: -------------------------------------------------------------------------------- 1 | tf 2 | == 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | tf_python 8 | transformations 9 | 10 | Indices and tables 11 | ================== 12 | 13 | * :ref:`genindex` 14 | * :ref:`modindex` 15 | * :ref:`search` 16 | 17 | -------------------------------------------------------------------------------- /tf/mainpage.dox: -------------------------------------------------------------------------------- 1 | 2 | /** \mainpage 3 | 4 | @htmlinclude manifest.html 5 | 6 | @b tf is a library for keeping track of coordinate frames. There are both C++ and Python bindings. 7 | 8 | @section usage Common Usage 9 | For most ROS use cases, the basic tf::Transformer library is not used directly. 10 | 11 | There are two helper classes to provide sending and recieving of ROS transform 12 | messages. tf::TransformBroadcaster and tf::TransformListener. 13 | 14 | @subsection listener TransformListener 15 | The tf::TransformListener class inherits from tf::Transformer to provide all the functionality of the 16 | basic library. In addition, it provides methods to transform data ROS messages directly and 17 | it automatically listens for transforms published on ROS. 18 | 19 | @subsection message_filter MessageFilter 20 | The tf::MessageFilter is the recommended method for receiving almost any sensor data from ROS. 21 | Data in ROS can be published with respect to any known frame. 22 | The tf::MessageFilter class makes it easy to use this data 23 | by providing callbacks only when it is possible to transform it into your desired 24 | target frame. 25 | 26 | The tf::MessageFilter class can subscribe to any ROS datatype that has a ROS Header. 27 | 28 | @subsection broadcaster TransformBroadcaster 29 | The tf::TransformBroadcaster class is the complement to the tf::TransformListener class. The broadcaster class provides a 30 | simple API for broadcasting coordinate frame transforms to other ROS nodes. 31 | 32 | @subsection send_transform send_transform 33 | The send_transform command is the easiest way to report transforms for fixed offsets. 34 | It is a simple command-line utility that repeatedly publishes the fixed-offset transform to ROS. 35 | 36 | @subsection datatypes Data Types used in tf 37 | - Quaternion typedef of btQuaternion 38 | - Vector3 typedef of btVector3 39 | - Point typedef of btVector3 40 | - Transform typedef of btTransform 41 | - Pose typedef of btTransform 42 | 43 | -Stamped version of all of the above inherits from the data type and also has: 44 | - ros::Time stamp_ 45 | - std::string frame_id_ 46 | - std::string child_frame_id_ (only used for Stamped ) 47 | 48 | - There are analogous ROS messages in std_msgs to the Stamped data types. 49 | 50 | - Time represented by ros::Time and ros::Duration in ros/time.h in roscpp 51 | 52 | 53 | 54 | */ 55 | -------------------------------------------------------------------------------- /tf/msg/tfMessage.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/TransformStamped[] transforms 2 | -------------------------------------------------------------------------------- /tf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | tf 3 | 1.13.4 4 | 5 | 6 | tf is a package that lets the user keep track of multiple coordinate 7 | frames over time. tf maintains the relationship between coordinate 8 | frames in a tree structure buffered in time, and lets the user 9 | transform points, vectors, etc between any two coordinate frames at 10 | any desired point in time. 11 | 12 |

Migration: Since ROS Hydro, tf has been "deprecated" in favor of tf2. tf2 is an iteration on tf providing generally the same feature set more efficiently. As well as adding a few new features.
13 | As tf2 is a major change the tf API has been maintained in its current form. Since tf2 has a superset of the tf features with a subset of the dependencies the tf implementation has been removed and replaced with calls to tf2 under the hood. This will mean that all users will be compatible with tf2. It is recommended for new work to use tf2 directly as it has a cleaner interface. However tf will continue to be supported for through at least J Turtle. 14 |

15 | 16 | Tully Foote 17 | Eitan Marder-Eppstein 18 | Wim Meeussen 19 | Tully Foote 20 | 21 | BSD 22 | http://www.ros.org/wiki/tf 23 | 24 | catkin 25 | 26 | angles 27 | geometry_msgs 28 | message_filters 29 | message_generation 30 | rosconsole 31 | roscpp 32 | rostime 33 | sensor_msgs 34 | std_msgs 35 | tf2_ros 36 | 37 | geometry_msgs 38 | graphviz 39 | message_filters 40 | message_runtime 41 | rosconsole 42 | roscpp 43 | roswtf 44 | sensor_msgs 45 | std_msgs 46 | tf2_ros 47 | 48 | rostest 49 | rosunit 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /tf/remap_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | - {old: "/asdf", 5 | new: "/a"} 6 | - {old: "/fdsa", 7 | new: "/b"} 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /tf/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | name: Python API 3 | output_dir: python 4 | - builder: doxygen 5 | name: C++ API 6 | output_dir: c++ 7 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' 8 | -------------------------------------------------------------------------------- /tf/scripts/bullet_migration_sed.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2011, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | 35 | 36 | # Run this script to update bullet datatypes from tf in Electric to 37 | # Fuerte/Unstable or newer 38 | 39 | # by default this assumes your files are not using the tf namespace. 40 | # If they are change the line below with the for loop to use the 41 | # namespaced_rules 42 | 43 | from __future__ import print_function 44 | 45 | import subprocess 46 | 47 | 48 | 49 | 50 | cmd = "find . -type f ! -name '*.svn-base' -a ! -name '*.hg' -a ! -name '*.git' -a \( -name '*.c*' -o -name '*.h*' \) -exec sed -i '%(rule)s' {} \;" 51 | 52 | rules = ['s|LinearMath/bt|tf/LinearMath/|g', # include path 53 | 's/btTransform\.h/Transform\.h/g', # include files 54 | 's/btMatrix3x3\.h/Matrix3x3\.h/g', 55 | 's/btScalar\.h/Scalar\.h/g', 56 | 's/btQuaternion\.h/Quaternion\.h/g', 57 | 's/btQuadWord\.h/QuadWord\.h/g', 58 | 's/btMinMax\.h/MinMax\.h/g', 59 | 's/btVector3\.h/Vector3\.h/g', 60 | 's/btScalar/tfScalar/g', 61 | ] 62 | 63 | unnamespaced_rules = [ 64 | 's/btTransform/tf::Transform/g', 65 | 's/btQuaternion/tf::Quaternion/g', 66 | 's/btVector3/tf::Vector3/g', 67 | 's/btMatrix3x3/tf::Matrix3x3/g', 68 | 's/btQuadWord/tf::QuadWord/g', 69 | 70 | ] 71 | 72 | namespaced_rules = [ 73 | 's/btTransform/Transform/g', 74 | 's/btQuaternion/Quaternion/g', 75 | 's/btVector3/Vector3/g', 76 | 's/btMatrix3x3/Matrix3x3/g', 77 | 's/btQuadWord/QuadWord/g', 78 | #'s/btScalar/Scalar/g', 79 | ] 80 | 81 | 82 | 83 | for rule in rules + unnamespaced_rules: #change me if using files with namespace tf set 84 | full_cmd = cmd%locals() 85 | print("Running {}".format(full_cmd)) 86 | ret_code = subprocess.call(full_cmd, shell=True) 87 | if ret_code == 0: 88 | print("success") 89 | else: 90 | print("failure") 91 | -------------------------------------------------------------------------------- /tf/scripts/groovy_compatibility/tf_remap: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | echo "Please use the package local script inside tf not the global one, this is deprecated." 4 | echo "Running [rosrun tf tf_remap] for you" 5 | exec rosrun tf tf_remap "$@" -------------------------------------------------------------------------------- /tf/scripts/groovy_compatibility/view_frames: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | echo "Please use the package local script inside tf not the global one, this is deprecated." 4 | echo "Running [rosrun tf view_frames] for you" 5 | exec rosrun tf view_frames "$@" -------------------------------------------------------------------------------- /tf/scripts/python_benchmark.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2011, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the Willow Garage nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | from __future__ import print_function 34 | 35 | import rostest 36 | import rospy 37 | import numpy 38 | import unittest 39 | import sys 40 | import time 41 | try: 42 | from cStringIO import StringIO 43 | except ImportError: 44 | from io import StringIO 45 | 46 | 47 | import tf.transformations 48 | import geometry_msgs.msg 49 | 50 | from tf.msg import tfMessage 51 | 52 | import tf 53 | 54 | iterations = 10000 55 | 56 | t = tf.Transformer() 57 | def mkm(): 58 | m = geometry_msgs.msg.TransformStamped() 59 | m.header.frame_id = "PARENT" 60 | m.child_frame_id = "THISFRAME" 61 | m.transform.translation.y = 5.0 62 | m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) 63 | return m 64 | 65 | tm = tfMessage([mkm() for i in range(20)]) 66 | 67 | def deserel_to_string(o): 68 | s = StringIO() 69 | o.serialize(s) 70 | return s.getvalue() 71 | 72 | mstr = deserel_to_string(tm) 73 | 74 | class Timer: 75 | def __init__(self, func): 76 | self.func = func 77 | def mean(self, iterations = 1000000): 78 | started = time.time() 79 | for i in range(iterations): 80 | self.func() 81 | took = time.time() - started 82 | return took / iterations 83 | 84 | import tf.msg 85 | import tf.cMsg 86 | for t in [tf.msg.tfMessage, tf.cMsg.tfMessage]: 87 | m2 = t() 88 | m2.deserialize(mstr) 89 | for m in m2.transforms: 90 | print(type(m), sys.getrefcount(m)) 91 | assert deserel_to_string(m2) == mstr, "deserel screwed up for type %s" % repr(t) 92 | 93 | m2 = t() 94 | print("deserialize only {} us each".format(1e6 * Timer(lambda: m2.deserialize(mstr)).mean()) 95 | 96 | sys.exit(0) 97 | 98 | started = time.time() 99 | for i in range(iterations): 100 | for m in tm.transforms: 101 | t.setTransform(m) 102 | took = time.time() - started 103 | print("setTransform only {} took {} us each".format(iterations, took, (1e6 * took / iterations))) 104 | 105 | started = time.time() 106 | for i in range(iterations): 107 | m2 = tfMessage() 108 | m2.deserialize(mstr) 109 | for m in m2.transforms: 110 | t.setTransform(m) 111 | took = time.time() - started 112 | print("deserialize+setTransform {} took {} us each".format(iterations, took, (1e6 * took / iterations))) 113 | 114 | from tf import TransformListener 115 | -------------------------------------------------------------------------------- /tf/scripts/tf_remap: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Willow Garage, Inc. nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | 35 | 36 | ## remap a tf topic 37 | 38 | from __future__ import print_function 39 | 40 | import rospy 41 | from tf.msg import tfMessage 42 | 43 | class TfRemapper: 44 | def __init__(self): 45 | self.pub = rospy.Publisher('/tf', tfMessage, queue_size=1) 46 | mappings = rospy.get_param('~mappings', []) 47 | self.mappings = {} 48 | 49 | for i in mappings: 50 | if "old" in i and "new" in i: 51 | self.mappings[i["old"]] = i["new"] 52 | 53 | print("Applying the following mappings to incoming tf frame ids", self.mappings) 54 | rospy.Subscriber("/tf_old", tfMessage, self.callback) 55 | 56 | def callback(self, tf_msg): 57 | for transform in tf_msg.transforms: 58 | if transform.header.frame_id in self.mappings: 59 | transform.header.frame_id = self.mappings[transform.header.frame_id] 60 | if transform.child_frame_id in self.mappings: 61 | transform.child_frame_id = self.mappings[transform.child_frame_id] 62 | 63 | self.pub.publish(tf_msg) 64 | 65 | def remap_tf(): 66 | pub.publish(Empty()) 67 | 68 | 69 | if __name__ == '__main__': 70 | rospy.init_node('tf_remapper') 71 | tfr = TfRemapper() 72 | rospy.spin() 73 | -------------------------------------------------------------------------------- /tf/scripts/view_frames: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | # Revision $Id: gossipbot.py 1013 2008-05-21 01:08:56Z sfkwc $ 35 | 36 | from __future__ import print_function, with_statement 37 | 38 | import sys, time 39 | import os 40 | import string 41 | import re 42 | import subprocess 43 | import distutils.version 44 | 45 | from optparse import OptionParser 46 | 47 | import rospy 48 | from tf.srv import * 49 | import tf 50 | 51 | def listen(duration): 52 | rospy.init_node("view_frames", anonymous=True) 53 | tf_listener = tf.TransformListener() 54 | print("WARNING: tf view_frames is deprecated. Use tf2_tools view_frames.py instead.") 55 | print("Listening to /tf for {} seconds".format(duration)) 56 | time.sleep(duration) 57 | print("Done Listening") 58 | return tf_listener.allFramesAsDot(rospy.Time.now()) 59 | 60 | def poll(node_name): 61 | print("Waiting for service {}/tf_frames".format(node_name)) 62 | rospy.wait_for_service('{}/tf_frames'.format(node_name)) 63 | 64 | try: 65 | print("Polling service") 66 | tf_frames_proxy = rospy.ServiceProxy('{}/tf_frames'.format(node_name), FrameGraph) 67 | output = tf_frames_proxy.call(FrameGraphRequest()) 68 | 69 | except rospy.ServiceException as e: 70 | print("Service call failed: {}".format(e)) 71 | 72 | return output.dot_graph 73 | 74 | def generate(dot_graph): 75 | 76 | with open('frames.gv', 'w') as outfile: 77 | outfile.write(dot_graph) 78 | 79 | try: 80 | subprocess.check_call(["dot", "-Tpdf", "frames.gv", "-o", "frames.pdf"]) 81 | print("frames.pdf generated") 82 | except subprocess.CalledProcessError: 83 | print("Failed to generate frames.pdf. Is graphviz installed?", file=sys.stderr) 84 | 85 | 86 | if __name__ == '__main__': 87 | parser = OptionParser(usage="usage: %prog [options]", prog='viewFrames.py') 88 | parser.add_option("--node", metavar="node name", 89 | type="string", help="Node to get frames from", 90 | dest="node") 91 | options, args = parser.parse_args() 92 | 93 | dot_graph = '' 94 | if not options.node: 95 | dot_graph = listen(5.0) 96 | else: 97 | print("Generating graph for node: ", options.node) 98 | dot_graph = poll(options.node) 99 | 100 | generate(dot_graph) 101 | -------------------------------------------------------------------------------- /tf/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['tf'], 8 | package_dir={'': 'src'}, 9 | requires=['genmsg', 'genpy', 'roslib', 'rospkg', 'geometry_msgs', 'sensor_msgs', 'std_msgs'], 10 | scripts=['scripts/groovy_compatibility/tf_remap', 11 | 'scripts/groovy_compatibility/view_frames'] 12 | ) 13 | 14 | setup(**d) 15 | -------------------------------------------------------------------------------- /tf/src/empty_listener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #include 30 | #include 31 | 32 | 33 | int main(int argc, char** argv) 34 | { 35 | ros::init(argc, argv, "my_tf_listener"); 36 | 37 | ros::NodeHandle node; 38 | 39 | tf::TransformListener listener; 40 | 41 | ros::Rate rate(10.0); 42 | while (node.ok()){ 43 | tf::StampedTransform transform; 44 | try 45 | { 46 | listener.lookupTransform("odom_combined", "base_link", ros::Time(0), transform); 47 | } 48 | catch (tf::TransformException ex) 49 | { 50 | ROS_ERROR("%s",ex.what()); 51 | } 52 | 53 | rate.sleep(); 54 | } 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /tf/src/static_transform_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include "tf/transform_broadcaster.h" 32 | 33 | class TransformSender 34 | { 35 | public: 36 | ros::NodeHandle node_; 37 | //constructor 38 | TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string& frame_id, const std::string& child_frame_id) 39 | { 40 | tf::Quaternion q; 41 | q.setRPY(roll, pitch,yaw); 42 | transform_ = tf::StampedTransform(tf::Transform(q, tf::Vector3(x,y,z)), time, frame_id, child_frame_id ); 43 | }; 44 | TransformSender(double x, double y, double z, double qx, double qy, double qz, double qw, ros::Time time, const std::string& frame_id, const std::string& child_frame_id) : 45 | transform_(tf::Transform(tf::Quaternion(qx,qy,qz,qw), tf::Vector3(x,y,z)), time, frame_id, child_frame_id){}; 46 | //Clean up ros connections 47 | ~TransformSender() { } 48 | 49 | //A pointer to the rosTFServer class 50 | tf::TransformBroadcaster broadcaster; 51 | 52 | 53 | 54 | // A function to call to send data periodically 55 | void send (ros::Time time) { 56 | transform_.stamp_ = time; 57 | broadcaster.sendTransform(transform_); 58 | }; 59 | 60 | private: 61 | tf::StampedTransform transform_; 62 | 63 | }; 64 | 65 | int main(int argc, char ** argv) 66 | { 67 | //Initialize ROS 68 | ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName); 69 | 70 | if(argc == 11) 71 | { 72 | ros::Duration sleeper(atof(argv[10])/1000.0); 73 | 74 | if (strcmp(argv[8], argv[9]) == 0) 75 | ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]); 76 | 77 | TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]), 78 | atof(argv[4]), atof(argv[5]), atof(argv[6]), atof(argv[7]), 79 | ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout 80 | argv[8], argv[9]); 81 | 82 | 83 | 84 | while(tf_sender.node_.ok()) 85 | { 86 | tf_sender.send(ros::Time::now() + sleeper); 87 | ROS_DEBUG("Sending transform from %s with parent %s\n", argv[8], argv[9]); 88 | sleeper.sleep(); 89 | } 90 | 91 | return 0; 92 | } 93 | else if (argc == 10) 94 | { 95 | ros::Duration sleeper(atof(argv[9])/1000.0); 96 | 97 | if (strcmp(argv[7], argv[8]) == 0) 98 | ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[7], argv[8]); 99 | 100 | TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]), 101 | atof(argv[4]), atof(argv[5]), atof(argv[6]), 102 | ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout 103 | argv[7], argv[8]); 104 | 105 | 106 | 107 | while(tf_sender.node_.ok()) 108 | { 109 | tf_sender.send(ros::Time::now() + sleeper); 110 | ROS_DEBUG("Sending transform from %s with parent %s\n", argv[7], argv[8]); 111 | sleeper.sleep(); 112 | } 113 | 114 | return 0; 115 | 116 | } 117 | else 118 | { 119 | printf("A command line utility for manually sending a transform.\n"); 120 | printf("It will periodicaly republish the given transform. \n"); 121 | printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period(milliseconds) \n"); 122 | printf("OR \n"); 123 | printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period(milliseconds) \n"); 124 | printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n"); 125 | printf("of the child_frame_id. \n"); 126 | ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments"); 127 | return -1; 128 | } 129 | 130 | 131 | } 132 | 133 | -------------------------------------------------------------------------------- /tf/src/tf/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | from __future__ import absolute_import 29 | 30 | from tf2_ros import TransformException as Exception, ConnectivityException, LookupException, ExtrapolationException 31 | from .listener import Transformer, TransformListener, TransformerROS 32 | from .broadcaster import TransformBroadcaster 33 | -------------------------------------------------------------------------------- /tf/src/tf/broadcaster.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2008, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the Willow Garage nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import geometry_msgs.msg 34 | import tf2_ros.transform_broadcaster 35 | 36 | class TransformBroadcaster: 37 | """ 38 | :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. 39 | """ 40 | 41 | def __init__(self, queue_size=100): 42 | self.tf2_broadcaster = tf2_ros.transform_broadcaster.TransformBroadcaster() 43 | 44 | def sendTransform(self, translation, rotation, time, child, parent): 45 | """ 46 | :param translation: the translation of the transformtion as a tuple (x, y, z) 47 | :param rotation: the rotation of the transformation as a tuple (x, y, z, w) 48 | :param time: the time of the transformation, as a rospy.Time() 49 | :param child: child frame in tf, string 50 | :param parent: parent frame in tf, string 51 | 52 | Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``. 53 | """ 54 | 55 | t = geometry_msgs.msg.TransformStamped() 56 | t.header.frame_id = parent 57 | t.header.stamp = time 58 | t.child_frame_id = child 59 | t.transform.translation.x = translation[0] 60 | t.transform.translation.y = translation[1] 61 | t.transform.translation.z = translation[2] 62 | 63 | t.transform.rotation.x = rotation[0] 64 | t.transform.rotation.y = rotation[1] 65 | t.transform.rotation.z = rotation[2] 66 | t.transform.rotation.w = rotation[3] 67 | 68 | self.sendTransformMessage(t) 69 | 70 | def sendTransformMessage(self, transform): 71 | """ 72 | :param transform: geometry_msgs.msg.TransformStamped 73 | Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``. 74 | """ 75 | self.tf2_broadcaster.sendTransform([transform]) 76 | -------------------------------------------------------------------------------- /tf/src/tf/tfwtf.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2009, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | # Revision $Id$ 34 | 35 | from __future__ import print_function 36 | 37 | import time 38 | 39 | from roswtf.rules import warning_rule, error_rule 40 | import rosgraph 41 | import rospy 42 | 43 | import tf.msg 44 | 45 | import math 46 | 47 | 48 | # global list of messages received 49 | _msgs = [] 50 | 51 | ################################################################################ 52 | # RULES 53 | 54 | def rostime_delta(ctx): 55 | deltas = {} 56 | for m, stamp, callerid in _msgs: 57 | for t in m.transforms: 58 | d = t.header.stamp - stamp 59 | secs = d.to_sec() 60 | if abs(secs) > 1.: 61 | if callerid in deltas: 62 | if abs(secs) > abs(deltas[callerid]): 63 | deltas[callerid] = secs 64 | else: 65 | deltas[callerid] = secs 66 | 67 | errors = [] 68 | for k, v in deltas.items(): 69 | errors.append("receiving transform from [{}] that differed from ROS time by {}s".format(k, v)) 70 | return errors 71 | 72 | def reparenting(ctx): 73 | errors = [] 74 | parent_id_map = {} 75 | for m, stamp, callerid in _msgs: 76 | for t in m.transforms: 77 | frame_id = t.child_frame_id 78 | parent_id = t.header.frame_id 79 | if frame_id in parent_id_map and parent_id_map[frame_id] != parent_id: 80 | msg = "reparenting of [{}] to [{}] by [{}]".format(frame_id, parent_id, callerid) 81 | parent_id_map[frame_id] = parent_id 82 | if msg not in errors: 83 | errors.append(msg) 84 | else: 85 | parent_id_map[frame_id] = parent_id 86 | return errors 87 | 88 | def cycle_detection(ctx): 89 | max_depth = 100 90 | errors = [] 91 | parent_id_map = {} 92 | for m, stamp, callerid in _msgs: 93 | for t in m.transforms: 94 | frame_id = t.child_frame_id 95 | parent_id = t.header.frame_id 96 | parent_id_map[frame_id] = parent_id 97 | 98 | for frame in parent_id_map: 99 | frame_list = [] 100 | current_frame = frame 101 | count = 0 102 | while count < max_depth + 2: 103 | count = count + 1 104 | frame_list.append(current_frame) 105 | try: 106 | current_frame = parent_id_map[current_frame] 107 | except KeyError: 108 | break 109 | if current_frame == frame: 110 | errors.append("Frame {} is in a loop. It's loop has elements:\n{}".format(frame, " -> ".join(frame_list))) 111 | break 112 | 113 | return errors 114 | 115 | def multiple_authority(ctx): 116 | errors = [] 117 | authority_map = {} 118 | for m, stamp, callerid in _msgs: 119 | for t in m.transforms: 120 | frame_id = t.child_frame_id 121 | parent_id = t.header.frame_id 122 | if frame_id in authority_map and authority_map[frame_id] != callerid: 123 | msg = "node [{}] publishing transform [{}] with parent [{}] already published by node [{}]".format(callerid, frame_id, parent_id, authority_map[frame_id]) 124 | authority_map[frame_id] = callerid 125 | if msg not in errors: 126 | errors.append(msg) 127 | else: 128 | authority_map[frame_id] = callerid 129 | return errors 130 | 131 | def no_msgs(ctx): 132 | return not _msgs 133 | 134 | def not_normalized(ctx): 135 | errors = [] 136 | for m, stamp, callerid in _msgs: 137 | for t in m.transforms: 138 | q = t.transform.rotation 139 | length = math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) 140 | if math.fabs(length - 1) > 1e-6: 141 | errors.append("rotation from [{}] to [{}] is not unit length, {}".format(t.header.frame_id, t.child_frame_id, length)) 142 | return errors 143 | 144 | ################################################################################ 145 | # roswtf PLUGIN 146 | 147 | # tf_warnings and tf_errors declare the rules that we actually check 148 | 149 | tf_warnings = [ 150 | (no_msgs, "No tf messages"), 151 | (rostime_delta, "Received out-of-date/future transforms:"), 152 | (not_normalized, "Received non-normalized rotation in transforms:"), 153 | ] 154 | tf_errors = [ 155 | (reparenting, "TF re-parenting contention:"), 156 | (cycle_detection, "TF cycle detection::"), 157 | (multiple_authority, "TF multiple authority contention:"), 158 | ] 159 | 160 | # rospy subscriber callback for /tf 161 | def _tf_handle(msg): 162 | _msgs.append((msg, rospy.get_rostime(), msg._connection_header['callerid'])) 163 | 164 | # @return bool: True if /tf has a publisher 165 | def is_tf_active(): 166 | master = rosgraph.Master('/tfwtf') 167 | if master is not None: 168 | try: 169 | val = master.getPublishedTopics('/') 170 | if filter(lambda x: x[0] == '/tf', val): 171 | return True 172 | except: 173 | pass 174 | return False 175 | 176 | # roswtf entry point for online checks 177 | def roswtf_plugin_online(ctx): 178 | # don't run plugin if tf isn't active as these checks take awhile 179 | if not is_tf_active(): 180 | return 181 | 182 | print("running tf checks, this will take a second...") 183 | sub1 = rospy.Subscriber('/tf', tf.msg.tfMessage, _tf_handle) 184 | time.sleep(1.0) 185 | sub1.unregister() 186 | print("... tf checks complete") 187 | 188 | for r in tf_warnings: 189 | warning_rule(r, r[0](ctx), ctx) 190 | for r in tf_errors: 191 | error_rule(r, r[0](ctx), ctx) 192 | 193 | # currently no static checks for tf 194 | #def roswtf_plugin_static(ctx): 195 | # for r in tf_warnings: 196 | # warning_rule(r, r[0](ctx), ctx) 197 | # for r in tf_errors: 198 | # error_rule(r, r[0](ctx), ctx) 199 | -------------------------------------------------------------------------------- /tf/src/tf_echo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include "tf/transform_listener.h" 32 | #include "ros/ros.h" 33 | 34 | #define _USE_MATH_DEFINES 35 | class echoListener 36 | { 37 | public: 38 | 39 | tf::TransformListener tf; 40 | 41 | //constructor with name 42 | echoListener() 43 | { 44 | 45 | } 46 | 47 | ~echoListener() 48 | { 49 | 50 | } 51 | 52 | private: 53 | 54 | }; 55 | 56 | 57 | int main(int argc, char ** argv) 58 | { 59 | //Initialize ROS 60 | ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName); 61 | 62 | // Allow 2 or 3 command line arguments 63 | if (argc < 3 || argc > 4) 64 | { 65 | printf("Usage: tf_echo source_frame target_frame [echo_rate]\n\n"); 66 | printf("This will echo the transform from the coordinate frame of the source_frame\n"); 67 | printf("to the coordinate frame of the target_frame. \n"); 68 | printf("Note: This is the transform to get data from target_frame into the source_frame.\n"); 69 | printf("Default echo rate is 1 if echo_rate is not given.\n"); 70 | return -1; 71 | } 72 | 73 | ros::NodeHandle nh("~"); 74 | 75 | double rate_hz; 76 | if (argc == 4) 77 | { 78 | // read rate from command line 79 | rate_hz = atof(argv[3]); 80 | } 81 | else 82 | { 83 | // read rate parameter 84 | nh.param("rate", rate_hz, 1.0); 85 | } 86 | if (rate_hz <= 0.0) 87 | { 88 | std::cerr << "Echo rate must be > 0.0\n"; 89 | return -1; 90 | } 91 | ros::Rate rate(rate_hz); 92 | 93 | int precision(3); 94 | if (nh.getParam("precision", precision)) 95 | { 96 | if (precision < 1) 97 | { 98 | std::cerr << "Precision must be > 0\n"; 99 | return -1; 100 | } 101 | printf("Precision default value was overriden, new value: %d\n", precision); 102 | } 103 | 104 | //Instantiate a local listener 105 | echoListener echoListener; 106 | 107 | 108 | std::string source_frameid = std::string(argv[1]); 109 | std::string target_frameid = std::string(argv[2]); 110 | 111 | // Wait for up to one second for the first transforms to become avaiable. 112 | echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0)); 113 | 114 | //Nothing needs to be done except wait for a quit 115 | //The callbacks withing the listener class 116 | //will take care of everything 117 | while(nh.ok()) 118 | { 119 | try 120 | { 121 | tf::StampedTransform echo_transform; 122 | echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform); 123 | std::cout.precision(precision); 124 | std::cout.setf(std::ios::fixed,std::ios::floatfield); 125 | std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl; 126 | double yaw, pitch, roll; 127 | echo_transform.getBasis().getRPY(roll, pitch, yaw); 128 | tf::Quaternion q = echo_transform.getRotation(); 129 | tf::Vector3 v = echo_transform.getOrigin(); 130 | std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl; 131 | std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", " 132 | << q.getZ() << ", " << q.getW() << "]" << std::endl 133 | << " in RPY (radian) [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl 134 | << " in RPY (degree) [" << roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl; 135 | 136 | //print transform 137 | } 138 | catch(tf::TransformException& ex) 139 | { 140 | std::cout << "Failure at "<< ros::Time::now() << std::endl; 141 | std::cout << "Exception thrown:" << ex.what()<< std::endl; 142 | std::cout << "The current list of frames is:" < 39 | 40 | 41 | 42 | namespace tf { 43 | 44 | TransformBroadcaster::TransformBroadcaster(): 45 | tf2_broadcaster_() 46 | { 47 | } 48 | 49 | void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf) 50 | { 51 | tf2_broadcaster_.sendTransform(msgtf); 52 | } 53 | 54 | void TransformBroadcaster::sendTransform(const StampedTransform & transform) 55 | { 56 | geometry_msgs::TransformStamped msgtf; 57 | transformStampedTFToMsg(transform, msgtf); 58 | tf2_broadcaster_.sendTransform(msgtf); 59 | } 60 | 61 | void TransformBroadcaster::sendTransform(const std::vector & msgtf) 62 | { 63 | tf2_broadcaster_.sendTransform(msgtf); 64 | } 65 | 66 | void TransformBroadcaster::sendTransform(const std::vector & transforms) 67 | { 68 | std::vector msgtfs; 69 | for (std::vector::const_iterator it = transforms.begin(); it != transforms.end(); ++it) 70 | { 71 | geometry_msgs::TransformStamped msgtf; 72 | transformStampedTFToMsg(*it, msgtf); 73 | msgtfs.push_back(msgtf); 74 | 75 | } 76 | tf2_broadcaster_.sendTransform(msgtfs); 77 | } 78 | 79 | 80 | 81 | 82 | } 83 | 84 | 85 | -------------------------------------------------------------------------------- /tf/srv/FrameGraph.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string dot_graph 3 | -------------------------------------------------------------------------------- /tf/test/method_test.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import rospy 4 | import tf 5 | import time 6 | import bullet 7 | import math 8 | 9 | 10 | try: 11 | transform_stamped = tf.TransformStamped() 12 | print("getting stamp") 13 | print(transform_stamped.stamp) 14 | # mytime = rospy.Time().now() 15 | mytime = rospy.Time(10,20) 16 | transform_stamped.stamp = mytime 17 | print(mytime) 18 | print("getting stamp", transform_stamped.stamp) 19 | print("transform:", transform_stamped.transform) 20 | transform_stamped.transform.setIdentity() 21 | print("after setIdentity()", transform_stamped.transform) 22 | # transform_stamped.transform.basis.setEulerZYX(0,0,0) 23 | quat = bullet.Quaternion(math.pi/2,0,0) 24 | print("quaternion ", quat) 25 | transform_stamped.transform.setRotation(quat) 26 | print("setting rotation to PI/2\n After setRotation") 27 | print(transform_stamped.transform) 28 | 29 | other_transform = bullet.Transform() 30 | other_transform.setIdentity() 31 | transform_stamped.transform = other_transform 32 | print("After assignment of Identity") 33 | print(transform_stamped.transform) 34 | 35 | other_transform = bullet.Transform() 36 | other_transform.setIdentity() 37 | other_transform.setRotation(quat) 38 | transform_stamped.transform = other_transform 39 | print("After assignment of Rotation Transformation") 40 | print(transform_stamped.transform) 41 | 42 | except ValueError as e: 43 | print("Exception {} Improperly thrown: {}".format(type(e), e)) 44 | -------------------------------------------------------------------------------- /tf/test/operator_overload.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #include "tf/LinearMath/Transform.h" 34 | 35 | 36 | void seed_rand() 37 | { 38 | //Seed random number generator with current microseond count 39 | timeval temp_time_struct; 40 | gettimeofday(&temp_time_struct,NULL); 41 | srand(temp_time_struct.tv_usec); 42 | }; 43 | 44 | 45 | int main(int argc, char **argv){ 46 | 47 | unsigned int runs = 400; 48 | seed_rand(); 49 | 50 | std::vector xvalues(runs), yvalues(runs), zvalues(runs); 51 | for ( unsigned int i = 0; i < runs ; i++ ) 52 | { 53 | xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 54 | yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 55 | zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 56 | } 57 | 58 | //Useful Operator Overload 59 | for ( unsigned int i = 0; i < runs ; i++ ) 60 | { 61 | tf::Transform transform(tf::Quaternion(0,0,0), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])); 62 | tf::Quaternion initial(xvalues[i],yvalues[i],zvalues[i]); 63 | tf::Quaternion final(xvalues[i],yvalues[i],zvalues[i]); 64 | final = transform * initial; 65 | std::printf("Useful Operator Overload: %f, angle between quaternions\n", initial.angle(final)); 66 | } 67 | 68 | 69 | return 0; 70 | } 71 | 72 | 73 | -------------------------------------------------------------------------------- /tf/test/python_debug_test.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | from __future__ import print_function 29 | 30 | import rospy 31 | import tf 32 | import time 33 | import bullet 34 | import math 35 | 36 | try: 37 | 38 | rospy.init_node("test_node") 39 | tfl = tf.TransformListener() 40 | time.sleep(1) 41 | 42 | # View all frames 43 | print("All frames are:\n", tfl.all_frames_as_string()) 44 | 45 | # dot based introspection 46 | print("All frame graph is:\n", tfl.all_frames_as_dot()) 47 | 48 | 49 | # test transforming pose 50 | po = tf.PoseStamped() 51 | po.frame_id = "base_link" 52 | print("calling transform pose") 53 | po2 = tfl.transform_pose("/map", po) 54 | 55 | print("po2.pose.this", po2.pose.this) 56 | print("po.pose.this", po.pose.this) 57 | 58 | # test transforming point 59 | po = tf.PointStamped() 60 | po.frame_id = "base_link" 61 | po3 = tfl.transform_point("/map", po) 62 | print("po3") 63 | print(po3.this) 64 | 65 | # test transforming vector 66 | po = tf.VectorStamped() 67 | po.frame_id = "base_link" 68 | po4 = tfl.transform_vector("/map", po) 69 | print(po4.this) 70 | 71 | # test transforming quaternion 72 | po = tf.QuaternionStamped() 73 | po.frame_id = "base_link" 74 | po5 = tfl.transform_quaternion("/map", po) 75 | print("po5", po5.this) 76 | 77 | tr = tf.TransformStamped() 78 | 79 | lps = tf.PoseStamped() 80 | lps.pose.setIdentity() 81 | print("setting stamp") 82 | mytime = rospy.Time(10,20) 83 | lps.stamp = mytime 84 | print(mytime) 85 | print("getting stamp") 86 | output = lps.stamp 87 | print(output) 88 | print(lps.pose) 89 | print("setting pose.positon to 1,2,3") 90 | lps.pose.setOrigin( bullet.Vector3(1,2,3)) 91 | print(lps.pose.getOrigin()) 92 | print(lps.pose) 93 | 94 | transform_stamped = tf.TransformStamped() 95 | print("getting stamp") 96 | print(transform_stamped.stamp) 97 | # mytime = rospy.Time().now() 98 | mytime = rospy.Time(10,20) 99 | transform_stamped.stamp = mytime 100 | print(mytime) 101 | print("getting stamp", transform_stamped.stamp) 102 | print("transform:", transform_stamped.transform) 103 | transform_stamped.transform.setIdentity() 104 | print("after setIdentity()", transform_stamped.transform) 105 | # transform_stamped.transform.basis.setEulerZYX(0,0,0) 106 | quat = bullet.Quaternion(math.pi/2,0,0) 107 | print("quaternion ", quat) 108 | transform_stamped.transform.setRotation(quat) 109 | print("setting rotation to PI/2",transform_stamped.transform) 110 | 111 | 112 | pointstamped = tf.PointStamped() 113 | print("getting stamp") 114 | print(pointstamped.stamp) 115 | # mytime = rospy.Time().now() 116 | mytime = rospy.Time(10,20) 117 | pointstamped.stamp = mytime 118 | print(mytime) 119 | print("getting stamp") 120 | output = pointstamped.stamp 121 | print(output) 122 | print(pointstamped.point) 123 | print(transform_stamped.transform * pointstamped.point) 124 | 125 | pose_only = bullet.Transform(transform_stamped.transform) 126 | print("destructing pose_only", pose_only.this ) 127 | pose_only = [] 128 | 129 | 130 | print("Creating copy") 131 | po2_copy = tf.PoseStamped(po2) 132 | print("po2_copy.pose", po2_copy.pose.this) 133 | print("po2.pose", po2.pose.this) 134 | 135 | print("Creating copy2") 136 | po2_copy2 = tf.PoseStamped(po2) 137 | print("po2_copy2.pose", po2_copy2.pose.this) 138 | 139 | 140 | print("destructing po2 po2.pose is", po2.pose.this) 141 | po2 = [] 142 | 143 | 144 | print("destructing po2_copy po2_copy.pose is", po2_copy.pose.this) 145 | po2_copy = [] 146 | 147 | 148 | 149 | 150 | 151 | 152 | print("done") 153 | 154 | except ValueError as e: 155 | print("Exception {} Improperly thrown: {}".format(type(e), e)) 156 | 157 | 158 | -------------------------------------------------------------------------------- /tf/test/quaternion.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "tf/LinearMath/Transform.h" 36 | 37 | double epsilon = 10E-6; 38 | 39 | 40 | void seed_rand() 41 | { 42 | //Seed random number generator with current microseond count 43 | timeval temp_time_struct; 44 | gettimeofday(&temp_time_struct,NULL); 45 | srand(temp_time_struct.tv_usec); 46 | } 47 | 48 | void testQuatRPY(tf::Quaternion q_baseline) 49 | { 50 | q_baseline.normalize(); 51 | tf::Matrix3x3 m(q_baseline); 52 | double roll, pitch, yaw; 53 | 54 | for (int solution = 1 ; solution < 2 ; ++solution) 55 | { 56 | m.getRPY(roll, pitch, yaw, solution); 57 | tf::Quaternion q_from_rpy; 58 | q_from_rpy.setRPY(roll, pitch, yaw); 59 | // use angleShortestPath() because angle() can return PI for equivalent 60 | // quaternions 61 | double angle1 = q_from_rpy.angleShortestPath(q_baseline); 62 | ASSERT_NEAR(0.0, angle1, epsilon); 63 | //std::printf("%f, angle between quaternions\n", angle1); 64 | 65 | tf::Matrix3x3 m2; 66 | m2.setRPY(roll, pitch, yaw); 67 | tf::Quaternion q_from_m_from_rpy; 68 | m2.getRotation(q_from_m_from_rpy); 69 | // use angleShortestPath() because angle() can return PI for equivalent 70 | // quaternions 71 | double angle2 = q_from_m_from_rpy.angleShortestPath(q_baseline); 72 | ASSERT_NEAR(0.0, angle2, epsilon); 73 | //std::printf("%f, angle between quaternions\n", angle2); 74 | } 75 | } 76 | 77 | TEST(tf, Quaternion) 78 | { 79 | unsigned int runs = 400; 80 | seed_rand(); 81 | 82 | std::vector xvalues(runs), yvalues(runs), zvalues(runs); 83 | for ( unsigned int i = 0; i < runs ; i++ ) 84 | { 85 | xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 86 | yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 87 | zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 88 | } 89 | 90 | 91 | for ( unsigned int i = 0; i < runs ; i++ ) 92 | { 93 | tf::Matrix3x3 mat; 94 | tf::Quaternion q_baseline; 95 | q_baseline.setRPY(zvalues[i],yvalues[i],xvalues[i]); 96 | mat.setRotation(q_baseline); 97 | tf::Quaternion q_from_m; 98 | mat.getRotation(q_from_m); 99 | double angle = q_from_m.angle(q_baseline); 100 | ASSERT_NEAR(0.0, angle, epsilon); 101 | testQuatRPY(q_baseline); 102 | } 103 | 104 | // test some corner cases 105 | testQuatRPY(tf::Quaternion( 0.5, 0.5, 0.5, -0.5)); 106 | testQuatRPY(tf::Quaternion( 0.5, 0.5, 0.5, 0.5)); 107 | testQuatRPY(tf::Quaternion( 0.5, -0.5, 0.5, 0.5)); 108 | testQuatRPY(tf::Quaternion( 0.5, 0.5, -0.5, 0.5)); 109 | testQuatRPY(tf::Quaternion(-0.5, 0.5, 0.5, 0.5)); 110 | } 111 | 112 | 113 | int main(int argc, char **argv){ 114 | testing::InitGoogleTest(&argc, argv); 115 | return RUN_ALL_TESTS(); 116 | } 117 | -------------------------------------------------------------------------------- /tf/test/testBroadcaster.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "tf/transform_broadcaster.h" 31 | #include "ros/ros.h" 32 | 33 | class testBroadcaster 34 | { 35 | public: 36 | //constructor 37 | testBroadcaster() : count(0), count1(0){}; 38 | //Clean up ros connections 39 | ~testBroadcaster() { } 40 | 41 | //A pointer to the rosTFServer class 42 | tf::TransformBroadcaster broadcaster; 43 | 44 | 45 | // A function to call to send data periodically 46 | void test () { 47 | broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "frame2", "frame1")); 48 | 49 | if (count > 9000) 50 | { 51 | count = 0; 52 | std::cerr<<"Counter 0 rolledover at 9000"<< std::endl; 53 | } 54 | else 55 | count ++; 56 | //std::cerr< vec; 62 | vec.push_back(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "vframe2", "vframe1")); 63 | vec.push_back(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "vframe1", "vframe0")); 64 | broadcaster.sendTransform(vec); 65 | 66 | if (count1 > 9000) 67 | { 68 | count1 = 0; 69 | std::cerr<<"Counter 1 rolledover at 9000"<< std::endl; 70 | } 71 | else 72 | count1 ++; 73 | //std::cerr< 34 | 35 | TEST(TransformBroadcaster, single_frame) 36 | { 37 | tf::TransformListener tfl; 38 | EXPECT_TRUE(tfl.waitForTransform("frame1", "frame2", ros::Time(), ros::Duration(2.0))); 39 | 40 | } 41 | 42 | TEST(TransformBroadcaster, multi_frame) 43 | { 44 | tf::TransformListener tfl; 45 | EXPECT_TRUE(tfl.waitForTransform("vframe0", "vframe2", ros::Time(), ros::Duration(2.0))); 46 | 47 | } 48 | 49 | int main(int argc, char ** argv) 50 | { 51 | testing::InitGoogleTest(&argc, argv); 52 | 53 | //Initialize ROS 54 | ros::init(argc, argv, "listener"); 55 | ros::NodeHandle nh; 56 | 57 | int ret = RUN_ALL_TESTS(); 58 | 59 | return ret; 60 | } 61 | 62 | -------------------------------------------------------------------------------- /tf/test/test_broadcaster.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /tf/test/test_message_filter.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /tf/test/test_transform_datatypes.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "tf/LinearMath/Vector3.h" 35 | 36 | using namespace tf; 37 | 38 | TEST(tf, set) 39 | { 40 | tf::Quaternion q = tf::createQuaternionFromRPY(0, 0, 0); 41 | EXPECT_EQ(q, tf::createIdentityQuaternion()); 42 | } 43 | 44 | 45 | 46 | int main(int argc, char **argv){ 47 | testing::InitGoogleTest(&argc, argv); 48 | return RUN_ALL_TESTS(); 49 | } 50 | -------------------------------------------------------------------------------- /tf/test/tf_unittest_future.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "tf/LinearMath/Vector3.h" 6 | 7 | using namespace tf; 8 | 9 | void seed_rand() 10 | { 11 | //Seed random number generator with current microseond count 12 | timeval temp_time_struct; 13 | gettimeofday(&temp_time_struct,NULL); 14 | srand(temp_time_struct.tv_usec); 15 | }; 16 | 17 | 18 | 19 | TEST(tf, SignFlipExtrapolate) 20 | { 21 | double epsilon = 1e-3; 22 | 23 | double truex, truey, trueyaw1, trueyaw2; 24 | 25 | truex = 5.220; 26 | truey = 1.193; 27 | trueyaw1 = 2.094; 28 | trueyaw2 = 2.199; 29 | ros::Time ts0; 30 | ts0.fromSec(46.6); 31 | ros::Time ts1; 32 | ts1.fromSec(46.7); 33 | ros::Time ts2; 34 | ts2.fromSec(46.8); 35 | 36 | TransformStorage tout; 37 | double yaw, pitch, roll; 38 | 39 | TransformStorage t0(StampedTransform 40 | (tf::Transform(tf::Quaternion(0.000, 0.000, -0.8386707128751809, 0.5446388118427071), 41 | tf::Vector3(1.0330764266905630, 5.2545257423922198, -0.000)), 42 | ts0, "odom", "other0"), 3); 43 | TransformStorage t1(StampedTransform 44 | (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8660255375641606, -0.4999997682866531), 45 | tf::Vector3(1.5766646418987809, 5.1177550046707436, -0.000)), 46 | ts1, "odom", "other1"), 3); 47 | TransformStorage t2(StampedTransform 48 | (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8910066733792211, -0.4539902069358919), 49 | tf::Vector3(2.1029791754869160, 4.9249128183465967, -0.000)), 50 | ts2, "odom", "other2"), 3); 51 | 52 | tf::TimeCache tc; 53 | tf::Transform res; 54 | 55 | tc.interpolate(t0, t1, ts1, tout); 56 | res = tout.inverse(); 57 | res.getBasis().getEulerZYX(yaw,pitch,roll); 58 | 59 | EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); 60 | EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); 61 | EXPECT_NEAR(yaw, trueyaw1, epsilon); 62 | 63 | tc.interpolate(t0, t1, ts2, tout); 64 | res = tout.inverse(); 65 | res.getBasis().getEulerZYX(yaw,pitch,roll); 66 | 67 | EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); 68 | EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); 69 | EXPECT_NEAR(yaw, trueyaw2, epsilon); 70 | 71 | tc.interpolate(t1, t2, ts2, tout); 72 | res = tout.inverse(); 73 | res.getBasis().getEulerZYX(yaw,pitch,roll); 74 | 75 | EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); 76 | EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); 77 | EXPECT_NEAR(yaw, trueyaw2, epsilon); 78 | } 79 | 80 | 81 | 82 | /** @todo Make this actually Assert something */ 83 | 84 | int main(int argc, char **argv){ 85 | testing::InitGoogleTest(&argc, argv); 86 | return RUN_ALL_TESTS(); 87 | } 88 | 89 | -------------------------------------------------------------------------------- /tf/test/transform_listener_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | 35 | void seed_rand() 36 | { 37 | //Seed random number generator with current microseond count 38 | timeval temp_time_struct; 39 | gettimeofday(&temp_time_struct,NULL); 40 | srand(temp_time_struct.tv_usec); 41 | } 42 | 43 | void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) 44 | { 45 | seed_rand(); 46 | for ( uint64_t i = 0; i < runs ; i++ ) 47 | { 48 | xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 49 | yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 50 | zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 51 | } 52 | } 53 | 54 | 55 | using namespace tf; 56 | 57 | 58 | TEST(transform_listener, resolve) 59 | { 60 | ros::NodeHandle n("~"); 61 | tf::TransformListener tl; 62 | 63 | //no prefix 64 | EXPECT_STREQ("id", tl.resolve("id").c_str()); 65 | 66 | n.setParam("tf_prefix", "a_tf_prefix"); 67 | tf::TransformListener tp; 68 | 69 | std::string prefix_str = tf::getPrefixParam(n); 70 | 71 | EXPECT_STREQ("a_tf_prefix", prefix_str.c_str()); 72 | 73 | EXPECT_STREQ("a_tf_prefix/id", tf::resolve(prefix_str, "id").c_str()); 74 | 75 | EXPECT_STREQ("a_tf_prefix/id", tp.resolve("id").c_str()); 76 | 77 | 78 | } 79 | 80 | 81 | int main(int argc, char **argv){ 82 | testing::InitGoogleTest(&argc, argv); 83 | ros::init(argc, argv, "transform_listener_unittest"); 84 | return RUN_ALL_TESTS(); 85 | } 86 | -------------------------------------------------------------------------------- /tf/test/transform_listener_unittest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /tf/test/transform_twist_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /tf/transformations.rst: -------------------------------------------------------------------------------- 1 | transformations 2 | ============== 3 | .. automodule:: tf.transformations 4 | :members: 5 | -------------------------------------------------------------------------------- /tf_conversions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(tf_conversions) 3 | 4 | find_package(orocos_kdl REQUIRED) 5 | find_package(catkin REQUIRED geometry_msgs kdl_conversions tf) 6 | find_package(Eigen3 REQUIRED) 7 | 8 | catkin_python_setup() 9 | 10 | catkin_package( 11 | INCLUDE_DIRS include 12 | LIBRARIES ${PROJECT_NAME} 13 | DEPENDS EIGEN3 orocos_kdl 14 | CATKIN_DEPENDS geometry_msgs kdl_conversions tf 15 | ) 16 | 17 | include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) 18 | 19 | # Needed due to no full filename in orocos_kdl pkg-config export 20 | link_directories(${orocos_kdl_LIBRARY_DIRS}) 21 | 22 | add_library(${PROJECT_NAME} 23 | src/tf_kdl.cpp 24 | src/tf_eigen.cpp 25 | ) 26 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 27 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) 28 | 29 | # Tests 30 | if(CATKIN_ENABLE_TESTING) 31 | 32 | 33 | catkin_add_gtest(test_eigen_tf test/test_eigen_tf.cpp) 34 | target_link_libraries(test_eigen_tf ${PROJECT_NAME} ${orocos_kdl_LIBRARIES}) 35 | 36 | catkin_add_gtest(test_kdl_tf test/test_kdl_tf.cpp) 37 | if(CMAKE_COMPILER_IS_GNUCXX) 38 | set_target_properties(test_kdl_tf PROPERTIES COMPILE_FLAGS "-Wno-deprecated-declarations") 39 | endif() 40 | target_link_libraries(test_kdl_tf ${PROJECT_NAME} ${orocos_kdl_LIBRARIES}) 41 | 42 | catkin_add_nosetests(test/posemath.py) 43 | 44 | endif() 45 | 46 | 47 | install(DIRECTORY include/${PROJECT_NAME}/ 48 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 49 | 50 | install(TARGETS ${PROJECT_NAME} 51 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 53 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 54 | -------------------------------------------------------------------------------- /tf_conversions/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # tf documentation build configuration file, created by 4 | # sphinx-quickstart on Mon Jun 1 14:21:53 2009. 5 | # 6 | # This file is execfile()d with the current directory set to its containing dir. 7 | # 8 | # Note that not all possible configuration values are present in this 9 | # autogenerated file. 10 | # 11 | # All configuration values have a default; values that are commented out 12 | # serve to show the default. 13 | 14 | import sys, os 15 | 16 | # If extensions (or modules to document with autodoc) are in another directory, 17 | # add these directories to sys.path here. If the directory is relative to the 18 | # documentation root, use os.path.abspath to make it absolute, like shown here. 19 | #sys.path.append(os.path.abspath('.')) 20 | 21 | # -- General configuration ----------------------------------------------------- 22 | 23 | # Add any Sphinx extension module names here, as strings. They can be extensions 24 | # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. 25 | extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] 26 | 27 | # Add any paths that contain templates here, relative to this directory. 28 | templates_path = ['_templates'] 29 | 30 | # The suffix of source filenames. 31 | source_suffix = '.rst' 32 | 33 | # The encoding of source files. 34 | #source_encoding = 'utf-8' 35 | 36 | # The master toctree document. 37 | master_doc = 'index' 38 | 39 | # General information about the project. 40 | project = u'tf_conversions' 41 | copyright = u'2010, Willow Garage, Inc.' 42 | 43 | # The version info for the project you're documenting, acts as replacement for 44 | # |version| and |release|, also used in various other places throughout the 45 | # built documents. 46 | # 47 | # The short X.Y version. 48 | version = '0.1' 49 | # The full version, including alpha/beta/rc tags. 50 | release = '0.1.0' 51 | 52 | # The language for content autogenerated by Sphinx. Refer to documentation 53 | # for a list of supported languages. 54 | #language = None 55 | 56 | # There are two options for replacing |today|: either, you set today to some 57 | # non-false value, then it is used: 58 | #today = '' 59 | # Else, today_fmt is used as the format for a strftime call. 60 | #today_fmt = '%B %d, %Y' 61 | 62 | # List of documents that shouldn't be included in the build. 63 | #unused_docs = [] 64 | 65 | # List of directories, relative to source directory, that shouldn't be searched 66 | # for source files. 67 | exclude_trees = ['_build'] 68 | 69 | # The reST default role (used for this markup: `text`) to use for all documents. 70 | #default_role = None 71 | 72 | # If true, '()' will be appended to :func: etc. cross-reference text. 73 | #add_function_parentheses = True 74 | 75 | # If true, the current module name will be prepended to all description 76 | # unit titles (such as .. function::). 77 | #add_module_names = True 78 | 79 | # If true, sectionauthor and moduleauthor directives will be shown in the 80 | # output. They are ignored by default. 81 | #show_authors = False 82 | 83 | # The name of the Pygments (syntax highlighting) style to use. 84 | pygments_style = 'sphinx' 85 | 86 | # A list of ignored prefixes for module index sorting. 87 | #modindex_common_prefix = [] 88 | 89 | 90 | # -- Options for HTML output --------------------------------------------------- 91 | 92 | # The theme to use for HTML and HTML Help pages. Major themes that come with 93 | # Sphinx are currently 'default' and 'sphinxdoc'. 94 | html_theme = 'default' 95 | 96 | # Theme options are theme-specific and customize the look and feel of a theme 97 | # further. For a list of options available for each theme, see the 98 | # documentation. 99 | #html_theme_options = {} 100 | 101 | # Add any paths that contain custom themes here, relative to this directory. 102 | #html_theme_path = [] 103 | 104 | # The name for this set of Sphinx documents. If None, it defaults to 105 | # " v documentation". 106 | #html_title = None 107 | 108 | # A shorter title for the navigation bar. Default is the same as html_title. 109 | #html_short_title = None 110 | 111 | # The name of an image file (relative to this directory) to place at the top 112 | # of the sidebar. 113 | #html_logo = None 114 | 115 | # The name of an image file (within the static path) to use as favicon of the 116 | # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 117 | # pixels large. 118 | #html_favicon = None 119 | 120 | # Add any paths that contain custom static files (such as style sheets) here, 121 | # relative to this directory. They are copied after the builtin static files, 122 | # so a file named "default.css" will overwrite the builtin "default.css". 123 | html_static_path = ['_static'] 124 | 125 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 126 | # using the given strftime format. 127 | #html_last_updated_fmt = '%b %d, %Y' 128 | 129 | # If true, SmartyPants will be used to convert quotes and dashes to 130 | # typographically correct entities. 131 | #html_use_smartypants = True 132 | 133 | # Custom sidebar templates, maps document names to template names. 134 | #html_sidebars = {} 135 | 136 | # Additional templates that should be rendered to pages, maps page names to 137 | # template names. 138 | #html_additional_pages = {} 139 | 140 | # If false, no module index is generated. 141 | #html_use_modindex = True 142 | 143 | # If false, no index is generated. 144 | #html_use_index = True 145 | 146 | # If true, the index is split into individual pages for each letter. 147 | #html_split_index = False 148 | 149 | # If true, links to the reST sources are added to the pages. 150 | #html_show_sourcelink = True 151 | 152 | # If true, an OpenSearch description file will be output, and all pages will 153 | # contain a tag referring to it. The value of this option must be the 154 | # base URL from which the finished HTML is served. 155 | #html_use_opensearch = '' 156 | 157 | # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). 158 | #html_file_suffix = '' 159 | 160 | # Output file base name for HTML help builder. 161 | htmlhelp_basename = 'tfdoc' 162 | 163 | 164 | # -- Options for LaTeX output -------------------------------------------------- 165 | 166 | # The paper size ('letter' or 'a4'). 167 | #latex_paper_size = 'letter' 168 | 169 | # The font size ('10pt', '11pt' or '12pt'). 170 | #latex_font_size = '10pt' 171 | 172 | # Grouping the document tree into LaTeX files. List of tuples 173 | # (source start file, target name, title, author, documentclass [howto/manual]). 174 | latex_documents = [ 175 | ('index', 'tf_conversions.tex', u'stereo\\_utils Documentation', 176 | u'James Bowman', 'manual'), 177 | ] 178 | 179 | # The name of an image file (relative to this directory) to place at the top of 180 | # the title page. 181 | #latex_logo = None 182 | 183 | # For "manual" documents, if this is true, then toplevel headings are parts, 184 | # not chapters. 185 | #latex_use_parts = False 186 | 187 | # Additional stuff for the LaTeX preamble. 188 | #latex_preamble = '' 189 | 190 | # Documents to append as an appendix to all manuals. 191 | #latex_appendices = [] 192 | 193 | # If false, no module index is generated. 194 | #latex_use_modindex = True 195 | 196 | 197 | # Example configuration for intersphinx: refer to the Python standard library. 198 | intersphinx_mapping = { 199 | 'http://docs.python.org/': None, 200 | 'http://docs.opencv.org/3.0-last-rst/': None, 201 | 'http://www.ros.org/doc/api/tf/html/python/' : None, 202 | 'http://docs.scipy.org/doc/numpy' : None, 203 | 'http://www.ros.org/doc/api/kdl/html/python/' : None 204 | } 205 | -------------------------------------------------------------------------------- /tf_conversions/include/tf_conversions/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** \mainpage 2 | 3 | @htmlinclude manifest.html 4 | 5 | The tf conversions package provide methods to convert between 6 | datatypes used by tf, and semantically identical data types of 7 | other libraries. The package now supports KDL and Eigen. 8 | 9 | The supported KDL data types are: 10 | \li Frame 11 | \li Vector 12 | \li Rotation 13 | \li Twist 14 | 15 | The supported Eigen data types are: 16 | \li Affine3d 17 | \li Vector3d 18 | \li Quaterniond 19 | 20 | For a complete list of available conversion methods, follow this link: 21 | tf 22 | 23 | */ 24 | -------------------------------------------------------------------------------- /tf_conversions/include/tf_conversions/tf_eigen.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009-2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // Author: Adam Leeper (and others) 31 | 32 | #ifndef CONVERSIONS_TF_EIGEN_H 33 | #define CONVERSIONS_TF_EIGEN_H 34 | 35 | #include "tf/transform_datatypes.h" 36 | #include "Eigen/Core" 37 | #include "Eigen/Geometry" 38 | 39 | namespace tf { 40 | 41 | /// Converts a tf Matrix3x3 into an Eigen Quaternion 42 | void matrixTFToEigen(const tf::Matrix3x3 &t, Eigen::Matrix3d &e); 43 | 44 | /// Converts an Eigen Quaternion into a tf Matrix3x3 45 | void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t); 46 | 47 | /// Converts a tf Pose into an Eigen Affine3d 48 | void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e); 49 | 50 | /// Converts a tf Pose into an Eigen Isometry3d 51 | void poseTFToEigen(const tf::Pose &t, Eigen::Isometry3d &e); 52 | 53 | /// Converts an Eigen Affine3d into a tf Transform 54 | void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t); 55 | 56 | /// Converts an Eigen Isometry3d into a tf Transform 57 | void poseEigenToTF(const Eigen::Isometry3d &e, tf::Pose &t); 58 | 59 | /// Converts a tf Quaternion into an Eigen Quaternion 60 | void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e); 61 | 62 | /// Converts an Eigen Quaternion into a tf Quaternion 63 | void quaternionEigenToTF(const Eigen::Quaterniond &e, tf::Quaternion &t); 64 | 65 | /// Converts a tf Transform into an Eigen Affine3d 66 | void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e); 67 | 68 | /// Converts a tf Transform into an Eigen Isometry3d 69 | void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e); 70 | 71 | /// Converts an Eigen Affine3d into a tf Transform 72 | void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t); 73 | 74 | /// Converts an Eigen Isometry3d into a tf Transform 75 | void transformEigenToTF(const Eigen::Isometry3d &e, tf::Transform &t); 76 | 77 | /// Converts a tf Vector3 into an Eigen Vector3d 78 | void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3d &e); 79 | 80 | /// Converts an Eigen Vector3d into a tf Vector3 81 | void vectorEigenToTF(const Eigen::Vector3d &e, tf::Vector3 &t); 82 | 83 | } // namespace tf 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /tf_conversions/include/tf_conversions/tf_kdl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef CONVERSIONS_TF_KDL_H 31 | #define CONVERSIONS_TF_KDL_H 32 | 33 | #include "kdl_conversions/kdl_msg.h" //backwards compatability for deprecated methods 34 | #include "tf/transform_datatypes.h" 35 | #include "kdl/frames.hpp" 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | namespace tf 43 | { 44 | 45 | /// Converts a tf Pose into a KDL Frame 46 | void poseTFToKDL(const tf::Pose &t, KDL::Frame &k); 47 | 48 | /// Converts a KDL Frame into a tf Pose 49 | void poseKDLToTF(const KDL::Frame &k, tf::Pose &t); 50 | 51 | /// Converts a tf Quaternion into a KDL Rotation 52 | void quaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k); 53 | 54 | /// Converts a tf Quaternion into a KDL Rotation 55 | void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t); 56 | 57 | /// Converts a tf Transform into a KDL Frame 58 | void transformTFToKDL(const tf::Transform &t, KDL::Frame &k); 59 | 60 | /// Converts a KDL Frame into a tf Transform 61 | void transformKDLToTF(const KDL::Frame &k, tf::Transform &t); 62 | 63 | /// Converts a tf Vector3 into a KDL Vector 64 | void vectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k); 65 | 66 | /// Converts a tf Vector3 into a KDL Vector 67 | void vectorKDLToTF(const KDL::Vector &k, tf::Vector3 &t); 68 | 69 | /* DEPRECATED FUNCTIONS */ 70 | /// Starting from a Pose from A to B, apply a Twist with reference frame A and reference point B, during a time t. 71 | ROS_DEPRECATED geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t); 72 | 73 | 74 | /// Converts a tf Pose into a KDL Frame 75 | ROS_DEPRECATED void PoseTFToKDL(const tf::Pose &t, KDL::Frame &k); 76 | void inline PoseTFToKDL(const tf::Pose &t, KDL::Frame &k) {poseTFToKDL(t, k);} 77 | 78 | /// Converts a KDL Frame into a tf Pose 79 | ROS_DEPRECATED void PoseKDLToTF(const KDL::Frame &k, tf::Pose &t); 80 | void inline PoseKDLToTF(const KDL::Frame &k, tf::Pose &t) {poseKDLToTF(k, t);} 81 | 82 | /// Converts a tf Quaternion into a KDL Rotation 83 | ROS_DEPRECATED void QuaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k); 84 | void inline QuaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k) {quaternionTFToKDL(t, k);} 85 | 86 | /// Converts a tf Quaternion into a KDL Rotation 87 | ROS_DEPRECATED void QuaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t); 88 | void inline QuaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t) {quaternionKDLToTF(k, t);} 89 | 90 | /// Converts a tf Transform into a KDL Frame 91 | ROS_DEPRECATED void TransformTFToKDL(const tf::Transform &t, KDL::Frame &k); 92 | void inline TransformTFToKDL(const tf::Transform &t, KDL::Frame &k) {transformTFToKDL(t, k);} 93 | 94 | /// Converts a KDL Frame into a tf Transform 95 | ROS_DEPRECATED void TransformKDLToTF(const KDL::Frame &k, tf::Transform &t); 96 | void inline TransformKDLToTF(const KDL::Frame &k, tf::Transform &t) {transformKDLToTF(k, t);} 97 | 98 | /// Converts a tf Vector3 into a KDL Vector 99 | ROS_DEPRECATED void VectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k); 100 | void inline VectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k) {vectorTFToKDL(t, k);} 101 | 102 | /// Converts a tf Vector3 into a KDL Vector 103 | ROS_DEPRECATED void VectorKDLToTF(const KDL::Vector &k, tf::Vector3 &t); 104 | void inline VectorKDLToTF(const KDL::Vector &k, tf::Vector3 &t) {vectorKDLToTF(k, t);} 105 | 106 | 107 | } // namespace tf 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /tf_conversions/index.rst: -------------------------------------------------------------------------------- 1 | tf_conversions 2 | ============== 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | PoseMath 8 | -------- 9 | 10 | PoseMath is a utility that makes it easy to work with :class:`PyKDL.Frame`'s. 11 | It can work with poses from a variety of sources: :meth:`tf.Transformer.lookupTransform`, 12 | :mod:`opencv` and ROS messages. It has utility functions to convert between these 13 | types and the :class:`PyKDL.Frame` pose representation. 14 | 15 | .. doctest:: 16 | :options: -ELLIPSIS, +NORMALIZE_WHITESPACE 17 | 18 | >>> from geometry_msgs.msg import Pose 19 | >>> import tf_conversions.posemath as pm 20 | >>> import PyKDL 21 | >>> 22 | >>> msg = Pose() 23 | >>> msg.position.x = 7.0 24 | >>> msg.orientation.w = 1.0 25 | >>> 26 | >>> frame = PyKDL.Frame(PyKDL.Rotation.RPY(2, 0, 1), PyKDL.Vector(1,2,4)) 27 | >>> 28 | >>> res = pm.toTf(pm.fromMsg(msg) * frame) 29 | >>> print res 30 | ((8.0, 2.0, 4.0), (0.73846026260412856, 0.40342268011133486, 0.25903472399992566, 0.4741598817790379)) 31 | 32 | 33 | 34 | .. automodule:: tf_conversions.posemath 35 | :members: fromTf, fromMsg, toMsg, fromMatrix, toMatrix, fromCameraParams 36 | 37 | Indices and tables 38 | ================== 39 | 40 | * :ref:`genindex` 41 | * :ref:`modindex` 42 | * :ref:`search` 43 | 44 | -------------------------------------------------------------------------------- /tf_conversions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | tf_conversions 3 | 1.13.4 4 | 5 | 6 | This package contains a set of conversion functions to convert 7 | common tf datatypes (point, vector, pose, etc) into semantically 8 | identical datatypes used by other libraries. The conversion functions 9 | make it easier for users of the transform library (tf) to work with 10 | the datatype of their choice. Currently this package has support for 11 | the Kinematics and Dynamics Library (KDL) and the Eigen matrix 12 | library. This package is stable, and will get integrated into tf in 13 | the next major release cycle (see roadmap). 14 | 15 | 16 | Tully Foote 17 | Tully Foote 18 | 19 | BSD 20 | http://www.ros.org/wiki/tf_conversions 21 | 22 | catkin 23 | 24 | eigen 25 | geometry_msgs 26 | kdl_conversions 27 | liborocos-kdl-dev 28 | tf 29 | 30 | liborocos-kdl-dev 31 | 32 | eigen 33 | geometry_msgs 34 | kdl_conversions 35 | liborocos-kdl 36 | python3-pykdl 37 | tf 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /tf_conversions/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | name: Python API 3 | output_dir: python 4 | - builder: doxygen 5 | name: C++ API 6 | output_dir: c++ 7 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' 8 | -------------------------------------------------------------------------------- /tf_conversions/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['tf_conversions'], 8 | package_dir={'': 'src'}, 9 | requires=['geometry_msgs', 'rospy', 'tf'] 10 | ) 11 | 12 | setup(**d) 13 | -------------------------------------------------------------------------------- /tf_conversions/src/tf_conversions/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2010, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | from __future__ import absolute_import 29 | 30 | from .posemath import * 31 | -------------------------------------------------------------------------------- /tf_conversions/src/tf_conversions/posemath.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2010, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | from geometry_msgs.msg import Pose, Point, Quaternion 29 | from tf import transformations 30 | import tf 31 | import rospy 32 | import numpy 33 | 34 | from PyKDL import * 35 | 36 | def fromTf(tf): 37 | """ 38 | :param tf: :class:`tf.Transformer` transform 39 | :type tf: tuple (translation, quaternion) 40 | :return: New :class:`PyKDL.Frame` object 41 | 42 | Convert a pose returned by :meth:`tf.Transformer.lookupTransform` to a :class:`PyKDL.Frame`. 43 | 44 | .. doctest:: 45 | 46 | >>> import rospy 47 | >>> import tf 48 | >>> import geometry_msgs.msg 49 | >>> t = tf.Transformer(True, rospy.Duration(10.0)) 50 | >>> m = geometry_msgs.msg.TransformStamped() 51 | >>> m.header.frame_id = 'THISFRAME' 52 | >>> m.child_frame_id = 'CHILD' 53 | >>> m.transform.translation.x = 668.5 54 | >>> m.transform.rotation.w = 1.0 55 | >>> t.setTransform(m) 56 | >>> t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0)) 57 | ((668.5, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) 58 | >>> import tf_conversions.posemath as pm 59 | >>> p = pm.fromTf(t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0))) 60 | >>> print(pm.toMsg(p * p)) 61 | position: 62 | x: 1337.0 63 | y: 0.0 64 | z: 0.0 65 | orientation: 66 | x: 0.0 67 | y: 0.0 68 | z: 0.0 69 | w: 1.0 70 | 71 | """ 72 | 73 | position, quaternion = tf 74 | x, y, z = position 75 | Qx, Qy, Qz, Qw = quaternion 76 | return Frame(Rotation.Quaternion(Qx, Qy, Qz, Qw), 77 | Vector(x, y, z)) 78 | 79 | def toTf(f): 80 | """ 81 | :param f: input pose 82 | :type f: :class:`PyKDL.Frame` 83 | 84 | Return a tuple (position, quaternion) for the pose. 85 | """ 86 | 87 | return ((f.p[0], f.p[1], f.p[2]), f.M.GetQuaternion()) 88 | 89 | # to and from pose message 90 | def fromMsg(p): 91 | """ 92 | :param p: input pose 93 | :type p: :class:`geometry_msgs.msg.Pose` 94 | :return: New :class:`PyKDL.Frame` object 95 | 96 | Convert a pose represented as a ROS Pose message to a :class:`PyKDL.Frame`. 97 | """ 98 | return Frame(Rotation.Quaternion(p.orientation.x, 99 | p.orientation.y, 100 | p.orientation.z, 101 | p.orientation.w), 102 | Vector(p.position.x, p.position.y, p.position.z)) 103 | 104 | def toMsg(f): 105 | """ 106 | :param f: input pose 107 | :type f: :class:`PyKDL.Frame` 108 | 109 | Return a ROS Pose message for the Frame f. 110 | 111 | """ 112 | p = Pose() 113 | p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w = f.M.GetQuaternion() 114 | p.position.x = f.p[0] 115 | p.position.y = f.p[1] 116 | p.position.z = f.p[2] 117 | return p 118 | 119 | 120 | # to and from matrix 121 | def fromMatrix(m): 122 | """ 123 | :param m: input 4x4 matrix 124 | :type m: :func:`numpy.array` 125 | :return: New :class:`PyKDL.Frame` object 126 | 127 | Convert a pose represented as a 4x4 numpy array to a :class:`PyKDL.Frame`. 128 | 129 | """ 130 | return Frame(Rotation(m[0,0], m[0,1], m[0,2], 131 | m[1,0], m[1,1], m[1,2], 132 | m[2,0], m[2,1], m[2,2]), 133 | Vector(m[0,3], m[1, 3], m[2, 3])) 134 | 135 | def toMatrix(f): 136 | """ 137 | :param f: input pose 138 | :type f: :class:`PyKDL.Frame` 139 | 140 | Return a numpy 4x4 array for the Frame F. 141 | """ 142 | return numpy.array([[f.M[0,0], f.M[0,1], f.M[0,2], f.p[0]], 143 | [f.M[1,0], f.M[1,1], f.M[1,2], f.p[1]], 144 | [f.M[2,0], f.M[2,1], f.M[2,2], f.p[2]], 145 | [0,0,0,1]]) 146 | 147 | 148 | # from camera parameters 149 | def fromCameraParams(cv, rvec, tvec): 150 | """ 151 | :param cv: OpenCV module 152 | :param rvec: A Rodrigues rotation vector - see :func:`Rodrigues2` 153 | :type rvec: 3x1 :class:`CvMat` 154 | :param tvec: A translation vector 155 | :type tvec: 3x1 :class:`CvMat` 156 | :return: New :class:`PyKDL.Frame` object 157 | 158 | For use with :func:`FindExtrinsicCameraParams2`:: 159 | 160 | import cv 161 | import tf_conversions.posemath as pm 162 | ... 163 | rvec = cv.CreateMat(3, 1, cv.CV_32FC1) 164 | tvec = cv.CreateMat(3, 1, cv.CV_32FC1) 165 | cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rvec, tvec) 166 | pose = pm.fromCameraParams(cv, rvec, tvec) 167 | 168 | """ 169 | m = numpy.array([ [ 0, 0, 0, tvec[0,0] ], 170 | [ 0, 0, 0, tvec[1,0] ], 171 | [ 0, 0, 0, tvec[2,0] ], 172 | [ 0, 0, 0, 1.0 ] ], dtype = numpy.float32) 173 | cv.Rodrigues2(rvec, m[:3,:3]) 174 | return fromMatrix(m) 175 | 176 | 177 | -------------------------------------------------------------------------------- /tf_conversions/src/tf_eigen.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009-2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // Author: Adam Leeper, Stuart Glaser 31 | 32 | #include "tf_conversions/tf_eigen.h" 33 | 34 | namespace tf { 35 | 36 | void matrixTFToEigen(const tf::Matrix3x3 &t, Eigen::Matrix3d &e) 37 | { 38 | for(int i=0; i<3; i++) 39 | for(int j=0; j<3; j++) 40 | e(i,j) = t[i][j]; 41 | } 42 | 43 | void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t) 44 | { 45 | for(int i=0; i<3; i++) 46 | for(int j=0; j<3; j++) 47 | t[i][j] = e(i,j); 48 | } 49 | 50 | void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e) 51 | { 52 | transformTFToEigen(t, e); 53 | } 54 | 55 | void poseTFToEigen(const tf::Pose &t, Eigen::Isometry3d &e) 56 | { 57 | transformTFToEigen(t, e); 58 | } 59 | 60 | void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t) 61 | { 62 | transformEigenToTF(e, t); 63 | } 64 | 65 | void poseEigenToTF(const Eigen::Isometry3d &e, tf::Pose &t) 66 | { 67 | transformEigenToTF(e, t); 68 | } 69 | 70 | void quaternionTFToEigen(const tf::Quaternion& t, Eigen::Quaterniond& e) 71 | { 72 | e = Eigen::Quaterniond(t[3],t[0],t[1],t[2]); 73 | } 74 | 75 | void quaternionEigenToTF(const Eigen::Quaterniond& e, tf::Quaternion& t) 76 | { 77 | t[0] = e.x(); 78 | t[1] = e.y(); 79 | t[2] = e.z(); 80 | t[3] = e.w(); 81 | } 82 | 83 | namespace { 84 | template 85 | void transformTFToEigenImpl(const tf::Transform &t, Transform & e) 86 | { 87 | for(int i=0; i<3; i++) 88 | { 89 | e.matrix()(i,3) = t.getOrigin()[i]; 90 | for(int j=0; j<3; j++) 91 | { 92 | e.matrix()(i,j) = t.getBasis()[i][j]; 93 | } 94 | } 95 | // Fill in identity in last row 96 | for (int col = 0 ; col < 3; col ++) 97 | e.matrix()(3, col) = 0; 98 | e.matrix()(3,3) = 1; 99 | } 100 | 101 | template 102 | void transformEigenToTFImpl(const T &e, tf::Transform &t) 103 | { 104 | t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3))); 105 | t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1), e.matrix()(0,2), 106 | e.matrix()(1,0), e.matrix()(1,1), e.matrix()(1,2), 107 | e.matrix()(2,0), e.matrix()(2,1), e.matrix()(2,2))); 108 | } 109 | } 110 | 111 | void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e) 112 | { 113 | transformTFToEigenImpl(t, e); 114 | }; 115 | 116 | void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e) 117 | { 118 | transformTFToEigenImpl(t, e); 119 | }; 120 | 121 | void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t) 122 | { 123 | transformEigenToTFImpl(e, t); 124 | } 125 | 126 | void transformEigenToTF(const Eigen::Isometry3d &e, tf::Transform &t) 127 | { 128 | transformEigenToTFImpl(e, t); 129 | } 130 | 131 | void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3d& e) 132 | { 133 | e(0) = t[0]; 134 | e(1) = t[1]; 135 | e(2) = t[2]; 136 | } 137 | 138 | void vectorEigenToTF(const Eigen::Vector3d& e, tf::Vector3& t) 139 | { 140 | t[0] = e(0); 141 | t[1] = e(1); 142 | t[2] = e(2); 143 | } 144 | 145 | } // namespace tf 146 | -------------------------------------------------------------------------------- /tf_conversions/src/tf_kdl.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "tf_conversions/tf_kdl.h" 31 | #include "kdl_conversions/kdl_msg.h" 32 | 33 | namespace tf { 34 | 35 | void poseTFToKDL(const tf::Pose& t, KDL::Frame& k) 36 | { 37 | for (unsigned int i = 0; i < 3; ++i) 38 | k.p[i] = t.getOrigin()[i]; 39 | for (unsigned int i = 0; i < 9; ++i) 40 | k.M.data[i] = t.getBasis()[i/3][i%3]; 41 | } 42 | 43 | void poseKDLToTF(const KDL::Frame& k, tf::Pose& t) 44 | { 45 | t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2])); 46 | t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2], 47 | k.M.data[3], k.M.data[4], k.M.data[5], 48 | k.M.data[6], k.M.data[7], k.M.data[8])); 49 | } 50 | 51 | void quaternionTFToKDL(const tf::Quaternion& t, KDL::Rotation& k) 52 | { 53 | k = KDL::Rotation::Quaternion(t[0], t[1], t[2], t[3]); 54 | } 55 | 56 | void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t) 57 | { 58 | double x, y, z, w; 59 | k.GetQuaternion(x, y, z, w); 60 | t = tf::Quaternion(x, y, z, w); 61 | } 62 | 63 | void transformTFToKDL(const tf::Transform &t, KDL::Frame &k) 64 | { 65 | for (unsigned int i = 0; i < 3; ++i) 66 | k.p[i] = t.getOrigin()[i]; 67 | for (unsigned int i = 0; i < 9; ++i) 68 | k.M.data[i] = t.getBasis()[i/3][i%3]; 69 | } 70 | 71 | void transformKDLToTF(const KDL::Frame &k, tf::Transform &t) 72 | { 73 | t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2])); 74 | t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2], 75 | k.M.data[3], k.M.data[4], k.M.data[5], 76 | k.M.data[6], k.M.data[7], k.M.data[8])); 77 | } 78 | 79 | void vectorTFToKDL(const tf::Vector3& t, KDL::Vector& k) 80 | { 81 | k[0] = t[0]; 82 | k[1] = t[1]; 83 | k[2] = t[2]; 84 | } 85 | 86 | void vectorKDLToTF(const KDL::Vector& k, tf::Vector3& t) 87 | { 88 | t[0] = k[0]; 89 | t[1] = k[1]; 90 | t[2] = k[2]; 91 | } 92 | 93 | 94 | /* DEPRECATED FUNCTIONS */ 95 | geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t) 96 | { 97 | geometry_msgs::Pose result; 98 | KDL::Twist kdl_twist; 99 | KDL::Frame kdl_pose_id, kdl_pose; 100 | 101 | poseMsgToKDL(pose,kdl_pose); 102 | twistMsgToKDL(twist,kdl_twist); 103 | kdl_pose = KDL::addDelta(kdl_pose_id,kdl_twist,t)*kdl_pose; 104 | poseKDLToMsg(kdl_pose,result); 105 | return result; 106 | } 107 | 108 | } // namespace tf 109 | 110 | -------------------------------------------------------------------------------- /tf_conversions/test/posemath.py: -------------------------------------------------------------------------------- 1 | import rostest 2 | import rospy 3 | import numpy 4 | import unittest 5 | import sys 6 | 7 | from tf import Transformer 8 | import tf_conversions.posemath as pm 9 | 10 | from geometry_msgs.msg import TransformStamped 11 | from PyKDL import Frame 12 | 13 | class TestPoseMath(unittest.TestCase): 14 | 15 | def setUp(self): 16 | pass 17 | 18 | def test_fromTf(self): 19 | transformer = Transformer(True, rospy.Duration(10.0)) 20 | m = TransformStamped() 21 | m.header.frame_id = 'wim' 22 | m.child_frame_id = 'james' 23 | m.transform.translation.x = 2.71828183 24 | m.transform.rotation.w = 1.0 25 | transformer.setTransform(m) 26 | b = pm.fromTf(transformer.lookupTransform('wim', 'james', rospy.Time(0))) 27 | 28 | def test_roundtrip(self): 29 | c = Frame() 30 | 31 | d = pm.fromMsg(pm.toMsg(c)) 32 | self.assertEqual(repr(c), repr(d)) 33 | 34 | d = pm.fromMatrix(pm.toMatrix(c)) 35 | self.assertEqual(repr(c), repr(d)) 36 | 37 | d = pm.fromTf(pm.toTf(c)) 38 | self.assertEqual(repr(c), repr(d)) 39 | 40 | if __name__ == '__main__': 41 | if len(sys.argv) == 1 or sys.argv[1].startswith('--gtest_output'): 42 | rostest.unitrun('tf', 'directed', TestPoseMath) 43 | else: 44 | suite = unittest.TestSuite() 45 | suite.addTest(TestPoseMath(sys.argv[1])) 46 | unittest.TextTestRunner(verbosity=2).run(suite) 47 | -------------------------------------------------------------------------------- /tf_conversions/test/test_eigen_tf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | using namespace tf; 35 | 36 | double gen_rand(double min, double max) 37 | { 38 | int rand_num = rand()%100+1; 39 | double result = min + (double)((max-min)*rand_num)/101.0; 40 | return result; 41 | } 42 | 43 | TEST(TFEigenConversions, tf_eigen_vector) 44 | { 45 | tf::Vector3 t; 46 | t[0] = gen_rand(-10,10); 47 | t[1] = gen_rand(-10,10); 48 | t[2] = gen_rand(-10,10); 49 | 50 | Eigen::Vector3d k; 51 | vectorTFToEigen(t,k); 52 | 53 | ASSERT_NEAR(t[0],k[0],1e-6); 54 | ASSERT_NEAR(t[1],k[1],1e-6); 55 | ASSERT_NEAR(t[2],k[2],1e-6); 56 | } 57 | 58 | TEST(TFEigenConversions, tf_eigen_quaternion) 59 | { 60 | tf::Quaternion t; 61 | t[0] = gen_rand(-1.0,1.0); 62 | t[1] = gen_rand(-1.0,1.0); 63 | t[2] = gen_rand(-1.0,1.0); 64 | t[3] = gen_rand(-1.0,1.0); 65 | t.normalize(); 66 | Eigen::Quaterniond k; 67 | quaternionTFToEigen(t,k); 68 | 69 | ASSERT_NEAR(t[0],k.coeffs()(0),1e-6); 70 | ASSERT_NEAR(t[1],k.coeffs()(1),1e-6); 71 | ASSERT_NEAR(t[2],k.coeffs()(2),1e-6); 72 | ASSERT_NEAR(t[3],k.coeffs()(3),1e-6); 73 | ASSERT_NEAR(k.norm(),1.0,1e-10); 74 | } 75 | 76 | TEST(TFEigenConversions, tf_eigen_transform) 77 | { 78 | tf::Transform t; 79 | tf::Quaternion tq; 80 | tq[0] = gen_rand(-1.0,1.0); 81 | tq[1] = gen_rand(-1.0,1.0); 82 | tq[2] = gen_rand(-1.0,1.0); 83 | tq[3] = gen_rand(-1.0,1.0); 84 | tq.normalize(); 85 | t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); 86 | t.setRotation(tq); 87 | 88 | Eigen::Affine3d affine; 89 | Eigen::Isometry3d isometry; 90 | transformTFToEigen(t, affine); 91 | transformTFToEigen(t, isometry); 92 | 93 | for(int i=0; i < 3; i++) 94 | { 95 | ASSERT_NEAR(t.getOrigin()[i],affine.matrix()(i,3),1e-6); 96 | ASSERT_NEAR(t.getOrigin()[i],isometry.matrix()(i,3),1e-6); 97 | for(int j=0; j < 3; j++) 98 | { 99 | ASSERT_NEAR(t.getBasis()[i][j],affine.matrix()(i,j),1e-6); 100 | ASSERT_NEAR(t.getBasis()[i][j],isometry.matrix()(i,j),1e-6); 101 | } 102 | } 103 | for (int col = 0 ; col < 3; col ++) 104 | { 105 | ASSERT_NEAR(affine.matrix()(3, col), 0, 1e-6); 106 | ASSERT_NEAR(isometry.matrix()(3, col), 0, 1e-6); 107 | } 108 | ASSERT_NEAR(affine.matrix()(3,3), 1, 1e-6); 109 | ASSERT_NEAR(isometry.matrix()(3,3), 1, 1e-6); 110 | } 111 | 112 | TEST(TFEigenConversions, eigen_tf_transform) 113 | { 114 | tf::Transform t1; 115 | tf::Transform t2; 116 | Eigen::Affine3d affine; 117 | Eigen::Isometry3d isometry; 118 | Eigen::Quaterniond kq; 119 | kq.coeffs()(0) = gen_rand(-1.0,1.0); 120 | kq.coeffs()(1) = gen_rand(-1.0,1.0); 121 | kq.coeffs()(2) = gen_rand(-1.0,1.0); 122 | kq.coeffs()(3) = gen_rand(-1.0,1.0); 123 | kq.normalize(); 124 | isometry.setIdentity(); 125 | isometry.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); 126 | isometry.rotate(kq); 127 | affine = isometry; 128 | 129 | transformEigenToTF(affine,t1); 130 | transformEigenToTF(isometry,t2); 131 | for(int i=0; i < 3; i++) 132 | { 133 | ASSERT_NEAR(t1.getOrigin()[i],affine.matrix()(i,3),1e-6); 134 | ASSERT_NEAR(t2.getOrigin()[i],isometry.matrix()(i,3),1e-6); 135 | for(int j=0; j < 3; j++) 136 | { 137 | ASSERT_NEAR(t1.getBasis()[i][j],affine.matrix()(i,j),1e-6); 138 | ASSERT_NEAR(t2.getBasis()[i][j],isometry.matrix()(i,j),1e-6); 139 | } 140 | } 141 | } 142 | 143 | 144 | int main(int argc, char **argv){ 145 | /* initialize random seed: */ 146 | srand ( time(NULL) ); 147 | testing::InitGoogleTest(&argc, argv); 148 | return RUN_ALL_TESTS(); 149 | } 150 | -------------------------------------------------------------------------------- /tf_conversions/test/test_kdl_tf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | using namespace tf; 35 | 36 | double gen_rand(double min, double max) 37 | { 38 | int rand_num = rand()%100+1; 39 | double result = min + (double)((max-min)*rand_num)/101.0; 40 | return result; 41 | } 42 | 43 | TEST(TFKDLConversions, tf_kdl_vector) 44 | { 45 | tf::Vector3 t; 46 | t[0] = gen_rand(-10,10); 47 | t[1] = gen_rand(-10,10); 48 | t[2] = gen_rand(-10,10); 49 | 50 | KDL::Vector k; 51 | VectorTFToKDL(t,k); 52 | 53 | ASSERT_NEAR(t[0],k[0],1e-6); 54 | ASSERT_NEAR(t[1],k[1],1e-6); 55 | ASSERT_NEAR(t[2],k[2],1e-6); 56 | } 57 | 58 | TEST(TFKDLConversions, tf_kdl_rotation) 59 | { 60 | tf::Quaternion t; 61 | t[0] = gen_rand(-1.0,1.0); 62 | t[1] = gen_rand(-1.0,1.0); 63 | t[2] = gen_rand(-1.0,1.0); 64 | t[3] = gen_rand(-1.0,1.0); 65 | t.normalize(); 66 | 67 | KDL::Rotation k; 68 | QuaternionTFToKDL(t,k); 69 | 70 | double x,y,z,w; 71 | k.GetQuaternion(x,y,z,w); 72 | 73 | ASSERT_NEAR(fabs(t[0]),fabs(x),1e-6); 74 | ASSERT_NEAR(fabs(t[1]),fabs(y),1e-6); 75 | ASSERT_NEAR(fabs(t[2]),fabs(z),1e-6); 76 | ASSERT_NEAR(fabs(t[3]),fabs(w),1e-6); 77 | ASSERT_NEAR(t[0]*t[1]*t[2]*t[3],x*y*z*w,1e-6); 78 | 79 | 80 | } 81 | 82 | TEST(TFKDLConversions, tf_kdl_transform) 83 | { 84 | tf::Transform t; 85 | tf::Quaternion tq; 86 | tq[0] = gen_rand(-1.0,1.0); 87 | tq[1] = gen_rand(-1.0,1.0); 88 | tq[2] = gen_rand(-1.0,1.0); 89 | tq[3] = gen_rand(-1.0,1.0); 90 | tq.normalize(); 91 | t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); 92 | t.setRotation(tq); 93 | 94 | //test tf->kdl 95 | KDL::Frame k; 96 | TransformTFToKDL(t,k); 97 | 98 | for(int i=0; i < 3; i++) 99 | { 100 | ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6); 101 | for(int j=0; j < 3; j++) 102 | { 103 | ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6); 104 | } 105 | } 106 | //test kdl->tf 107 | tf::Transform r; 108 | TransformKDLToTF(k,r); 109 | 110 | for(int i=0; i< 3; i++) 111 | { 112 | ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6); 113 | for(int j=0; j < 3; j++) 114 | { 115 | ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6); 116 | } 117 | 118 | } 119 | } 120 | 121 | TEST(TFKDLConversions, tf_kdl_pose) 122 | { 123 | tf::Pose t; 124 | tf::Quaternion tq; 125 | tq[0] = gen_rand(-1.0,1.0); 126 | tq[1] = gen_rand(-1.0,1.0); 127 | tq[2] = gen_rand(-1.0,1.0); 128 | tq[3] = gen_rand(-1.0,1.0); 129 | tq.normalize(); 130 | t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); 131 | t.setRotation(tq); 132 | 133 | //test tf->kdl 134 | KDL::Frame k; 135 | PoseTFToKDL(t,k); 136 | 137 | for(int i=0; i < 3; i++) 138 | { 139 | ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6); 140 | for(int j=0; j < 3; j++) 141 | { 142 | ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6); 143 | } 144 | } 145 | 146 | //test kdl->tf 147 | tf::Pose r; 148 | PoseKDLToTF(k,r); 149 | 150 | for(int i=0; i< 3; i++) 151 | { 152 | ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6); 153 | for(int j=0; j < 3; j++) 154 | { 155 | ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6); 156 | } 157 | 158 | } 159 | } 160 | 161 | TEST(TFKDLConversions, msg_kdl_twist) 162 | { 163 | geometry_msgs::Twist m; 164 | m.linear.x = gen_rand(-10,10); 165 | m.linear.y = gen_rand(-10,10); 166 | m.linear.z = gen_rand(-10,10); 167 | m.angular.x = gen_rand(-10,10); 168 | m.angular.y = gen_rand(-10,10); 169 | m.angular.y = gen_rand(-10,10); 170 | 171 | //test msg->kdl 172 | KDL::Twist t; 173 | TwistMsgToKDL(m,t); 174 | 175 | ASSERT_NEAR(m.linear.x,t.vel[0],1e-6); 176 | ASSERT_NEAR(m.linear.y,t.vel[1],1e-6); 177 | ASSERT_NEAR(m.linear.z,t.vel[2],1e-6); 178 | ASSERT_NEAR(m.angular.x,t.rot[0],1e-6); 179 | ASSERT_NEAR(m.angular.y,t.rot[1],1e-6); 180 | ASSERT_NEAR(m.angular.z,t.rot[2],1e-6); 181 | 182 | 183 | //test msg->kdl 184 | geometry_msgs::Twist r; 185 | TwistKDLToMsg(t,r); 186 | 187 | ASSERT_NEAR(r.linear.x,t.vel[0],1e-6); 188 | ASSERT_NEAR(r.linear.y,t.vel[1],1e-6); 189 | ASSERT_NEAR(r.linear.z,t.vel[2],1e-6); 190 | ASSERT_NEAR(r.angular.x,t.rot[0],1e-6); 191 | ASSERT_NEAR(r.angular.y,t.rot[1],1e-6); 192 | ASSERT_NEAR(r.angular.z,t.rot[2],1e-6); 193 | 194 | } 195 | 196 | 197 | 198 | int main(int argc, char **argv){ 199 | /* initialize random seed: */ 200 | srand ( time(NULL) ); 201 | testing::InitGoogleTest(&argc, argv); 202 | return RUN_ALL_TESTS(); 203 | } 204 | --------------------------------------------------------------------------------