pax_global_header00006660000000000000000000000064136675612510014526gustar00rootroot0000000000000052 comment=86618c7f902ef17b91bfb414652b3a82f7a40d67 geometry-1.13.2/000077500000000000000000000000001366756125100134455ustar00rootroot00000000000000geometry-1.13.2/.gitignore000066400000000000000000000000051366756125100154300ustar00rootroot00000000000000*.pycgeometry-1.13.2/eigen_conversions/000077500000000000000000000000001366756125100171645ustar00rootroot00000000000000geometry-1.13.2/eigen_conversions/CHANGELOG.rst000066400000000000000000000111521366756125100212050ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package eigen_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.13.2 (2020-06-08) ------------------- 1.13.1 (2020-05-15) ------------------- * Format 2 and build_export_depend orocos kdl (`#210 `_) * Contributors: Shane Loretz 1.13.0 (2020-03-10) ------------------- * Replace kdl packages with rosdep keys (`#206 `_) * Contributors: Shane Loretz 1.12.1 (2020-03-10) ------------------- * Bump CMake version to avoid CMP0048 warning (`#204 `_) * Contributors: Shane Loretz 1.12.0 (2018-05-02) ------------------- * Remove dependency on cmake_modules (`#157 `_) This is not needed anymore with the move from Eigen to Eigen3 in 707eb41. * Contributors: Jochen Sprickerhof 1.11.9 (2017-07-14) ------------------- * Fix cmake dependency export usage * Contributors: Timo Röhling 1.11.8 (2016-03-04) ------------------- * eigen_conversions: Add conversions for Eigen::Isometry3d * Contributors: Maarten de Vries 1.11.7 (2015-04-21) ------------------- 1.11.6 (2015-03-25) ------------------- 1.11.5 (2015-03-17) ------------------- 1.11.4 (2014-12-23) ------------------- 1.11.3 (2014-05-07) ------------------- 1.11.2 (2014-02-25) ------------------- * alphabetization and updating cmake to find Eigen * Contributors: Tully Foote 1.11.1 (2014-02-23) ------------------- * add dep on cmake_modules for finding eigen * Contributors: Ioan Sucan 1.11.0 (2014-02-14) ------------------- * fixed Eigen-KDL 3D Vector conversion functions * Contributors: fevb 1.10.8 (2014-02-01) ------------------- 1.10.7 (2013-12-27) ------------------- 1.10.6 (2013-08-28) ------------------- 1.10.5 (2013-07-19) ------------------- 1.10.4 (2013-07-11) ------------------- 1.10.3 (2013-07-09) ------------------- 1.10.2 (2013-07-09) ------------------- 1.10.1 (2013-07-05) ------------------- 1.10.0 (2013-07-05) ------------------- 1.9.31 (2013-04-18 18:16) ------------------------- * add link directories 1.9.30 (2013-04-18 16:26) ------------------------- * Fixes `#11 `_: orocos_kdl now has cmake rules oroco-kdl update cmake to use orocos_kdl as plain cmake package instead of catkin package Signed-off-by: Ruben Smits 1.9.29 (2013-01-13) ------------------- 1.9.28 (2013-01-02) ------------------- 1.9.27 (2012-12-21) ------------------- 1.9.26 (2012-12-14) ------------------- * add missing dep to catkin 1.9.25 (2012-12-13) ------------------- 1.9.24 (2012-12-11) ------------------- * Version 1.9.24 * Fixes to CMakeLists.txt's while building from source 1.9.23 (2012-11-22) ------------------- * Releaseing version 1.9.23 1.9.22 (2012-11-04 09:14) ------------------------- 1.9.21 (2012-11-04 01:19) ------------------------- 1.9.20 (2012-11-02) ------------------- 1.9.19 (2012-10-31) ------------------- * Removed deprecated 'brief' attribute from tags. 1.9.18 (2012-10-16) ------------------- 1.9.17 (2012-10-02) ------------------- * fix several dependency issues 1.9.16 (2012-09-29) ------------------- * adding geometry metapackage and updating to 1.9.16 1.9.15 (2012-09-30) ------------------- * fix a few dependency/catkin problems * remove old API files * comply to the new catkin API 1.9.14 (2012-09-18) ------------------- 1.9.13 (2012-09-17) ------------------- 1.9.12 (2012-09-16) ------------------- 1.9.11 (2012-09-14 22:49) ------------------------- 1.9.10 (2012-09-14 22:30) ------------------------- 1.9.9 (2012-09-11) ------------------ * update depends * minor patches for new build system 1.9.8 (2012-09-03) ------------------ 1.9.7 (2012-08-10 12:19) ------------------------ * minor build fixes * completed set of eigen conversions; added KDL conversions * adding additional conversion functions 1.9.6 (2012-08-02 19:59) ------------------------ 1.9.5 (2012-08-02 19:48) ------------------------ 1.9.4 (2012-08-02 18:29) ------------------------ 1.9.3 (2012-08-02 18:28) ------------------------ * forgot to install some things * also using DEPENDS 1.9.2 (2012-08-01 21:05) ------------------------ 1.9.1 (2012-08-01 19:16) ------------------------ * install manifest.xml 1.9.0 (2012-08-01 18:52) ------------------------ * catkin build system * added some additional conversion routines between eigen and messages * removing dependency on eigen package * compiling with eigen3 * missed a rename * more extensive search * applying patch from sed script for eigen3 compatability * Moving eigen_conversions from sandbox. geometry-1.13.2/eigen_conversions/CMakeLists.txt000066400000000000000000000016641366756125100217330ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(eigen_conversions) find_package(orocos_kdl REQUIRED) find_package(catkin REQUIRED geometry_msgs std_msgs) find_package(Eigen3 REQUIRED) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) include_directories(include ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) link_directories(${catkin_LIBRARY_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) catkin_package( INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS geometry_msgs std_msgs DEPENDS orocos_kdl ) add_library(${PROJECT_NAME} src/eigen_msg.cpp src/eigen_kdl.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) geometry-1.13.2/eigen_conversions/include/000077500000000000000000000000001366756125100206075ustar00rootroot00000000000000geometry-1.13.2/eigen_conversions/include/eigen_conversions/000077500000000000000000000000001366756125100243265ustar00rootroot00000000000000geometry-1.13.2/eigen_conversions/include/eigen_conversions/eigen_kdl.h000066400000000000000000000063561366756125100264320ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Author: Adam Leeper, Stuart Glaser */ #ifndef EIGEN_KDL_CONVERSIONS_H #define EIGEN_KDL_CONVERSIONS_H #include #include #include namespace tf { /// Converts a KDL rotation into an Eigen quaternion void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e); /// Converts an Eigen quaternion into a KDL rotation void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k); /// Converts a KDL frame into an Eigen Affine3d void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e); /// Converts a KDL frame into an Eigen Isometry3d void transformKDLToEigen(const KDL::Frame &k, Eigen::Isometry3d &e); /// Converts an Eigen Affine3d into a KDL frame void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k); /// Converts an Eigen Isometry3d into a KDL frame void transformEigenToKDL(const Eigen::Isometry3d &e, KDL::Frame &k); /// Converts a KDL twist into an Eigen matrix void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix &e); /// Converts an Eigen matrix into a KDL Twist void twistEigenToKDL(const Eigen::Matrix &e, KDL::Twist &k); /// Converts a KDL vector into an Eigen matrix void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix &e); /// Converts an Eigen matrix into a KDL vector void vectorEigenToKDL(const Eigen::Matrix &e, KDL::Vector &k); /// Converts a KDL wrench into an Eigen matrix void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix &e); /// Converts an Eigen matrix into a KDL wrench void wrenchEigenToKDL(const Eigen::Matrix &e, KDL::Wrench &k); } // namespace #endif geometry-1.13.2/eigen_conversions/include/eigen_conversions/eigen_msg.h000066400000000000000000000117161366756125100264420ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /* * Author: Stuart Glaser */ #ifndef EIGEN_MSG_CONVERSIONS_H #define EIGEN_MSG_CONVERSIONS_H #include #include #include #include #include #include #include #include #include #include namespace tf { /// Converts a Point message into an Eigen Vector void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e); /// Converts an Eigen Vector into a Point message void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m); /// Converts a Pose message into an Eigen Affine3d void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e); /// Converts a Pose message into an Eigen Isometry3d void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Isometry3d &e); /// Converts an Eigen Affine3d into a Pose message void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m); /// Converts an Eigen Isometry3d into a Pose message void poseEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Pose &m); /// Converts a Quaternion message into an Eigen Quaternion void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e); /// Converts an Eigen Quaternion into a Quaternion message void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m); /// Converts a Transform message into an Eigen Affine3d void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e); /// Converts a Transform message into an Eigen Isometry3d void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Isometry3d &e); /// Converts an Eigen Affine3d into a Transform message void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m); /// Converts an Eigen Isometry3d into a Transform message void transformEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Transform &m); /// Converts a Twist message into an Eigen matrix void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix &e); /// Converts an Eigen matrix into a Twist message void twistEigenToMsg(const Eigen::Matrix &e, geometry_msgs::Twist &m); /// Converts a Vector message into an Eigen Vector void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e); /// Converts an Eigen Vector into a Vector message void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m); /// Converts a Wrench message into an Eigen matrix void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix &e); /// Converts an Eigen matrix into a Wrench message void wrenchEigenToMsg(const Eigen::Matrix &e, geometry_msgs::Wrench &m); /// Converts an Eigen matrix into a Float64MultiArray message template void matrixEigenToMsg(const Eigen::MatrixBase &e, std_msgs::Float64MultiArray &m) { if (m.layout.dim.size() != 2) m.layout.dim.resize(2); m.layout.dim[0].stride = e.rows() * e.cols(); m.layout.dim[0].size = e.rows(); m.layout.dim[1].stride = e.cols(); m.layout.dim[1].size = e.cols(); if ((int)m.data.size() != e.size()) m.data.resize(e.size()); int ii = 0; for (int i = 0; i < e.rows(); ++i) for (int j = 0; j < e.cols(); ++j) m.data[ii++] = e.coeff(i, j); } } // namespace #endif geometry-1.13.2/eigen_conversions/mainpage.dox000066400000000000000000000011231366756125100214560ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b eigen_conversions is ... \section codeapi Code API */ geometry-1.13.2/eigen_conversions/package.xml000066400000000000000000000015521366756125100213040ustar00rootroot00000000000000 eigen_conversions 1.13.2 Conversion functions between: - Eigen and KDL - Eigen and geometry_msgs. Stuart Glaser Adam Leeper Tully Foote BSD http://ros.org/wiki/eigen_conversions catkin geometry_msgs eigen liborocos-kdl-dev std_msgs liborocos-kdl-dev geometry_msgs eigen liborocos-kdl std_msgs geometry-1.13.2/eigen_conversions/src/000077500000000000000000000000001366756125100177535ustar00rootroot00000000000000geometry-1.13.2/eigen_conversions/src/eigen_kdl.cpp000066400000000000000000000074561366756125100224140ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include namespace tf { void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e) { // // is it faster to do this? k.GetQuaternion(e.x(), e.y(), e.z(), e.w()); // or this? //double x, y, z, w; //k.GetQuaternion(x, y, z, w); //e = Eigen::Quaterniond(w, x, y, z); } void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k) { k = KDL::Rotation::Quaternion(e.x(), e.y(), e.z(), e.w()); } namespace { template void transformKDLToEigenImpl(const KDL::Frame &k, T &e) { // translation for (unsigned int i = 0; i < 3; ++i) e(i, 3) = k.p[i]; // rotation matrix for (unsigned int i = 0; i < 9; ++i) e(i/3, i%3) = k.M.data[i]; // "identity" row e(3,0) = 0.0; e(3,1) = 0.0; e(3,2) = 0.0; e(3,3) = 1.0; } template void transformEigenToKDLImpl(const T &e, KDL::Frame &k) { for (unsigned int i = 0; i < 3; ++i) k.p[i] = e(i, 3); for (unsigned int i = 0; i < 9; ++i) k.M.data[i] = e(i/3, i%3); } } void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e) { transformKDLToEigenImpl(k, e); } void transformKDLToEigen(const KDL::Frame &k, Eigen::Isometry3d &e) { transformKDLToEigenImpl(k, e); } void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k) { transformEigenToKDLImpl(e, k); } void transformEigenToKDL(const Eigen::Isometry3d &e, KDL::Frame &k) { transformEigenToKDLImpl(e, k); } void twistEigenToKDL(const Eigen::Matrix &e, KDL::Twist &k) { for(int i = 0; i < 6; ++i) k[i] = e[i]; } void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix &e) { for(int i = 0; i < 6; ++i) e[i] = k[i]; } void vectorEigenToKDL(const Eigen::Matrix &e, KDL::Vector &k) { for(int i = 0; i < 3; ++i) k[i] = e[i]; } void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix &e) { for(int i = 0; i < 3; ++i) e[i] = k[i]; } void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix &e) { for(int i = 0; i < 6; ++i) e[i] = k[i]; } void wrenchEigenToKDL(const Eigen::Matrix &e, KDL::Wrench &k) { for(int i = 0; i < 6; ++i) k[i] = e[i]; } } // namespace geometry-1.13.2/eigen_conversions/src/eigen_msg.cpp000066400000000000000000000137071366756125100224240ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include namespace tf { void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e) { e(0) = m.x; e(1) = m.y; e(2) = m.z; } void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m) { m.x = e(0); m.y = e(1); m.z = e(2); } namespace { template void poseMsgToEigenImpl(const geometry_msgs::Pose &m, T &e) { e = Eigen::Translation3d(m.position.x, m.position.y, m.position.z) * Eigen::Quaterniond(m.orientation.w, m.orientation.x, m.orientation.y, m.orientation.z); } template void poseEigenToMsgImpl(const T &e, geometry_msgs::Pose &m) { m.position.x = e.translation()[0]; m.position.y = e.translation()[1]; m.position.z = e.translation()[2]; Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear(); m.orientation.x = q.x(); m.orientation.y = q.y(); m.orientation.z = q.z(); m.orientation.w = q.w(); if (m.orientation.w < 0) { m.orientation.x *= -1; m.orientation.y *= -1; m.orientation.z *= -1; m.orientation.w *= -1; } } template void transformMsgToEigenImpl(const geometry_msgs::Transform &m, T &e) { e = Eigen::Translation3d(m.translation.x, m.translation.y, m.translation.z) * Eigen::Quaterniond(m.rotation.w, m.rotation.x, m.rotation.y, m.rotation.z); } template void transformEigenToMsgImpl(const T &e, geometry_msgs::Transform &m) { m.translation.x = e.translation()[0]; m.translation.y = e.translation()[1]; m.translation.z = e.translation()[2]; Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear(); m.rotation.x = q.x(); m.rotation.y = q.y(); m.rotation.z = q.z(); m.rotation.w = q.w(); if (m.rotation.w < 0) { m.rotation.x *= -1; m.rotation.y *= -1; m.rotation.z *= -1; m.rotation.w *= -1; } } } void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e) { poseMsgToEigenImpl(m, e); } void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Isometry3d &e) { poseMsgToEigenImpl(m, e); } void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m) { poseEigenToMsgImpl(e, m); } void poseEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Pose &m) { poseEigenToMsgImpl(e, m); } void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e) { e = Eigen::Quaterniond(m.w, m.x, m.y, m.z); } void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m) { m.x = e.x(); m.y = e.y(); m.z = e.z(); m.w = e.w(); } void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e) { transformMsgToEigenImpl(m, e); } void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Isometry3d &e) { transformMsgToEigenImpl(m, e); } void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m) { transformEigenToMsgImpl(e, m); } void transformEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Transform &m) { transformEigenToMsgImpl(e, m); } void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e) { e(0) = m.x; e(1) = m.y; e(2) = m.z; } void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m) { m.x = e(0); m.y = e(1); m.z = e(2); } void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix &e) { e[0] = m.linear.x; e[1] = m.linear.y; e[2] = m.linear.z; e[3] = m.angular.x; e[4] = m.angular.y; e[5] = m.angular.z; } void twistEigenToMsg(const Eigen::Matrix &e, geometry_msgs::Twist &m) { m.linear.x = e[0]; m.linear.y = e[1]; m.linear.z = e[2]; m.angular.x = e[3]; m.angular.y = e[4]; m.angular.z = e[5]; } void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix &e) { e[0] = m.force.x; e[1] = m.force.y; e[2] = m.force.z; e[3] = m.torque.x; e[4] = m.torque.y; e[5] = m.torque.z; } void wrenchEigenToMsg(const Eigen::Matrix &e, geometry_msgs::Wrench &m) { m.force.x = e[0]; m.force.y = e[1]; m.force.z = e[2]; m.torque.x = e[3]; m.torque.y = e[4]; m.torque.z = e[5]; } } // namespace geometry-1.13.2/geometry/000077500000000000000000000000001366756125100153005ustar00rootroot00000000000000geometry-1.13.2/geometry/CHANGELOG.rst000066400000000000000000000057421366756125100173310ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package geometry ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.13.2 (2020-06-08) ------------------- 1.13.1 (2020-05-15) ------------------- 1.13.0 (2020-03-10) ------------------- 1.12.1 (2020-03-10) ------------------- * Bump CMake version to avoid CMP0048 warning (`#204 `_) * Contributors: Shane Loretz 1.12.0 (2018-05-02) ------------------- 1.11.9 (2017-07-14) ------------------- * [doc] Add migration notice in manifest. (`#129 `_) * Contributors: Isaac I.Y. Saito 1.11.8 (2016-03-04) ------------------- 1.11.7 (2015-04-21) ------------------- 1.11.6 (2015-03-25) ------------------- 1.11.5 (2015-03-17) ------------------- 1.11.4 (2014-12-23) ------------------- * Update package.xml * Contributors: David Lu!! 1.11.3 (2014-05-07) ------------------- 1.11.2 (2014-02-25) ------------------- 1.11.1 (2014-02-23) ------------------- 1.11.0 (2014-02-14) ------------------- 1.10.8 (2014-02-01) ------------------- 1.10.7 (2013-12-27) ------------------- 1.10.6 (2013-08-28) ------------------- 1.10.5 (2013-07-19) ------------------- 1.10.4 (2013-07-11) ------------------- 1.10.3 (2013-07-09) ------------------- 1.10.2 (2013-07-09) ------------------- 1.10.1 (2013-07-05) ------------------- 1.10.0 (2013-07-05) ------------------- 1.9.31 (2013-04-18 18:16) ------------------------- 1.9.30 (2013-04-18 16:26) ------------------------- * add buildtool_depend * add CMakeLists.txt for metapackage 1.9.29 (2013-01-13) ------------------- 1.9.28 (2013-01-02) ------------------- 1.9.27 (2012-12-21) ------------------- 1.9.26 (2012-12-14) ------------------- 1.9.25 (2012-12-13) ------------------- 1.9.24 (2012-12-11) ------------------- * Version 1.9.24 1.9.23 (2012-11-22) ------------------- * Releaseing version 1.9.23 1.9.22 (2012-11-04 09:14) ------------------------- 1.9.21 (2012-11-04 01:19) ------------------------- 1.9.20 (2012-11-02) ------------------- * depend on angles 1.9.19 (2012-10-31) ------------------- 1.9.18 (2012-10-16) ------------------- 1.9.17 (2012-10-02) ------------------- 1.9.16 (2012-09-29) ------------------- * fixing xml syntax * adding geometry metapackage and updating to 1.9.16 1.9.15 (2012-09-30) ------------------- 1.9.14 (2012-09-18) ------------------- 1.9.13 (2012-09-17) ------------------- 1.9.12 (2012-09-16) ------------------- 1.9.11 (2012-09-14 22:49) ------------------------- 1.9.10 (2012-09-14 22:30) ------------------------- 1.9.9 (2012-09-11) ------------------ 1.9.8 (2012-09-03) ------------------ 1.9.7 (2012-08-10 12:19) ------------------------ 1.9.6 (2012-08-02 19:59) ------------------------ 1.9.5 (2012-08-02 19:48) ------------------------ 1.9.4 (2012-08-02 18:29) ------------------------ 1.9.3 (2012-08-02 18:28) ------------------------ 1.9.2 (2012-08-01 21:05) ------------------------ 1.9.1 (2012-08-01 19:16) ------------------------ 1.9.0 (2012-08-01 18:52) ------------------------ geometry-1.13.2/geometry/CMakeLists.txt000066400000000000000000000001531366756125100200370ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(geometry) find_package(catkin REQUIRED) catkin_metapackage() geometry-1.13.2/geometry/package.xml000066400000000000000000000031031366756125100174120ustar00rootroot00000000000000 geometry 1.13.2

A metapackage for geometry library suite.

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.
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.

Tully Foote BSD http://www.ros.org/wiki/geometry https://code.ros.org/trac/ros-pkg/query?component=geometry&status=assigned&status=new&status=reopened https://kforge.ros.org/geometry/geometry Tully Foote catkin angles eigen_conversions kdl_conversions tf tf_conversions geometry-1.13.2/kdl_conversions/000077500000000000000000000000001366756125100166475ustar00rootroot00000000000000geometry-1.13.2/kdl_conversions/CHANGELOG.rst000066400000000000000000000076711366756125100207030ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package kdl_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.13.2 (2020-06-08) ------------------- 1.13.1 (2020-05-15) ------------------- * Format 2 and build_export_depend orocos kdl (`#210 `_) * Contributors: Shane Loretz 1.13.0 (2020-03-10) ------------------- * Replace kdl packages with rosdep keys (`#206 `_) * Contributors: Shane Loretz 1.12.1 (2020-03-10) ------------------- * Bump CMake version to avoid CMP0048 warning (`#204 `_) * windows bring up, use ROS_DEPRECATED * Contributors: James Xu, Shane Loretz 1.12.0 (2018-05-02) ------------------- 1.11.9 (2017-07-14) ------------------- * Fix cmake dependency export usage * Contributors: Timo Röhling 1.11.8 (2016-03-04) ------------------- 1.11.7 (2015-04-21) ------------------- 1.11.6 (2015-03-25) ------------------- 1.11.5 (2015-03-17) ------------------- 1.11.4 (2014-12-23) ------------------- * Update package.xml * Contributors: David Lu!! 1.11.3 (2014-05-07) ------------------- 1.11.2 (2014-02-25) ------------------- 1.11.1 (2014-02-23) ------------------- 1.11.0 (2014-02-14) ------------------- 1.10.8 (2014-02-01) ------------------- 1.10.7 (2013-12-27) ------------------- 1.10.6 (2013-08-28) ------------------- 1.10.5 (2013-07-19) ------------------- 1.10.4 (2013-07-11) ------------------- 1.10.3 (2013-07-09) ------------------- 1.10.2 (2013-07-09) ------------------- 1.10.1 (2013-07-05) ------------------- 1.10.0 (2013-07-05) ------------------- 1.9.31 (2013-04-18 18:16) ------------------------- * add link directories 1.9.30 (2013-04-18 16:26) ------------------------- * Fixes `#11 `_: orocos_kdl now has cmake rules oroco-kdl update cmake to use orocos_kdl as plain cmake package instead of catkin package Signed-off-by: Ruben Smits 1.9.29 (2013-01-13) ------------------- 1.9.28 (2013-01-02) ------------------- 1.9.27 (2012-12-21) ------------------- 1.9.26 (2012-12-14) ------------------- * add missing dep to catkin 1.9.25 (2012-12-13) ------------------- 1.9.24 (2012-12-11) ------------------- * Version 1.9.24 * Fixes to CMakeLists.txt's while building from source 1.9.23 (2012-11-22) ------------------- * Releaseing version 1.9.23 1.9.22 (2012-11-04 09:14) ------------------------- * more backwards compatible conversions and an include to make sure it gets into the old header location 1.9.21 (2012-11-04 01:19) ------------------------- 1.9.20 (2012-11-02) ------------------- 1.9.19 (2012-10-31) ------------------- * Removed deprecated 'brief' attribute from tags. 1.9.18 (2012-10-16) ------------------- 1.9.17 (2012-10-02) ------------------- * fix several dependency issues 1.9.16 (2012-09-29) ------------------- * adding geometry metapackage and updating to 1.9.16 1.9.15 (2012-09-30) ------------------- * fix a few dependency/catkin problems * remove old API files * comply to the new catkin API 1.9.14 (2012-09-18) ------------------- 1.9.13 (2012-09-17) ------------------- * update manifests 1.9.12 (2012-09-16) ------------------- 1.9.11 (2012-09-14 22:49) ------------------------- 1.9.10 (2012-09-14 22:30) ------------------------- 1.9.9 (2012-09-11) ------------------ * minor patches for new build system 1.9.8 (2012-09-03) ------------------ 1.9.7 (2012-08-10 12:19) ------------------------ * minor build fixes * fixed some minor errors from last commit * completed set of eigen conversions; added KDL conversions 1.9.6 (2012-08-02 19:59) ------------------------ 1.9.5 (2012-08-02 19:48) ------------------------ 1.9.4 (2012-08-02 18:29) ------------------------ 1.9.3 (2012-08-02 18:28) ------------------------ 1.9.2 (2012-08-01 21:05) ------------------------ 1.9.1 (2012-08-01 19:16) ------------------------ 1.9.0 (2012-08-01 18:52) ------------------------ geometry-1.13.2/kdl_conversions/CMakeLists.txt000066400000000000000000000014411366756125100214070ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(kdl_conversions) find_package(catkin REQUIRED geometry_msgs) find_package(orocos_kdl REQUIRED) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS geometry_msgs DEPENDS orocos_kdl ) include_directories(include ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) link_directories(${catkin_LIBRARY_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) add_library(${PROJECT_NAME} src/kdl_msg.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) geometry-1.13.2/kdl_conversions/include/000077500000000000000000000000001366756125100202725ustar00rootroot00000000000000geometry-1.13.2/kdl_conversions/include/kdl_conversions/000077500000000000000000000000001366756125100234745ustar00rootroot00000000000000geometry-1.13.2/kdl_conversions/include/kdl_conversions/kdl_msg.h000066400000000000000000000103561366756125100252720ustar00rootroot00000000000000/* * Copyright (c) 2012, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Adam Leeper */ #ifndef CONVERSIONS_KDL_MSG_H #define CONVERSIONS_KDL_MSG_H #include "kdl/frames.hpp" #include #include #include #include #include #include #include #include namespace tf { /// Conversion functions from/to the corresponding KDL and geometry_msgs types. /// Converts a geometry_msgs Point into a KDL Vector void pointMsgToKDL(const geometry_msgs::Point &m, KDL::Vector &k); /// Converts a KDL Vector into a geometry_msgs Vector3 void pointKDLToMsg(const KDL::Vector &k, geometry_msgs::Point &m); /// Converts a Pose message into a KDL Frame void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k); /// Converts a KDL Frame into a Pose message void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m); /// Converts a quaternion message into a KDL Rotation void quaternionMsgToKDL(const geometry_msgs::Quaternion &m, KDL::Rotation &k); /// Converts a KDL Rotation into a message quaternion void quaternionKDLToMsg(const KDL::Rotation &k, geometry_msgs::Quaternion &m); /// Converts a Transform message into a KDL Frame void transformMsgToKDL(const geometry_msgs::Transform &m, KDL::Frame &k); /// Converts a KDL Frame into a Transform message void transformKDLToMsg(const KDL::Frame &k, geometry_msgs::Transform &m); /// Converts a Twist message into a KDL Twist void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k); /// Converts a KDL Twist into a Twist message void twistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m); /// Converts a Vector3 message into a KDL Vector void vectorMsgToKDL(const geometry_msgs::Vector3 &m, KDL::Vector &k); /// Converts a KDL Vector into a Vector3 message void vectorKDLToMsg(const KDL::Vector &k, geometry_msgs::Vector3 &m); /// Converts a Wrench message into a KDL Wrench void wrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &k); /// Converts a KDL Wrench into a Wrench message void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m); //Deprecated methods use above: /// Converts a Pose message into a KDL Frame ROS_DEPRECATED void PoseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k); /// Converts a KDL Frame into a Pose message ROS_DEPRECATED void PoseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m); /// Converts a Twist message into a KDL Twist ROS_DEPRECATED void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k); /// Converts a KDL Twist into a Twist message ROS_DEPRECATED void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m); } #endif geometry-1.13.2/kdl_conversions/mainpage.dox000066400000000000000000000001701366756125100211420ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b kdl_conversions --> */ geometry-1.13.2/kdl_conversions/package.xml000066400000000000000000000012241366756125100207630ustar00rootroot00000000000000 kdl_conversions 1.13.2 Conversion functions between KDL and geometry_msgs types. Adam Leeper Tully Foote BSD http://ros.org/wiki/kdl_conversions catkin geometry_msgs liborocos-kdl-dev liborocos-kdl-dev geometry_msgs liborocos-kdl geometry-1.13.2/kdl_conversions/src/000077500000000000000000000000001366756125100174365ustar00rootroot00000000000000geometry-1.13.2/kdl_conversions/src/kdl_msg.cpp000066400000000000000000000116331366756125100215660ustar00rootroot00000000000000/* * Copyright (c) 2012, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "kdl_conversions/kdl_msg.h" namespace tf { void pointMsgToKDL(const geometry_msgs::Point &m, KDL::Vector &k) { k[0] = m.x; k[1] = m.y; k[2] = m.z; } void pointKDLToMsg(const KDL::Vector &k, geometry_msgs::Point &m) { m.x = k[0]; m.y = k[1]; m.z = k[2]; } void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k) { k.p[0] = m.position.x; k.p[1] = m.position.y; k.p[2] = m.position.z; k.M = KDL::Rotation::Quaternion( m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w); } void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m) { m.position.x = k.p[0]; m.position.y = k.p[1]; m.position.z = k.p[2]; k.M.GetQuaternion(m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w); } void quaternionMsgToKDL(const geometry_msgs::Quaternion &m, KDL::Rotation &k) { k = KDL::Rotation::Quaternion(m.x, m.y, m.z, m.w); } void quaternionKDLToMsg(const KDL::Rotation &k, geometry_msgs::Quaternion &m) { k.GetQuaternion(m.x, m.y, m.z, m.w); } void transformMsgToKDL(const geometry_msgs::Transform &m, KDL::Frame &k) { k.p[0] = m.translation.x; k.p[1] = m.translation.y; k.p[2] = m.translation.z; k.M = KDL::Rotation::Quaternion( m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w); } void transformKDLToMsg(const KDL::Frame &k, geometry_msgs::Transform &m) { m.translation.x = k.p[0]; m.translation.y = k.p[1]; m.translation.z = k.p[2]; k.M.GetQuaternion(m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w); } void twistKDLToMsg(const KDL::Twist &t, geometry_msgs::Twist &m) { m.linear.x = t.vel[0]; m.linear.y = t.vel[1]; m.linear.z = t.vel[2]; m.angular.x = t.rot[0]; m.angular.y = t.rot[1]; m.angular.z = t.rot[2]; } void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &t) { t.vel[0] = m.linear.x; t.vel[1] = m.linear.y; t.vel[2] = m.linear.z; t.rot[0] = m.angular.x; t.rot[1] = m.angular.y; t.rot[2] = m.angular.z; } void vectorMsgToKDL(const geometry_msgs::Vector3 &m, KDL::Vector &k) { k[0] = m.x; k[1] = m.y; k[2] = m.z; } void vectorKDLToMsg(const KDL::Vector &k, geometry_msgs::Vector3 &m) { m.x = k[0]; m.y = k[1]; m.z = k[2]; } void wrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &k) { k[0] = m.force.x; k[1] = m.force.y; k[2] = m.force.z; k[3] = m.torque.x; k[4] = m.torque.y; k[5] = m.torque.z; } void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m) { m.force.x = k[0]; m.force.y = k[1]; m.force.z = k[2]; m.torque.x = k[3]; m.torque.y = k[4]; m.torque.z = k[5]; } ///// Deprecated methods for backwards compatability /// Converts a Pose message into a KDL Frame void PoseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k) { poseMsgToKDL(m, k);} /// Converts a KDL Frame into a Pose message void PoseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m) { poseKDLToMsg(k, m);} /// Converts a Twist message into a KDL Twist void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k) {twistMsgToKDL(m, k);}; /// Converts a KDL Twist into a Twist message void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m){twistKDLToMsg(k, m);}; } // namespace tf geometry-1.13.2/tf/000077500000000000000000000000001366756125100140565ustar00rootroot00000000000000geometry-1.13.2/tf/CHANGELOG.rst000066400000000000000000000717421366756125100161120ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf ^^^^^^^^^^^^^^^^^^^^^^^^ 1.13.2 (2020-06-08) ------------------- * fix shebang line for python3 (`#212 `_) * Contributors: Mikael Arguedas 1.13.1 (2020-05-15) ------------------- * Fix ring45 test expectations (`#211 `_) Copying https://github.com/ros/geometry2/commit/04625380bdff3f3e9e860fc0e85f71674ddd1587 * import setup from setuptools instead of distutils-core (`#209 `_) * Contributors: Alejandro Hernández Cordero, Shane Loretz 1.13.0 (2020-03-10) ------------------- 1.12.1 (2020-03-10) ------------------- * Use process_time() for Python 3.8 compatibility (`#205 `_) * Bump CMake version to avoid CMP0048 warning (`#204 `_) * Add rostest include dirs (`#195 `_) * Remove trailing semicolons from tf sources (`#187 `_) * [tf] Removed trailing semicolons after functions from all sources Used the -Wpedantic compiler flag to find all occurrences * Allow to choose output precision in tf_echo (`#186 `_) * Allow to choose output precision in tf_echo * update how c++11 requirement is added (`#184 `_) * update install destination in CMakeLists.txt (`#183 `_) * export binary to right locations * specify archive and runtime destinations, update whitespace (`#5 `_) * add visibility macro * windows bring up, use ROS_DEPRECATED * Remove `signals` from find_package(Boost COMPONENTS ...) * fixing error of casting away constness in method void tfSwapScalarEndian(const tfScalar& sourceVal, tfScalar& destVal) in line 626 of Vector3.h (`#179 `_) * Fix log output typo: message_notifier -> message_filter (`#177 `_) Almost all the log outputs use message_filter, except one. The warning text still referred to message_notifier. This commit fixes that. * Contributors: C-NR, James Xu, Maarten de Vries, Martin Günther, Shane Loretz, Victor Lamoine, Yoshua Nava 1.12.0 (2018-05-02) ------------------- * Adapt to new xmlrpcpp header location (`#164 `_) * Maintain & expose tf2 Buffer in shared_ptr for tf (`#163 `_) - Adds a tf2_ros::Buffer via a public accessor method to expose to customers of Transformer - Maintains the tf2_ros::Buffer in a shared_ptr to safely share access to the Buffer object - As this is targeting Melodic, adds c++11 compile flags to grant access to std::shared_ptr's - Reorders the include_directories in the CMakeLists to ensure the headers exposed in this package are read *before* the system installed catkin_INCLUDE_DIRS (otherwise changes to tf source headers are never detected during a catkin_make on a system with ros-*-geometry installed) * Prevent rates that result in core dump (0.0) or no limit on update rate at all (<0.0) `#159 `_ (`#160 `_) * Fix empty yaml parsing (`#153 `_) Fixes `#152 `_ The empty yaml was coming through as a list not a dict so was breaking the expectations. I used the shorthand `or {}` since I know any valid data won't evaluate to zero. A more complete solution is described here: https://stackoverflow.com/a/35777649/604099 * Make python scripts Python3 compatible. (`#151 `_) * Python 3 fixes * Prefer str.format over operator % and small python3 fixes. * Contributors: Ian McMahon, Lucas Walter, Maarten de Vries, Tully Foote 1.11.9 (2017-07-14) ------------------- * Replace legacy python code with appropriate calls to tf2_ros (`#149 `_) (`#134 `_) * Replace deprecated Eigen module with Eigen3 * Update minimum version for run dependency on tf2_ros * Add support for static_transforms in tf_monitor. Fixes `#136 `_ with `#134 `_ for tf_echo and view_frames. * Pass through allFramesAsDot time argument optionally. * remove vestigial includes. Fixes `#146 `_ (`#147 `_) * Commented code caused error in documentation (`#142 `_) * [doc] Add migration notice in manifest. (`#129 `_) * Fix "stdlib.h: No such file or directory" errors in GCC-6 * Fix error for null conversion. * Change version regex for graphviz in view_frames * fix for issue in getAngleShortestPath(), closes `#102 `_ * Contributors: AndyZe, Edward Venator, Hodorgasm, Isaac I.Y. Saito, Michael Korn, Mike Purvis, Tom Moore, Tully Foote, Timo Röhling 1.11.8 (2016-03-04) ------------------- * Update assertQuaternionValid to check for NaNs * Remove outdated manifest loading in python files * update unit tests to catch https://github.com/ros/geometry_experimental/issues/102 * Contributors: Chris Mansley, Michael Hwang, Tully Foote 1.11.7 (2015-04-21) ------------------- * add a unit test for pytf wait_for_transform * removed msg serv installation from cmakelists * generated autodoc * Fixed Vector3 documentation * display RPY in both radian and degree * Fixed command line arguments * using TimeStamp and FrameId in message filter this allows to use tf::MessageFilter with pcl::PointCloud see `#55 `_ * Added and optional third argument to specify publishing frequency * Contributors: Adnan Munawar, Brice Rebsamen, Jackie Kay, Tully Foote, Ying Lu 1.11.6 (2015-03-25) ------------------- * reenable python tests * Broadcaster: Rospy fix `#84 `_. Add sendTransformMessage. * Contributors: Tully Foote, lsouchet 1.11.5 (2015-03-17) ------------------- * Strip leading slash get parent `#79 `_ * Make frameExists strip leading slash going into tf2.`#63 `_ * Update broadcaster.py, Added ability to use TransformStamped * update view_frames to use AllFramesAsDot(rospy.time.now()) `#77 `_ * Contributors: David Lu!!, Gaël Ecorchard, Kei Okada, Tully Foote 1.11.4 (2014-12-23) ------------------- * Install static lib and remove test for Android * Larger default queue size in broadcaster With queue_size=1 when two transforms are sent in quick succession, the second is often lost. The C++ code uses a default queue_size of 100, so switch to that default here as well. If that is not appropriate, a queue_size constructor argument is provided. * Update package.xml * add rate parameter to tf_echo * Added check for normalized quaternion in roswtf plugin * Contributors: David Lu!!, Gary Servin, Kevin Hallenbeck, Stephan Wirth, contradict 1.11.3 (2014-05-07) ------------------- * convert to boost signals2 following `ros/ros_comm#267 `_ Fixes `#23 `_. Requires `ros/geometry_experimental#61 `_ as well. * add rospy publisher queue_size argument `ros/ros_comm#169 `_ * add queue_size to tf publisher `ros/ros_comm#169 `_ * make rostest in CMakeLists optional (`ros/rosdistro#3010 `_) * Contributors: Lukas Bulwahn, Tully Foote 1.11.2 (2014-02-25) ------------------- * fixing test linking * Contributors: Tully Foote 1.11.1 (2014-02-23) ------------------- 1.11.0 (2014-02-14) ------------------- * TF uses ros::MessageEvent to get connection information * Contributors: Kevin Watts, Tully Foote 1.10.8 (2014-02-01) ------------------- * Port groovy-devel patch to hydro-devel * Added rosconsole as catkin dependency for catkin_package * Add rosconsole as runtime dependency * Contributors: Michael Ferguson, Mirza Shah 1.10.7 (2013-12-27) ------------------- * fix bug in tf::Matrix3x3::getEulerYPR() Fixes a bug in tf::Matrix3x3::getEulerYPR() implementation's handling of gimbal lock cases (when the new x axis aligns with the old +/-z axis). * add test that demonstrated bug in tf::Matrix3x3 tf::Matrix3x3::getEulerYPR() has a bug which returns an incorrect rpy for certain corner case inputs. This test demonstrates that bug. * Fix const correctness of tf::Vector3 rotate() method The method does not modify the class thus should be const. This has already been fixed in Bullet itself. * add automatic tf buffer cleaning on bag loop for python This logic was already implemented for c++ but not for the python module. * Contributors: Acorn Pooley, Timo Rohling, Tully Foote, v4hn 1.10.6 (2013-08-28) ------------------- * switching to wrapper scripts which will provide a deprecation warning for `#3 `_ * add missing roswtf dependency to really export the plugin (fix `#27 `_) * Update listener.py Fix the tf listener service exception in rospy. See: http://answers.ros.org/question/10777/service-exception-using-tf-listener-in-rospy/ * Fix MessageFilter race condition If MessageFilter does not explicitly stop its callback timer when it's being destroyed, there is a race condition when that timer is processed in a callback queue run by a different thread. Specifically, maxRateTimerCallback() may be called after messages_mutex_ has been destroyed, causing a unrecoverable error. 1.10.5 (2013-07-19) ------------------- * tf: export dependency on tf2_ros Fixes `#21 `_ * added run dependency on graphviz closes `#19 `_ 1.10.4 (2013-07-11) ------------------- * fixing erase syntax * resolving https://github.com/ros/geometry/issues/18 using implementation added in tf2::BufferCore, adding dependency on next version of tf2 for this 1.10.3 (2013-07-09) ------------------- * fixing unittest for new resolve syntax 1.10.2 (2013-07-09) ------------------- * strip leading slashes in resolve, and also any time a method is passed from tf to tf2 assert the leading slash is stripped as well. tf::resolve with two arguments will end up with foo/bar instead of /foo/bar. Fixes https://github.com/ros/geometry_experimental/issues/12 * added two whitespaces to make message_filter compile with c++11 more on this here: http://stackoverflow.com/questions/10329942/error-unable-to-find-string-literal-operator-slashes * using CATKIN_ENABLE_TESTING to optionally configure tests in tf 1.10.1 (2013-07-05) ------------------- * updating dependency requirement to tf2_ros 0.4.3 * removing unused functions removing unused private methods removing ``max_extrapolation_distance_`` removing unused data storage _frameIDs frameIDS_reverse ``frame_authority_`` removing cache_time from tf, passing through method to tf2 buffer_core removing unused variables ``frames_`` and ``frame_mutex_`` and ``interpolating_`` removing unused mutex and transformchanged signaling commenting on deprecation of MAX_EXTRAPOLATION_DISTANCE 1.10.0 (2013-07-05) ------------------- * adding versioned dependency on recent geometry_experimental changes * fixing test dependencies * fixing callbacks for message filters * remove extra invalid comment * dedicated thread logic all implemented * removing commented out code * mostly completed conversion of tf::TransformListener to use tf2 under the hood * lookuptwist working * tf::Transformer converted to use tf2::Buffer under the hood. passing tf_unittest.cpp * making tf exceptions typedefs of tf2 exceptions for compatability * first stage of converting Transformer to Buffer * switching to use tf2's TransformBroadcaster * adding dependency on tf2_ros to start moving over contents * fixing unit tests 1.9.31 (2013-04-18 18:16) ------------------------- 1.9.30 (2013-04-18 16:26) ------------------------- * Adding correct install targets for tf scripts * Removing scripts from setup.py install 1.9.29 (2013-01-13) ------------------- * use CATKIN_DEVEL_PREFIX instead of obsolete CATKIN_BUILD_PREFIX 1.9.28 (2013-01-02) ------------------- 1.9.27 (2012-12-21) ------------------- * set addditional python version * added license headers to various files 1.9.26 (2012-12-14) ------------------- * add missing dep to catkin 1.9.25 (2012-12-13) ------------------- * add missing downstream depend * update setup.py 1.9.24 (2012-12-11) ------------------- * Version 1.9.24 1.9.23 (2012-11-22) ------------------- * Releaseing version 1.9.23 * tf depended on angles but did not find_package it 1.9.22 (2012-11-04 09:14) ------------------------- 1.9.21 (2012-11-04 01:19) ------------------------- 1.9.20 (2012-11-02) ------------------- 1.9.19 (2012-10-31) ------------------- * fix catkin function order * Removed deprecated 'brief' attribute from tags. 1.9.18 (2012-10-16) ------------------- * tf: Fixed wrong install directory for python message files. * tf: fixed bug where generated python message code was not being installed. * tf: added setup.py file and changed CMakeLists.txt to install python files and bound library (_tf.so, also known as pytf_py in CMakeLists.txt) which must have been missed during the previous catkin-ization. 1.9.17 (2012-10-02) ------------------- * fix several dependency issues 1.9.16 (2012-09-29) ------------------- * adding geometry metapackage and updating to 1.9.16 1.9.15 (2012-09-30) ------------------- * fix a few dependency/catkin problems * remove old API files * comply to the new catkin API 1.9.14 (2012-09-18) ------------------- * patch from Tom Ruehr from tf sig * patch from `#5401 `_ for c++0x support 1.9.13 (2012-09-17) ------------------- * update manifests 1.9.12 (2012-09-16) ------------------- * use the proper angles package 1.9.11 (2012-09-14 22:49) ------------------------- * no need for angles anymore 1.9.10 (2012-09-14 22:30) ------------------------- * no need for bullet anymore 1.9.9 (2012-09-11) ------------------ * update depends * minor patches for new build system 1.9.8 (2012-09-03) ------------------ * fixes for groovy's catkin 1.9.7 (2012-08-10 12:19) ------------------------ 1.9.6 (2012-08-02 19:59) ------------------------ * changing how we install bins 1.9.5 (2012-08-02 19:48) ------------------------ * fix the header to be compiled properly * using PROGRAMS insteas of TARGETS 1.9.4 (2012-08-02 18:29) ------------------------ 1.9.3 (2012-08-02 18:28) ------------------------ * forgot to install some things * also using DEPENDS 1.9.2 (2012-08-01 21:05) ------------------------ * make sure the tf target depends on the messages (and clean some include_directories too) 1.9.1 (2012-08-01 19:16) ------------------------ * install manifest.xml 1.9.0 (2012-08-01 18:52) ------------------------ * catkin build system * remove bullet dep * fix bug `#5089 `_ * add link flag for OSX * tf: MessageFilter: added public getter/setter for ``queue_size_`` * adding btQuaternion constructor for ease of use * fixing method naming for camelCase and adding bt* Constructor methods * tf.tfwtf now uses rosgraph.Master instead of roslib * Added tf and angles to catkin * cleanup up last errors * ``SIMD_`` -> ``TFSIMD_`` defines to not conflict * write in bullet assignment and return methods * executable bit on conversion script * changing defines from BT to TF * removing BULLET_VERSION info * changing all bt* to tf* in LinearMath to avoid collisions * convert btScalar to tfScalar to avoid definition conflicts * deleting GEN_clamp and GEN_clamped as they're unused and would conflict * non conflicting minmax functions * the migration script * applied bullet_migration_sed.py to LinearMath include dir with namespaced rules and everything with Namespaced rules and all 152 tests pass * removing all BT_USE_DOUBLE_PRECISION ifs and hardcoding them to the double case * adding tf namespaces to moved files * breaking bullet dependency * removing redundant typedefs with new datatypes * moving filenames to not collide in search and replaces * changing include guards * moving linear math into tf namespace * copying in bullet datatypes * switching to a recursive mutex and actually holding locks for the right amount of time. ticket:5 * Giving error message when time cache is empty for lookup failures * Moving ``lct_cache_`` to local variable from class member. As class member, using this variable makes lookupTransform not thread-safe * velocity test precision a little lower requirements * Fix to error message for earliest extrapolation time exception, ros-pkg5085 * Fixing epsilon to prevent test failures * Reducing epsilon value for velocity tests * add missing empty_listener.cpp file * Not calling ros::Time::now() in tf.cpp, causes problems with pytf * fix for ROS_BREAK include * Adding faster point cloud transform, as specified in ros-pkg`#4958 `_ * Cache unittest reenabled * Adding speed_test from tf2 to check lookupTransform/canTransform * Josh's optimizations from tf2 merged into tf. Tests pass * Benchmark test includes tests of lookupTransform * Adding ros::Time::init to benchmark test * Testing compound transforms with lookupTransform * Adding helix test of time-varying transforms, with interpolation, to test lookupTransform * Moving test executables to bin/. Cleanup in tf_unittest. Removed deprecated calls to bullet, added 'ring45' test from tf2 as lookupTransform test * patch for `#4952 `_ * kevin's patch for #ros-pkg4882 * Fix for TransformListener hanging on simulation shutdown, `#4882 `_ * removing old srv export * removing old srv includ path * this should never have been passing in an error string here -- likely one of the reasons MessageFilter is so slow * Adding to author list to create branch * removing reset_time topic and catching negative time change to reset the tf buffer * `#4277 `_ transformPointCloud * revert patch that uses ros::ok in waitForTransform. ticket `#4235 `_ * make tf unittest a ros node * fix lockup in waitForTransform. ticket 4235 * reverting r30406 and r30407, they are redundant with standardized functionality and break previous functionality * sse detection `#4114 `_ * tf: change_notifier should sleep after an exception * created common place for ROS Pose/Point/Quaternion to numpy arrays transformations * added TransformBroadcaster.sendTransform for PoseStamped * one more patch for `#4183 `_ * new unit test * waitforTransform now polls on walltime to avoid ros::Time initialization issues. basic unit test for waitForTransform in python. * fix for stricter time * fix ros::Time unit test problem with ROS 1.1.9 * `#4103 `_ method getTFPrefix() added, documented, tested * moving patch to trunk from tag r30172 * Added Ubuntu platform tags * Update MessageFilter to use traits and MessageEvent * `#4039 `_, moved PoseMath from tf to tf_conversions * `#4031 `_ add lookupTwist and lookupTwistFull * fixing zero time edge case of lookupTwist, thanks james * commenting debug statement * Typo in comment * documentation * fixing up unit tests * lookup twist for `#4010 `_ * commenting twist test while the code is being refactored * removing transform twist as per api review in ticket `#4010 `_ * Added doctest for PoseMath creation from message * Doc for PoseMath * Double module tf * Remove expect_exception * comment for operator * opeartor == for StampedTransform too `#3990 `_ * First cut at posemath * adding operator== to Stamped with unit tests * adding methods for vectorized publishing of transforms `#3954 `_ * fix thread-safety of add() * Re-add message filter test that was accidentally removed when the message notifier was deleted * Fix message filter in the case where messages are arriving faster than the update timer is running (exacerbated by rosbag play --clock not actually broadcasting the clock at 100hz). (`#3810 `_) * Tiny refactor for callerid->authority * `#3942 `_ testcase * Add doc for Transformer.clear * Missing initializer from TransformListener * New test test_cache_time * fixing quaternion checking and adding unittests `#3758 `_ * review status `#3776 `_ * tf: change_notifier now supports multiple frames; publishes tfMessages * passing basic tests for transformtwist * adding transformTwist method * all tests passing on lookupVelocity * tests for values calculated by hand * linear velocity to multiple other targets * expanding to all three dimentions and asserting others are zero * first cut velocity, basic test architecture layed out. * searchparam when publishing * noting deprecations better and changing frame_id to frame_name for unresolved * removing /tf_message since it's been deprecated * returning remap capability to remap `#3602 `_ * inlining helper function * tf: changed manifest to have lower-case tf * comment * more documentation * adding helper function for getting tf_prefix * patches for tf_monitor to correctly display the chain, thanks for the help Sachin. * asserting that incoming frameids are resolved, currently at debug level as this is not fully implemented in othe code. This level will escalate slowly as compliance is increased `#3169 `_ * not using my own deprecated function * more usage * tf_echo usage expanded * fixing typo in documentation * removing include of message_notifier * removing deprecated message_notifier `#3046 `_ * removing deprecated data type and constructor `#3046 `_ * removing deprecated sendTransform calls * fixing test for usage of deprecated APIs `#3046 `_ * removing deprecated setTransform method `#3046 `_ * removing deprecated lookupTransform methods `#3046 `_ * removed deprecated canTransform method `#3046 `_ * removing deprecated canTransform `#3046 `_ * removing deprecated transform_sender `#3046 `_ * removing deprecated transformStampedMsgToTF and transformStampedTFToMsg `#3046 `_ * fixing startup race condition `#3168 `_ * adding InvalidArgument exception for transformMethods, currently it only throws if w in quaternions are w <= 0 or w > 1 `#3236 `_ * reving for release * commenting all velocity work for it's not ready to be released * adding in deprecated call which I removed accidentally * renaming tf::remap to tf::resolve as per `#3190 `_ with backwards compatability. Also Standardizing to only do searchparam at startup `#3167 `_ * Switch MessageFilter back to using a Timer instead of a WallTimer, since the time-jumping is now fixed (`#2430 `_) * adding createQuaternionFromRPY method to help deprecation transition `#2992 `_ * Added specific tes for quaternion types * Switching refernece frame and moving frame ordering for lookup transform call to actually be correct * adding test to the back * fixing lookupVelocity special cases for zero time * documention improvements * Doc clarifications * removing debugging * lookupVelocity Python first cut * transformVector3 * switching tf_prefix to searchParam so you can set it for a whole namespace `#2921 `_ * removing .py extension from script * simpler topic name * adding tf_remapping script to remap frame ids `#870 `_ * fixing manifest loading to right package * uncommenting lookup velocity and fixing implementation * removing redundant angles package dependency `#3334 `_ * Patch from `#3337 `_ * fixing ~ usage * commenting out lookupvelocity while it's still not working for release of patches * angles needed for velocity lookup * Switch from to_seconds to to_sec, `#3324 `_ * updating for 0.10 changes to python and hudson * fixing deprecated to_seconds call in tfwtf * merging 0.4x changes into trunk * a first trial of lookupVelocity * added createQuaternionMsgFromRollPitchYaw helper function * removing wait_for_transform_death test from default, for it doesn't work under release * switching to Release from Debug * fixing usage message of static_transform_sender * Warn about received messages with frame_ids that are not fully qualified, but resolve them locally * moving deprecation note to top of summary * * Remap target frames (`#3119 `_) * Throw out messages immediately if they have an empty frame_id * fixing display of chain to show all links * documentation for `#2072 `_ * fixing frequency output of tf_monitor * making remapping on send more consistent * removing unused variable * Doxygen comments for the failure reasons * Add a failure callback to tf::MessageFilter * fixing `#2990 `_ deprecated ~ call * update tf error strings. Still need review and user testing * notifier should subscribe to tf and tf_message * doc: updated setTransform to properly list child_frame_id * Doc clearer on exceptions * restoring caller_id to graph view in python * Set daemon on listener thread * better command line outputs * Removed turtlesim reference from python broadcaster * removing useages of deprecated bullet APIs * Add rosdoc to manifest * Fix build break * New Sphinx docs * changing display of legend to be above the tree * make output consistent with view frames * tweak output of tf_echo * tweek output of tf_echo * update output string * update output of view frames * make tf_echo wait for up to one second before throwing exceptions * Fixes for pytf: exception distinction, waitForTransform, threaded listener * Switch MessageFilter back to a WallTimer... shouldn't have been checked in with my last checkin * Remove last remnants of Node use * Fix compiler warnings * removing last warnings relating to `#2477 `_ * tf monitor working, and a little bit cleaner display * fixing useage of ~ params * cleaning up tf_echo output * fixing warning * static_transform_publsher replacing transform_sender for backwards compatability, and fixing new StampedTransform * update tf description * remove extra / in method def. Ticket `#2778 `_ * fixed deprecation of Stamped<> 4 constructor vs 3 constructor. and switched three usages `#2477 `_ * converting transformPointCloud to use new StampedTransform `#2477 `_ * fixing warnings related to `#2477 `_ * internally switching to StampedTransform for `#2477 `_ cleanup * fixing usage of Stamped to StampedTransform * switching Stamped to StampedTransform, deprecating usage, and changing all APIs to the new one with backwards compatabilty `#2477 `_. It's working but lots of warnings left to fix * removing warning * fixing deprecated function call usage * one less node API call * one less node usage * fixing urls for new server * Rename tf message from \tf_message to \tf. Listener is backwards compatible, broadcaster is not. See ticket `#2381 `_ * migration part 1 geometry-1.13.2/tf/CMakeLists.txt000066400000000000000000000117341366756125100166240ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf) include(CheckCXXCompilerFlag) unset(COMPILER_SUPPORTS_CXX11 CACHE) if(MSVC) # https://docs.microsoft.com/en-us/cpp/build/reference/std-specify-language-standard-version # MSVC has c++14 enabled by default, no need to specify c++11 else() check_cxx_compiler_flag(-std=c++11 COMPILER_SUPPORTS_CXX11) if(COMPILER_SUPPORTS_CXX11) add_compile_options(-std=c++11) endif() endif() find_package(catkin REQUIRED COMPONENTS angles geometry_msgs message_filters message_generation rosconsole roscpp rostime sensor_msgs std_msgs tf2_ros ) find_package(Boost REQUIRED COMPONENTS thread system) catkin_python_setup() include_directories( include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ) add_message_files(DIRECTORY msg FILES tfMessage.msg) add_service_files(DIRECTORY srv FILES FrameGraph.srv) generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS geometry_msgs message_filters message_runtime roscpp sensor_msgs std_msgs tf2_ros rosconsole ) add_library(${PROJECT_NAME} src/cache.cpp src/tf.cpp src/transform_broadcaster.cpp src/transform_listener.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # Debug add_executable(tf_empty_listener src/empty_listener.cpp) target_link_libraries(tf_empty_listener ${PROJECT_NAME}) add_executable(tf_echo src/tf_echo.cpp) target_link_libraries(tf_echo ${PROJECT_NAME}) add_executable(tf_change_notifier src/change_notifier.cpp) target_link_libraries(tf_change_notifier ${PROJECT_NAME}) add_executable(tf_monitor src/tf_monitor.cpp) target_link_libraries(tf_monitor ${PROJECT_NAME}) add_executable(static_transform_publisher src/static_transform_publisher.cpp) target_link_libraries(static_transform_publisher ${PROJECT_NAME}) # Tests if(CATKIN_ENABLE_TESTING AND NOT ANDROID) find_package(rostest REQUIRED) catkin_add_gtest(tf_unittest test/tf_unittest.cpp) target_include_directories(tf_unittest PRIVATE ${rostest_INCLUDE_DIRS}) target_link_libraries(tf_unittest ${PROJECT_NAME}) catkin_add_gtest(tf_quaternion_unittest test/quaternion.cpp) target_link_libraries(tf_quaternion_unittest ${PROJECT_NAME}) catkin_add_gtest(test_transform_datatypes test/test_transform_datatypes.cpp) target_link_libraries(test_transform_datatypes ${PROJECT_NAME} ${Boost_LIBRARIES}) add_executable(transform_listener_unittest test/transform_listener_unittest.cpp) target_link_libraries(transform_listener_unittest ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_rostest(test/transform_listener_unittest.launch) # Disabled because of changes in TransformStorage #catkin_add_gtest_future(tf_unittest_future test/tf_unittest_future.cpp) #target_link_libraries(tf_unittest_future ${PROJECT_NAME}) catkin_add_gtest(test_velocity test/velocity_test.cpp) target_link_libraries(test_velocity ${PROJECT_NAME}) #add_executable(test_transform_twist test/transform_twist_test.cpp) #target_link_libraries(test_transform_twist ${PROJECT_NAME}) #catkin_add_gtest_build_flags(test_transform_twist) #add_rostest(test/transform_twist_test.launch) catkin_add_gtest(cache_unittest test/cache_unittest.cpp) target_link_libraries(cache_unittest ${PROJECT_NAME}) add_executable(test_message_filter EXCLUDE_FROM_ALL test/test_message_filter.cpp) target_link_libraries(test_message_filter tf ${Boost_LIBRARIES} ${GTEST_LIBRARIES}) add_rostest(test/test_message_filter.xml) ### Benchmarking #catkin_add_gtest_future(tf_benchmark test/tf_benchmark.cpp) #target_link_libraries(tf_benchmark ${PROJECT_NAME}) add_executable(testListener test/testListener.cpp) target_link_libraries(testListener ${PROJECT_NAME} ${GTEST_LIBRARIES}) add_rostest(test/test_broadcaster.launch) add_executable(testBroadcaster test/testBroadcaster.cpp) target_link_libraries(testBroadcaster ${PROJECT_NAME}) catkin_add_nosetests(test/testPython.py) add_executable(tf_speed_test EXCLUDE_FROM_ALL test/speed_test.cpp) target_link_libraries(tf_speed_test ${PROJECT_NAME}) if(TARGET tests) add_dependencies(tests testBroadcaster testListener transform_listener_unittest test_message_filter) endif() endif() # CATKIN_ENABLE_TESTING AND NOT ANDROID install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) # Install library install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) # Install executable install(TARGETS tf_echo tf_empty_listener tf_change_notifier tf_monitor static_transform_publisher RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) # Install rosrun-able scripts catkin_install_python(PROGRAMS scripts/bullet_migration_sed.py scripts/tf_remap scripts/view_frames DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) geometry-1.13.2/tf/conf.py000066400000000000000000000147341366756125100153660ustar00rootroot00000000000000# -*- coding: utf-8 -*- # # tf documentation build configuration file, created by # sphinx-quickstart on Mon Jun 1 14:21:53 2009. # # This file is execfile()d with the current directory set to its containing dir. # # Note that not all possible configuration values are present in this # autogenerated file. # # All configuration values have a default; values that are commented out # serve to show the default. import sys, os # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. #sys.path.append(os.path.abspath('.')) # -- General configuration ----------------------------------------------------- # Add any Sphinx extension module names here, as strings. They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] # The suffix of source filenames. source_suffix = '.rst' # The encoding of source files. #source_encoding = 'utf-8' # The master toctree document. master_doc = 'index' # General information about the project. project = u'tf' copyright = u'2009, Willow Garage, Inc.' # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. # # The short X.Y version. version = '0.1' # The full version, including alpha/beta/rc tags. release = '0.1.0' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. #language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: #today = '' # Else, today_fmt is used as the format for a strftime call. #today_fmt = '%B %d, %Y' # List of documents that shouldn't be included in the build. #unused_docs = [] # List of directories, relative to source directory, that shouldn't be searched # for source files. exclude_trees = ['_build'] # The reST default role (used for this markup: `text`) to use for all documents. #default_role = None # If true, '()' will be appended to :func: etc. cross-reference text. #add_function_parentheses = True # If true, the current module name will be prepended to all description # unit titles (such as .. function::). #add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. #show_authors = False # The name of the Pygments (syntax highlighting) style to use. pygments_style = 'sphinx' # A list of ignored prefixes for module index sorting. #modindex_common_prefix = [] # -- Options for HTML output --------------------------------------------------- # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. html_theme = 'default' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the # documentation. #html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. #html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". #html_title = None # A shorter title for the navigation bar. Default is the same as html_title. #html_short_title = None # The name of an image file (relative to this directory) to place at the top # of the sidebar. #html_logo = None # The name of an image file (within the static path) to use as favicon of the # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. #html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ['_static'] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. #html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. #html_use_smartypants = True # Custom sidebar templates, maps document names to template names. #html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. #html_additional_pages = {} # If false, no module index is generated. #html_use_modindex = True # If false, no index is generated. #html_use_index = True # If true, the index is split into individual pages for each letter. #html_split_index = False # If true, links to the reST sources are added to the pages. #html_show_sourcelink = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. #html_use_opensearch = '' # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). #html_file_suffix = '' # Output file base name for HTML help builder. htmlhelp_basename = 'tfdoc' # -- Options for LaTeX output -------------------------------------------------- # The paper size ('letter' or 'a4'). #latex_paper_size = 'letter' # The font size ('10pt', '11pt' or '12pt'). #latex_font_size = '10pt' # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, author, documentclass [howto/manual]). latex_documents = [ ('index', 'tf.tex', u'stereo\\_utils Documentation', u'James Bowman', 'manual'), ] # The name of an image file (relative to this directory) to place at the top of # the title page. #latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. #latex_use_parts = False # Additional stuff for the LaTeX preamble. #latex_preamble = '' # Documents to append as an appendix to all manuals. #latex_appendices = [] # If false, no module index is generated. #latex_use_modindex = True # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = { 'http://docs.python.org/': None, 'http://docs.opencv.org/3.0-last-rst/': None, 'http://docs.scipy.org/doc/numpy' : None } geometry-1.13.2/tf/doc/000077500000000000000000000000001366756125100146235ustar00rootroot00000000000000geometry-1.13.2/tf/doc/bifrucation.gv000066400000000000000000000013571366756125100174740ustar00rootroot00000000000000digraph G{ label = "Effect of publishing Inverted Parent Child Relationship on TF tree"; subgraph cluster0{ a [label="A"]; aa [label="AA"]; b [label="B"]; bb [label="BB"]; c [ label="C"]; d [ label="D"]; a -> b; aa -> b; bb -> c; b -> c; c -> d; label = "a) Example Tree"; color=black; } subgraph cluster1{ a1 [label="A"]; aa1 [label="AA"]; b1 [label="B"]; bb1 [label="BB"]; c1 [ label="C"]; d1 [ label="D"]; a1 -> b1; aa1 -> b1; bb1 -> c1; c1 -> b1; label = "b) C-B Inverted Case1 (Part of the time)"; color=black; } subgraph cluster2{ a2 [label="A"]; aa2 [label="AA"]; b2 [label="B"]; bb2 [label="BB"]; c2 [ label="C"]; d2 [ label="D"]; a2 -> b2; aa2 -> b2; bb2 -> c2; c2 -> d2; label = "c) C-B Inverted Case2 (part of the time)"; color=black; } } geometry-1.13.2/tf/doc/bifrucation.pdf000066400000000000000000001452221366756125100176310ustar00rootroot00000000000000%PDF-1.4 % 3 0 obj << /Length 4 0 R /Filter /FlateDecode >> stream xZˮD+qF#1XWbeq37%g'30(d\˧OW9:|;:<91${H~{7|t_ߟWG7(|c)1W[u.qx|s#_[|1)C> /_8b|x^_ާ{w._?|=ǟn|??<Ǹ{~z~5~l{v.ϗܼ_u.nsU}7}ܹ'.Wֵ˙dF1P'^O[כW;v>~mo8썟boi8{Cyia% ;y{\[,1ƹlf0'{d̃&7E,1f_]g=RU0mUPν/80Y#ݿlFƥH:r"ׂ@,1Yt ]F^*"᱄M(;C0PfN59jtGz;HFj[ٸiĪP@(=\EIP4G< @]蠀<7(^hw)s j(E 1m 1X-` _zfR^:s<@Hu XBǿ\zma J!䢉t =@x+".Ԩ:s3/] т-CCBqJCČ X*wNV`) }m6rw˅ M(I5bBzA1G wy%vW% xF1 #x:⹻ ifHb.*B]paJ]8/Dꈰ$ ȏp) m 2TAtj MԄGs tGLEwAW:T<3tPvx -p86Kau L3`pk"8P]Zٲ@1nuD-9I8XQޭ`\%d9'0CգBr '8'ʈwcCYĖ27bߣsBR(O'BTu8Ѣ#̗H<ԋouRRacKu4D74 >MD4-URP O 59jtP@ ^e 7P[89q!|MCjޣ55ߕWxlF;N &mDs$NeTeO8iRFI_.fYOK Gkč\q )\w^- *|Z+{'ՔrXB镐HKp'Yf .3R!CA9' şs PDLJW|9OX$0+xͫ(A,J FtA\j[eTqFEຼ'MK IZ^Z-/8sKDz% \WIƶF`K('ȡ"HlY˨<]BjPpKAåL͐%[ViHN!ND[HyhtMt& 9EH"U͐JҐ9Tl6ϩJY`Rzs󜊤5Q>Ǔf˾E8gqRam7i##W Z桬'm#]5:ۈ[8f͠OKcGcףD!b}= >d.R;{# |:R Ce|EHׄr党Ra [l}U0IkȲ}xd#Ω^& ~ a$C,֚A92h!o21Vh,#E~0Q:#S(MNAl7sNbK[1.k=nW8u [zH ;[ZeaGfˠ풖AAזspе endstream endobj 4 0 obj 2234 endobj 2 0 obj << /ExtGState << /a0 << /CA 1 /ca 1 >> >> /Font << /f-0-0 5 0 R >> >> endobj 6 0 obj << /Type /Page /Parent 1 0 R /MediaBox [ 0 0 727 424 ] /Contents 3 0 R /Group << /Type /Group /S /Transparency /CS /DeviceRGB >> /Resources 2 0 R >> endobj 7 0 obj << /Length 8 0 R /Filter /FlateDecode /Length1 1880 /Length2 46280 /Length3 0 >> stream xeT\Q-wpwwww[Npw A}oUQ5מ{>c"%TP45821p̭ ll9eh΀8,)K%U:8pw3 b9B l 0YX8L h0q5%sw075s@NIMM7 0to t47ghekg q̍Vvfcc?4 1s+s;;[0|H K >[,)031(̜M>)Pt&t6@'OMRQca[T "@5usL0vW1wJ+`gk01r+ }w=?{x7s_Yѧ]swOO2rvk>&>-@9 $4%nUUk*^erXԖͩ؍ `\/HpP98qMP>`5f= >Z$p9""~H9ͬG>rKvyն{#05%Mk"N΢Jް`Z537uL_x?BqގL26^1nAJe/۶,o,{9 G&U"h=o4U &=kߓoIЏ~GY}"ā\px1N`$D-Kׂsro?`>5bͳl8!/Н%Yl[*(f nB!8͈eFZb2IÌ.(غԊ{/%xUxp:in0>H S3{/yGV8\{ 9B)<n AyXV {/PB['鐁uV43Vo:Y=qNfF_ڎ-﹄4E5𸀏TPT:yFEۊmZ;eR팆nT\;q}{g|4h XC֢&j Yrjֿ+Z:pACRwɂHL#]r2*n)#pN<9Qzy(.9?O.FKC/壱O,\KvgݲF( 2l÷fז5fņCm߻:c8 j:юbz'g~s*ljB%xڔ(_S#tAQWGQp9=AG-Vq>EZP#DŽ_֣1t*kSg˳'>F+:9Q9Pk8g1 L^ O_>%s/מkKBTin9=cSɪ!;ˏv~gF4 ~޽OaqM/8l)PGj[ؑ^^fVŸ~$zO|vY &G ;SuL.Tbݔy{[/ ?8l\`RBUK?Wԋ T2 *])y\hP?g, 2)RC3/SͅB0 Rz›G4~N6'.fDE~Uy- {m`sKr!d5Po!my>5 U'.㮬TQ{{BV6j 5鳾HfDjiRp(X9)-j: Ωtו)!=ffdDk V t-ؤ6ֺG^)._u VЛ̔_2 u&X8eN EE7 (@2 ZAv#.:jzAnӪ6._h*Vd-HIVr2)<21},.YGUEtmǍiø$(8xW/S`-{F",C  υZLk+$Y]cړޯMk#-:^H(߮T??K1ז%}Gԇd"}:k _k?7į8-tU.> @W51 Fty=Z0;>oHRY͙7?5QY$[qNHTjXIbavBd ƹ׉~:x%`2dqJݰx^I8&n_t.((f{ktԡVr YNV/@Ƨp,h碪] He $⛩xK^+Z}SShf.rlJ /"H߼ 9_pS3ݣ]vi6ƓPRFzзeՅĞl]CoJɥk\՘C.] vAհs,Oڮ )5ak%(8]%ND]"(%hzw]W^o|X=YIԦfG1o/+C|O4! Yދ*t2Qyk3hFdsGdVZ/-2 Xn@)p?rDa;5<`z'ޛyxMn9 2)} {wBg)&F kX^ߊ\_)фgg y_OEn9;N)gHըH;pl9ˏ5IIQS3kұtLs'{-"O9 T ] 4mO*[!Ɩ<4% H(%&#-h4{T\sa\ tM$zi=}'aD=#uL9Ľ'ˉX1‚L>T Q}g3q`eeOC Z6 2uQ䯫IcA4]^vvi*[H.@7/e\<{PKt,[⴩k5XzZs>͔)JSQRmij1xJ_gY$ ơ:fHͫWå/|2$ tf3x Z y=q-#fF7~ߐzP»{!Z+ pvI&G0 <{I3el|MAomTH/pКo|GEG0Y|`R Ef? 9y\Hb|tZ6VcL0!a "T~Q> `ȟdR!).)X~zOiADGJ3 X|yry/RAa L蔧 '4ϧuZ0YdI8Yx+M.CH#Z:e\"qނ<#bpL<] s+!I@z&hD!s?D٨I\#֡ > YϘEHD@BLG8^ kq%G($s vj#WtCr=^z`욓>8c9B[pcke&#߃SZF1Rawk#O,gTVxY0犑N#jP%y𼣥 &(͗$M:[ZB qS]m2铝 m?tt "85m~kVKe4Ǫ [3#@j'$˵n=tߥЧ[ +:diЬ D/IlKp?۔gɞY0>X@5+G`$plMn;"k Qȉ*jaj΍K)E7a%LѶ E'N η9% @@n #ѕ#~xCa6=m=n68F2Au7H\C} B]f+bqqFPƥU̷¨ @+<[ibyg3h8"UrwcFxԔoSk[(HJ>i,>#c|HUDUO*_0;bvM}I1;9qYmδb)I!e1Vڄ_&>m% 3:;qnkB2]:sZ{!Hܐ]*nJ1Udt%LRPߌNh~de0ߧ O`{CAazg-(qA,vK+40V/E7a2ؑ5k莰 ҙ>yq.w"˺}%'!k "-)KB KɍG\Gx0nswJC)~v6͐&_ t9Jlm_qb971Gr$Lk?5ƶ-~Vנ/Έ02`bssMb؛o<7hH%:(t$v2=*o36BԴyS2 hS!|2>^L_т$WPF‡/xnyS@O4~4x 2h\-VlAY 'sobJ4Vl=!OOZ|'Z^;bLj*n66GbsϢݎ2"?vi[:ryt-> -F\ m<}IBTԀV/iqX5pQAy(,ޟQ Lw_5AY |t56$i0ɑޤy*h:i /z0'5Lm>fY7Bm+Vl(3at?sNw0ܵY$H.oIhgZ(]V_kU_9:- P:g7YZ>6YC=];Aot񘑔0Bds$۟T/BQŠNq!;]i#=&B~>}wD;v=Q!$ Ri5 eg2N{d*t^CGZ[G"~b! %Sh1&ι_m^] ? 1W昏?p:qh~ A8$`nڟ?|:矰Y3kU<(1Ҟ D|پڴaQ |Dgg0͐ )v6 pĔ/0 qCd|%a׽ý! fj(k' ?x.nY/CvHKw=D<]>Tv2!e> 8+r~c o Vn',t)xᗮ8БSYXP.EA;.\r@y*},Mx8&8˕Jzn,fҀq{TGŽ )9&ͻFU;. V *ޥmY'Q$m р[7k凪K3?(p0n%o*lJ])@1Ƴu<)SP3AM DQ&f)s1?^W]<е3 m>= W@3o!׾S&rR+bBDMR%rh?'(\-4;Ҧ?;:#ݭI".4%V]LSb) V5n0U:_f 308Ϙ%q Q ynr$b8sr$>CbتdvP UFo<{,qC *=q`ut^uۧXXWsT1֪:S-~$d@gI Q2X_5-M1wRfO(Pu/k$xNb>qϥZkK>0YxiMM >4'W4pT2շw5rP豞%G&1X$Ov-^KwM^nԒf{lwF7w0Y (}oM^X(;eF4VTRe9Yu ONKe?-~%XxsМ]kBdfx=j>ѼuFyybOݚqV0Dw`dBBy}ZEJ}QAoDۘOֈv9v[!Wtӻ\2FQAωsϳ$c#Œ y;[N]\.͟]wfl\7{'HEx xHLS `&]-^ܟmHҗ,0)U 0i)/X[KUWҦD`̏op<`tQޛsuz$Z;˪V_ڕJj1W<܊Q$$/oK &seHO?p]fFl =R a, k A!߄q8x)fil_<*:(zL2q[Z +s2:"Uًc1J3,vV8 k^d']s}tE3{;@hpprt0 dlaϺfGe3vk8cQDa.TEݖ~{8?:%xb'c͆n=//Q45f)0[@8$a k]dȮr2΋|"a!+aD}q'.2Qrѹ^t5d>dZ dYۦt[üF}X^ȡj OԀa l=^Ϗ'=-\3ۯxXz Km7Ŏ@2B_;z9TYɡWi'q!v5*ܸ4pz}ԉ THM5\"u̶I3y[¸0"ޞhj`VEayNU+]kre0[6 ,ΘWRErk]Pox5՞}H^Ӿ[+U-e_Wrؑg_pwQh$ R'T#XeW$iMT֤5K1ɴ;@AMb` ~(^~C=S@>cQ+>el`;j2 }iEQˀ۩1\It7fN ;lJ}z*ך5Xpi. Bk1[2&_Xx&&, F.׻S:g,^˒uRJslqJT:;A s]D_lnjEu.[͟,Vn7,዗&Ήv3a0jr /дewmӒ7}Ԟbk'c"[S@8O,Y19yQ5ށ(ȩ/q*N (ݬfI"\.sq )84̔ qX1`2gƝ' ,܄!!P4yD?Hⵁ6fi#yrqƜzzoyjJ㳶D%$+3]N3I6 "ro7|YgڇLw>VmۉCrPSgl+[i)~#AHf|`C۵S ?zm.Pκ-cQFSN<BVLGpib"?zNMcpIȆf[ڌBKd/-EX|5y`ln|I.} >M RwEEئBξO 3NG ?5.G^NOӊ1>.5LHQQOUEd Bq: sT|F*NGQɬJbnc؜;tik(!}H$a9,bORMiǓ6.ox'.ڒ`^%C6B@믱k,\nr;> l *y΀.gX{0`mF!UJ "s0'&+yv[\gLi3m9GB'9QG_9t3A~ZcP ͻ,8k6K,7v L=@J,Oa)B03&nri /P_ٴ'yu#5Q s;SǗqG볓tEG}m@C]gaqzW,w˕ŨqxƋJKjҗ T'wpV=2S=0`5;OF-& ?1fU8)/1>^},ܤ+`V6!5u 'Yk)qzIu-KٛRGKD`XNO LG$dQy\~[ {Ճ+wzhe)4V ,aPAzBg2.}lW WYIf!śB@V(GiU}]7=ӟNSIQ NB:)XCa=\)xmǾӐmx:ѫѢL3{[\֐r͏ל=1aY&+(h  (s6zEҾ>_BLFD ,lXW}pcU[!LjM\>ŭ碃?] 5.+eUȆs`$\Fk ZP ɞNV3#/f/{㗇d:LGUt^$~%i@spr2o8l|*S̩.Ԫ-:jPenTGh_^LG%mLR=-h>H)_"軂%E{3 %vJr+?mb'UQtN$̩GTTK8ۿ+w-t5͟7<|=uCDskkB|+5&$?.liZWT{(6F_[7c+0 êU( o%'a Ũةid=Ǫ|խN=C+BӼ(H~_?/PH;CR]e >KZlǨP&̈́_Ky.T5 >~v< }pWsi r[߿iˆ'}ZQCSuhЍ*A)) 7裟ncѸX|w|RE!n[ԍsO6q @6mܶ:; m6 a]R|1q>AL*ݾ,@|),8=YNpH|Xujؼo*#4]RLv4I@]ЁS{Q-)E;̳p 0VRKO"x߮FXMdL=~_O3swc306UbnIψZ<k.co~0ێXh nΕg!;ዻ;^Zw{>R.OZ|i]_Sˁ|-=I@Ɩr~a(scXۘue:Qic$WaIJOxZgx-_DNjLYeFQ{(ѥ5Z=t+: H(tU7$WoUfݲҟׁ;V2)H?d6'ygHزyattcOV?ú7KK=}w7~J# wE#_/X>NSJHĶP>*f@+):2`gT&RuDxği\xrox**eG}w*| cf~t?Q"̛CoGl8#טPܼLBc@C4$ 4CCy- M O[ŗqLXM"v, 71o]'x6Bs`a"iZ–w4rkI=yocJn?P}TCҦqD5)i`+xDj^ºE]D!Tǰ=·xKT gWCa=l,Mߘ\9buR|a?@ׂ^l8x~65( 3yn9H3fF T448f2Xi)ҏ+}1fJ ]JDM'k;lbAኤ2XK[rJыai#JH}EMtDZGIkMǝ59FzC9y:&w)d|1w󴨭X6:0~fPkPڤ\E0C%h@7b$vŅpDŽ28D=\7:umͩJyۼעcIj<v7ZbENa%o*e$hFoW~*&{#ZrAfx,\SS2%kvsOh  T%&s/C?3t_> zyʟtߘw>ݳF5tXK 4/.ZNg =ʈFG|ɀ< DK !Kt:+@)㝻(]eAr$?a .6j^Y MIpFr!H` 8z#y// :ld}el>6E7@RKO*zapSe+BdS7+{p }O&ݛvdE*ϔx5.HFؓߞCϙSbV7YԐ=]Ac4ǯ3 V7"ჩݮӷgK`Jqnk$aW% +;j Ox H j2Yoj|tG>sH&JF|ƶFAS{Hk> a @b㳒1Ri޹p@mֳ_ Bu%b lQû;OF>/)itG³W|(Ilc'-JcT4A˔CY2yE:r3p?o`4gF*{}Z j'vD3mBrԮ&8l@#+IT F *)fe$`v=' 0bh^_X#ZnydjFb61(d,Cz -LKC%l<-IvW^ZtZG3KJ@eSԓ/()hS P5}1q#T;}\쵁G3QeeArS#*m'X<󍉇s#d.%G"as_.m[z z+vf X /BYr(!:kڴ7Py6Fj{ T B4S~=e: hsxC+Iǒ EdYCLT0 $G ht=8"]_X\dν9&%j(5F}zkob"3oh%"~-אQc^SϘ> !.d8jxM˞e95xCd0`uJv9Gﴤ,o/6"I$UȄ&RAR|ZQ9Wԁѡڹr%pEy$85חj-`Y][)*')(XTIsg8"|c%Em0l҉\;u9]I3vTBuk =TlNr<r 櫆S\|_Sjvf}!MΌcH櫄MtQ??*m)e̞نe +{}yx:nma믷J#l΀X7#Ѻ=4z &O*K k}oׯ=DK@^>?~Ɩޙ,؂ Ɛ  v F(u͘BBՠ)qILvDqn*[i˰Zg ٲf|0n z;{ |Dè}H 9{2$Ki`Xc$W ~ .Ei`_]f^iЧ;]M|Dܲ@lM9Q6*O KO'߃|υ sݑ>p|6Ö;Ugrb$Dtpwe.: #%7kEC[IQ@fU X]gKE]p{:̀Ǝ+-41񳺭kv(pC ROBnX!wڂ#!`Ob\DƟgmQyqM'[SXeKRr >ɯh} ֹhwt!WlطJ %r6:ҜC2! )TѓfJ*V uY tP&.أ-+M͸Q84@zM-8݀$]yvU{:z$P5=ohểw`uE6 \:5HPm "mv ̑> ,d =^g}3LLkP<wh^$0}WpA78wjͼ6HPQy1ŅSϹ-:h/7$^ܬ^;/="M N2&Z_@O UEX6w1.Q-MQ#=؆sKm~M"ѪM'f[l͙ kWFUq|N~=yGqCfu(NR/; .[ dB]Senxi6f .-S%5-=L"UDoPitXoji@z'Yae.Z)X,qxe%KX gpt(IeY`-)O=.|lEkR<%+Ÿ!ƀYM_jm\Lo1RY|7ɤ_J1%39q&%sh_ӫP,=p36q2|ݶP *Qu-tISԡe# lJpMǸXʽ$ͩIuK􁪍p4QuQt1i:˾9ȡTWX^΃A=}H_s*R*s%JQX?rͿXw<ʡ6. pH'd@_&n%iҙB~ҦO|[}@u4j@uD /Xn;EcM-O@T1/,dFʔJw_f&F5v՚J3T6e+vgI"e+X@5Kh:_hHo3n%!Uo>!n&?C/iNByTPTHo!;x^[9z56m;:,<^E'*uSfm>Ǖ(8x8WN٢:9;Tj=_D~/Yq־h }T.P㎼*;6#KFݐP-]4Tw< Eym1(.]/k]؎Ӷ-N!%ӵSNpOHs U)jX5p|_DQ|q)t@JV I.qz#L̬BaݧӢ-yh??N'&^@` fӫ,dEfUk*D3M?tΝ8(|+;B>/eG okj˜q<,xTJ}:3aad}o|_,<ِPb5i'V&}a60_A5|;ђ@;e@l("?ml2)]sf'yerN}qK}(MQ̋%jVF[(@Il. rHj:G녫?xr30(b66l7-UOI͚iMf9>GF~dSuCm9Gq %ɷ2 Ũ3/sKk+3@}9|$bSeC$?@1VmM~vR3n-Ү8J )~rk"DQ'M "VX&]g500I:/ٕ( 쮧R"EWywup?fE$h`NS7ώ>̚&uI~NbFD+cg?Ug@{17RufY0hac0=.BҒ:+"XljKhY1'ӟc/XP ׷KP2 S4"eE)J!n1Iꆠ %mv&^g5=qQY%yem*˰)(s}.T* gx((;j@,e uڗ,9Wvrd9@$ Rb{&2`"һ,1e([TCs74huo96Hx x"½`9PWh,6K/ "^y'q5C*Y,op ӀJ?ml*:+!턗V?uHrO4_wPCBQpq VSp!%e*|5_-i#(~rjKwR͚UuX"zu1&%!1AcH, -~FPj$tg=[ĊN}ީ~xЙ_ *\,@)|{v?AꖴUWx *)(6^|k%a軺貢 {n, T1.W'=ݢs-m~N<\'~s؉VXi"M/Ě;r6>aI@bs|l[XudyMg{6Цl*u%/T5@'TH[gXv~\Y'P vl-6%SO,hPN؆w@4;]0x_;a\ق[/r!墒%^91z^` %A+"p-Wwᥔ\7^ Rˠ+{X`xB IՍiT"~:rk8e\epʚ2S@{cΫ'9Dq}XI6aiI2%ӪNti.O*✡>Ү„09*bzQq3 ^ |caX XOkڝ%t^l#Ig+Rpp)Wq}mB-0/{6vO,^ )B,)bh;H@ZHqC#}؃Dr+uh@A[w)B!L3hUũb4d-U+by!>PKuoqumϏ}tGMg"Ͼd1cVKL?y* byPA=As 7"X¼)胆)᚝OwXS]i>Kw*qNvHtǰyr qᑜ=bS]YyG,>'ҩ8g p!oJ`}wijۺ8{ZM( %&R+z(Nf[Ri-iHe=O׍x*^:Uod`W }Gzbq!O9@EG^QH.`ٟĶ<(qyqmN]=O)^ou@%0'ٹnb;qGzY(=ωn}f{ez3v]1٣ 7 $h4E",/?<"76!z4nX~kg͓֬.0}5`C^NP;=RE (Vvg-!S ̣KEX) <)1+uaTyc^P-o%Gzh(%}88D~WqD)l3 yr}䠍oؿ%Tb) ٻ |җ\0*a3b_N a5Om ~;׋㵣J9). ଂwxWί*>U-+2%< ?"؟Nr+ x:7ͷAS[ '/R:\^~VD ѧ lQv )|9kڻvZ@#xԍGޞ Y¿ّU$Gx*NtdusxL1˜. zs;Z14w9rSOw3a@&If^!; *uJ"M5z0@\,ic"U|偞^jb4ᦐumӄ徛իK=^ܵʃ;7oz#̺P6 m% Ȭ\ϫnKIș3e&Ziaecqqo@OgdP%vDk&F3Oy宄a,6 %IWM;4p23gA"-29(ohvz|mt}Iv}TixY‚`/j y6D3ӰsHZsW\3<9kgp~9:bMcdtVVѡ/gݷxEpm6:#6t`WV 6k3hbk%72 1HFN@C~/yu( w<)Y銂$hì._5]ʝ.J|TG(Z)zFN6, >I:5-r^nA3(HC9- "ͧxl2D|3XtDiMı% _%~=eb.t}UΘnTԲ)hhGRhަp,lS>nsF%^ I=x[p!2Ih!u?'Lє`֠*팉t3@ IUv6[f| \uN<=8Ѳ_C / E ȟX9~c!Q\6(eG}D]ħ_V5"{ƖцxO b%Lʡŋ&? FLر۰B'=^SLзQ. R,B}Hl"hj#ɮ؀jr]V|,M^FWEI?F܀, .(:1J۱`ʐrHwݒ7wy,6,8ӨK :R"Cf:-A6nwAE+L<"f\uGzO2uvJȫ[ t}^ʙng gifDfx_`l G9 TDm̠@>K>'j=HnpN\eb  HqE=څ5sj8@UAP-0 3w,F΄UXI9']~JrMW:fy2͕p-āxhŞ4]1s aڇ%e@˦[3*f`$~qn)6GP: c, mȚ $ToTfK?,vr4~jp|<{XTX[q%JB2KX=)ZtvR|R2 |VbhOUV#Ŷn`O:w&aTEڕHH0x:ؠ6"n<-Ҹ}e*s BH6{b|ݥ)C&>NMذe#6G#x, *&G6I)mw=Lz@K` Yۡr?aqNYE/F Uv7R0E!y3mRiR pc,!!ƦK~S,e@$X }gE_FI @Yչ"^FlQFq#SS۪pxQίg!vz5Z0IN[ T W^ØN Ww_J.9%YY jQmZ6& Ġ;P?L:%~#!obt(s{n_R4 ]6keX150BcyZ>TSgKGoN ZznRm1DQy>I`֠6tŀG(Lnq&L*| K(W^`(tJ(jXmm*RG!=)ՙtdaDĚgqsDJOI2`}`!IGMkWYre-_%d0㚻2)q*;i_ms5Rpb1ClAа !LwW@ZV+#} !ܶ/@ȏ nnƱy~v%(a#6s)*O ֕ 4#l[XuR ia"N$0HnR#!9a$Sj18.N2Ґo}#%ia!08p%>\c&.1|HcA | έFCgC#EcG\VTxܟ$5FpջRFd ii_PY% ><izo/-F*%C~ڤV@7U2rO3 )X63Piçn< ,Y Rh,in].|"ܟcߢQ*/Hk*/ɘs#SAN*ƋTUd LGvl748Q>HvEH3C6n6^WhIepbksg| E@+6 @w%Fӈi>^GDyg1L~ U%d Ֆ'Ф}͋~ QI i8F7Ve;ceO4 3 X6c s2>hqDQ`Nx)EfMw@j\eH!c}zڕo(go;mpOz%)RDZ5b M~0R-V-Vg~keTE ej@J3D5cRDD^#H~"w~$&e+7c@p6g8>/@Xx0Koz;T ̃AJUiU8Qq n@FG0|n.jFa!2s{5{e^prN\t~ZUEazq')ʬ7bXhtV Ƅ)Oj+"ַSaCȍvm7j&u$vl?m_J:W` Z?@r<ېCw->GpV ~E z|05_~Gpl"F'nć6.Ίg U3 [[.A1/' f\=;Ƣ{ /UD Fn A9]'[Z`}s><~Ccr޳bnQCph=̖ T*S!WzC;$`{3r]:9#XfX }X(?~iKV #;f +@^#zְmelh1^Z0GtL"rcA|E@&3SgMq뜘gul$׬:ͷB]vp>y2@j}*_xgs咞ĵ RWh6e7fȲj%7 @y#q2EN!*2&i &oZguSGYXt%CZ Z(eE'K ԩԖ"H-PQOXՍRz\ե!JJTĹ5\ ȵ8HTH -˚pqYwۉG2`#U+zu[b:Iӈ5ջG[C .P{ 0rqxh(ӌKZ %_#A57{a9k'Vc gn)Hd/\DkcGQzEk]5^ݾq[<@9_Q< -yaׅFÙBXIvؙNi9i{!~c%CIŗ.q"0.U!]ŅGyd(\W&r1ME/ \X0U&c0ݢA>{Y}φQ#@ɓtm>c]lEsC'y7!ۘe pG _ʿow{] HHɊGխt^8a0Zt2҉m]ZrA~KQ䗋-= @9*,-ICUEj՝!n;PMjx;Օ;[gcϝwxN~8ᨩA+n4{B0бk;Fù`J0=4if7?沬k1%T} <͖J/&ʱM Il'ɍ_Оb\W[' t_P1D{Q(N )؄o:<[0_yfg޼6rmJM#s$)v.ڕsäGNydژoHүsd)>J7VKHH+h }U to5u _&  !%)NqK%Y(+ F.ė ]CR* ;dDxann`?N ` OH7ՋXDTۋ{֣ B#(v!B7 !LF|s󯩦cdYs]ڸPs*x0tQ`W)Z7gpAuR%EUBq=Jؙn^ o&s\́ZIS0ftf;lic*J VI@ ٻ۷[­nL@t^Y]VA1TY&|l;A44:H.䛛ִ݁I]jvWiFkqހ𑇱GWm.Y@lԃc9g5sAɍA3`r\RJߊcOOf]>(SCx7n҉kn <`/җ7ۚ7U͹6" JwuR5 [W'G IU7t06FdQ*T{kiՔ͖k"U JV DchLcR'+<.mlz6Ź+- i.* =#L4ɬu`OBlp85c"w.؅_wxKNjG drX %?b";HsXHE]'Eǻ$`Kzb[Vm`AZ)˦%L5%B[n(2#+ BTx\L~]]c)q`t3vǘg0 (1CzhВL!i"2,\d>@{Ƅ3g?4CFC\fuvOO.QWz/{:4rk=.7#oE Zʠyx}Y9AhzJ^L+.SDCE7_ò@  @f#Xԣ6VGbox d9A؟LyyAR\J i9ӟbL%j>}C4˚F2^.@>ILQƥ IDHʟ6it(?ۺ4wIO*f]-t&y{ =^Q5t(@sN m݊: Cynٱ+𷌤g!w!V ,wC\2I1.a-_eKE ]Dl+is#HnCfoqJ[?p H2aOŽv;2%զR|1S`WTS}Dy+1+KJEuy>4* 9el f*RiA%h!rk~1o͚i9#}&6\S~8.Cksi㳄@YzB7^.(X|D)=((`aͱ|$Q5&ʫ<}<m*?EMk_f\i3_%>𥍹o&ME*Pve`loelFk'UߖZc0krvQvƱrf~}ɫ {vN#eu=?Z;FCgfJƗ,Q󞓂-)gX/׊mZDR[޲G ZնGS4CES^]W Ҋ–su3 3߾:KL\:ڡ|v`Us-RCftV~͕3ۤy|1tw"tZ;}8 % teb4o%NT\Sp?3V@ F?n`p& sS^Wr';%̄^ B'dv)'ihvLca g]4YKف|fY>V|Fɍ[ ni-o簊㛵ká:No.*ըj.GfG\AR>/ tSuLͱvUo\?C{#I]ۭ]AG65o+jȆ}ٰm"w!%޸NX^<4u1nJTbs|1~8=V\a"%[DJ wan~QفS|%#ԓ59\:l+IC{}w3=_L̯EA:az?7?pU-:dp1p--t i lW2;:xkE2.@ϓܖ@;#du67[ ê,wxs`7nlݐr_2U4}WzȤbzÁZT(!2+TCιNu. a-sॊ5"=\*Ij7<1-w{OCH&2ؑ='.e؉D6fp7K!+{u ,5'11y JE!yIYe.FOHxi"sGBMvv'76>Gz91bJJ]_"ϟ[)O(zw\ayogAh$,)}9?)AW\„XZmUcڕ2/<^Z5Cv<֗Jwjj5CΊ. ߐ9TK~{{ K+g+{J%@b1ꞑiWp54$9|כrkUECJPBG)cW1'咪Ɂ5jt_.Y=E w o3͙dzd?ƤDn/R}K?Φف;&&7ɀgTB+Y~xi=)v9cO̤xx%2oPS2Ǜ5΋&mp#uPNxb9"a9iH6V={Ut誤Vb|̓@j J`|D#ehRF[M.1#0pz.jyg8(~|ռ5Mh7yG:`ecP`Iw}0˲ ==Z@?2zq܉Aes}+]?9"̢@ Z~*g8PGpLM}deT󻛾10wm<8dz~NVxuK3c(P1 DUUJ{h>9B[GC[^+HrP*EI1ƼЅ*5;Q kЇ ,`~HWL߶  Fˤ!b1'8(  -`w?Fq n՚UW#Nk098شhy!Dyʛ'RY<4*{F<6=s/k9k}eze ߕ5O1L,gUldI^p&vb֢h_%9\? ?#u"%  +QU;!vʂO''$Elc5&~GLP|e2 Pke:1R'aUv YA#ua+w;Ua Ȫv' bXރ͐*,Ef;Xfq>-0 <lBkh>(b6R9TƖ]MN{Aɼ2q\B!z݌J#o|m\p޺z}qAx뿼[©΄$Ho͚Z(F-#pOCa~71%i']>'1#AR>r8^ 3'%@22ԚHAcE V.bam 6şc0$!A=yK2->="H/Y喙qG8L_=[. BЂIBL9Ru zE_?_HP z jY ?iBcbmNHM}Uk}UtlR!Y3 h\n%51I.A(yQ6#&9蝄_A0@x!/=+;Ͳ:~N9*vIY] V0Sn+^yq8Q#X`q&Ek Rp6 wFwUˈ'8CjQe3-(T];1Ur_{l(΃,K粆3Aڴ_\w3;j+JNE:@t['e]8&b1ǫ1otp&yi<"oyA7= --rK}erXbir\>3 m;LG(K\ș dGt^]}!t w87WiYrr9\ͤ7A0|z[BI٥¾9x)ي.:! f q(I&eW e̔^&D'#iQH4&l-Qfnr: N'4_:4TZu'x"7jzֺ`j@`/U{|J[f@53_mKn PYV4{Qt1iqt)^Eyu[T9xk W(۪ll> } $tXCpVݕfSB7Dd]!½y vVԿXPǾ]gж ЉnX1+*Y`!,Oz2()@Ϗ;'c0`2r~XMN=|9Lt8o"f )EЅ3,Xw`iQb!_Fy`9e*bg \+3)tfX[!%a+hREPWo A7 22& >zct*ʡ.vh@HDRA-FF[>q^9yg+H^i֎ fo][[EJvzwA"Ni"{+l(#,ye'US++^Gk1鐢p*(*r??s+6 d FsQٳǕSր1*(<:J3:i"0gzmfF3E0)5-CVς*vJ&hPgCh=vxA._` Lò\} -6"/=OZ2$/%X5פ9l]k"PW [sM G:3eF3K|{"rӱ}%t 3ѽ';ܟ Ԯk|P+ 7Jzp&\좲IƳ>GljkhC"|ۼbFSb!S[dmU!J`v jׯsӠ-~KxQtq}& &0r3ci" ,kuG&W3~hMťC2+Fۓ*ֱvs[Heh]UhP%/Zgp: .N#|O*dZɓTm^4իM_2yyHMZ 27u)E,PU65õ٠9_gāF.J(Q,Gö BZi{y;* ^ c20 9ӁY)\"4hTǫ񬼨tT`KO>L{%Hu"q7HdCmχ٬ pXƃZ|XO?n?rf PuV}iJGHif ӳO^CFrc >Ί<-Bvсz]@(|1Pv:a=5E=M|IŶ@վl /,k.xOm!tA(WWA,X{0!RA&RP?Hl%Q]ov=dU)r7tpf ׶Mb $Pdͩ-L̂xL굳ـz19¥`,D=&uq.@6CfUfJֹWl'-t {]v?EbPO{#ZϰI0I:/&KH㗝-I7f:k\0(zU\0nO?YS(@v%zΪ48s0/$)yp:od["Q4pNF*F?Ǎy"ŷ QNdd|VVG:&NCԙ\lZ 1eXc3soʺV"T;aۙ&pٔ򷇓_zr.^<)\~4GxHnf:WYy}&{6)׎ԿA!@vm7"ʲe,\wt0$LsP7܎xݾdUഇwDs!OV }.r0iBU> Bnhn"N MozֆE8Li:EnQ铺rD:?VYިol$qV 8- Q}8qv 0Q+&12>*'p*[w=3/t}; -m5m+t< !6~!Y: -?`ygF#PO=Mj_<]*lnhØInQG Jե~aR֏':Q~0I"iҖџ}#~I^@$D?AofgT_ sϥycW-yEOj^fcLBY VT>M0]nY3z$gׁ[O<snxy̶^p`'+Vwh,Xfl5sP1=?t6wa}~(o;cIxfqNۑ™iV2}IH 0/{kW0AbYXcd'&9\v4c-E͠}.S[DsoL~+HLQU[ -mTZ% FOp^ -8kYNI:TDT0+:H*GU߮sRcOyz{3tx\2p%E_˞ +nde%N.fӫ^rMSM;~cnzzN9`$-&b{J^Vס􅸱}cA҄1)p bfi}&XQ ,7<`4$) 12q^8<9hyLXgDYo[]sv&8h%k,jpCjܶNOwMdF'EՉ<i;l)V&'߫lYЮ\9]ciQW!'%+́;2\ɰqqYx?߶Pv 5lY TV|Ŧҫ3$ ?w hgOVXA~$dBKۈ_~T6&tS>9pϿ/C1Yˣ4|ױ}!|ʱjށ|d9F+ϔhcs xSlH7ٹM6½-hE݌Y 7YQsL9Nf,; [\FΘK_ôֲk2>iV0+ܞi< _|)H^L/r1bcAg0 Ύ7 s3ʬs'͖Ɇ!ǵI|:gs`xr:ΟlaǼ:P IafGiSn#syn.^-0EhPOJQb1/= S=oׁAE'1W(LqjO1V{,|^ S endstream endobj 8 0 obj 47401 endobj 9 0 obj << /Length 10 0 R /Filter /FlateDecode >> stream x]j0E -E%vPM}д`KFqj9jhyEqrZ0<]#q 6ʏnNݜ)|:0eۭ?yWzSOR*j;`p:QXT4^;%t>.uU|^gRFJnt;G GʶEѪ0שּׁTSiQتMl뚹˾̕pck`nP30?F7`Caف7ҋ<). ͟ k7KYÿb7 Z3r_ J$ +H_7_{fa[J ?-}f%4.22'> endobj 5 0 obj << /Type /Font /Subtype /Type1 /BaseFont /NimbusRomanNo9L /FirstChar 0 /LastChar 36 /FontDescriptor 11 0 R /Widths [ 500 611 333 444 444 278 250 500 500 500 500 278 278 389 500 500 500 333 500 333 500 556 444 667 667 611 556 333 500 778 333 667 500 333 500 722 722 ] /ToUnicode 9 0 R >> endobj 1 0 obj << /Type /Pages /Kids [ 6 0 R ] /Count 1 >> endobj 12 0 obj << /Creator (cairo 1.8.0 (http://cairographics.org)) /Producer (cairo 1.8.0 (http://cairographics.org)) >> endobj 13 0 obj << /Type /Catalog /Pages 1 0 R >> endobj xref 0 14 0000000000 65535 f 0000051247 00000 n 0000002348 00000 n 0000000015 00000 n 0000002325 00000 n 0000050923 00000 n 0000002457 00000 n 0000002657 00000 n 0000050183 00000 n 0000050207 00000 n 0000050665 00000 n 0000050688 00000 n 0000051312 00000 n 0000051438 00000 n trailer << /Size 14 /Root 13 0 R /Info 12 0 R >> startxref 51491 %%EOF geometry-1.13.2/tf/include/000077500000000000000000000000001366756125100155015ustar00rootroot00000000000000geometry-1.13.2/tf/include/tf/000077500000000000000000000000001366756125100161125ustar00rootroot00000000000000geometry-1.13.2/tf/include/tf/LinearMath/000077500000000000000000000000001366756125100201365ustar00rootroot00000000000000geometry-1.13.2/tf/include/tf/LinearMath/Matrix3x3.h000066400000000000000000000514551366756125100221230ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 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. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF_MATRIX3x3_H #define TF_MATRIX3x3_H #include "Vector3.h" #include "Quaternion.h" #include namespace tf { #define Matrix3x3Data Matrix3x3DoubleData /**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. * Make sure to only include a pure orthogonal matrix without scaling. */ class Matrix3x3 { ///Data storage for the matrix, each vector is a row of the matrix Vector3 m_el[3]; public: /** @brief No initializaion constructor */ Matrix3x3 () {} // explicit Matrix3x3(const tfScalar *m) { setFromOpenGLSubMatrix(m); } /**@brief Constructor from Quaternion */ explicit Matrix3x3(const Quaternion& q) { setRotation(q); } /* template Matrix3x3(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) { setEulerYPR(yaw, pitch, roll); } */ /** @brief Constructor with row major formatting */ Matrix3x3(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz, const tfScalar& yx, const tfScalar& yy, const tfScalar& yz, const tfScalar& zx, const tfScalar& zy, const tfScalar& zz) { setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz); } /** @brief Copy constructor */ TFSIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other) { m_el[0] = other.m_el[0]; m_el[1] = other.m_el[1]; m_el[2] = other.m_el[2]; } /** @brief Assignment Operator */ TFSIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other) { m_el[0] = other.m_el[0]; m_el[1] = other.m_el[1]; m_el[2] = other.m_el[2]; return *this; } /** @brief Get a column of the matrix as a vector * @param i Column number 0 indexed */ TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const { return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]); } /** @brief Get a row of the matrix as a vector * @param i Row number 0 indexed */ TFSIMD_FORCE_INLINE const Vector3& getRow(int i) const { tfFullAssert(0 <= i && i < 3); return m_el[i]; } /** @brief Get a mutable reference to a row of the matrix as a vector * @param i Row number 0 indexed */ TFSIMD_FORCE_INLINE Vector3& operator[](int i) { tfFullAssert(0 <= i && i < 3); return m_el[i]; } /** @brief Get a const reference to a row of the matrix as a vector * @param i Row number 0 indexed */ TFSIMD_FORCE_INLINE const Vector3& operator[](int i) const { tfFullAssert(0 <= i && i < 3); return m_el[i]; } /** @brief Multiply by the target matrix on the right * @param m Rotation matrix to be applied * Equivilant to this = this * m */ Matrix3x3& operator*=(const Matrix3x3& m); /** @brief Set from a carray of tfScalars * @param m A pointer to the beginning of an array of 9 tfScalars */ void setFromOpenGLSubMatrix(const tfScalar *m) { m_el[0].setValue(m[0],m[4],m[8]); m_el[1].setValue(m[1],m[5],m[9]); m_el[2].setValue(m[2],m[6],m[10]); } /** @brief Set the values of the matrix explicitly (row major) * @param xx Top left * @param xy Top Middle * @param xz Top Right * @param yx Middle Left * @param yy Middle Middle * @param yz Middle Right * @param zx Bottom Left * @param zy Bottom Middle * @param zz Bottom Right*/ void setValue(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz, const tfScalar& yx, const tfScalar& yy, const tfScalar& yz, const tfScalar& zx, const tfScalar& zy, const tfScalar& zz) { m_el[0].setValue(xx,xy,xz); m_el[1].setValue(yx,yy,yz); m_el[2].setValue(zx,zy,zz); } /** @brief Set the matrix from a quaternion * @param q The Quaternion to match */ void setRotation(const Quaternion& q) { tfScalar d = q.length2(); tfFullAssert(d != tfScalar(0.0)); tfScalar s = tfScalar(2.0) / d; tfScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; tfScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; tfScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; tfScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; setValue(tfScalar(1.0) - (yy + zz), xy - wz, xz + wy, xy + wz, tfScalar(1.0) - (xx + zz), yz - wx, xz - wy, yz + wx, tfScalar(1.0) - (xx + yy)); } /** @brief Set the matrix from euler angles using YPR around ZYX respectively * @param yaw Yaw about Z axis * @param pitch Pitch about Y axis * @param roll Roll about X axis */ ROS_DEPRECATED void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) { setEulerYPR(yaw, pitch, roll); } /** @brief Set the matrix from euler angles YPR around ZYX axes * @param eulerZ Yaw aboud Z axis * @param eulerY Pitch around Y axis * @param eulerX Roll about X axis * * These angles are used to produce a rotation matrix. The euler * angles are applied in ZYX order. I.e a vector is first rotated * about X then Y and then Z **/ void setEulerYPR(tfScalar eulerZ, tfScalar eulerY,tfScalar eulerX) { tfScalar ci ( tfCos(eulerX)); tfScalar cj ( tfCos(eulerY)); tfScalar ch ( tfCos(eulerZ)); tfScalar si ( tfSin(eulerX)); tfScalar sj ( tfSin(eulerY)); tfScalar sh ( tfSin(eulerZ)); tfScalar cc = ci * ch; tfScalar cs = ci * sh; tfScalar sc = si * ch; tfScalar ss = si * sh; setValue(cj * ch, sj * sc - cs, sj * cc + ss, cj * sh, sj * ss + cc, sj * cs - sc, -sj, cj * si, cj * ci); } /** @brief Set the matrix using RPY about XYZ fixed axes * @param roll Roll about X axis * @param pitch Pitch around Y axis * @param yaw Yaw aboud Z axis * **/ void setRPY(tfScalar roll, tfScalar pitch,tfScalar yaw) { setEulerYPR(yaw, pitch, roll); } /**@brief Set the matrix to the identity */ void setIdentity() { setValue(tfScalar(1.0), tfScalar(0.0), tfScalar(0.0), tfScalar(0.0), tfScalar(1.0), tfScalar(0.0), tfScalar(0.0), tfScalar(0.0), tfScalar(1.0)); } static const Matrix3x3& getIdentity() { static const Matrix3x3 identityMatrix(tfScalar(1.0), tfScalar(0.0), tfScalar(0.0), tfScalar(0.0), tfScalar(1.0), tfScalar(0.0), tfScalar(0.0), tfScalar(0.0), tfScalar(1.0)); return identityMatrix; } /**@brief Fill the values of the matrix into a 9 element array * @param m The array to be filled */ void getOpenGLSubMatrix(tfScalar *m) const { m[0] = tfScalar(m_el[0].x()); m[1] = tfScalar(m_el[1].x()); m[2] = tfScalar(m_el[2].x()); m[3] = tfScalar(0.0); m[4] = tfScalar(m_el[0].y()); m[5] = tfScalar(m_el[1].y()); m[6] = tfScalar(m_el[2].y()); m[7] = tfScalar(0.0); m[8] = tfScalar(m_el[0].z()); m[9] = tfScalar(m_el[1].z()); m[10] = tfScalar(m_el[2].z()); m[11] = tfScalar(0.0); } /**@brief Get the matrix represented as a quaternion * @param q The quaternion which will be set */ void getRotation(Quaternion& q) const { tfScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); tfScalar temp[4]; if (trace > tfScalar(0.0)) { tfScalar s = tfSqrt(trace + tfScalar(1.0)); temp[3]=(s * tfScalar(0.5)); s = tfScalar(0.5) / s; temp[0]=((m_el[2].y() - m_el[1].z()) * s); temp[1]=((m_el[0].z() - m_el[2].x()) * s); temp[2]=((m_el[1].x() - m_el[0].y()) * s); } else { int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; tfScalar s = tfSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tfScalar(1.0)); temp[i] = s * tfScalar(0.5); s = tfScalar(0.5) / s; temp[3] = (m_el[k][j] - m_el[j][k]) * s; temp[j] = (m_el[j][i] + m_el[i][j]) * s; temp[k] = (m_el[k][i] + m_el[i][k]) * s; } q.setValue(temp[0],temp[1],temp[2],temp[3]); } /**@brief Get the matrix represented as euler angles around ZYX * @param yaw Yaw around Z axis * @param pitch Pitch around Y axis * @param roll around X axis * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ ROS_DEPRECATED void getEulerZYX(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const { getEulerYPR(yaw, pitch, roll, solution_number); }; /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR * @param yaw Yaw around Z axis * @param pitch Pitch around Y axis * @param roll around X axis */ void getEulerYPR(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const { struct Euler { tfScalar yaw; tfScalar pitch; tfScalar roll; }; Euler euler_out; Euler euler_out2; //second solution //get the pointer to the raw data // Check that pitch is not at a singularity // Check that pitch is not at a singularity if (tfFabs(m_el[2].x()) >= 1) { euler_out.yaw = 0; euler_out2.yaw = 0; // From difference of angles formula if (m_el[2].x() < 0) //gimbal locked down { tfScalar delta = tfAtan2(m_el[0].y(),m_el[0].z()); euler_out.pitch = TFSIMD_PI / tfScalar(2.0); euler_out2.pitch = TFSIMD_PI / tfScalar(2.0); euler_out.roll = delta; euler_out2.roll = delta; } else // gimbal locked up { tfScalar delta = tfAtan2(-m_el[0].y(),-m_el[0].z()); euler_out.pitch = -TFSIMD_PI / tfScalar(2.0); euler_out2.pitch = -TFSIMD_PI / tfScalar(2.0); euler_out.roll = delta; euler_out2.roll = delta; } } else { euler_out.pitch = - tfAsin(m_el[2].x()); euler_out2.pitch = TFSIMD_PI - euler_out.pitch; euler_out.roll = tfAtan2(m_el[2].y()/tfCos(euler_out.pitch), m_el[2].z()/tfCos(euler_out.pitch)); euler_out2.roll = tfAtan2(m_el[2].y()/tfCos(euler_out2.pitch), m_el[2].z()/tfCos(euler_out2.pitch)); euler_out.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out.pitch), m_el[0].x()/tfCos(euler_out.pitch)); euler_out2.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out2.pitch), m_el[0].x()/tfCos(euler_out2.pitch)); } if (solution_number == 1) { yaw = euler_out.yaw; pitch = euler_out.pitch; roll = euler_out.roll; } else { yaw = euler_out2.yaw; pitch = euler_out2.pitch; roll = euler_out2.roll; } } /**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ * @param roll around X axis * @param pitch Pitch around Y axis * @param yaw Yaw around Z axis * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ void getRPY(tfScalar& roll, tfScalar& pitch, tfScalar& yaw, unsigned int solution_number = 1) const { getEulerYPR(yaw, pitch, roll, solution_number); } /**@brief Create a scaled copy of the matrix * @param s Scaling vector The elements of the vector will scale each column */ Matrix3x3 scaled(const Vector3& s) const { return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); } /**@brief Return the determinant of the matrix */ tfScalar determinant() const; /**@brief Return the adjoint of the matrix */ Matrix3x3 adjoint() const; /**@brief Return the matrix with all values non negative */ Matrix3x3 absolute() const; /**@brief Return the transpose of the matrix */ Matrix3x3 transpose() const; /**@brief Return the inverse of the matrix */ Matrix3x3 inverse() const; Matrix3x3 transposeTimes(const Matrix3x3& m) const; Matrix3x3 timesTranspose(const Matrix3x3& m) const; TFSIMD_FORCE_INLINE tfScalar tdotx(const Vector3& v) const { return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); } TFSIMD_FORCE_INLINE tfScalar tdoty(const Vector3& v) const { return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); } TFSIMD_FORCE_INLINE tfScalar tdotz(const Vector3& v) const { return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); } /**@brief diagonalizes this matrix by the Jacobi method. * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original * coordinate system, i.e., old_this = rot * new_this * rot^T. * @param threshold See iteration * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. * * Note that this matrix is assumed to be symmetric. */ void diagonalize(Matrix3x3& rot, tfScalar threshold, int maxSteps) { rot.setIdentity(); for (int step = maxSteps; step > 0; step--) { // find off-diagonal element [p][q] with largest magnitude int p = 0; int q = 1; int r = 2; tfScalar max = tfFabs(m_el[0][1]); tfScalar v = tfFabs(m_el[0][2]); if (v > max) { q = 2; r = 1; max = v; } v = tfFabs(m_el[1][2]); if (v > max) { p = 1; q = 2; r = 0; max = v; } tfScalar t = threshold * (tfFabs(m_el[0][0]) + tfFabs(m_el[1][1]) + tfFabs(m_el[2][2])); if (max <= t) { if (max <= TFSIMD_EPSILON * t) { return; } step = 1; } // compute Jacobi rotation J which leads to a zero for element [p][q] tfScalar mpq = m_el[p][q]; tfScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); tfScalar theta2 = theta * theta; tfScalar cos; tfScalar sin; if (theta2 * theta2 < tfScalar(10 / TFSIMD_EPSILON)) { t = (theta >= 0) ? 1 / (theta + tfSqrt(1 + theta2)) : 1 / (theta - tfSqrt(1 + theta2)); cos = 1 / tfSqrt(1 + t * t); sin = cos * t; } else { // approximation for large theta-value, i.e., a nearly diagonal matrix t = 1 / (theta * (2 + tfScalar(0.5) / theta2)); cos = 1 - tfScalar(0.5) * t * t; sin = cos * t; } // apply rotation to matrix (this = J^T * this * J) m_el[p][q] = m_el[q][p] = 0; m_el[p][p] -= t * mpq; m_el[q][q] += t * mpq; tfScalar mrp = m_el[r][p]; tfScalar mrq = m_el[r][q]; m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; // apply rotation to rot (rot = rot * J) for (int i = 0; i < 3; i++) { Vector3& row = rot[i]; mrp = row[p]; mrq = row[q]; row[p] = cos * mrp - sin * mrq; row[q] = cos * mrq + sin * mrp; } } } /**@brief Calculate the matrix cofactor * @param r1 The first row to use for calculating the cofactor * @param c1 The first column to use for calculating the cofactor * @param r1 The second row to use for calculating the cofactor * @param c1 The second column to use for calculating the cofactor * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details */ tfScalar cofac(int r1, int c1, int r2, int c2) const { return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; } void serialize(struct Matrix3x3Data& dataOut) const; void serializeFloat(struct Matrix3x3FloatData& dataOut) const; void deSerialize(const struct Matrix3x3Data& dataIn); void deSerializeFloat(const struct Matrix3x3FloatData& dataIn); void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn); }; TFSIMD_FORCE_INLINE Matrix3x3& Matrix3x3::operator*=(const Matrix3x3& m) { setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); return *this; } TFSIMD_FORCE_INLINE tfScalar Matrix3x3::determinant() const { return tfTriple((*this)[0], (*this)[1], (*this)[2]); } TFSIMD_FORCE_INLINE Matrix3x3 Matrix3x3::absolute() const { return Matrix3x3( tfFabs(m_el[0].x()), tfFabs(m_el[0].y()), tfFabs(m_el[0].z()), tfFabs(m_el[1].x()), tfFabs(m_el[1].y()), tfFabs(m_el[1].z()), tfFabs(m_el[2].x()), tfFabs(m_el[2].y()), tfFabs(m_el[2].z())); } TFSIMD_FORCE_INLINE Matrix3x3 Matrix3x3::transpose() const { return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), m_el[0].y(), m_el[1].y(), m_el[2].y(), m_el[0].z(), m_el[1].z(), m_el[2].z()); } TFSIMD_FORCE_INLINE Matrix3x3 Matrix3x3::adjoint() const { return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); } TFSIMD_FORCE_INLINE Matrix3x3 Matrix3x3::inverse() const { Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); tfScalar det = (*this)[0].dot(co); tfFullAssert(det != tfScalar(0.0)); tfScalar s = tfScalar(1.0) / det; return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); } TFSIMD_FORCE_INLINE Matrix3x3 Matrix3x3::transposeTimes(const Matrix3x3& m) const { return Matrix3x3( m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); } TFSIMD_FORCE_INLINE Matrix3x3 Matrix3x3::timesTranspose(const Matrix3x3& m) const { return Matrix3x3( m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); } TFSIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3& m, const Vector3& v) { return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); } TFSIMD_FORCE_INLINE Vector3 operator*(const Vector3& v, const Matrix3x3& m) { return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); } TFSIMD_FORCE_INLINE Matrix3x3 operator*(const Matrix3x3& m1, const Matrix3x3& m2) { return Matrix3x3( m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); } /* TFSIMD_FORCE_INLINE Matrix3x3 tfMultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) { return Matrix3x3( m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); } */ /**@brief Equality operator between two matrices * It will test all elements are equal. */ TFSIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2) { return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); } ///for serialization struct Matrix3x3FloatData { Vector3FloatData m_el[3]; }; ///for serialization struct Matrix3x3DoubleData { Vector3DoubleData m_el[3]; }; TFSIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const { for (int i=0;i<3;i++) m_el[i].serialize(dataOut.m_el[i]); } TFSIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const { for (int i=0;i<3;i++) m_el[i].serializeFloat(dataOut.m_el[i]); } TFSIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn) { for (int i=0;i<3;i++) m_el[i].deSerialize(dataIn.m_el[i]); } TFSIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn) { for (int i=0;i<3;i++) m_el[i].deSerializeFloat(dataIn.m_el[i]); } TFSIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn) { for (int i=0;i<3;i++) m_el[i].deSerializeDouble(dataIn.m_el[i]); } } #endif //TF_MATRIX3x3_H geometry-1.13.2/tf/include/tf/LinearMath/MinMax.h000066400000000000000000000025671366756125100215120ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 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. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF_MINMAX_H #define TF_MINMAX_H template TFSIMD_FORCE_INLINE const T& tfMin(const T& a, const T& b) { return a < b ? a : b ; } template TFSIMD_FORCE_INLINE const T& tfMax(const T& a, const T& b) { return a > b ? a : b; } template TFSIMD_FORCE_INLINE void tfSetMin(T& a, const T& b) { if (b < a) { a = b; } } template TFSIMD_FORCE_INLINE void tfSetMax(T& a, const T& b) { if (a < b) { a = b; } } #endif geometry-1.13.2/tf/include/tf/LinearMath/QuadWord.h000066400000000000000000000135671366756125100220510ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 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. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF_QUADWORD_H #define TF_QUADWORD_H #include "Scalar.h" #include "MinMax.h" #if defined (__CELLOS_LV2) && defined (__SPU__) #include #endif namespace tf { /**@brief The QuadWord class is base class for Vector3 and Quaternion. * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. */ #ifndef USE_LIBSPE2 ATTRIBUTE_ALIGNED16(class) QuadWord #else class QuadWord #endif { protected: #if defined (__SPU__) && defined (__CELLOS_LV2__) union { vec_float4 mVec128; tfScalar m_floats[4]; }; public: vec_float4 get128() const { return mVec128; } protected: #else //__CELLOS_LV2__ __SPU__ tfScalar m_floats[4]; #endif //__CELLOS_LV2__ __SPU__ public: /**@brief Return the x value */ TFSIMD_FORCE_INLINE const tfScalar& getX() const { return m_floats[0]; } /**@brief Return the y value */ TFSIMD_FORCE_INLINE const tfScalar& getY() const { return m_floats[1]; } /**@brief Return the z value */ TFSIMD_FORCE_INLINE const tfScalar& getZ() const { return m_floats[2]; } /**@brief Set the x value */ TFSIMD_FORCE_INLINE void setX(tfScalar x) { m_floats[0] = x;}; /**@brief Set the y value */ TFSIMD_FORCE_INLINE void setY(tfScalar y) { m_floats[1] = y;}; /**@brief Set the z value */ TFSIMD_FORCE_INLINE void setZ(tfScalar z) { m_floats[2] = z;}; /**@brief Set the w value */ TFSIMD_FORCE_INLINE void setW(tfScalar w) { m_floats[3] = w;}; /**@brief Return the x value */ TFSIMD_FORCE_INLINE const tfScalar& x() const { return m_floats[0]; } /**@brief Return the y value */ TFSIMD_FORCE_INLINE const tfScalar& y() const { return m_floats[1]; } /**@brief Return the z value */ TFSIMD_FORCE_INLINE const tfScalar& z() const { return m_floats[2]; } /**@brief Return the w value */ TFSIMD_FORCE_INLINE const tfScalar& w() const { return m_floats[3]; } //TFSIMD_FORCE_INLINE tfScalar& operator[](int i) { return (&m_floats[0])[i]; } //TFSIMD_FORCE_INLINE const tfScalar& operator[](int i) const { return (&m_floats[0])[i]; } ///operator tfScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. TFSIMD_FORCE_INLINE operator tfScalar *() { return &m_floats[0]; } TFSIMD_FORCE_INLINE operator const tfScalar *() const { return &m_floats[0]; } TFSIMD_FORCE_INLINE bool operator==(const QuadWord& other) const { 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])); } TFSIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const { return !(*this == other); } /**@brief Set x,y,z and zero w * @param x Value of x * @param y Value of y * @param z Value of z */ TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z) { m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; m_floats[3] = 0.f; } /* void getValue(tfScalar *m) const { m[0] = m_floats[0]; m[1] = m_floats[1]; m[2] = m_floats[2]; } */ /**@brief Set the values * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) { m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; m_floats[3]=w; } /**@brief No initialization constructor */ TFSIMD_FORCE_INLINE QuadWord() // :m_floats[0](tfScalar(0.)),m_floats[1](tfScalar(0.)),m_floats[2](tfScalar(0.)),m_floats[3](tfScalar(0.)) { } /**@brief Three argument constructor (zeros w) * @param x Value of x * @param y Value of y * @param z Value of z */ TFSIMD_FORCE_INLINE QuadWord(const tfScalar& x, const tfScalar& y, const tfScalar& z) { m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; } /**@brief Initializing constructor * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ TFSIMD_FORCE_INLINE QuadWord(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) { m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; } /**@brief Set each element to the max of the current values and the values of another QuadWord * @param other The other QuadWord to compare with */ TFSIMD_FORCE_INLINE void setMax(const QuadWord& other) { tfSetMax(m_floats[0], other.m_floats[0]); tfSetMax(m_floats[1], other.m_floats[1]); tfSetMax(m_floats[2], other.m_floats[2]); tfSetMax(m_floats[3], other.m_floats[3]); } /**@brief Set each element to the min of the current values and the values of another QuadWord * @param other The other QuadWord to compare with */ TFSIMD_FORCE_INLINE void setMin(const QuadWord& other) { tfSetMin(m_floats[0], other.m_floats[0]); tfSetMin(m_floats[1], other.m_floats[1]); tfSetMin(m_floats[2], other.m_floats[2]); tfSetMin(m_floats[3], other.m_floats[3]); } }; } #endif //TFSIMD_QUADWORD_H geometry-1.13.2/tf/include/tf/LinearMath/Quaternion.h000066400000000000000000000371271366756125100224460ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 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. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF_QUATERNION_H_ #define TF_QUATERNION_H_ #include "Vector3.h" #include "QuadWord.h" #include namespace tf { /**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */ class Quaternion : public QuadWord { public: /**@brief No initialization constructor */ Quaternion() {} // template // explicit Quaternion(const tfScalar *v) : Tuple4(v) {} /**@brief Constructor from scalars */ Quaternion(const tfScalar& x, const tfScalar& y, const tfScalar& z, const tfScalar& w) : QuadWord(x, y, z, w) {} /**@brief Axis angle Constructor * @param axis The axis which the rotation is around * @param angle The magnitude of the rotation around the angle (Radians) */ Quaternion(const Vector3& axis, const tfScalar& angle) { setRotation(axis, angle); } /**@brief Constructor from Euler angles * @param yaw Angle around Y unless TF_EULER_DEFAULT_ZYX defined then Z * @param pitch Angle around X unless TF_EULER_DEFAULT_ZYX defined then Y * @param roll Angle around Z unless TF_EULER_DEFAULT_ZYX defined then X */ ROS_DEPRECATED Quaternion(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) { #ifndef TF_EULER_DEFAULT_ZYX setEuler(yaw, pitch, roll); #else setRPY(roll, pitch, yaw); #endif } /**@brief Set the rotation using axis angle notation * @param axis The axis around which to rotate * @param angle The magnitude of the rotation in Radians */ void setRotation(const Vector3& axis, const tfScalar& angle) { tfScalar d = axis.length(); tfAssert(d != tfScalar(0.0)); tfScalar s = tfSin(angle * tfScalar(0.5)) / d; setValue(axis.x() * s, axis.y() * s, axis.z() * s, tfCos(angle * tfScalar(0.5))); } /**@brief Set the quaternion using Euler angles * @param yaw Angle around Y * @param pitch Angle around X * @param roll Angle around Z */ void setEuler(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) { tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5); tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5); tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5); tfScalar cosYaw = tfCos(halfYaw); tfScalar sinYaw = tfSin(halfYaw); tfScalar cosPitch = tfCos(halfPitch); tfScalar sinPitch = tfSin(halfPitch); tfScalar cosRoll = tfCos(halfRoll); tfScalar sinRoll = tfSin(halfRoll); setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); } /**@brief Set the quaternion using fixed axis RPY * @param roll Angle around X * @param pitch Angle around Y * @param yaw Angle around Z*/ void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw) { tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5); tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5); tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5); tfScalar cosYaw = tfCos(halfYaw); tfScalar sinYaw = tfSin(halfYaw); tfScalar cosPitch = tfCos(halfPitch); tfScalar sinPitch = tfSin(halfPitch); tfScalar cosRoll = tfCos(halfRoll); tfScalar sinRoll = tfSin(halfRoll); setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx } /**@brief Set the quaternion using euler angles * @param yaw Angle around Z * @param pitch Angle around Y * @param roll Angle around X */ ROS_DEPRECATED void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) { setRPY(roll, pitch, yaw); } /**@brief Add two quaternions * @param q The quaternion to add to this one */ TFSIMD_FORCE_INLINE Quaternion& operator+=(const Quaternion& q) { m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; return *this; } /**@brief Sutfract out a quaternion * @param q The quaternion to sutfract from this one */ Quaternion& operator-=(const Quaternion& q) { m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; return *this; } /**@brief Scale this quaternion * @param s The scalar to scale by */ Quaternion& operator*=(const tfScalar& s) { m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; return *this; } /**@brief Multiply this quaternion by q on the right * @param q The other quaternion * Equivilant to this = this * q */ Quaternion& operator*=(const Quaternion& q) { setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); return *this; } /**@brief Return the dot product between this quaternion and another * @param q The other quaternion */ tfScalar dot(const Quaternion& q) const { return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; } /**@brief Return the length squared of the quaternion */ tfScalar length2() const { return dot(*this); } /**@brief Return the length of the quaternion */ tfScalar length() const { return tfSqrt(length2()); } /**@brief Normalize the quaternion * Such that x^2 + y^2 + z^2 +w^2 = 1 */ Quaternion& normalize() { return *this /= length(); } /**@brief Return a scaled version of this quaternion * @param s The scale factor */ TFSIMD_FORCE_INLINE Quaternion operator*(const tfScalar& s) const { return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s); } /**@brief Return an inversely scaled versionof this quaternion * @param s The inverse scale factor */ Quaternion operator/(const tfScalar& s) const { tfAssert(s != tfScalar(0.0)); return *this * (tfScalar(1.0) / s); } /**@brief Inversely scale this quaternion * @param s The scale factor */ Quaternion& operator/=(const tfScalar& s) { tfAssert(s != tfScalar(0.0)); return *this *= tfScalar(1.0) / s; } /**@brief Return a normalized version of this quaternion */ Quaternion normalized() const { return *this / length(); } /**@brief Return the ***half*** angle between this quaternion and the other * @param q The other quaternion */ tfScalar angle(const Quaternion& q) const { tfScalar s = tfSqrt(length2() * q.length2()); tfAssert(s != tfScalar(0.0)); return tfAcos(dot(q) / s); } /**@brief Return the angle between this quaternion and the other along the shortest path * @param q The other quaternion */ tfScalar angleShortestPath(const Quaternion& q) const { tfScalar s = tfSqrt(length2() * q.length2()); tfAssert(s != tfScalar(0.0)); if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp return tfAcos(dot(-q) / s) * tfScalar(2.0); else return tfAcos(dot(q) / s) * tfScalar(2.0); } /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */ tfScalar getAngle() const { tfScalar s = tfScalar(2.) * tfAcos(m_floats[3]); return s; } /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path*/ tfScalar getAngleShortestPath() const { tfScalar s; if (m_floats[3] < 0) s = tfScalar(2.) * tfAcos(-m_floats[3]); else s = tfScalar(2.) * tfAcos(m_floats[3]); return s; } /**@brief Return the axis of the rotation represented by this quaternion */ Vector3 getAxis() const { tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.)); if (s_squared < tfScalar(10.) * TFSIMD_EPSILON) //Check for divide by zero return Vector3(1.0, 0.0, 0.0); // Arbitrary tfScalar s = tfSqrt(s_squared); return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); } /**@brief Return the inverse of this quaternion */ Quaternion inverse() const { return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); } /**@brief Return the sum of this quaternion and the other * @param q2 The other quaternion */ TFSIMD_FORCE_INLINE Quaternion operator+(const Quaternion& q2) const { const Quaternion& q1 = *this; return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); } /**@brief Return the difference between this quaternion and the other * @param q2 The other quaternion */ TFSIMD_FORCE_INLINE Quaternion operator-(const Quaternion& q2) const { const Quaternion& q1 = *this; return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); } /**@brief Return the negative of this quaternion * This simply negates each element */ TFSIMD_FORCE_INLINE Quaternion operator-() const { const Quaternion& q2 = *this; return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); } /**@todo document this and it's use */ TFSIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const { Quaternion diff,sum; diff = *this - qd; sum = *this + qd; if( diff.dot(diff) > sum.dot(sum) ) return qd; return (-qd); } /**@todo document this and it's use */ TFSIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const { Quaternion diff,sum; diff = *this - qd; sum = *this + qd; if( diff.dot(diff) < sum.dot(sum) ) return qd; return (-qd); } /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion * @param q The other quaternion to interpolate with * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. * Slerp interpolates assuming constant velocity. */ Quaternion slerp(const Quaternion& q, const tfScalar& t) const { tfScalar theta = angleShortestPath(q) / tfScalar(2.0); if (theta != tfScalar(0.0)) { tfScalar d = tfScalar(1.0) / tfSin(theta); tfScalar s0 = tfSin((tfScalar(1.0) - t) * theta); tfScalar s1 = tfSin(t * theta); if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d, (m_floats[1] * s0 + -q.y() * s1) * d, (m_floats[2] * s0 + -q.z() * s1) * d, (m_floats[3] * s0 + -q.m_floats[3] * s1) * d); else return Quaternion((m_floats[0] * s0 + q.x() * s1) * d, (m_floats[1] * s0 + q.y() * s1) * d, (m_floats[2] * s0 + q.z() * s1) * d, (m_floats[3] * s0 + q.m_floats[3] * s1) * d); } else { return *this; } } static const Quaternion& getIdentity() { static const Quaternion identityQuat(tfScalar(0.),tfScalar(0.),tfScalar(0.),tfScalar(1.)); return identityQuat; } TFSIMD_FORCE_INLINE const tfScalar& getW() const { return m_floats[3]; } }; /**@brief Return the negative of a quaternion */ TFSIMD_FORCE_INLINE Quaternion operator-(const Quaternion& q) { return Quaternion(-q.x(), -q.y(), -q.z(), -q.w()); } /**@brief Return the product of two quaternions */ TFSIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q1, const Quaternion& q2) { return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); } TFSIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q, const Vector3& w) { return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); } TFSIMD_FORCE_INLINE Quaternion operator*(const Vector3& w, const Quaternion& q) { return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); } /**@brief Calculate the dot product between two quaternions */ TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion& q1, const Quaternion& q2) { return q1.dot(q2); } /**@brief Return the length of a quaternion */ TFSIMD_FORCE_INLINE tfScalar length(const Quaternion& q) { return q.length(); } /**@brief Return the ***half*** angle between two quaternions*/ TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion& q1, const Quaternion& q2) { return q1.angle(q2); } /**@brief Return the shortest angle between two quaternions*/ TFSIMD_FORCE_INLINE tfScalar angleShortestPath(const Quaternion& q1, const Quaternion& q2) { return q1.angleShortestPath(q2); } /**@brief Return the inverse of a quaternion*/ TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion& q) { return q.inverse(); } /**@brief Return the result of spherical linear interpolation betwen two quaternions * @param q1 The first quaternion * @param q2 The second quaternion * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 * Slerp assumes constant velocity between positions. */ TFSIMD_FORCE_INLINE Quaternion slerp(const Quaternion& q1, const Quaternion& q2, const tfScalar& t) { return q1.slerp(q2, t); } TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion& rotation, const Vector3& v) { Quaternion q = rotation * v; q *= rotation.inverse(); return Vector3(q.getX(),q.getY(),q.getZ()); } TFSIMD_FORCE_INLINE Quaternion shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized { Vector3 c = v0.cross(v1); tfScalar d = v0.dot(v1); if (d < -1.0 + TFSIMD_EPSILON) { Vector3 n,unused; tfPlaneSpace1(v0,n,unused); return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 } tfScalar s = tfSqrt((1.0f + d) * 2.0f); tfScalar rs = 1.0f / s; return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); } TFSIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3& v0,Vector3& v1) { v0.normalize(); v1.normalize(); return shortestArcQuat(v0,v1); } } #endif geometry-1.13.2/tf/include/tf/LinearMath/Scalar.h000066400000000000000000000316131366756125100215200ustar00rootroot00000000000000/* Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 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. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF_SCALAR_H #define TF_SCALAR_H #ifdef TF_MANAGED_CODE //Aligned data types not supported in managed code #pragma unmanaged #endif #include #include //size_t for MSVC 6.0 #include #include #include #if defined(DEBUG) || defined (_DEBUG) #define TF_DEBUG #endif #ifdef _WIN32 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) #define TFSIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #else //#define TF_HAS_ALIGNED_ALLOCATOR #pragma warning(disable : 4324) // disable padding warning // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. // #pragma warning(disable:4996) //Turn off warnings about deprecated C routines // #pragma warning(disable:4786) // Disable the "debug name too long" warning #define TFSIMD_FORCE_INLINE __forceinline #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a #ifdef _XBOX #define TF_USE_VMX128 #include #define TF_HAVE_NATIVE_FSEL #define tfFsel(a,b,c) __fsel((a),(b),(c)) #else #endif//_XBOX #endif //__MINGW32__ #include #ifdef TF_DEBUG #define tfAssert assert #else #define tfAssert(x) #endif //tfFullAssert is optional, slows down a lot #define tfFullAssert(x) #define tfLikely(_c) _c #define tfUnlikely(_c) _c #else #if defined (__CELLOS_LV2__) #define TFSIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include #endif #ifdef TF_DEBUG #define tfAssert assert #else #define tfAssert(x) #endif //tfFullAssert is optional, slows down a lot #define tfFullAssert(x) #define tfLikely(_c) _c #define tfUnlikely(_c) _c #else #ifdef USE_LIBSPE2 #define TFSIMD_FORCE_INLINE __inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include #endif #ifdef TF_DEBUG #define tfAssert assert #else #define tfAssert(x) #endif //tfFullAssert is optional, slows down a lot #define tfFullAssert(x) #define tfLikely(_c) __builtin_expect((_c), 1) #define tfUnlikely(_c) __builtin_expect((_c), 0) #else //non-windows systems #define TFSIMD_FORCE_INLINE inline ///@todo: check out alignment methods for other platforms/compilers ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #ifndef assert #include #endif #if defined(DEBUG) || defined (_DEBUG) #define tfAssert assert #else #define tfAssert(x) #endif //tfFullAssert is optional, slows down a lot #define tfFullAssert(x) #define tfLikely(_c) _c #define tfUnlikely(_c) _c #endif // LIBSPE2 #endif //__CELLOS_LV2__ #endif ///The tfScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. typedef double tfScalar; //this number could be bigger in double precision #define TF_LARGE_FLOAT 1e30 #define TF_DECLARE_ALIGNED_ALLOCATOR() \ TFSIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return tfAlignedAlloc(sizeInBytes,16); } \ TFSIMD_FORCE_INLINE void operator delete(void* ptr) { tfAlignedFree(ptr); } \ TFSIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ TFSIMD_FORCE_INLINE void operator delete(void*, void*) { } \ TFSIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return tfAlignedAlloc(sizeInBytes,16); } \ TFSIMD_FORCE_INLINE void operator delete[](void* ptr) { tfAlignedFree(ptr); } \ TFSIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ TFSIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ TFSIMD_FORCE_INLINE tfScalar tfSqrt(tfScalar x) { return sqrt(x); } TFSIMD_FORCE_INLINE tfScalar tfFabs(tfScalar x) { return fabs(x); } TFSIMD_FORCE_INLINE tfScalar tfCos(tfScalar x) { return cos(x); } TFSIMD_FORCE_INLINE tfScalar tfSin(tfScalar x) { return sin(x); } TFSIMD_FORCE_INLINE tfScalar tfTan(tfScalar x) { return tan(x); } TFSIMD_FORCE_INLINE tfScalar tfAcos(tfScalar x) { if (xtfScalar(1)) x=tfScalar(1); return acos(x); } TFSIMD_FORCE_INLINE tfScalar tfAsin(tfScalar x) { if (xtfScalar(1)) x=tfScalar(1); return asin(x); } TFSIMD_FORCE_INLINE tfScalar tfAtan(tfScalar x) { return atan(x); } TFSIMD_FORCE_INLINE tfScalar tfAtan2(tfScalar x, tfScalar y) { return atan2(x, y); } TFSIMD_FORCE_INLINE tfScalar tfExp(tfScalar x) { return exp(x); } TFSIMD_FORCE_INLINE tfScalar tfLog(tfScalar x) { return log(x); } TFSIMD_FORCE_INLINE tfScalar tfPow(tfScalar x,tfScalar y) { return pow(x,y); } TFSIMD_FORCE_INLINE tfScalar tfFmod(tfScalar x,tfScalar y) { return fmod(x,y); } #define TFSIMD_2_PI tfScalar(6.283185307179586232) #define TFSIMD_PI (TFSIMD_2_PI * tfScalar(0.5)) #define TFSIMD_HALF_PI (TFSIMD_2_PI * tfScalar(0.25)) #define TFSIMD_RADS_PER_DEG (TFSIMD_2_PI / tfScalar(360.0)) #define TFSIMD_DEGS_PER_RAD (tfScalar(360.0) / TFSIMD_2_PI) #define TFSIMDSQRT12 tfScalar(0.7071067811865475244008443621048490) #define tfRecipSqrt(x) ((tfScalar)(tfScalar(1.0)/tfSqrt(tfScalar(x)))) /* reciprocal square root */ #define TFSIMD_EPSILON DBL_EPSILON #define TFSIMD_INFINITY DBL_MAX TFSIMD_FORCE_INLINE tfScalar tfAtan2Fast(tfScalar y, tfScalar x) { tfScalar coeff_1 = TFSIMD_PI / 4.0f; tfScalar coeff_2 = 3.0f * coeff_1; tfScalar abs_y = tfFabs(y); tfScalar angle; if (x >= 0.0f) { tfScalar r = (x - abs_y) / (x + abs_y); angle = coeff_1 - coeff_1 * r; } else { tfScalar r = (x + abs_y) / (abs_y - x); angle = coeff_2 - coeff_1 * r; } return (y < 0.0f) ? -angle : angle; } TFSIMD_FORCE_INLINE bool tfFuzzyZero(tfScalar x) { return tfFabs(x) < TFSIMD_EPSILON; } TFSIMD_FORCE_INLINE bool tfEqual(tfScalar a, tfScalar eps) { return (((a) <= eps) && !((a) < -eps)); } TFSIMD_FORCE_INLINE bool tfGreaterEqual (tfScalar a, tfScalar eps) { return (!((a) <= eps)); } TFSIMD_FORCE_INLINE int tfIsNegative(tfScalar x) { return x < tfScalar(0.0) ? 1 : 0; } TFSIMD_FORCE_INLINE tfScalar tfRadians(tfScalar x) { return x * TFSIMD_RADS_PER_DEG; } TFSIMD_FORCE_INLINE tfScalar tfDegrees(tfScalar x) { return x * TFSIMD_DEGS_PER_RAD; } #define TF_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name #ifndef tfFsel TFSIMD_FORCE_INLINE tfScalar tfFsel(tfScalar a, tfScalar b, tfScalar c) { return a >= 0 ? b : c; } #endif #define tfFsels(a,b,c) (tfScalar)tfFsel(a,b,c) TFSIMD_FORCE_INLINE bool tfMachineIsLittleEndian() { long int i = 1; const char *p = (const char *) &i; if (p[0] == 1) // Lowest address contains the least significant byte return true; else return false; } ///tfSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html TFSIMD_FORCE_INLINE unsigned tfSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) { // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero // Rely on positive value or'ed with its negative having sign bit on // and zero value or'ed with its negative (which is still zero) having sign bit off // Use arithmetic shift right, shifting the sign bit through all 32 bits unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); unsigned testEqz = ~testNz; return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); } TFSIMD_FORCE_INLINE int tfSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) { unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); unsigned testEqz = ~testNz; return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); } TFSIMD_FORCE_INLINE float tfSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) { #ifdef TF_HAVE_NATIVE_FSEL return (float)tfFsel((tfScalar)condition - tfScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); #else return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; #endif } template TFSIMD_FORCE_INLINE void tfSwap(T& a, T& b) { T tmp = a; a = b; b = tmp; } //PCK: endian swapping functions TFSIMD_FORCE_INLINE unsigned tfSwapEndian(unsigned val) { return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); } TFSIMD_FORCE_INLINE unsigned short tfSwapEndian(unsigned short val) { return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); } TFSIMD_FORCE_INLINE unsigned tfSwapEndian(int val) { return tfSwapEndian((unsigned)val); } TFSIMD_FORCE_INLINE unsigned short tfSwapEndian(short val) { return tfSwapEndian((unsigned short) val); } ///tfSwapFloat uses using char pointers to swap the endianness ////tfSwapFloat/tfSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values ///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. ///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. ///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. ///so instead of returning a float/double, we return integer/long long integer TFSIMD_FORCE_INLINE unsigned int tfSwapEndianFloat(float d) { unsigned int a = 0; unsigned char *dst = (unsigned char *)&a; unsigned char *src = (unsigned char *)&d; dst[0] = src[3]; dst[1] = src[2]; dst[2] = src[1]; dst[3] = src[0]; return a; } // unswap using char pointers TFSIMD_FORCE_INLINE float tfUnswapEndianFloat(unsigned int a) { float d = 0.0f; unsigned char *src = (unsigned char *)&a; unsigned char *dst = (unsigned char *)&d; dst[0] = src[3]; dst[1] = src[2]; dst[2] = src[1]; dst[3] = src[0]; return d; } // swap using char pointers TFSIMD_FORCE_INLINE void tfSwapEndianDouble(double d, unsigned char* dst) { unsigned char *src = (unsigned char *)&d; dst[0] = src[7]; dst[1] = src[6]; dst[2] = src[5]; dst[3] = src[4]; dst[4] = src[3]; dst[5] = src[2]; dst[6] = src[1]; dst[7] = src[0]; } // unswap using char pointers TFSIMD_FORCE_INLINE double tfUnswapEndianDouble(const unsigned char *src) { double d = 0.0; unsigned char *dst = (unsigned char *)&d; dst[0] = src[7]; dst[1] = src[6]; dst[2] = src[5]; dst[3] = src[4]; dst[4] = src[3]; dst[5] = src[2]; dst[6] = src[1]; dst[7] = src[0]; return d; } // returns normalized value in range [-TFSIMD_PI, TFSIMD_PI] TFSIMD_FORCE_INLINE tfScalar tfNormalizeAngle(tfScalar angleInRadians) { angleInRadians = tfFmod(angleInRadians, TFSIMD_2_PI); if(angleInRadians < -TFSIMD_PI) { return angleInRadians + TFSIMD_2_PI; } else if(angleInRadians > TFSIMD_PI) { return angleInRadians - TFSIMD_2_PI; } else { return angleInRadians; } } ///rudimentary class to provide type info struct tfTypedObject { tfTypedObject(int objectType) :m_objectType(objectType) { } int m_objectType; inline int getObjectType() const { return m_objectType; } }; #endif //TFSIMD___SCALAR_H geometry-1.13.2/tf/include/tf/LinearMath/Transform.h000066400000000000000000000205631366756125100222700ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 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. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef tfTransform_H #define tfTransform_H #include "Matrix3x3.h" namespace tf { #define TransformData TransformDoubleData /**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear. *It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */ class Transform { ///Storage for the rotation Matrix3x3 m_basis; ///Storage for the translation Vector3 m_origin; public: /**@brief No initialization constructor */ Transform() {} /**@brief Constructor from Quaternion (optional Vector3 ) * @param q Rotation from quaternion * @param c Translation from Vector (default 0,0,0) */ explicit TFSIMD_FORCE_INLINE Transform(const Quaternion& q, const Vector3& c = Vector3(tfScalar(0), tfScalar(0), tfScalar(0))) : m_basis(q), m_origin(c) {} /**@brief Constructor from Matrix3x3 (optional Vector3) * @param b Rotation from Matrix * @param c Translation from Vector default (0,0,0)*/ explicit TFSIMD_FORCE_INLINE Transform(const Matrix3x3& b, const Vector3& c = Vector3(tfScalar(0), tfScalar(0), tfScalar(0))) : m_basis(b), m_origin(c) {} /**@brief Copy constructor */ TFSIMD_FORCE_INLINE Transform (const Transform& other) : m_basis(other.m_basis), m_origin(other.m_origin) { } /**@brief Assignment Operator */ TFSIMD_FORCE_INLINE Transform& operator=(const Transform& other) { m_basis = other.m_basis; m_origin = other.m_origin; return *this; } /**@brief Set the current transform as the value of the product of two transforms * @param t1 Transform 1 * @param t2 Transform 2 * This = Transform1 * Transform2 */ TFSIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) { m_basis = t1.m_basis * t2.m_basis; m_origin = t1(t2.m_origin); } /* void multInverseLeft(const Transform& t1, const Transform& t2) { Vector3 v = t2.m_origin - t1.m_origin; m_basis = tfMultTransposeLeft(t1.m_basis, t2.m_basis); m_origin = v * t1.m_basis; } */ /**@brief Return the transform of the vector */ TFSIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const { return Vector3(m_basis[0].dot(x) + m_origin.x(), m_basis[1].dot(x) + m_origin.y(), m_basis[2].dot(x) + m_origin.z()); } /**@brief Return the transform of the vector */ TFSIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const { return (*this)(x); } /**@brief Return the transform of the Quaternion */ TFSIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const { return getRotation() * q; } /**@brief Return the basis matrix for the rotation */ TFSIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; } /**@brief Return the basis matrix for the rotation */ TFSIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; } /**@brief Return the origin vector translation */ TFSIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; } /**@brief Return the origin vector translation */ TFSIMD_FORCE_INLINE const Vector3& getOrigin() const { return m_origin; } /**@brief Return a quaternion representing the rotation */ Quaternion getRotation() const { Quaternion q; m_basis.getRotation(q); return q; } /**@brief Set from an array * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void setFromOpenGLMatrix(const tfScalar *m) { m_basis.setFromOpenGLSubMatrix(m); m_origin.setValue(m[12],m[13],m[14]); } /**@brief Fill an array representation * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void getOpenGLMatrix(tfScalar *m) const { m_basis.getOpenGLSubMatrix(m); m[12] = m_origin.x(); m[13] = m_origin.y(); m[14] = m_origin.z(); m[15] = tfScalar(1.0); } /**@brief Set the translational element * @param origin The vector to set the translation to */ TFSIMD_FORCE_INLINE void setOrigin(const Vector3& origin) { m_origin = origin; } TFSIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const; /**@brief Set the rotational element by Matrix3x3 */ TFSIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis) { m_basis = basis; } /**@brief Set the rotational element by Quaternion */ TFSIMD_FORCE_INLINE void setRotation(const Quaternion& q) { m_basis.setRotation(q); } /**@brief Set this transformation to the identity */ void setIdentity() { m_basis.setIdentity(); m_origin.setValue(tfScalar(0.0), tfScalar(0.0), tfScalar(0.0)); } /**@brief Multiply this Transform by another(this = this * another) * @param t The other transform */ Transform& operator*=(const Transform& t) { m_origin += m_basis * t.m_origin; m_basis *= t.m_basis; return *this; } /**@brief Return the inverse of this transform */ Transform inverse() const { Matrix3x3 inv = m_basis.transpose(); return Transform(inv, inv * -m_origin); } /**@brief Return the inverse of this transform times the other transform * @param t The other transform * return this.inverse() * the other */ Transform inverseTimes(const Transform& t) const; /**@brief Return the product of this transform and the other */ Transform operator*(const Transform& t) const; /**@brief Return an identity transform */ static const Transform& getIdentity() { static const Transform identityTransform(Matrix3x3::getIdentity()); return identityTransform; } void serialize(struct TransformData& dataOut) const; void serializeFloat(struct TransformFloatData& dataOut) const; void deSerialize(const struct TransformData& dataIn); void deSerializeDouble(const struct TransformDoubleData& dataIn); void deSerializeFloat(const struct TransformFloatData& dataIn); }; TFSIMD_FORCE_INLINE Vector3 Transform::invXform(const Vector3& inVec) const { Vector3 v = inVec - m_origin; return (m_basis.transpose() * v); } TFSIMD_FORCE_INLINE Transform Transform::inverseTimes(const Transform& t) const { Vector3 v = t.getOrigin() - m_origin; return Transform(m_basis.transposeTimes(t.m_basis), v * m_basis); } TFSIMD_FORCE_INLINE Transform Transform::operator*(const Transform& t) const { return Transform(m_basis * t.m_basis, (*this)(t.m_origin)); } /**@brief Test if two transforms have all elements equal */ TFSIMD_FORCE_INLINE bool operator==(const Transform& t1, const Transform& t2) { return ( t1.getBasis() == t2.getBasis() && t1.getOrigin() == t2.getOrigin() ); } ///for serialization struct TransformFloatData { Matrix3x3FloatData m_basis; Vector3FloatData m_origin; }; struct TransformDoubleData { Matrix3x3DoubleData m_basis; Vector3DoubleData m_origin; }; TFSIMD_FORCE_INLINE void Transform::serialize(TransformData& dataOut) const { m_basis.serialize(dataOut.m_basis); m_origin.serialize(dataOut.m_origin); } TFSIMD_FORCE_INLINE void Transform::serializeFloat(TransformFloatData& dataOut) const { m_basis.serializeFloat(dataOut.m_basis); m_origin.serializeFloat(dataOut.m_origin); } TFSIMD_FORCE_INLINE void Transform::deSerialize(const TransformData& dataIn) { m_basis.deSerialize(dataIn.m_basis); m_origin.deSerialize(dataIn.m_origin); } TFSIMD_FORCE_INLINE void Transform::deSerializeFloat(const TransformFloatData& dataIn) { m_basis.deSerializeFloat(dataIn.m_basis); m_origin.deSerializeFloat(dataIn.m_origin); } TFSIMD_FORCE_INLINE void Transform::deSerializeDouble(const TransformDoubleData& dataIn) { m_basis.deSerializeDouble(dataIn.m_basis); m_origin.deSerializeDouble(dataIn.m_origin); } } #endif geometry-1.13.2/tf/include/tf/LinearMath/Vector3.h000066400000000000000000000472361366756125100216500ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 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. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF_VECTOR3_H #define TF_VECTOR3_H #include "Scalar.h" #include "MinMax.h" namespace tf{ #define Vector3Data Vector3DoubleData #define Vector3DataName "Vector3DoubleData" /** * @class Vector3 * @brief Vector3 can be used to represent 3D points and vectors. * It has an un-used w component to suit 16-byte alignment when Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers */ ATTRIBUTE_ALIGNED16(class) Vector3 { public: #if defined (__SPU__) && defined (__CELLOS_LV2__) tfScalar m_floats[4]; public: TFSIMD_FORCE_INLINE const vec_float4& get128() const { return *((const vec_float4*)&m_floats[0]); } public: #else //__CELLOS_LV2__ __SPU__ #ifdef TF_USE_SSE // _WIN32 union { __m128 mVec128; tfScalar m_floats[4]; }; TFSIMD_FORCE_INLINE __m128 get128() const { return mVec128; } TFSIMD_FORCE_INLINE void set128(__m128 v128) { mVec128 = v128; } #else tfScalar m_floats[4]; #endif #endif //__CELLOS_LV2__ __SPU__ public: /**@brief No initialization constructor */ TFSIMD_FORCE_INLINE Vector3() {} /**@brief Constructor from scalars * @param x X value * @param y Y value * @param z Z value */ TFSIMD_FORCE_INLINE Vector3(const tfScalar& x, const tfScalar& y, const tfScalar& z) { m_floats[0] = x; m_floats[1] = y; m_floats[2] = z; m_floats[3] = tfScalar(0.); } /**@brief Add a vector to this one * @param The vector to add to this one */ TFSIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v) { m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; return *this; } /**@brief Sutfract a vector from this one * @param The vector to sutfract */ TFSIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v) { m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; return *this; } /**@brief Scale the vector * @param s Scale factor */ TFSIMD_FORCE_INLINE Vector3& operator*=(const tfScalar& s) { m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; return *this; } /**@brief Inversely scale the vector * @param s Scale factor to divide by */ TFSIMD_FORCE_INLINE Vector3& operator/=(const tfScalar& s) { tfFullAssert(s != tfScalar(0.0)); return *this *= tfScalar(1.0) / s; } /**@brief Return the dot product * @param v The other vector in the dot product */ TFSIMD_FORCE_INLINE tfScalar dot(const Vector3& v) const { return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; } /**@brief Return the length of the vector squared */ TFSIMD_FORCE_INLINE tfScalar length2() const { return dot(*this); } /**@brief Return the length of the vector */ TFSIMD_FORCE_INLINE tfScalar length() const { return tfSqrt(length2()); } /**@brief Return the distance squared between the ends of this and another vector * This is symantically treating the vector like a point */ TFSIMD_FORCE_INLINE tfScalar distance2(const Vector3& v) const; /**@brief Return the distance between the ends of this and another vector * This is symantically treating the vector like a point */ TFSIMD_FORCE_INLINE tfScalar distance(const Vector3& v) const; /**@brief Normalize this vector * x^2 + y^2 + z^2 = 1 */ TFSIMD_FORCE_INLINE Vector3& normalize() { return *this /= length(); } /**@brief Return a normalized version of this vector */ TFSIMD_FORCE_INLINE Vector3 normalized() const; /**@brief Rotate this vector * @param wAxis The axis to rotate about * @param angle The angle to rotate by */ TFSIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tfScalar angle ) const; /**@brief Return the angle between this and another vector * @param v The other vector */ TFSIMD_FORCE_INLINE tfScalar angle(const Vector3& v) const { tfScalar s = tfSqrt(length2() * v.length2()); tfFullAssert(s != tfScalar(0.0)); return tfAcos(dot(v) / s); } /**@brief Return a vector will the absolute values of each element */ TFSIMD_FORCE_INLINE Vector3 absolute() const { return Vector3( tfFabs(m_floats[0]), tfFabs(m_floats[1]), tfFabs(m_floats[2])); } /**@brief Return the cross product between this and another vector * @param v The other vector */ TFSIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const { return Vector3( m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1], m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); } TFSIMD_FORCE_INLINE tfScalar triple(const Vector3& v1, const Vector3& v2) const { return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); } /**@brief Return the axis with the smallest value * Note return values are 0,1,2 for x, y, or z */ TFSIMD_FORCE_INLINE int minAxis() const { return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ TFSIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tfScalar& t) const { return Vector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, m_floats[2] + (v.m_floats[2] -m_floats[2]) * t); } /**@brief Elementwise multiply this vector by the other * @param v The other vector */ TFSIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v) { m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; return *this; } /**@brief Return the x value */ TFSIMD_FORCE_INLINE const tfScalar& getX() const { return m_floats[0]; } /**@brief Return the y value */ TFSIMD_FORCE_INLINE const tfScalar& getY() const { return m_floats[1]; } /**@brief Return the z value */ TFSIMD_FORCE_INLINE const tfScalar& getZ() const { return m_floats[2]; } /**@brief Set the x value */ TFSIMD_FORCE_INLINE void setX(tfScalar x) { m_floats[0] = x;}; /**@brief Set the y value */ TFSIMD_FORCE_INLINE void setY(tfScalar y) { m_floats[1] = y;}; /**@brief Set the z value */ TFSIMD_FORCE_INLINE void setZ(tfScalar z) {m_floats[2] = z;}; /**@brief Set the w value */ TFSIMD_FORCE_INLINE void setW(tfScalar w) { m_floats[3] = w;}; /**@brief Return the x value */ TFSIMD_FORCE_INLINE const tfScalar& x() const { return m_floats[0]; } /**@brief Return the y value */ TFSIMD_FORCE_INLINE const tfScalar& y() const { return m_floats[1]; } /**@brief Return the z value */ TFSIMD_FORCE_INLINE const tfScalar& z() const { return m_floats[2]; } /**@brief Return the w value */ TFSIMD_FORCE_INLINE const tfScalar& w() const { return m_floats[3]; } //TFSIMD_FORCE_INLINE tfScalar& operator[](int i) { return (&m_floats[0])[i]; } //TFSIMD_FORCE_INLINE const tfScalar& operator[](int i) const { return (&m_floats[0])[i]; } ///operator tfScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. TFSIMD_FORCE_INLINE operator tfScalar *() { return &m_floats[0]; } TFSIMD_FORCE_INLINE operator const tfScalar *() const { return &m_floats[0]; } TFSIMD_FORCE_INLINE bool operator==(const Vector3& other) const { 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])); } TFSIMD_FORCE_INLINE bool operator!=(const Vector3& other) const { return !(*this == other); } /**@brief Set each element to the max of the current values and the values of another Vector3 * @param other The other Vector3 to compare with */ TFSIMD_FORCE_INLINE void setMax(const Vector3& other) { tfSetMax(m_floats[0], other.m_floats[0]); tfSetMax(m_floats[1], other.m_floats[1]); tfSetMax(m_floats[2], other.m_floats[2]); tfSetMax(m_floats[3], other.w()); } /**@brief Set each element to the min of the current values and the values of another Vector3 * @param other The other Vector3 to compare with */ TFSIMD_FORCE_INLINE void setMin(const Vector3& other) { tfSetMin(m_floats[0], other.m_floats[0]); tfSetMin(m_floats[1], other.m_floats[1]); tfSetMin(m_floats[2], other.m_floats[2]); tfSetMin(m_floats[3], other.w()); } TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z) { m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; m_floats[3] = tfScalar(0.); } void getSkewSymmetricMatrix(Vector3* v0,Vector3* v1,Vector3* v2) const { v0->setValue(0. ,-z() ,y()); v1->setValue(z() ,0. ,-x()); v2->setValue(-y() ,x() ,0.); } void setZero() { setValue(tfScalar(0.),tfScalar(0.),tfScalar(0.)); } TFSIMD_FORCE_INLINE bool isZero() const { return m_floats[0] == tfScalar(0) && m_floats[1] == tfScalar(0) && m_floats[2] == tfScalar(0); } TFSIMD_FORCE_INLINE bool fuzzyZero() const { return length2() < TFSIMD_EPSILON; } TFSIMD_FORCE_INLINE void serialize(struct Vector3Data& dataOut) const; TFSIMD_FORCE_INLINE void deSerialize(const struct Vector3Data& dataIn); TFSIMD_FORCE_INLINE void serializeFloat(struct Vector3FloatData& dataOut) const; TFSIMD_FORCE_INLINE void deSerializeFloat(const struct Vector3FloatData& dataIn); TFSIMD_FORCE_INLINE void serializeDouble(struct Vector3DoubleData& dataOut) const; TFSIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn); }; /**@brief Return the sum of two vectors (Point symantics)*/ TFSIMD_FORCE_INLINE Vector3 operator+(const Vector3& v1, const Vector3& v2) { return Vector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); } /**@brief Return the elementwise product of two vectors */ TFSIMD_FORCE_INLINE Vector3 operator*(const Vector3& v1, const Vector3& v2) { return Vector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); } /**@brief Return the difference between two vectors */ TFSIMD_FORCE_INLINE Vector3 operator-(const Vector3& v1, const Vector3& v2) { return Vector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); } /**@brief Return the negative of the vector */ TFSIMD_FORCE_INLINE Vector3 operator-(const Vector3& v) { return Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); } /**@brief Return the vector scaled by s */ TFSIMD_FORCE_INLINE Vector3 operator*(const Vector3& v, const tfScalar& s) { return Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); } /**@brief Return the vector scaled by s */ TFSIMD_FORCE_INLINE Vector3 operator*(const tfScalar& s, const Vector3& v) { return v * s; } /**@brief Return the vector inversely scaled by s */ TFSIMD_FORCE_INLINE Vector3 operator/(const Vector3& v, const tfScalar& s) { tfFullAssert(s != tfScalar(0.0)); return v * (tfScalar(1.0) / s); } /**@brief Return the vector inversely scaled by s */ TFSIMD_FORCE_INLINE Vector3 operator/(const Vector3& v1, const Vector3& v2) { return Vector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); } /**@brief Return the dot product between two vectors */ TFSIMD_FORCE_INLINE tfScalar tfDot(const Vector3& v1, const Vector3& v2) { return v1.dot(v2); } /**@brief Return the distance squared between two vectors */ TFSIMD_FORCE_INLINE tfScalar tfDistance2(const Vector3& v1, const Vector3& v2) { return v1.distance2(v2); } /**@brief Return the distance between two vectors */ TFSIMD_FORCE_INLINE tfScalar tfDistance(const Vector3& v1, const Vector3& v2) { return v1.distance(v2); } /**@brief Return the angle between two vectors */ TFSIMD_FORCE_INLINE tfScalar tfAngle(const Vector3& v1, const Vector3& v2) { return v1.angle(v2); } /**@brief Return the cross product of two vectors */ TFSIMD_FORCE_INLINE Vector3 tfCross(const Vector3& v1, const Vector3& v2) { return v1.cross(v2); } TFSIMD_FORCE_INLINE tfScalar tfTriple(const Vector3& v1, const Vector3& v2, const Vector3& v3) { return v1.triple(v2, v3); } /**@brief Return the linear interpolation between two vectors * @param v1 One vector * @param v2 The other vector * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ TFSIMD_FORCE_INLINE Vector3 lerp(const Vector3& v1, const Vector3& v2, const tfScalar& t) { return v1.lerp(v2, t); } TFSIMD_FORCE_INLINE tfScalar Vector3::distance2(const Vector3& v) const { return (v - *this).length2(); } TFSIMD_FORCE_INLINE tfScalar Vector3::distance(const Vector3& v) const { return (v - *this).length(); } TFSIMD_FORCE_INLINE Vector3 Vector3::normalized() const { return *this / length(); } TFSIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tfScalar angle ) const { // wAxis must be a unit lenght vector Vector3 o = wAxis * wAxis.dot( *this ); Vector3 x = *this - o; Vector3 y; y = wAxis.cross( *this ); return ( o + x * tfCos( angle ) + y * tfSin( angle ) ); } class tfVector4 : public Vector3 { public: TFSIMD_FORCE_INLINE tfVector4() {} TFSIMD_FORCE_INLINE tfVector4(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) : Vector3(x,y,z) { m_floats[3] = w; } TFSIMD_FORCE_INLINE tfVector4 absolute4() const { return tfVector4( tfFabs(m_floats[0]), tfFabs(m_floats[1]), tfFabs(m_floats[2]), tfFabs(m_floats[3])); } tfScalar getW() const { return m_floats[3];} TFSIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; tfScalar maxVal = tfScalar(-TF_LARGE_FLOAT); if (m_floats[0] > maxVal) { maxIndex = 0; maxVal = m_floats[0]; } if (m_floats[1] > maxVal) { maxIndex = 1; maxVal = m_floats[1]; } if (m_floats[2] > maxVal) { maxIndex = 2; maxVal =m_floats[2]; } if (m_floats[3] > maxVal) { maxIndex = 3; maxVal = m_floats[3]; } return maxIndex; } TFSIMD_FORCE_INLINE int minAxis4() const { int minIndex = -1; tfScalar minVal = tfScalar(TF_LARGE_FLOAT); if (m_floats[0] < minVal) { minIndex = 0; minVal = m_floats[0]; } if (m_floats[1] < minVal) { minIndex = 1; minVal = m_floats[1]; } if (m_floats[2] < minVal) { minIndex = 2; minVal =m_floats[2]; } if (m_floats[3] < minVal) { minIndex = 3; minVal = m_floats[3]; } return minIndex; } TFSIMD_FORCE_INLINE int closestAxis4() const { return absolute4().maxAxis4(); } /**@brief Set x,y,z and zero w * @param x Value of x * @param y Value of y * @param z Value of z */ /* void getValue(tfScalar *m) const { m[0] = m_floats[0]; m[1] = m_floats[1]; m[2] =m_floats[2]; } */ /**@brief Set the values * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) { m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; m_floats[3]=w; } }; ///tfSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization TFSIMD_FORCE_INLINE void tfSwapScalarEndian(const tfScalar& sourceVal, tfScalar& destVal) { unsigned char* dest = (unsigned char*) &destVal; const unsigned char* src = (const unsigned char*) &sourceVal; dest[0] = src[7]; dest[1] = src[6]; dest[2] = src[5]; dest[3] = src[4]; dest[4] = src[3]; dest[5] = src[2]; dest[6] = src[1]; dest[7] = src[0]; } ///tfSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization TFSIMD_FORCE_INLINE void tfSwapVector3Endian(const Vector3& sourceVec, Vector3& destVec) { for (int i=0;i<4;i++) { tfSwapScalarEndian(sourceVec[i],destVec[i]); } } ///tfUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization TFSIMD_FORCE_INLINE void tfUnSwapVector3Endian(Vector3& vector) { Vector3 swappedVec; for (int i=0;i<4;i++) { tfSwapScalarEndian(vector[i],swappedVec[i]); } vector = swappedVec; } TFSIMD_FORCE_INLINE void tfPlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q) { if (tfFabs(n.z()) > TFSIMDSQRT12) { // choose p in y-z plane tfScalar a = n[1]*n[1] + n[2]*n[2]; tfScalar k = tfRecipSqrt (a); p.setValue(0,-n[2]*k,n[1]*k); // set q = n x p q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); } else { // choose p in x-y plane tfScalar a = n.x()*n.x() + n.y()*n.y(); tfScalar k = tfRecipSqrt (a); p.setValue(-n.y()*k,n.x()*k,0); // set q = n x p q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); } } struct Vector3FloatData { float m_floats[4]; }; struct Vector3DoubleData { double m_floats[4]; }; TFSIMD_FORCE_INLINE void Vector3::serializeFloat(struct Vector3FloatData& dataOut) const { ///could also do a memcpy, check if it is worth it for (int i=0;i<4;i++) dataOut.m_floats[i] = float(m_floats[i]); } TFSIMD_FORCE_INLINE void Vector3::deSerializeFloat(const struct Vector3FloatData& dataIn) { for (int i=0;i<4;i++) m_floats[i] = tfScalar(dataIn.m_floats[i]); } TFSIMD_FORCE_INLINE void Vector3::serializeDouble(struct Vector3DoubleData& dataOut) const { ///could also do a memcpy, check if it is worth it for (int i=0;i<4;i++) dataOut.m_floats[i] = double(m_floats[i]); } TFSIMD_FORCE_INLINE void Vector3::deSerializeDouble(const struct Vector3DoubleData& dataIn) { for (int i=0;i<4;i++) m_floats[i] = tfScalar(dataIn.m_floats[i]); } TFSIMD_FORCE_INLINE void Vector3::serialize(struct Vector3Data& dataOut) const { ///could also do a memcpy, check if it is worth it for (int i=0;i<4;i++) dataOut.m_floats[i] = m_floats[i]; } TFSIMD_FORCE_INLINE void Vector3::deSerialize(const struct Vector3Data& dataIn) { for (int i=0;i<4;i++) m_floats[i] = dataIn.m_floats[i]; } } #endif //TFSIMD__VECTOR3_H geometry-1.13.2/tf/include/tf/exceptions.h000066400000000000000000000040601366756125100204440ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF_EXCEPTIONS_H #define TF_EXCEPTIONS_H #include #include namespace tf{ // Pass through exceptions from tf2 typedef tf2::TransformException TransformException; typedef tf2::LookupException LookupException; typedef tf2::ConnectivityException ConnectivityException; typedef tf2::ExtrapolationException ExtrapolationException; typedef tf2::InvalidArgumentException InvalidArgument; } #endif //TF_EXCEPTIONS_H geometry-1.13.2/tf/include/tf/message_filter.h000066400000000000000000000453611366756125100212650ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Josh Faust */ #ifndef TF_MESSAGE_FILTER_H #define TF_MESSAGE_FILTER_H #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define TF_MESSAGEFILTER_DEBUG(fmt, ...) \ ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__) #define TF_MESSAGEFILTER_WARN(fmt, ...) \ ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__) namespace tf { namespace filter_failure_reasons { enum FilterFailureReason { /// The message buffer overflowed, and this message was pushed off the back of the queue, but the reason it was unable to be transformed is unknown. Unknown, /// The timestamp on the message is more than the cache length earlier than the newest data in the transform cache OutTheBack, /// The frame_id on the message is empty EmptyFrameID, }; } typedef filter_failure_reasons::FilterFailureReason FilterFailureReason; class MessageFilterBase { public: virtual ~MessageFilterBase(){} virtual void clear() = 0; virtual void setTargetFrame(const std::string& target_frame) = 0; virtual void setTargetFrames(const std::vector& target_frames) = 0; virtual void setTolerance(const ros::Duration& tolerance) = 0; virtual void setQueueSize( uint32_t new_queue_size ) = 0; virtual uint32_t getQueueSize() = 0; }; /** * \brief Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available * * The callbacks used in this class are of the same form as those used by roscpp's message callbacks. * * MessageFilter is templated on a message type. * * \section example_usage Example Usage * * If you want to hook a MessageFilter into a ROS topic: \verbatim message_filters::Subscriber sub(node_handle_, "topic", 10); tf::MessageFilter tf_filter(sub, tf_listener_, "/map", 10); tf_filter.registerCallback(&MyClass::myCallback, this); \endverbatim */ template class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter { public: typedef boost::shared_ptr MConstPtr; typedef ros::MessageEvent MEvent; typedef boost::function FailureCallback; typedef boost::signals2::signal FailureSignal; /** * \brief Constructor * * \param tf The tf::Transformer this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param nh The NodeHandle to use for any necessary operations * \param max_rate The maximum rate to check for newly transformable messages */ MessageFilter(Transformer& tf, const std::string& target_frame, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle(), ros::Duration max_rate = ros::Duration(0.01)) : tf_(tf) , nh_(nh) , max_rate_(max_rate) , queue_size_(queue_size) { init(); setTargetFrame(target_frame); } /** * \brief Constructor * * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. * \param tf The tf::Transformer this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param nh The NodeHandle to use for any necessary operations * \param max_rate The maximum rate to check for newly transformable messages */ template MessageFilter(F& f, Transformer& tf, const std::string& target_frame, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle(), ros::Duration max_rate = ros::Duration(0.01)) : tf_(tf) , nh_(nh) , max_rate_(max_rate) , queue_size_(queue_size) { init(); setTargetFrame(target_frame); connectInput(f); } /** * \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first. */ template void connectInput(F& f) { message_connection_.disconnect(); message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this); } /** * \brief Destructor */ ~MessageFilter() { // Explicitly stop callbacks; they could execute after we're destroyed max_rate_timer_.stop(); message_connection_.disconnect(); tf_.removeTransformsChangedListener(tf_connection_); clear(); TF_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Failed Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu", (long long unsigned int)successful_transform_count_, (long long unsigned int)failed_transform_count_, (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_, (long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_); } /** * \brief Set the frame you need to be able to transform to before getting a message callback */ void setTargetFrame(const std::string& target_frame) { std::vector frames; frames.push_back(target_frame); setTargetFrames(frames); } /** * \brief Set the frames you need to be able to transform to before getting a message callback */ void setTargetFrames(const std::vector& target_frames) { boost::mutex::scoped_lock list_lock(messages_mutex_); boost::mutex::scoped_lock string_lock(target_frames_string_mutex_); target_frames_ = target_frames; std::stringstream ss; for (std::vector::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) { ss << *it << " "; } target_frames_string_ = ss.str(); } /** * \brief Get the target frames as a string for debugging */ std::string getTargetFramesString() { boost::mutex::scoped_lock lock(target_frames_string_mutex_); return target_frames_string_; }; /** * \brief Set the required tolerance for the notifier to return true */ void setTolerance(const ros::Duration& tolerance) { time_tolerance_ = tolerance; } /** * \brief Clear any messages currently in the queue */ void clear() { boost::mutex::scoped_lock lock(messages_mutex_); TF_MESSAGEFILTER_DEBUG("%s", "Cleared"); messages_.clear(); message_count_ = 0; warned_about_unresolved_name_ = false; warned_about_empty_frame_id_ = false; } void add(const MEvent& evt) { boost::mutex::scoped_lock lock(messages_mutex_); testMessages(); if (!testMessage(evt)) { // If this message is about to push us past our queue size, erase the oldest message if (queue_size_ != 0 && message_count_ + 1 > queue_size_) { ++dropped_message_count_; const MEvent& front = messages_.front(); TF_MESSAGEFILTER_DEBUG( "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_, ros::message_traits::FrameId::value(*front.getMessage()).c_str(), ros::message_traits::TimeStamp::value(*front.getMessage()).toSec()); signalFailure(messages_.front(), filter_failure_reasons::Unknown); messages_.pop_front(); --message_count_; } // Add the message to our list messages_.push_back(evt); ++message_count_; } TF_MESSAGEFILTER_DEBUG( "Added message in frame %s at time %.3f, count now %d", ros::message_traits::FrameId::value(*evt.getMessage()).c_str(), ros::message_traits::TimeStamp::value(*evt.getMessage()).toSec(), message_count_); ++incoming_message_count_; } /** * \brief Manually add a message into this filter. * \note If the message (or any other messages in the queue) are immediately transformable this will immediately call through to the output callback, possibly * multiple times */ void add(const MConstPtr& message) { boost::shared_ptr > header(new std::map); (*header)["callerid"] = "unknown"; add(MEvent(message, header, ros::Time::now())); } /** * \brief Register a callback to be called when a message is about to be dropped * \param callback The callback to call */ message_filters::Connection registerFailureCallback(const FailureCallback& callback) { boost::mutex::scoped_lock lock(failure_signal_mutex_); return message_filters::Connection(boost::bind(&MessageFilter::disconnectFailure, this, _1), failure_signal_.connect(callback)); } virtual void setQueueSize( uint32_t new_queue_size ) { queue_size_ = new_queue_size; } virtual uint32_t getQueueSize() { return queue_size_; } private: void init() { message_count_ = 0; new_transforms_ = false; successful_transform_count_ = 0; failed_transform_count_ = 0; failed_out_the_back_count_ = 0; transform_message_count_ = 0; incoming_message_count_ = 0; dropped_message_count_ = 0; time_tolerance_ = ros::Duration(0.0); warned_about_unresolved_name_ = false; warned_about_empty_frame_id_ = false; tf_connection_ = tf_.addTransformsChangedListener(boost::bind(&MessageFilter::transformsChanged, this)); max_rate_timer_ = nh_.createTimer(max_rate_, &MessageFilter::maxRateTimerCallback, this); } typedef std::list L_Event; bool testMessage(const MEvent& evt) { const MConstPtr& message = evt.getMessage(); std::string callerid = evt.getPublisherName(); std::string frame_id = ros::message_traits::FrameId::value(*message); ros::Time stamp = ros::message_traits::TimeStamp::value(*message); //Throw out messages which have an empty frame_id if (frame_id.empty()) { if (!warned_about_empty_frame_id_) { warned_about_empty_frame_id_ = true; TF_MESSAGEFILTER_WARN("Discarding message from [%s] due to empty frame_id. This message will only print once.", callerid.c_str()); } signalFailure(evt, filter_failure_reasons::EmptyFrameID); return true; } //Throw out messages which are too old //! \todo combine getLatestCommonTime call with the canTransform call for (std::vector::iterator target_it = target_frames_.begin(); target_it != target_frames_.end(); ++target_it) { const std::string& target_frame = *target_it; if (target_frame != frame_id && stamp != ros::Time(0)) { ros::Time latest_transform_time ; tf_.getLatestCommonTime(frame_id, target_frame, latest_transform_time, 0) ; if (stamp + tf_.getCacheLength() < latest_transform_time) { ++failed_out_the_back_count_; ++dropped_message_count_; TF_MESSAGEFILTER_DEBUG( "Discarding Message, in frame %s, Out of the back of Cache Time " "(stamp: %.3f + cache_length: %.3f < latest_transform_time %.3f. " "Message Count now: %d", ros::message_traits::FrameId::value(*message).c_str(), ros::message_traits::TimeStamp::value(*message).toSec(), tf_.getCacheLength().toSec(), latest_transform_time.toSec(), message_count_); last_out_the_back_stamp_ = stamp; last_out_the_back_frame_ = frame_id; signalFailure(evt, filter_failure_reasons::OutTheBack); return true; } } } bool ready = !target_frames_.empty(); for (std::vector::iterator target_it = target_frames_.begin(); ready && target_it != target_frames_.end(); ++target_it) { std::string& target_frame = *target_it; if (time_tolerance_ != ros::Duration(0.0)) { ready = ready && (tf_.canTransform(target_frame, frame_id, stamp) && tf_.canTransform(target_frame, frame_id, stamp + time_tolerance_) ); } else { ready = ready && tf_.canTransform(target_frame, frame_id, stamp); } } if (ready) { TF_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_); ++successful_transform_count_; this->signalMessage(evt); } else { ++failed_transform_count_; } return ready; } void testMessages() { if (!messages_.empty() && getTargetFramesString() == " ") { ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: empty target frame", getTargetFramesString().c_str()); } int i = 0; typename L_Event::iterator it = messages_.begin(); for (; it != messages_.end(); ++i) { MEvent& evt = *it; if (testMessage(evt)) { --message_count_; it = messages_.erase(it); } else { ++it; } } } void maxRateTimerCallback(const ros::TimerEvent&) { boost::mutex::scoped_lock list_lock(messages_mutex_); if (new_transforms_) { testMessages(); new_transforms_ = false; } checkFailures(); } /** * \brief Callback that happens when we receive a message on the message topic */ void incomingMessage(const ros::MessageEvent& evt) { add(evt); } void transformsChanged() { new_transforms_ = true; ++transform_message_count_; } void checkFailures() { if (next_failure_warning_.isZero()) { next_failure_warning_ = ros::Time::now() + ros::Duration(15); } if (ros::Time::now() >= next_failure_warning_) { if (incoming_message_count_ - message_count_ == 0) { return; } double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_); if (dropped_pct > 0.95) { TF_MESSAGEFILTER_WARN("Dropped %.2f%% of messages so far. Please turn the [%s.message_filter] rosconsole logger to DEBUG for more information.", dropped_pct*100, ROSCONSOLE_DEFAULT_NAME); next_failure_warning_ = ros::Time::now() + ros::Duration(60); if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5) { TF_MESSAGEFILTER_WARN(" The majority of dropped messages were due to messages growing older than the TF cache time. The last message's timestamp was: %f, and the last frame_id was: %s", last_out_the_back_stamp_.toSec(), last_out_the_back_frame_.c_str()); } } } } void disconnectFailure(const message_filters::Connection& c) { boost::mutex::scoped_lock lock(failure_signal_mutex_); c.getBoostConnection().disconnect(); } void signalFailure(const MEvent& evt, FilterFailureReason reason) { boost::mutex::scoped_lock lock(failure_signal_mutex_); failure_signal_(evt.getMessage(), reason); } Transformer& tf_; ///< The Transformer used to determine if transformation data is available ros::NodeHandle nh_; ///< The node used to subscribe to the topic ros::Duration max_rate_; ros::Timer max_rate_timer_; std::vector target_frames_; ///< The frames we need to be able to transform to before a message is ready std::string target_frames_string_; boost::mutex target_frames_string_mutex_; uint32_t queue_size_; ///< The maximum number of messages we queue up L_Event messages_; ///< The message list uint32_t message_count_; ///< The number of messages in the list. Used because messages_.size() has linear cost boost::mutex messages_mutex_; ///< The mutex used for locking message list operations bool new_messages_; ///< Used to skip waiting on new_data_ if new messages have come in while calling back volatile bool new_transforms_; ///< Used to skip waiting on new_data_ if new transforms have come in while calling back or transforming data bool warned_about_unresolved_name_; bool warned_about_empty_frame_id_; uint64_t successful_transform_count_; uint64_t failed_transform_count_; uint64_t failed_out_the_back_count_; uint64_t transform_message_count_; uint64_t incoming_message_count_; uint64_t dropped_message_count_; ros::Time last_out_the_back_stamp_; std::string last_out_the_back_frame_; ros::Time next_failure_warning_; ros::Duration time_tolerance_; ///< Provide additional tolerance on time for messages which are stamped but can have associated duration boost::signals2::connection tf_connection_; message_filters::Connection message_connection_; FailureSignal failure_signal_; boost::mutex failure_signal_mutex_; }; } // namespace tf #endif geometry-1.13.2/tf/include/tf/tf.h000066400000000000000000000506611366756125100167040ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF_TF_H #define TF_TF_H #include #include #include #include #include #include #include #include #include "tf/time_cache.h" #include #include #include "geometry_msgs/TwistStamped.h" #include #include // Import/export for windows dll's and visibility for gcc shared libraries. #ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries #ifdef tf_EXPORTS // we are building a shared lib/dll #define TF_DECL ROS_HELPER_EXPORT #else // we are using shared lib/dll #define TF_DECL ROS_HELPER_IMPORT #endif #else // ros is being built around static libraries #define TF_DECL #endif namespace tf { /** \brief resolve tf names */ std::string resolve(const std::string& prefix, const std::string& frame_name); /** strip a leading slash for */ std::string strip_leading_slash(const std::string& frame_name); /** \deprecated This has been renamed to tf::resolve */ ROS_DEPRECATED static inline std::string remap(const std::string& prefix, const std::string& frame_name) { return tf::resolve(prefix, frame_name);} enum ErrorValues { NO_ERROR = 0, LOOKUP_ERROR, CONNECTIVITY_ERROR, EXTRAPOLATION_ERROR}; /** \brief An internal representation of transform chains * * This struct is how the list of transforms are stored before being passed to computeTransformFromList. */ typedef struct { std::vector inverseTransforms; std::vector forwardTransforms; } TransformLists; /** \brief A Class which provides coordinate transforms between any two frames in a system. * * This class provides a simple interface to allow recording and lookup of * relationships between arbitrary frames of the system. * * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames. * For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head. * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand. * libTF is designed to take care of all the intermediate steps for you. * * Internal Representation * libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame. * Frames are designated using an std::string * 0 is a frame without a parent (the top of a tree) * The positions of frames over time must be pushed in. * * All function calls which pass frame ids can potentially throw the exception tf::LookupException */ class TF_DECL Transformer { public: /************* Constants ***********************/ static const unsigned int MAX_GRAPH_DEPTH = 100UL; //!< The maximum number of time to recurse before assuming the tree has a loop. static const double DEFAULT_CACHE_TIME; //!< 10.0 is the default amount of time to cache data in seconds, set in cpp file. static const int64_t DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL; //!< The default amount of time to extrapolate //deprecated since integration with tf2 /** Constructor * \param interpolating Unused, legacy always true * \param cache_time How long to keep a history of transforms in nanoseconds * */ Transformer(bool interpolating = true, ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME)); virtual ~Transformer(void); /** \brief Clear all data */ void clear(); /** \brief Add transform information to the tf data structure * \param transform The transform to store * \param authority The source of the information for this transform * returns true unless an error occured */ bool setTransform(const StampedTransform& transform, const std::string & authority = "default_authority"); /*********** Accessors *************/ /** \brief Get the transform between two frames by frame ID. * \param target_frame The frame to which data should be transformed * \param source_frame The frame where the data originated * \param time The time at which the value of the transform is desired. (0 will get the latest) * \param transform The transform reference to fill. * * Possible exceptions tf::LookupException, tf::ConnectivityException, * tf::MaxDepthException, tf::ExtrapolationException */ void lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, StampedTransform& transform) const; /** \brief Get the transform between two frames by frame ID assuming fixed frame. * \param target_frame The frame to which data should be transformed * \param target_time The time to which the data should be transformed. (0 will get the latest) * \param source_frame The frame where the data originated * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) * \param fixed_frame The frame in which to assume the transform is constant in time. * \param transform The transform reference to fill. * * Possible exceptions tf::LookupException, tf::ConnectivityException, * tf::MaxDepthException, tf::ExtrapolationException */ void lookupTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const; /** \brief Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point * \param tracking_frame The frame to track * \param observation_frame The frame from which to measure the twist * \param reference_frame The reference frame in which to express the twist * \param reference_point The reference point with which to express the twist * \param reference_point_frame The frame_id in which the reference point is expressed * \param time The time at which to get the velocity * \param duration The period over which to average * \param twist The twist output * * This will compute the average velocity on the interval * (time - duration/2, time+duration/2). If that is too close to the most * recent reading, in which case it will shift the interval up to * duration/2 to prevent extrapolation. Possible exceptions * tf::LookupException, tf::ConnectivityException, * tf::MaxDepthException, tf::ExtrapolationException * * New in geometry 1.1 */ void lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame, const tf::Point & reference_point, const std::string& reference_point_frame, const ros::Time& time, const ros::Duration& averaging_interval, geometry_msgs::Twist& twist) const; /** \brief lookup the twist of the tracking frame with respect to the observational frame * * This is a simplified version of * lookupTwist with it assumed that the reference point is the * origin of the tracking frame, and the reference frame is the * observation frame. * * New in geometry 1.1 */ void lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const ros::Time& time, const ros::Duration& averaging_interval, geometry_msgs::Twist& twist) const; /** \brief Block until a transform is possible or it times out * \param target_frame The frame into which to transform * \param source_frame The frame from which to transform * \param time The time at which to transform * \param timeout How long to block before failing * \param polling_sleep_duration How often to retest if failed * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL */ bool waitForTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01), std::string* error_msg = NULL) const; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param source_frame The frame from which to transform * \param time The time at which to transform * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL */ bool canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, std::string* error_msg = NULL) const; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param target_time The time into which to transform * \param source_frame The frame from which to transform * \param source_time The time from which to transform * \param fixed_frame The frame in which to treat the transform as constant in time * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL */ bool canTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, std::string* error_msg = NULL) const; /** \brief Block until a transform is possible or it times out * \param target_frame The frame into which to transform * \param target_time The time into which to transform * \param source_frame The frame from which to transform * \param source_time The time from which to transform * \param fixed_frame The frame in which to treat the transform as constant in time * \param timeout How long to block before failing * \param polling_sleep_duration How often to retest if failed * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL */ bool waitForTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01), std::string* error_msg = NULL) const; /**@brief Return the latest rostime which is common across the spanning set * zero if fails to cross */ int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time& time, std::string* error_string) const; /** \brief Transform a Stamped Quaternion into the target frame * This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. */ void transformQuaternion(const std::string& target_frame, const Stamped& stamped_in, Stamped& stamped_out) const; /** \brief Transform a Stamped Vector3 into the target frame * This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.*/ void transformVector(const std::string& target_frame, const Stamped& stamped_in, Stamped& stamped_out) const; /** \brief Transform a Stamped Point into the target frame * This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.*/ void transformPoint(const std::string& target_frame, const Stamped& stamped_in, Stamped& stamped_out) const; /** \brief Transform a Stamped Pose into the target frame * This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.*/ void transformPose(const std::string& target_frame, const Stamped& stamped_in, Stamped& stamped_out) const; /** \brief Transform a Stamped Quaternion into the target frame * This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.*/ void transformQuaternion(const std::string& target_frame, const ros::Time& target_time, const Stamped& stamped_in, const std::string& fixed_frame, Stamped& stamped_out) const; /** \brief Transform a Stamped Vector3 into the target frame * This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.*/ void transformVector(const std::string& target_frame, const ros::Time& target_time, const Stamped& stamped_in, const std::string& fixed_frame, Stamped& stamped_out) const; /** \brief Transform a Stamped Point into the target frame * This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.*/ void transformPoint(const std::string& target_frame, const ros::Time& target_time, const Stamped& stamped_in, const std::string& fixed_frame, Stamped& stamped_out) const; /** \brief Transform a Stamped Pose into the target frame * This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.*/ void transformPose(const std::string& target_frame, const ros::Time& target_time, const Stamped& stamped_in, const std::string& fixed_frame, Stamped& stamped_out) const; /** \brief Debugging function that will print the spanning chain of transforms. * Possible exceptions tf::LookupException, tf::ConnectivityException, * tf::MaxDepthException */ //std::string chainAsString(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame) const; /** \brief Debugging function that will print the spanning chain of transforms. * Possible exceptions tf::LookupException, tf::ConnectivityException, * tf::MaxDepthException */ void chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame, std::vector& output) const; /** \brief A way to see what frames have been cached * Useful for debugging */ std::string allFramesAsString() const; /** \brief A way to see what frames have been cached * Useful for debugging */ std::string allFramesAsDot(double current_time = 0) const; /** \brief A way to get a std::vector of available frame ids */ void getFrameStrings(std::vector& ids) const; /**@brief Fill the parent of a frame. * @param frame_id The frame id of the frame in question * @param parent The reference to the string to fill the parent * Returns true unless "NO_PARENT" */ bool getParent(const std::string& frame_id, ros::Time time, std::string& parent) const; /**@brief Check if a frame exists in the tree * @param frame_id_str The frame id in question */ bool frameExists(const std::string& frame_id_str) const; /**@brief Set the distance which tf is allow to extrapolate * \param distance How far to extrapolate before throwing an exception * default is zero */ void setExtrapolationLimit(const ros::Duration& distance); /**@brief Get the duration over which this transformer will cache */ ros::Duration getCacheLength() { return tf2_buffer_ptr_->getCacheLength();} /** * \brief Add a callback that happens when a new transform has arrived * * \param callback The callback, of the form void func(); * \return A boost::signals2::connection object that can be used to remove this * listener */ boost::signals2::connection addTransformsChangedListener(boost::function callback); void removeTransformsChangedListener(boost::signals2::connection c); /** * \brief Get the tf_prefix this is running with */ std::string getTFPrefix() const { return tf_prefix_;}; //Declare that it is safe to call waitForTransform void setUsingDedicatedThread(bool value) { tf2_buffer_ptr_->setUsingDedicatedThread(value);}; // Get the state of using_dedicated_thread_ from the buffer bool isUsingDedicatedThread() { return tf2_buffer_ptr_->isUsingDedicatedThread();}; // Get a copy of the shared_ptr containing the tf2_ros::Buffer object std::shared_ptr getTF2BufferPtr() { return tf2_buffer_ptr_;}; protected: /** \brief The internal storage class for ReferenceTransform. * * An instance of this class is created for each frame in the system. * This class natively handles the relationship between frames. * * The derived class Pose3DCache provides a buffered history of positions * with interpolation. * */ /******************** Internal Storage ****************/ /// transform prefix to apply as necessary std::string tf_prefix_; public: // A flag to allow falling back to wall time bool fall_back_to_wall_time_; protected: /** Hack method to work around #4150 */ ros::Time now() const { if (!fall_back_to_wall_time_) return ros::Time::now() ; else { ros::WallTime wt = ros::WallTime::now(); return ros::Time(wt.sec, wt.nsec); }; } // Allows broadcaster to check ok() before wait for transform // Always returns true in base class virtual bool ok() const; /************************* Internal Functions ****************************/ protected: std::shared_ptr tf2_buffer_ptr_; }; /** \brief Throw InvalidArgument if quaternion is malformed */ inline void assertQuaternionValid(const tf::Quaternion & q) { if(std::isnan(q.x()) || std::isnan(q.y()) || std::isnan(q.z()) || std::isnan(q.w())) { std::stringstream ss; ss << "Quaternion contains a NaN" << std::endl; throw tf::InvalidArgument(ss.str()); } if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01) { std::stringstream ss; ss << "Quaternion malformed, magnitude: " << q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() << " should be 1.0" < 0.01) { std::stringstream ss; ss << "Quaternion malformed, magnitude: " << q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w << " should be 1.0" < #include #include "tf/transform_datatypes.h" #include "tf/exceptions.h" #include "tf/LinearMath/Transform.h" #include namespace tf { enum ExtrapolationMode { ONE_VALUE, INTERPOLATE, EXTRAPOLATE_BACK, EXTRAPOLATE_FORWARD }; typedef uint32_t CompactFrameID; typedef std::pair P_TimeAndFrameID; /** \brief Storage for transforms and their parent */ class TransformStorage { public: TransformStorage(); TransformStorage(const StampedTransform& data, CompactFrameID frame_id, CompactFrameID child_frame_id); TransformStorage(const TransformStorage& rhs) { *this = rhs; } TransformStorage& operator=(const TransformStorage& rhs) { #if 01 rotation_ = rhs.rotation_; translation_ = rhs.translation_; stamp_ = rhs.stamp_; frame_id_ = rhs.frame_id_; child_frame_id_ = rhs.child_frame_id_; #endif return *this; } bool operator< (const TransformStorage &b) const { return this->stamp_ < b.stamp_; } tf::Quaternion rotation_; tf::Vector3 translation_; ros::Time stamp_; CompactFrameID frame_id_; CompactFrameID child_frame_id_; }; /** \brief A class to keep a sorted linked list in time * This builds and maintains a list of timestamped * data. And provides lookup functions to get * data out as a function of time. */ class TimeCache { public: static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below. 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. static const int64_t DEFAULT_MAX_STORAGE_TIME = 1ULL * 1000000000LL; //!< default value of 10 seconds storage TimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME)); bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); bool insertData(const TransformStorage& new_data); void clearList(); CompactFrameID getParent(ros::Time time, std::string* error_str); P_TimeAndFrameID getLatestTimeAndParent(); /// Debugging information methods unsigned int getListLength(); ros::Time getLatestTimestamp(); ros::Time getOldestTimestamp(); private: typedef std::set L_TransformStorage; L_TransformStorage storage_; ros::Duration max_storage_time_; /// A helper function for getData //Assumes storage is already locked for it inline uint8_t findClosest(const TransformStorage*& one, const TransformStorage*& two, ros::Time target_time, std::string* error_str); inline void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output); void pruneList(); }; } #endif // TF_TIME_CACHE_H geometry-1.13.2/tf/include/tf/transform_broadcaster.h000066400000000000000000000060731366756125100226550ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF_TRANSFORMBROADCASTER_H #define TF_TRANSFORMBROADCASTER_H #include "tf/tf.h" #include "tf/tfMessage.h" #include namespace tf { /** \brief This class provides an easy way to publish coordinate frame transform information. * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the * necessary data needed for each message. */ class TransformBroadcaster{ public: /** \brief Constructor (needs a ros::Node reference) */ TransformBroadcaster(); /** \brief Send a StampedTransform * The stamped data structure includes frame_id, and time, and parent_id already. */ void sendTransform(const StampedTransform & transform); /** \brief Send a vector of StampedTransforms * The stamped data structure includes frame_id, and time, and parent_id already. */ void sendTransform(const std::vector & transforms); /** \brief Send a TransformStamped message * The stamped data structure includes frame_id, and time, and parent_id already. */ void sendTransform(const geometry_msgs::TransformStamped & transform); /** \brief Send a vector of TransformStamped messages * The stamped data structure includes frame_id, and time, and parent_id already. */ void sendTransform(const std::vector & transforms); private: tf2_ros::TransformBroadcaster tf2_broadcaster_; }; } #endif //TF_TRANSFORMBROADCASTER_H geometry-1.13.2/tf/include/tf/transform_datatypes.h000066400000000000000000000270761366756125100223700ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF_TRANSFORM_DATATYPES_H #define TF_TRANSFORM_DATATYPES_H #include #include "geometry_msgs/PointStamped.h" #include "geometry_msgs/Vector3Stamped.h" #include "geometry_msgs/QuaternionStamped.h" #include "geometry_msgs/TransformStamped.h" #include "geometry_msgs/PoseStamped.h" #include "tf/LinearMath/Transform.h" #include "ros/time.h" #include "ros/console.h" namespace tf { typedef tf::Vector3 Point; typedef tf::Transform Pose; static const double QUATERNION_TOLERANCE = 0.1f; /** \brief The data type which will be cross compatable with geometry_msgs * This is the tf datatype equivilant of a MessageStamped */ template class Stamped : public T{ public: ros::Time stamp_; ///< The timestamp associated with this data std::string frame_id_; ///< The frame_id associated this data /** Default constructor */ Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation /** Full constructor */ Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id) : T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ; /** Set the data element */ void setData(const T& input){*static_cast(this) = input;}; }; /** \brief Comparison Operator for Stamped datatypes */ template bool operator==(const Stamped &a, const Stamped &b) { return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast(a) == static_cast(b); } /** \brief The Stamped Transform datatype used by tf */ class StampedTransform : public tf::Transform { public: ros::Time stamp_; ///< The timestamp associated with this transform std::string frame_id_; ///< The frame_id of the coordinate frame in which this transform is defined std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id): tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ } /** \brief Default constructor only to be used for preallocation */ StampedTransform() { }; /** \brief Set the inherited Traonsform data */ void setData(const tf::Transform& input){*static_cast(this) = input;}; }; /** \brief Comparison operator for StampedTransform */ static inline bool operator==(const StampedTransform &a, const StampedTransform &b) { return a.frame_id_ == b.frame_id_ && a.child_frame_id_ == b.child_frame_id_ && a.stamp_ == b.stamp_ && static_cast(a) == static_cast(b); } /** \brief convert Quaternion msg to Quaternion */ static inline void quaternionMsgToTF(const geometry_msgs::Quaternion& msg, Quaternion& bt) { bt = Quaternion(msg.x, msg.y, msg.z, msg.w); if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE) { ROS_WARN("MSG to TF: Quaternion Not Properly Normalized"); bt.normalize(); } } /** \brief convert Quaternion to Quaternion msg*/ static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quaternion& msg) { if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE) { ROS_WARN("TF to MSG: Quaternion Not Properly Normalized"); Quaternion bt_temp = bt; bt_temp.normalize(); msg.x = bt_temp.x(); msg.y = bt_temp.y(); msg.z = bt_temp.z(); msg.w = bt_temp.w(); } else { msg.x = bt.x(); msg.y = bt.y(); msg.z = bt.z(); msg.w = bt.w(); } } /** \brief Helper function for getting yaw from a Quaternion */ static inline double getYaw(const Quaternion& bt_q){ tfScalar useless_pitch, useless_roll, yaw; tf::Matrix3x3(bt_q).getRPY( useless_roll, useless_pitch,yaw); return yaw; } /** \brief Helper function for getting yaw from a Quaternion message*/ static inline double getYaw(const geometry_msgs::Quaternion& msg_q){ Quaternion bt_q; quaternionMsgToTF(msg_q, bt_q); return getYaw(bt_q); } /** \brief construct a Quaternion from Fixed angles * \param roll The roll about the X axis * \param pitch The pitch about the Y axis * \param yaw The yaw about the Z axis * \return The quaternion constructed */ static inline tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw){ Quaternion q; q.setRPY(roll, pitch, yaw); return q; } /** \brief construct a Quaternion from yaw only * \param yaw The yaw about the Z axis * \return The quaternion constructed */ static inline Quaternion createQuaternionFromYaw(double yaw){ Quaternion q; q.setRPY(0.0, 0.0, yaw); return q; } /** \brief construct a Quaternion Message from yaw only * \param yaw The yaw about the Z axis * \return The quaternion constructed */ static inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw){ Quaternion q; q.setRPY(0.0, 0.0, yaw); geometry_msgs::Quaternion q_msg; quaternionTFToMsg(q, q_msg); return q_msg; } /** \brief construct a Quaternion Message from Fixed angles * \param roll The roll about the X axis * \param pitch The pitch about the Y axis * \param yaw The yaw about the Z axis * \return The quaternion constructed */ static inline geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw){ geometry_msgs::Quaternion q_msg; quaternionTFToMsg(createQuaternionFromRPY(roll, pitch, yaw), q_msg); return q_msg; } /** \brief construct an Identity Quaternion * \return The quaternion constructed */ static inline tf::Quaternion createIdentityQuaternion() { Quaternion q; q.setRPY(0,0,0); return q; } /** \brief convert QuaternionStamped msg to Stamped */ static inline void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped & msg, Stamped& bt) {quaternionMsgToTF(msg.quaternion, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;} /** \brief convert Stamped to QuaternionStamped msg*/ static inline void quaternionStampedTFToMsg(const Stamped& bt, geometry_msgs::QuaternionStamped & msg) {quaternionTFToMsg(bt, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;} /** \brief convert Vector3 msg to Vector3 */ static inline void vector3MsgToTF(const geometry_msgs::Vector3& msg_v, Vector3& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);} /** \brief convert Vector3 to Vector3 msg*/ static inline void vector3TFToMsg(const Vector3& bt_v, geometry_msgs::Vector3& msg_v) {msg_v.x = bt_v.x(); msg_v.y = bt_v.y(); msg_v.z = bt_v.z();} /** \brief convert Vector3Stamped msg to Stamped */ static inline void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped & msg, Stamped& bt) {vector3MsgToTF(msg.vector, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;} /** \brief convert Stamped to Vector3Stamped msg*/ static inline void vector3StampedTFToMsg(const Stamped& bt, geometry_msgs::Vector3Stamped & msg) {vector3TFToMsg(bt, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;} /** \brief convert Point msg to Point */ static inline void pointMsgToTF(const geometry_msgs::Point& msg_v, Point& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);} /** \brief convert Point to Point msg*/ static inline void pointTFToMsg(const Point& bt_v, geometry_msgs::Point& msg_v) {msg_v.x = bt_v.x(); msg_v.y = bt_v.y(); msg_v.z = bt_v.z();} /** \brief convert PointStamped msg to Stamped */ static inline void pointStampedMsgToTF(const geometry_msgs::PointStamped & msg, Stamped& bt) {pointMsgToTF(msg.point, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;} /** \brief convert Stamped to PointStamped msg*/ static inline void pointStampedTFToMsg(const Stamped& bt, geometry_msgs::PointStamped & msg) {pointTFToMsg(bt, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;} /** \brief convert Transform msg to Transform */ static inline void transformMsgToTF(const geometry_msgs::Transform& msg, Transform& bt) {bt = Transform(Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), Vector3(msg.translation.x, msg.translation.y, msg.translation.z));} /** \brief convert Transform to Transform msg*/ static inline void transformTFToMsg(const Transform& bt, geometry_msgs::Transform& msg) {vector3TFToMsg(bt.getOrigin(), msg.translation); quaternionTFToMsg(bt.getRotation(), msg.rotation);} /** \brief convert TransformStamped msg to tf::StampedTransform */ static inline void transformStampedMsgToTF(const geometry_msgs::TransformStamped & msg, StampedTransform& bt) {transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id; bt.child_frame_id_ = msg.child_frame_id;} /** \brief convert tf::StampedTransform to TransformStamped msg*/ static inline void transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg) {transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.child_frame_id = bt.child_frame_id_;} /** \brief convert Pose msg to Pose */ static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt) {bt = Transform(Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w), Vector3(msg.position.x, msg.position.y, msg.position.z));} /** \brief convert Pose to Pose msg*/ static inline void poseTFToMsg(const Pose& bt, geometry_msgs::Pose& msg) {pointTFToMsg(bt.getOrigin(), msg.position); quaternionTFToMsg(bt.getRotation(), msg.orientation);} /** \brief convert PoseStamped msg to Stamped */ static inline void poseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, Stamped& bt) {poseMsgToTF(msg.pose, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;} /** \brief convert Stamped to PoseStamped msg*/ static inline void poseStampedTFToMsg(const Stamped& bt, geometry_msgs::PoseStamped & msg) {poseTFToMsg(bt, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;} } #endif //TF_TRANSFORM_DATATYPES_H geometry-1.13.2/tf/include/tf/transform_listener.h000066400000000000000000000210161366756125100222030ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF_TRANSFORMLISTENER_H #define TF_TRANSFORMLISTENER_H #include "sensor_msgs/PointCloud.h" #include "std_msgs/Empty.h" #include "tf/tfMessage.h" #include "tf/tf.h" #include "ros/ros.h" #include "ros/callback_queue.h" #include "ros/macros.h" #include "tf/FrameGraph.h" //frame graph service #include "boost/thread.hpp" #include namespace tf{ /** \brief Get the tf_prefix from the parameter server * \param nh The node handle to use to lookup the parameter. * \return The tf_prefix value for this NodeHandle */ inline std::string getPrefixParam(ros::NodeHandle & nh) { std::string param; if (!nh.searchParam("tf_prefix", param)) return ""; std::string return_val; nh.getParam(param, return_val); return return_val; } /** \brief resolve names * \deprecated Use TransformListener::remap instead */ ROS_DEPRECATED std::string remap(const std::string& frame_id); /** \brief This class inherits from Transformer and automatically subscribes to ROS transform messages */ class TransformListener : public Transformer { //subscribes to message and automatically stores incoming data public: /**@brief Constructor for transform listener * \param max_cache_time How long to store transform information */ TransformListener(ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true); /** * \brief Alternate constructor for transform listener * \param nh The NodeHandle to use for any ROS interaction * \param max_cache_time How long to store transform information */ TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true); ~TransformListener(); /* Methods from transformer unhiding them here */ using Transformer::transformQuaternion; using Transformer::transformVector; using Transformer::transformPoint; using Transformer::transformPose; /** \brief Transform a Stamped Quaternion Message into the target frame * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformQuaternion(const std::string& target_frame, const geometry_msgs::QuaternionStamped& stamped_in, geometry_msgs::QuaternionStamped& stamped_out) const; /** \brief Transform a Stamped Vector Message into the target frame * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformVector(const std::string& target_frame, const geometry_msgs::Vector3Stamped& stamped_in, geometry_msgs::Vector3Stamped& stamped_out) const; /** \brief Transform a Stamped Point Message into the target frame * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const; /** \brief Transform a Stamped Pose Message into the target frame * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformPose(const std::string& target_frame, const geometry_msgs::PoseStamped& stamped_in, geometry_msgs::PoseStamped& stamped_out) const; /* \brief Transform a Stamped Twist Message into the target frame * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ // http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review // void transformTwist(const std::string& target_frame, const geometry_msgs::TwistStamped& stamped_in, geometry_msgs::TwistStamped& stamped_out) const; /** \brief Transform a Stamped Quaternion Message into the target frame * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformQuaternion(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::QuaternionStamped& qin, const std::string& fixed_frame, geometry_msgs::QuaternionStamped& qout) const; /** \brief Transform a Stamped Vector Message into the target frame and time * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformVector(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::Vector3Stamped& vin, const std::string& fixed_frame, geometry_msgs::Vector3Stamped& vout) const; /** \brief Transform a Stamped Point Message into the target frame and time * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformPoint(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::PointStamped& pin, const std::string& fixed_frame, geometry_msgs::PointStamped& pout) const; /** \brief Transform a Stamped Pose Message into the target frame and time * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformPose(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::PoseStamped& pin, const std::string& fixed_frame, geometry_msgs::PoseStamped& pout) const; /** \brief Transform a sensor_msgs::PointCloud natively * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformPointCloud(const std::string& target_frame, const sensor_msgs::PointCloud& pcin, sensor_msgs::PointCloud& pcout) const; /** @brief Transform a sensor_msgs::PointCloud in space and time * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ void transformPointCloud(const std::string& target_frame, const ros::Time& target_time, const sensor_msgs::PointCloud& pcin, const std::string& fixed_frame, sensor_msgs::PointCloud& pcout) const; ///\todo move to high precision laser projector class void projectAndTransformLaserScan(const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& pcout); bool getFrames(tf::FrameGraph::Request&, tf::FrameGraph::Response& res) { res.dot_graph = allFramesAsDot(); return true; } /* \brief Resolve frame_name into a frame_id using tf_prefix parameter */ std::string resolve(const std::string& frame_name) { ros::NodeHandle n("~"); std::string prefix = tf::getPrefixParam(n); return tf::resolve(prefix, frame_name); }; protected: bool ok() const; private: // Must be above the listener ros::NodeHandle node_; /// replacing implementation with tf2_ros' tf2_ros::TransformListener tf2_listener_; /** @brief a helper function to be used for both transfrom pointCloud methods */ void transformPointCloud(const std::string & target_frame, const Transform& transform, const ros::Time& target_time, const sensor_msgs::PointCloud& pcin, sensor_msgs::PointCloud& pcout) const; }; } #endif //TF_TRANSFORMLISTENER_H geometry-1.13.2/tf/index.rst000066400000000000000000000002451366756125100157200ustar00rootroot00000000000000tf == .. toctree:: :maxdepth: 2 tf_python transformations Indices and tables ================== * :ref:`genindex` * :ref:`modindex` * :ref:`search` geometry-1.13.2/tf/mainpage.dox000066400000000000000000000041751366756125100163620ustar00rootroot00000000000000 /** \mainpage @htmlinclude manifest.html @b tf is a library for keeping track of coordinate frames. There are both C++ and Python bindings. @section usage Common Usage For most ROS use cases, the basic tf::Transformer library is not used directly. There are two helper classes to provide sending and recieving of ROS transform messages. tf::TransformBroadcaster and tf::TransformListener. @subsection listener TransformListener The tf::TransformListener class inherits from tf::Transformer to provide all the functionality of the basic library. In addition, it provides methods to transform data ROS messages directly and it automatically listens for transforms published on ROS. @subsection message_filter MessageFilter The tf::MessageFilter is the recommended method for receiving almost any sensor data from ROS. Data in ROS can be published with respect to any known frame. The tf::MessageFilter class makes it easy to use this data by providing callbacks only when it is possible to transform it into your desired target frame. The tf::MessageFilter class can subscribe to any ROS datatype that has a ROS Header. @subsection broadcaster TransformBroadcaster The tf::TransformBroadcaster class is the complement to the tf::TransformListener class. The broadcaster class provides a simple API for broadcasting coordinate frame transforms to other ROS nodes. @subsection send_transform send_transform The send_transform command is the easiest way to report transforms for fixed offsets. It is a simple command-line utility that repeatedly publishes the fixed-offset transform to ROS. @subsection datatypes Data Types used in tf - Quaternion typedef of btQuaternion - Vector3 typedef of btVector3 - Point typedef of btVector3 - Transform typedef of btTransform - Pose typedef of btTransform -Stamped version of all of the above inherits from the data type and also has: - ros::Time stamp_ - std::string frame_id_ - std::string child_frame_id_ (only used for Stamped ) - There are analogous ROS messages in std_msgs to the Stamped data types. - Time represented by ros::Time and ros::Duration in ros/time.h in roscpp */ geometry-1.13.2/tf/msg/000077500000000000000000000000001366756125100146445ustar00rootroot00000000000000geometry-1.13.2/tf/msg/tfMessage.msg000066400000000000000000000000541366756125100172710ustar00rootroot00000000000000geometry_msgs/TransformStamped[] transforms geometry-1.13.2/tf/package.xml000066400000000000000000000046311366756125100161770ustar00rootroot00000000000000 tf 1.13.2 tf is a package that lets the user keep track of multiple coordinate frames over time. tf maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.

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.
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.

Tully Foote Eitan Marder-Eppstein Wim Meeussen Tully Foote BSD http://www.ros.org/wiki/tf catkin angles geometry_msgs message_filters message_generation rosconsole roscpp rostime sensor_msgs std_msgs tf2_ros geometry_msgs graphviz message_filters message_runtime rosconsole roscpp roswtf sensor_msgs std_msgs tf2_ros rostest rosunit geometry-1.13.2/tf/remap_tf.launch000066400000000000000000000003501366756125100170450ustar00rootroot00000000000000 - {old: "/asdf", new: "/a"} - {old: "/fdsa", new: "/b"} geometry-1.13.2/tf/rosdoc.yaml000066400000000000000000000002731366756125100162350ustar00rootroot00000000000000 - builder: rosmake - builder: sphinx name: Python API output_dir: python - builder: doxygen name: C++ API output_dir: c++ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' geometry-1.13.2/tf/scripts/000077500000000000000000000000001366756125100155455ustar00rootroot00000000000000geometry-1.13.2/tf/scripts/bullet_migration_sed.py000077500000000000000000000063361366756125100223250ustar00rootroot00000000000000#!/usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2011, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of the Willow Garage nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # Run this script to update bullet datatypes from tf in Electric to # Fuerte/Unstable or newer # by default this assumes your files are not using the tf namespace. # If they are change the line below with the for loop to use the # namespaced_rules from __future__ import print_function import subprocess cmd = "find . -type f ! -name '*.svn-base' -a ! -name '*.hg' -a ! -name '*.git' -a \( -name '*.c*' -o -name '*.h*' \) -exec sed -i '%(rule)s' {} \;" rules = ['s|LinearMath/bt|tf/LinearMath/|g', # include path 's/btTransform\.h/Transform\.h/g', # include files 's/btMatrix3x3\.h/Matrix3x3\.h/g', 's/btScalar\.h/Scalar\.h/g', 's/btQuaternion\.h/Quaternion\.h/g', 's/btQuadWord\.h/QuadWord\.h/g', 's/btMinMax\.h/MinMax\.h/g', 's/btVector3\.h/Vector3\.h/g', 's/btScalar/tfScalar/g', ] unnamespaced_rules = [ 's/btTransform/tf::Transform/g', 's/btQuaternion/tf::Quaternion/g', 's/btVector3/tf::Vector3/g', 's/btMatrix3x3/tf::Matrix3x3/g', 's/btQuadWord/tf::QuadWord/g', ] namespaced_rules = [ 's/btTransform/Transform/g', 's/btQuaternion/Quaternion/g', 's/btVector3/Vector3/g', 's/btMatrix3x3/Matrix3x3/g', 's/btQuadWord/QuadWord/g', #'s/btScalar/Scalar/g', ] for rule in rules + unnamespaced_rules: #change me if using files with namespace tf set full_cmd = cmd%locals() print("Running {}".format(full_cmd)) ret_code = subprocess.call(full_cmd, shell=True) if ret_code == 0: print("success") else: print("failure") geometry-1.13.2/tf/scripts/groovy_compatibility/000077500000000000000000000000001366756125100220235ustar00rootroot00000000000000geometry-1.13.2/tf/scripts/groovy_compatibility/tf_remap000077500000000000000000000002601366756125100235440ustar00rootroot00000000000000#!/bin/sh echo "Please use the package local script inside tf not the global one, this is deprecated." echo "Running [rosrun tf tf_remap] for you" exec rosrun tf tf_remap "$@"geometry-1.13.2/tf/scripts/groovy_compatibility/view_frames000077500000000000000000000002661366756125100242640ustar00rootroot00000000000000#!/bin/sh echo "Please use the package local script inside tf not the global one, this is deprecated." echo "Running [rosrun tf view_frames] for you" exec rosrun tf view_frames "$@"geometry-1.13.2/tf/scripts/python_benchmark.py000066400000000000000000000070401366756125100214530ustar00rootroot00000000000000# Software License Agreement (BSD License) # # Copyright (c) 2011, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of the Willow Garage nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from __future__ import print_function import rostest import rospy import numpy import unittest import sys import time try: from cStringIO import StringIO except ImportError: from io import StringIO import tf.transformations import geometry_msgs.msg from tf.msg import tfMessage import tf iterations = 10000 t = tf.Transformer() def mkm(): m = geometry_msgs.msg.TransformStamped() m.header.frame_id = "PARENT" m.child_frame_id = "THISFRAME" m.transform.translation.y = 5.0 m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) return m tm = tfMessage([mkm() for i in range(20)]) def deserel_to_string(o): s = StringIO() o.serialize(s) return s.getvalue() mstr = deserel_to_string(tm) class Timer: def __init__(self, func): self.func = func def mean(self, iterations = 1000000): started = time.time() for i in range(iterations): self.func() took = time.time() - started return took / iterations import tf.msg import tf.cMsg for t in [tf.msg.tfMessage, tf.cMsg.tfMessage]: m2 = t() m2.deserialize(mstr) for m in m2.transforms: print(type(m), sys.getrefcount(m)) assert deserel_to_string(m2) == mstr, "deserel screwed up for type %s" % repr(t) m2 = t() print("deserialize only {} us each".format(1e6 * Timer(lambda: m2.deserialize(mstr)).mean()) sys.exit(0) started = time.time() for i in range(iterations): for m in tm.transforms: t.setTransform(m) took = time.time() - started print("setTransform only {} took {} us each".format(iterations, took, (1e6 * took / iterations))) started = time.time() for i in range(iterations): m2 = tfMessage() m2.deserialize(mstr) for m in m2.transforms: t.setTransform(m) took = time.time() - started print("deserialize+setTransform {} took {} us each".format(iterations, took, (1e6 * took / iterations))) from tf import TransformListener geometry-1.13.2/tf/scripts/tf_remap000077500000000000000000000052611366756125100172740ustar00rootroot00000000000000#!/usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # ## remap a tf topic from __future__ import print_function import rospy from tf.msg import tfMessage class TfRemapper: def __init__(self): self.pub = rospy.Publisher('/tf', tfMessage, queue_size=1) mappings = rospy.get_param('~mappings', []) self.mappings = {} for i in mappings: if "old" in i and "new" in i: self.mappings[i["old"]] = i["new"] print("Applying the following mappings to incoming tf frame ids", self.mappings) rospy.Subscriber("/tf_old", tfMessage, self.callback) def callback(self, tf_msg): for transform in tf_msg.transforms: if transform.header.frame_id in self.mappings: transform.header.frame_id = self.mappings[transform.header.frame_id] if transform.child_frame_id in self.mappings: transform.child_frame_id = self.mappings[transform.child_frame_id] self.pub.publish(tf_msg) def remap_tf(): pub.publish(Empty()) if __name__ == '__main__': rospy.init_node('tf_remapper') tfr = TfRemapper() rospy.spin() geometry-1.13.2/tf/scripts/view_frames000077500000000000000000000105741366756125100200110ustar00rootroot00000000000000#!/usr/bin/env python # Software License Agreement (BSD License) # # Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of the Willow Garage nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Revision $Id: gossipbot.py 1013 2008-05-21 01:08:56Z sfkwc $ from __future__ import print_function, with_statement import sys, time import os import string import re import subprocess import distutils.version from optparse import OptionParser import rospy from tf.srv import * import tf def listen(duration): rospy.init_node("view_frames", anonymous=True) tf_listener = tf.TransformListener() print("Listening to /tf for {} seconds".format(duration)) time.sleep(duration) print("Done Listening") return tf_listener.allFramesAsDot(rospy.Time.now()) def poll(node_name): print("Waiting for service {}/tf_frames".format(node_name)) rospy.wait_for_service('{}/tf_frames'.format(node_name)) try: print("Polling service") tf_frames_proxy = rospy.ServiceProxy('{}/tf_frames'.format(node_name), FrameGraph) output = tf_frames_proxy.call(FrameGraphRequest()) except rospy.ServiceException as e: print("Service call failed: {}".format(e)) return output.dot_graph def generate(dot_graph): with open('frames.gv', 'w') as outfile: outfile.write(dot_graph) try: # Check version, make postscript if too old to make pdf args = ["dot", "-V"] try: vstr = subprocess.Popen(args, stdout=subprocess.PIPE, stderr=subprocess.PIPE).communicate()[1] except OSError as ex: print("Warning: Could not execute `dot -V`. Is graphviz installed?") sys.exit(-1) v = distutils.version.StrictVersion('2.16') r = re.compile(".*version (\d+\.?\d*)") print(vstr) m = r.search(vstr) if not m or not m.group(1): print('Warning: failed to determine your version of dot. Assuming v2.16') else: version = distutils.version.StrictVersion(m.group(1)) print('Detected dot version {}'.format(version)) if version > distutils.version.StrictVersion('2.8'): subprocess.check_call(["dot", "-Tpdf", "frames.gv", "-o", "frames.pdf"]) print("frames.pdf generated") else: subprocess.check_call(["dot", "-Tps2", "frames.gv", "-o", "frames.ps"]) print("frames.ps generated") except subprocess.CalledProcessError: print("failed to generate frames.pdf", file=sys.stderr) if __name__ == '__main__': parser = OptionParser(usage="usage: %prog [options]", prog='viewFrames.py') parser.add_option("--node", metavar="node name", type="string", help="Node to get frames from", dest="node") options, args = parser.parse_args() dot_graph = '' if not options.node: dot_graph = listen(5.0) else: print("Generating graph for node: ", options.node) dot_graph = poll(options.node) generate(dot_graph) geometry-1.13.2/tf/setup.py000066400000000000000000000006411366756125100155710ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['tf'], package_dir={'': 'src'}, requires=['genmsg', 'genpy', 'roslib', 'rospkg', 'geometry_msgs', 'sensor_msgs', 'std_msgs'], scripts=['scripts/groovy_compatibility/tf_remap', 'scripts/groovy_compatibility/view_frames'] ) setup(**d) geometry-1.13.2/tf/src/000077500000000000000000000000001366756125100146455ustar00rootroot00000000000000geometry-1.13.2/tf/src/cache.cpp000066400000000000000000000202641366756125100164200ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "tf/time_cache.h" #include "tf/exceptions.h" #include "tf/LinearMath/Transform.h" #include #include "ros/assert.h" using namespace tf; TransformStorage::TransformStorage() { } TransformStorage::TransformStorage(const StampedTransform& data, CompactFrameID frame_id, CompactFrameID child_frame_id) : rotation_(data.getRotation()) , translation_(data.getOrigin()) , stamp_(data.stamp_) , frame_id_(frame_id) , child_frame_id_(child_frame_id) { } TimeCache::TimeCache(ros::Duration max_storage_time) : max_storage_time_(max_storage_time) {} // hoisting these into separate functions causes an ~8% speedup. Removing calling them altogether adds another ~10% void createEmptyException(std::string *error_str) { if (error_str) { *error_str = "Unable to lookup transform, cache is empty"; } } void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* error_str) { if (error_str) { std::stringstream ss; ss << "Lookup would require extrapolation at time " << t0 << ", but only time " << t1 << " is in the buffer"; *error_str = ss.str(); } } void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str) { if (error_str) { std::stringstream ss; ss << "Lookup would require extrapolation into the future. Requested time " << t0 << " but the latest data is at time " << t1; *error_str = ss.str(); } } void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str) { if (error_str) { std::stringstream ss; ss << "Lookup would require extrapolation into the past. Requested time " << t0 << " but the earliest data is at time " << t1; *error_str = ss.str(); } } uint8_t TimeCache::findClosest(const TransformStorage*& one, const TransformStorage*& two, ros::Time target_time, std::string* error_str) { //No values stored if (storage_.empty()) { createEmptyException(error_str); return 0; } //If time == 0 return the latest if (target_time.isZero()) { one = &(*storage_.rbegin()); return 1; } // One value stored if (++storage_.begin() == storage_.end()) { const TransformStorage& ts = *storage_.begin(); if (ts.stamp_ == target_time) { one = &ts; return 1; } else { createExtrapolationException1(target_time, ts.stamp_, error_str); return 0; } } ros::Time latest_time = (*storage_.rbegin()).stamp_; ros::Time earliest_time = (*(storage_.begin())).stamp_; if (target_time == latest_time) { one = &(*storage_.rbegin()); return 1; } else if (target_time == earliest_time) { one = &(*storage_.begin()); return 1; } // Catch cases that would require extrapolation else if (target_time > latest_time) { createExtrapolationException2(target_time, latest_time, error_str); return 0; } else if (target_time < earliest_time) { createExtrapolationException3(target_time, earliest_time, error_str); return 0; } //Create a temporary object to compare to when searching the lower bound via std::set TransformStorage tmp; tmp.stamp_ = target_time; //Find the first value equal or higher than the target value L_TransformStorage::iterator storage_it = storage_.upper_bound(tmp); //Finally the case were somewhere in the middle Guarenteed no extrapolation :-) two = &*(storage_it); //Newer one = &*(--storage_it); //Older return 2; } void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output) { // Check for zero distance case if( two.stamp_ == one.stamp_ ) { output = two; return; } //Calculate the ratio tfScalar ratio = (time.toSec() - one.stamp_.toSec()) / (two.stamp_.toSec() - one.stamp_.toSec()); //Interpolate translation output.translation_.setInterpolate3(one.translation_, two.translation_, ratio); //Interpolate rotation output.rotation_ = slerp( one.rotation_, two.rotation_, ratio); output.stamp_ = one.stamp_; output.frame_id_ = one.frame_id_; output.child_frame_id_ = one.child_frame_id_; } bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available { const TransformStorage* p_temp_1 = NULL; const TransformStorage* p_temp_2 = NULL; int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); if (num_nodes == 0) { return false; } else if (num_nodes == 1) { data_out = *p_temp_1; } else if (num_nodes == 2) { if( p_temp_1->frame_id_ == p_temp_2->frame_id_) { interpolate(*p_temp_1, *p_temp_2, time, data_out); } else { data_out = *p_temp_1; } } else { ROS_BREAK(); } return true; } CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str) { const TransformStorage* p_temp_1 = NULL; const TransformStorage* p_temp_2 = NULL; int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); if (num_nodes == 0) { return 0; } return p_temp_1->frame_id_; } bool TimeCache::insertData(const TransformStorage& new_data) { if (storage_.begin() != storage_.end()) { // trying to add data that dates back longer than we want to keep history if (storage_.rbegin()->stamp_ > new_data.stamp_ + max_storage_time_) return false; // if we already have data at that exact time, delete it to ensure the latest data is stored if (storage_.rbegin()->stamp_ >= new_data.stamp_) { L_TransformStorage::iterator storage_it = storage_.find(new_data); if (storage_it != storage_.end()) storage_.erase(storage_it); } } storage_.insert(storage_.end(), new_data); pruneList(); return true; } void TimeCache::clearList() { storage_.clear(); } unsigned int TimeCache::getListLength() { return storage_.size(); } P_TimeAndFrameID TimeCache::getLatestTimeAndParent() { if (storage_.empty()) { return std::make_pair(ros::Time(), 0); } const TransformStorage& ts = *storage_.rbegin(); return std::make_pair(ts.stamp_, ts.frame_id_); } ros::Time TimeCache::getLatestTimestamp() { if (storage_.empty()) return ros::Time(); //empty list case return storage_.rbegin()->stamp_; } ros::Time TimeCache::getOldestTimestamp() { if (storage_.empty()) return ros::Time(); //empty list case return storage_.begin()->stamp_; } void TimeCache::pruneList() { ros::Time latest_time = storage_.rbegin()->stamp_; while(!storage_.empty() && storage_.begin()->stamp_ + max_storage_time_ < latest_time) { storage_.erase(storage_.begin()); } } geometry-1.13.2/tf/src/change_notifier.cpp000066400000000000000000000212031366756125100204730ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "ros/ros.h" #include "tf/transform_listener.h" #include "xmlrpcpp/XmlRpcValue.h" class FramePair { public: FramePair(const std::string& source_frame, const std::string& target_frame, double translational_update_distance, double angular_update_distance) : source_frame_(source_frame), target_frame_(target_frame), translational_update_distance_(translational_update_distance), angular_update_distance_(angular_update_distance) { pose_in_ = tf::Stamped(tf::Pose(tf::createIdentityQuaternion(), tf::Vector3(0, 0, 0)), ros::Time(), source_frame_); } public: std::string source_frame_; std::string target_frame_; tf::Stamped pose_in_; tf::Stamped pose_out_; tf::Stamped last_sent_pose_; double translational_update_distance_; double angular_update_distance_; }; bool getFramePairs(const ros::NodeHandle& local_node, std::vector& frame_pairs, double default_translational_update_distance, double default_angular_update_distance) { XmlRpc::XmlRpcValue frame_pairs_param; if (!local_node.getParam("frame_pairs", frame_pairs_param)) { // No frame_pairs parameter provided. Default to base_link->map. frame_pairs.push_back(FramePair("base_link", "map", default_translational_update_distance, default_angular_update_distance)); return true; } if (frame_pairs_param.getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR("Expecting a list for frame_pairs parameter"); return false; } for (int i = 0; i < frame_pairs_param.size(); i++) { XmlRpc::XmlRpcValue frame_pair_param = frame_pairs_param[i]; if (frame_pair_param.getType() != XmlRpc::XmlRpcValue::TypeStruct) { ROS_ERROR("frame_pairs must be specified as maps, but they are XmlRpcType: %d", frame_pair_param.getType()); return false; } // Get the source_frame if (!frame_pair_param.hasMember("source_frame")) { ROS_ERROR("frame_pair does not specified source_frame"); return false; } XmlRpc::XmlRpcValue source_frame_param = frame_pair_param["source_frame"]; if (source_frame_param.getType() != XmlRpc::XmlRpcValue::TypeString) { ROS_ERROR("source_frame must be a string, but it is XmlRpcType: %d", source_frame_param.getType()); return false; } std::string source_frame = source_frame_param; // Get the target_frame if (!frame_pair_param.hasMember("target_frame")) { ROS_ERROR("frame_pair does not specified target_frame"); return false; } XmlRpc::XmlRpcValue target_frame_param = frame_pair_param["target_frame"]; if (target_frame_param.getType() != XmlRpc::XmlRpcValue::TypeString) { ROS_ERROR("target_frame must be a string, but it is XmlRpcType: %d", target_frame_param.getType()); return false; } std::string target_frame = target_frame_param; // Get the (optional) translational_update_distance double translational_update_distance = default_translational_update_distance; if (frame_pair_param.hasMember("translational_update_distance")) { XmlRpc::XmlRpcValue translational_update_distance_param = frame_pair_param["translational_update_distance"]; if (translational_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeDouble && translational_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeInt) { ROS_ERROR("translational_update_distance must be either an integer or a double, but it is XmlRpcType: %d", translational_update_distance_param.getType()); return false; } translational_update_distance = translational_update_distance_param; } // Get the (optional) angular_update_distance double angular_update_distance = default_angular_update_distance; if (frame_pair_param.hasMember("angular_update_distance")) { XmlRpc::XmlRpcValue angular_update_distance_param = frame_pair_param["angular_update_distance"]; if (angular_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeDouble && angular_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeInt) { ROS_ERROR("angular_update_distance must be either an integer or a double, but it is XmlRpcType: %d", angular_update_distance_param.getType()); return false; } angular_update_distance = angular_update_distance_param; } ROS_INFO("Notifying change on %s -> %s (translational update distance: %.4f, angular update distance: %.4f)", source_frame.c_str(), target_frame.c_str(), translational_update_distance, angular_update_distance); frame_pairs.push_back(FramePair(source_frame, target_frame, translational_update_distance, angular_update_distance)); } return true; } /** This is a program to provide notifications of changes of state within tf * It was written for providing an easy way to on demand update a web graphic of * where the robot is located. It's not designed or recommended for use in live * operation for feedback. */ int main(int argc, char** argv) { ros::init(argc, argv, "change_notifier", ros::init_options::AnonymousName); ros::NodeHandle node; ros::NodeHandle local_node("~"); double polling_frequency, translational_update_distance, angular_update_distance; local_node.param(std::string("polling_frequency"), polling_frequency, 10.0); local_node.param(std::string("translational_update_distance"), translational_update_distance, 0.10); local_node.param(std::string("angular_update_distance"), angular_update_distance, 0.10); std::vector frame_pairs; if (!getFramePairs(local_node, frame_pairs, translational_update_distance, angular_update_distance)) return 1; tf::TransformListener tfl(node); // Advertise the service ros::Publisher pub = node.advertise("tf_changes", 1, true); while (node.ok()) { try { tf::tfMessage msg; for (std::vector::iterator i = frame_pairs.begin(); i != frame_pairs.end(); i++) { FramePair& fp = *i; tfl.transformPose(fp.target_frame_, fp.pose_in_, fp.pose_out_); const tf::Vector3& origin = fp.pose_out_.getOrigin(); const tf::Quaternion& rotation = fp.pose_out_.getRotation(); if (origin.distance(fp.last_sent_pose_.getOrigin()) > fp.translational_update_distance_ || rotation.angle(fp.last_sent_pose_.getRotation()) > fp.angular_update_distance_) { fp.last_sent_pose_ = fp.pose_out_; tf::StampedTransform stampedTf(tf::Transform(rotation, origin), fp.pose_out_.stamp_, "/" + fp.target_frame_, "/" + fp.source_frame_); geometry_msgs::TransformStamped msgtf; transformStampedTFToMsg(stampedTf, msgtf); msg.transforms.push_back(msgtf); } } if (msg.transforms.size() > 0) pub.publish(msg); } catch (tf::TransformException& ex) { ROS_DEBUG("Exception: %s\n", ex.what()); } // Sleep until next polling if (polling_frequency > 0) ros::Duration().fromSec(1.0 / polling_frequency).sleep(); } return 0; } geometry-1.13.2/tf/src/empty_listener.cpp000066400000000000000000000041071366756125100204160ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include int main(int argc, char** argv) { ros::init(argc, argv, "my_tf_listener"); ros::NodeHandle node; tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()){ tf::StampedTransform transform; try { listener.lookupTransform("odom_combined", "base_link", ros::Time(0), transform); } catch (tf::TransformException ex) { ROS_ERROR("%s",ex.what()); } rate.sleep(); } return 0; } geometry-1.13.2/tf/src/static_transform_publisher.cpp000066400000000000000000000117031366756125100230120ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include "tf/transform_broadcaster.h" class TransformSender { public: ros::NodeHandle node_; //constructor 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) { tf::Quaternion q; q.setRPY(roll, pitch,yaw); transform_ = tf::StampedTransform(tf::Transform(q, tf::Vector3(x,y,z)), time, frame_id, child_frame_id ); }; 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) : transform_(tf::Transform(tf::Quaternion(qx,qy,qz,qw), tf::Vector3(x,y,z)), time, frame_id, child_frame_id){}; //Clean up ros connections ~TransformSender() { } //A pointer to the rosTFServer class tf::TransformBroadcaster broadcaster; // A function to call to send data periodically void send (ros::Time time) { transform_.stamp_ = time; broadcaster.sendTransform(transform_); }; private: tf::StampedTransform transform_; }; int main(int argc, char ** argv) { //Initialize ROS ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName); if(argc == 11) { ros::Duration sleeper(atof(argv[10])/1000.0); if (strcmp(argv[8], argv[9]) == 0) ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]); TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]), atof(argv[4]), atof(argv[5]), atof(argv[6]), atof(argv[7]), ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout argv[8], argv[9]); while(tf_sender.node_.ok()) { tf_sender.send(ros::Time::now() + sleeper); ROS_DEBUG("Sending transform from %s with parent %s\n", argv[8], argv[9]); sleeper.sleep(); } return 0; } else if (argc == 10) { ros::Duration sleeper(atof(argv[9])/1000.0); if (strcmp(argv[7], argv[8]) == 0) ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[7], argv[8]); TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]), atof(argv[4]), atof(argv[5]), atof(argv[6]), ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout argv[7], argv[8]); while(tf_sender.node_.ok()) { tf_sender.send(ros::Time::now() + sleeper); ROS_DEBUG("Sending transform from %s with parent %s\n", argv[7], argv[8]); sleeper.sleep(); } return 0; } else { printf("A command line utility for manually sending a transform.\n"); printf("It will periodicaly republish the given transform. \n"); printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period(milliseconds) \n"); printf("OR \n"); printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period(milliseconds) \n"); printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n"); printf("of the child_frame_id. \n"); ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments"); return -1; } } geometry-1.13.2/tf/src/tf.cpp000066400000000000000000000470761366756125100160000ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "tf/tf.h" #include #include "ros/assert.h" #include "ros/ros.h" #include using namespace tf; // Must provide storage for non-integral static const class members. // Otherwise you get undefined symbol errors on OS X (why not on Linux?). // Thanks to Rob for pointing out the right way to do this. // In C++0x this must be initialized here #5401 const double tf::Transformer::DEFAULT_CACHE_TIME = 10.0; enum WalkEnding { Identity, TargetParentOfSource, SourceParentOfTarget, FullPath, }; struct CanTransformAccum { CompactFrameID gather(TimeCache* cache, ros::Time time, std::string* error_string) { return cache->getParent(time, error_string); } void accum(bool source) { } void finalize(WalkEnding end, ros::Time _time) { } TransformStorage st; }; struct TransformAccum { TransformAccum() : source_to_top_quat(0.0, 0.0, 0.0, 1.0) , source_to_top_vec(0.0, 0.0, 0.0) , target_to_top_quat(0.0, 0.0, 0.0, 1.0) , target_to_top_vec(0.0, 0.0, 0.0) , result_quat(0.0, 0.0, 0.0, 1.0) , result_vec(0.0, 0.0, 0.0) { } CompactFrameID gather(TimeCache* cache, ros::Time time, std::string* error_string) { if (!cache->getData(time, st, error_string)) { return 0; } return st.frame_id_; } void accum(bool source) { if (source) { source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_; source_to_top_quat = st.rotation_ * source_to_top_quat; } else { target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_; target_to_top_quat = st.rotation_ * target_to_top_quat; } } void finalize(WalkEnding end, ros::Time _time) { switch (end) { case Identity: break; case TargetParentOfSource: result_vec = source_to_top_vec; result_quat = source_to_top_quat; break; case SourceParentOfTarget: { tf::Quaternion inv_target_quat = target_to_top_quat.inverse(); tf::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); result_vec = inv_target_vec; result_quat = inv_target_quat; break; } case FullPath: { tf::Quaternion inv_target_quat = target_to_top_quat.inverse(); tf::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec; result_quat = inv_target_quat * source_to_top_quat; } break; }; time = _time; } TransformStorage st; ros::Time time; tf::Quaternion source_to_top_quat; tf::Vector3 source_to_top_vec; tf::Quaternion target_to_top_quat; tf::Vector3 target_to_top_vec; tf::Quaternion result_quat; tf::Vector3 result_vec; }; std::string assert_resolved(const std::string& prefix, const std::string& frame_id) { ROS_DEBUG("tf::assert_resolved just calls tf::resolve"); return tf::resolve(prefix, frame_id); } std::string tf::resolve(const std::string& prefix, const std::string& frame_name) { // printf ("resolveping prefix:%s with frame_name:%s\n", prefix.c_str(), frame_name.c_str()); if (frame_name.size() > 0) if (frame_name[0] == '/') { return strip_leading_slash(frame_name); } if (prefix.size() > 0) { if (prefix[0] == '/') { std::string composite = strip_leading_slash(prefix); composite.append("/"); composite.append(frame_name); return composite; } else { std::string composite; composite.append(prefix); composite.append("/"); composite.append(frame_name); return composite; } } else { std::string composite; composite.append(frame_name); return composite; } } std::string tf::strip_leading_slash(const std::string& frame_name) { if (frame_name.size() > 0) if (frame_name[0] == '/') { std::string shorter = frame_name; shorter.erase(0,1); return shorter; } return frame_name; } Transformer::Transformer(bool interpolating, ros::Duration cache_time): fall_back_to_wall_time_(false), tf2_buffer_ptr_(std::make_shared(cache_time)) { } Transformer::~Transformer() { } void Transformer::clear() { tf2_buffer_ptr_->clear(); } bool Transformer::setTransform(const StampedTransform& transform, const std::string& authority) { geometry_msgs::TransformStamped msgtf; transformStampedTFToMsg(transform, msgtf); return tf2_buffer_ptr_->setTransform(msgtf, authority); } void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, StampedTransform& transform) const { geometry_msgs::TransformStamped output = tf2_buffer_ptr_->lookupTransform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time); transformStampedMsgToTF(output, transform); return; } void Transformer::lookupTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const { geometry_msgs::TransformStamped output = tf2_buffer_ptr_->lookupTransform(strip_leading_slash(target_frame), target_time, strip_leading_slash(source_frame), source_time, strip_leading_slash(fixed_frame)); transformStampedMsgToTF(output, transform); } void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const ros::Time& time, const ros::Duration& averaging_interval, geometry_msgs::Twist& twist) const { // ref point is origin of tracking_frame, ref_frame = obs_frame lookupTwist(tracking_frame, observation_frame, observation_frame, tf::Point(0,0,0), tracking_frame, time, averaging_interval, twist); } void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame, const tf::Point & reference_point, const std::string& reference_point_frame, const ros::Time& time, const ros::Duration& averaging_interval, geometry_msgs::Twist& twist) const { ros::Time latest_time, target_time; getLatestCommonTime(observation_frame, tracking_frame, latest_time, NULL); ///\TODO check time on reference point too if (ros::Time() == time) target_time = latest_time; else target_time = time; ros::Time end_time = std::min(target_time + averaging_interval *0.5 , latest_time); ros::Time start_time = std::max(ros::Time().fromSec(.00001) + averaging_interval, end_time) - averaging_interval; // don't collide with zero ros::Duration corrected_averaging_interval = end_time - start_time; //correct for the possiblity that start time was truncated above. StampedTransform start, end; lookupTransform(observation_frame, tracking_frame, start_time, start); lookupTransform(observation_frame, tracking_frame, end_time, end); tf::Matrix3x3 temp = start.getBasis().inverse() * end.getBasis(); tf::Quaternion quat_temp; temp.getRotation(quat_temp); tf::Vector3 o = start.getBasis() * quat_temp.getAxis(); tfScalar ang = quat_temp.getAngle(); double delta_x = end.getOrigin().getX() - start.getOrigin().getX(); double delta_y = end.getOrigin().getY() - start.getOrigin().getY(); double delta_z = end.getOrigin().getZ() - start.getOrigin().getZ(); tf::Vector3 twist_vel ((delta_x)/corrected_averaging_interval.toSec(), (delta_y)/corrected_averaging_interval.toSec(), (delta_z)/corrected_averaging_interval.toSec()); tf::Vector3 twist_rot = o * (ang / corrected_averaging_interval.toSec()); // This is a twist w/ reference frame in observation_frame and reference point is in the tracking_frame at the origin (at start_time) //correct for the position of the reference frame tf::StampedTransform inverse; lookupTransform(reference_frame,tracking_frame, target_time, inverse); tf::Vector3 out_rot = inverse.getBasis() * twist_rot; tf::Vector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().cross(out_rot); //Rereference the twist about a new reference point // Start by computing the original reference point in the reference frame: tf::Stamped rp_orig(tf::Point(0,0,0), target_time, tracking_frame); transformPoint(reference_frame, rp_orig, rp_orig); // convert the requrested reference point into the right frame tf::Stamped rp_desired(reference_point, target_time, reference_point_frame); transformPoint(reference_frame, rp_desired, rp_desired); // compute the delta tf::Point delta = rp_desired - rp_orig; // Correct for the change in reference point out_vel = out_vel + out_rot * delta; // out_rot unchanged /* printf("KDL: Rotation %f %f %f, Translation:%f %f %f\n", out_rot.x(),out_rot.y(),out_rot.z(), out_vel.x(),out_vel.y(),out_vel.z()); */ twist.linear.x = out_vel.x(); twist.linear.y = out_vel.y(); twist.linear.z = out_vel.z(); twist.angular.x = out_rot.x(); twist.angular.y = out_rot.y(); twist.angular.z = out_rot.z(); } bool Transformer::waitForTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration, std::string* error_msg) const { return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time, timeout, error_msg); } bool Transformer::canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, std::string* error_msg) const { return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time, error_msg); } bool Transformer::canTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, std::string* error_msg) const { return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame), target_time, strip_leading_slash(source_frame), source_time, strip_leading_slash(fixed_frame), error_msg); } bool Transformer::waitForTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration, std::string* error_msg) const { return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame), target_time, strip_leading_slash(source_frame), source_time, strip_leading_slash(fixed_frame), timeout, error_msg); } bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent) const { return tf2_buffer_ptr_->_getParent(strip_leading_slash(frame_id), time, parent); } bool Transformer::frameExists(const std::string& frame_id_str) const { return tf2_buffer_ptr_->_frameExists(strip_leading_slash(frame_id_str)); } void Transformer::setExtrapolationLimit(const ros::Duration& distance) { ROS_WARN("Transformer::setExtrapolationLimit is deprecated and does not do anything"); } struct TimeAndFrameIDFrameComparator { TimeAndFrameIDFrameComparator(CompactFrameID id) : id(id) {} bool operator()(const P_TimeAndFrameID& rhs) const { return rhs.second == id; } CompactFrameID id; }; int Transformer::getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time& time, std::string* error_string) const { CompactFrameID target_id = tf2_buffer_ptr_->_lookupFrameNumber(strip_leading_slash(target_frame)); CompactFrameID source_id = tf2_buffer_ptr_->_lookupFrameNumber(strip_leading_slash(source_frame)); return tf2_buffer_ptr_->_getLatestCommonTime(source_id, target_id, time, error_string); } //@todo - Fix this to work with new data structures void Transformer::chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string& fixed_frame, std::vector& output) const { tf2_buffer_ptr_->_chainAsVector(target_frame, target_time, source_frame, source_time, fixed_frame, output); } std::string Transformer::allFramesAsString() const { return tf2_buffer_ptr_->allFramesAsString(); } std::string Transformer::allFramesAsDot(double current_time) const { return tf2_buffer_ptr_->_allFramesAsDot(current_time); } bool Transformer::ok() const { return true; } void Transformer::getFrameStrings(std::vector & vec) const { tf2_buffer_ptr_->_getFrameStrings(vec); } void Transformer::transformQuaternion(const std::string& target_frame, const Stamped& stamped_in, Stamped& stamped_out) const { tf::assertQuaternionValid(stamped_in); StampedTransform transform; lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform); stamped_out.setData( transform * stamped_in); stamped_out.stamp_ = transform.stamp_; stamped_out.frame_id_ = target_frame; } void Transformer::transformVector(const std::string& target_frame, const Stamped& stamped_in, Stamped& stamped_out) const { StampedTransform transform; lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform); /** \todo may not be most efficient */ tf::Vector3 end = stamped_in; tf::Vector3 origin = tf::Vector3(0,0,0); tf::Vector3 output = (transform * end) - (transform * origin); stamped_out.setData( output); stamped_out.stamp_ = transform.stamp_; stamped_out.frame_id_ = target_frame; } void Transformer::transformPoint(const std::string& target_frame, const Stamped& stamped_in, Stamped& stamped_out) const { StampedTransform transform; lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform); stamped_out.setData(transform * stamped_in); stamped_out.stamp_ = transform.stamp_; stamped_out.frame_id_ = target_frame; } void Transformer::transformPose(const std::string& target_frame, const Stamped& stamped_in, Stamped& stamped_out) const { StampedTransform transform; lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform); stamped_out.setData(transform * stamped_in); stamped_out.stamp_ = transform.stamp_; stamped_out.frame_id_ = target_frame; } void Transformer::transformQuaternion(const std::string& target_frame, const ros::Time& target_time, const Stamped& stamped_in, const std::string& fixed_frame, Stamped& stamped_out) const { tf::assertQuaternionValid(stamped_in); StampedTransform transform; lookupTransform(target_frame, target_time, stamped_in.frame_id_,stamped_in.stamp_, fixed_frame, transform); stamped_out.setData( transform * stamped_in); stamped_out.stamp_ = transform.stamp_; stamped_out.frame_id_ = target_frame; } void Transformer::transformVector(const std::string& target_frame, const ros::Time& target_time, const Stamped& stamped_in, const std::string& fixed_frame, Stamped& stamped_out) const { StampedTransform transform; lookupTransform(target_frame, target_time, stamped_in.frame_id_,stamped_in.stamp_, fixed_frame, transform); /** \todo may not be most efficient */ tf::Vector3 end = stamped_in; tf::Vector3 origin = tf::Vector3(0,0,0); tf::Vector3 output = (transform * end) - (transform * origin); stamped_out.setData( output); stamped_out.stamp_ = transform.stamp_; stamped_out.frame_id_ = target_frame; } void Transformer::transformPoint(const std::string& target_frame, const ros::Time& target_time, const Stamped& stamped_in, const std::string& fixed_frame, Stamped& stamped_out) const { StampedTransform transform; lookupTransform(target_frame, target_time, stamped_in.frame_id_,stamped_in.stamp_, fixed_frame, transform); stamped_out.setData(transform * stamped_in); stamped_out.stamp_ = transform.stamp_; stamped_out.frame_id_ = target_frame; } void Transformer::transformPose(const std::string& target_frame, const ros::Time& target_time, const Stamped& stamped_in, const std::string& fixed_frame, Stamped& stamped_out) const { StampedTransform transform; lookupTransform(target_frame, target_time, stamped_in.frame_id_,stamped_in.stamp_, fixed_frame, transform); stamped_out.setData(transform * stamped_in); stamped_out.stamp_ = transform.stamp_; stamped_out.frame_id_ = target_frame; } boost::signals2::connection Transformer::addTransformsChangedListener(boost::function callback) { return tf2_buffer_ptr_->_addTransformsChangedListener(callback); } void Transformer::removeTransformsChangedListener(boost::signals2::connection c) { tf2_buffer_ptr_->_removeTransformsChangedListener(c); } geometry-1.13.2/tf/src/tf/000077500000000000000000000000001366756125100152565ustar00rootroot00000000000000geometry-1.13.2/tf/src/tf/__init__.py000066400000000000000000000034751366756125100174000ustar00rootroot00000000000000# Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from __future__ import absolute_import from tf2_ros import TransformException as Exception, ConnectivityException, LookupException, ExtrapolationException from .listener import Transformer, TransformListener, TransformerROS from .broadcaster import TransformBroadcaster geometry-1.13.2/tf/src/tf/broadcaster.py000066400000000000000000000063211366756125100201230ustar00rootroot00000000000000# Software License Agreement (BSD License) # # Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of the Willow Garage nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import geometry_msgs.msg import tf2_ros.transform_broadcaster class TransformBroadcaster: """ :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. """ def __init__(self, queue_size=100): self.tf2_broadcaster = tf2_ros.transform_broadcaster.TransformBroadcaster() def sendTransform(self, translation, rotation, time, child, parent): """ :param translation: the translation of the transformtion as a tuple (x, y, z) :param rotation: the rotation of the transformation as a tuple (x, y, z, w) :param time: the time of the transformation, as a rospy.Time() :param child: child frame in tf, string :param parent: parent frame in tf, string Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``. """ t = geometry_msgs.msg.TransformStamped() t.header.frame_id = parent t.header.stamp = time t.child_frame_id = child t.transform.translation.x = translation[0] t.transform.translation.y = translation[1] t.transform.translation.z = translation[2] t.transform.rotation.x = rotation[0] t.transform.rotation.y = rotation[1] t.transform.rotation.z = rotation[2] t.transform.rotation.w = rotation[3] self.sendTransformMessage(t) def sendTransformMessage(self, transform): """ :param transform: geometry_msgs.msg.TransformStamped Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``. """ self.tf2_broadcaster.sendTransform([transform]) geometry-1.13.2/tf/src/tf/listener.py000066400000000000000000000425501366756125100174630ustar00rootroot00000000000000# Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import rospy import numpy import yaml import geometry_msgs.msg import sensor_msgs.msg import tf2_ros from . import transformations def xyz_to_mat44(pos): return transformations.translation_matrix((pos.x, pos.y, pos.z)) def xyzw_to_mat44(ori): return transformations.quaternion_matrix((ori.x, ori.y, ori.z, ori.w)) def strip_leading_slash(s): return s[1:] if s.startswith("/") else s ## Proxy Transformer class to call TF2 methods class Transformer(object): def __init__(self, interpolate=True, cache_time=None): self._buffer = tf2_ros.Buffer(cache_time, debug=False) self._using_dedicated_thread = False def allFramesAsDot(self, current_time=None): if current_time: return self._buffer._allFramesAsDot(current_time) return self._buffer._allFramesAsDot() def allFramesAsString(self): return self._buffer.all_frames_as_string() def setTransform(self, transform, authority="default_authority"): self._buffer.set_transform(transform, authority) def canTransform(self, target_frame, source_frame, time): return self._buffer.can_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time) def canTransformFull(self, target_frame, target_time, source_frame, source_time, fixed_frame): return self._buffer.can_transform_full(strip_leading_slash(target_frame), target_time, strip_leading_slash(source_frame), source_time, strip_leading_slash(fixed_frame)) def waitForTransform(self, target_frame, source_frame, time, timeout, polling_sleep_duration=None): if not self._using_dedicated_thread: raise tf2_ros.TransformException("cannot wait for transform without a dedicated thread that listens to incoming TF messages") can_transform, error_msg = self._buffer.can_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time, timeout, return_debug_tuple=True) if not can_transform: raise tf2_ros.TransformException(error_msg or "no such transformation: \"{}\" -> \"{}\"".format(source_frame, target_frame)) def waitForTransformFull(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout, polling_sleep_duration=None): if not self._using_dedicated_thread: raise tf2_ros.TransformException("cannot wait for transform without a dedicated thread that listens to incoming TF messages") can_transform, error_msg = self._buffer.can_transform_full(strip_leading_slash(target_frame), target_time, strip_leading_slash(source_frame), source_time, strip_leading_slash(fixed_frame), timeout, return_debug_tuple=True) if not can_transform: raise tf2_ros.TransformException(error_msg or "no such transformation: \"{}\" -> \"{}\"".format(source_frame, target_frame)) def chain(self, target_frame, target_time, source_frame, source_time, fixed_frame): return self._buffer._chain( target_frame, target_time, source_frame, source_time, fixed_frame) def clear(self): self._buffer.clear() def frameExists(self, frame_id): """ Not a recommended API, only here for backwards compatibility """ return frame_id in self.getFrameStrings() def getFrameStrings(self): """ Not a recommended API, only here for backwards compatibility """ data = yaml.load(self._buffer.all_frames_as_yaml()) or {} return [p for p, _ in data.items()] def getLatestCommonTime(self, source_frame, dest_frame): return self._buffer.get_latest_common_time(strip_leading_slash(source_frame), strip_leading_slash(dest_frame)) def lookupTransform(self, target_frame, source_frame, time): msg = self._buffer.lookup_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time) t = msg.transform.translation r = msg.transform.rotation return [t.x, t.y, t.z], [r.x, r.y, r.z, r.w] def lookupTransformFull(self, target_frame, target_time, source_frame, source_time, fixed_frame): msg = self._buffer.lookup_transform_full(strip_leading_slash(target_frame), target_time, strip_leading_slash(source_frame), source_time, strip_leading_slash(fixed_frame)) t = msg.transform.translation r = msg.transform.rotation return [t.x, t.y, t.z], [r.x, r.y, r.z, r.w] def lookupTwist(self, tracking_frame, observation_frame, time, averaging_interval): return self.lookupTwistFull(tracking_frame, observation_frame, observation_frame, (0, 0, 0), tracking_frame, time, averaging_interval) def lookupTwistFull(self, tracking_frame, observation_frame, reference_frame, ref_point, reference_point_frame, time, averaging_interval): latest_time = self.getLatestCommonTime(observation_frame, tracking_frame) target_time = time or latest_time end_time = min(target_time + rospy.Duration(0.5 * averaging_interval.to_sec()), latest_time) start_time = max(rospy.Time(0.0001) + averaging_interval, end_time) - averaging_interval delta_t = (end_time - start_time).to_sec() start_tr, start_rt = self.lookupTransform(observation_frame, tracking_frame, start_time) end_tr, end_rt = self.lookupTransform(observation_frame, tracking_frame, end_time) dR = numpy.dot(numpy.linalg.inv(transformations.quaternion_matrix(start_rt)), transformations.quaternion_matrix(end_rt)) ang, o, _ = transformations.rotation_from_matrix(dR) delta_x, delta_y, delta_z = end_tr[0] - start_tr[0], end_tr[1] - start_tr[1], end_tr[2] - start_tr[2] # Compute twist in observation_frame w.r.t. tracking_frame vel0 = delta_x / delta_t, delta_y / delta_t, delta_z / delta_t rot0 = o[0] * ang / delta_t, o[1] * ang / delta_t, o[2] * ang / delta_t # Shift to reference_frame inverse_tr, inverse_rt = self.lookupTransform(reference_frame, tracking_frame, target_time) iR = transformations.quaternion_matrix(inverse_rt)[:3, :3] rot = numpy.dot(iR, rot0) vel = numpy.dot(iR, vel0) + numpy.cross(inverse_tr, rot) # Correct for reference point rp_orig = numpy.array((inverse_tr[0], inverse_tr[1], inverse_tr[2], 1)) rp_tr, rp_rt = self.lookupTransform(reference_frame, reference_point_frame, target_time) T = numpy.dot(transformations.translation_matrix(rp_tr), transformations.quaternion_matrix(rp_rt)) rp_desired = numpy.dot(T, (rp_orig[0], rp_orig[1], rp_orig[2], 1)) delta = rp_desired - rp_orig vel += numpy.dot(rot, delta[:3]) return (vel[0], vel[1], vel[2]), (rot[0], rot[1], rot[2]) def setUsingDedicatedThread(self, value): self._using_dedicated_thread = value def getTFPrefix(self): # The tf2 resolver does not support TF prefixes, so we return the empty prefix here return "" ## Extends tf's Transformer, adding transform methods for ROS message ## types PointStamped, QuaternionStamped and PoseStamped. class TransformerROS(Transformer): """ TransformerROS extends the base class :class:`tf.Transformer`, adding methods for handling ROS messages. """ ## Looks up the transform for ROS message header hdr to frame ## target_frame, and returns the transform as a Numpy 4x4 matrix. # @param target_frame The target frame # @param hdr A ROS message header object def asMatrix(self, target_frame, hdr): """ :param target_frame: the tf target frame, a string :param hdr: a message header :return: a :class:`numpy.matrix` 4x4 representation of the transform :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Uses :meth:`lookupTransform` to look up the transform for ROS message header hdr to frame target_frame, and returns the transform as a :class:`numpy.matrix` 4x4. """ translation,rotation = self.lookupTransform(target_frame, hdr.frame_id, hdr.stamp) return self.fromTranslationRotation(translation, rotation) ## Returns a Numpy 4x4 matrix for a transform. # @param translation translation as (x,y,z) # @param rotation rotation as (x,y,z,w) def fromTranslationRotation(self, translation, rotation): """ :param translation: translation expressed as a tuple (x,y,z) :param rotation: rotation quaternion expressed as a tuple (x,y,z,w) :return: a :class:`numpy.matrix` 4x4 representation of the transform :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Converts a transformation from :class:`tf.Transformer` into a representation as a 4x4 matrix. """ return numpy.dot(transformations.translation_matrix(translation), transformations.quaternion_matrix(rotation)) ## Transforms a geometry_msgs PointStamped message to frame target_frame, returns the resulting PointStamped. # @param target_frame The target frame # @param ps geometry_msgs.msg.PointStamped object def transformPoint(self, target_frame, ps): """ :param target_frame: the tf target frame, a string :param ps: the geometry_msgs.msg.PointStamped message :return: new geometry_msgs.msg.PointStamped message, in frame target_frame :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Transforms a geometry_msgs PointStamped message to frame target_frame, returns a new PointStamped message. """ mat44 = self.asMatrix(target_frame, ps.header) xyz = tuple(numpy.dot(mat44, numpy.array([ps.point.x, ps.point.y, ps.point.z, 1.0])))[:3] r = geometry_msgs.msg.PointStamped() r.header.stamp = ps.header.stamp r.header.frame_id = target_frame r.point = geometry_msgs.msg.Point(*xyz) return r ## Transforms a geometry_msgs Vector3Stamped message to frame target_frame, returns the resulting Vector3Stamped. # @param target_frame The target frame # @param ps geometry_msgs.msg.Vector3Stamped object def transformVector3(self, target_frame, v3s): """ :param target_frame: the tf target frame, a string :param v3s: the geometry_msgs.msg.Vector3Stamped message :return: new geometry_msgs.msg.Vector3Stamped message, in frame target_frame :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Transforms a geometry_msgs Vector3Stamped message to frame target_frame, returns a new Vector3Stamped message. """ mat44 = self.asMatrix(target_frame, v3s.header) mat44[0,3] = 0.0 mat44[1,3] = 0.0 mat44[2,3] = 0.0 xyz = tuple(numpy.dot(mat44, numpy.array([v3s.vector.x, v3s.vector.y, v3s.vector.z, 1.0])))[:3] r = geometry_msgs.msg.Vector3Stamped() r.header.stamp = v3s.header.stamp r.header.frame_id = target_frame r.vector = geometry_msgs.msg.Vector3(*xyz) return r ## Transforms a geometry_msgs QuaternionStamped message to frame target_frame, returns the resulting QuaternionStamped. # @param target_frame The target frame # @param ps geometry_msgs.msg.QuaternionStamped object def transformQuaternion(self, target_frame, ps): """ :param target_frame: the tf target frame, a string :param ps: the geometry_msgs.msg.QuaternionStamped message :return: new geometry_msgs.msg.QuaternionStamped message, in frame target_frame :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Transforms a geometry_msgs QuaternionStamped message to frame target_frame, returns a new QuaternionStamped message. """ # mat44 is frame-to-frame transform as a 4x4 mat44 = self.asMatrix(target_frame, ps.header) # pose44 is the given quat as a 4x4 pose44 = xyzw_to_mat44(ps.quaternion) # txpose is the new pose in target_frame as a 4x4 txpose = numpy.dot(mat44, pose44) # quat is orientation of txpose quat = tuple(transformations.quaternion_from_matrix(txpose)) # assemble return value QuaternionStamped r = geometry_msgs.msg.QuaternionStamped() r.header.stamp = ps.header.stamp r.header.frame_id = target_frame r.quaternion = geometry_msgs.msg.Quaternion(*quat) return r ## Transforms a geometry_msgs PoseStamped message to frame target_frame, returns the resulting PoseStamped. # @param target_frame The target frame # @param ps geometry_msgs.msg.PoseStamped object def transformPose(self, target_frame, ps): """ :param target_frame: the tf target frame, a string :param ps: the geometry_msgs.msg.PoseStamped message :return: new geometry_msgs.msg.PoseStamped message, in frame target_frame :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message. """ # mat44 is frame-to-frame transform as a 4x4 mat44 = self.asMatrix(target_frame, ps.header) # pose44 is the given pose as a 4x4 pose44 = numpy.dot(xyz_to_mat44(ps.pose.position), xyzw_to_mat44(ps.pose.orientation)) # txpose is the new pose in target_frame as a 4x4 txpose = numpy.dot(mat44, pose44) # xyz and quat are txpose's position and orientation xyz = tuple(transformations.translation_from_matrix(txpose))[:3] quat = tuple(transformations.quaternion_from_matrix(txpose)) # assemble return value PoseStamped r = geometry_msgs.msg.PoseStamped() r.header.stamp = ps.header.stamp r.header.frame_id = target_frame r.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat)) return r def transformPointCloud(self, target_frame, point_cloud): """ :param target_frame: the tf target frame, a string :param ps: the sensor_msgs.msg.PointCloud message :return: new sensor_msgs.msg.PointCloud message, in frame target_frame :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message. """ r = sensor_msgs.msg.PointCloud() r.header.stamp = point_cloud.header.stamp r.header.frame_id = target_frame r.channels = point_cloud.channels mat44 = self.asMatrix(target_frame, point_cloud.header) def xf(p): xyz = tuple(numpy.dot(mat44, numpy.array([p.x, p.y, p.z, 1.0])))[:3] return geometry_msgs.msg.Point(*xyz) r.points = [xf(p) for p in point_cloud.points] return r class TransformListener(TransformerROS): """ TransformListener is a subclass of :class:`tf.TransformerROS` that subscribes to the ``"/tf"`` message topic, and calls :meth:`tf.Transformer.setTransform` with each incoming transformation message. In this way a TransformListener object automatically stays up to to date with all current transforms. Typical usage might be:: import tf from geometry_msgs.msg import PointStamped class MyNode: def __init__(self): self.tl = tf.TransformListener() rospy.Subscriber("/sometopic", PointStamped, self.some_message_handler) ... def some_message_handler(self, point_stamped): # want to work on the point in the "world" frame point_in_world = self.tl.transformPoint("world", point_stamped) ... """ def __init__(self, *args, **kwargs): TransformerROS.__init__(self, *args, **kwargs) self._listener = tf2_ros.TransformListener(self._buffer) self.setUsingDedicatedThread(True) geometry-1.13.2/tf/src/tf/tfwtf.py000077500000000000000000000152271366756125100167740ustar00rootroot00000000000000# Software License Agreement (BSD License) # # Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Revision $Id$ from __future__ import print_function import time from roswtf.rules import warning_rule, error_rule import rosgraph import rospy import tf.msg import math # global list of messages received _msgs = [] ################################################################################ # RULES def rostime_delta(ctx): deltas = {} for m, stamp, callerid in _msgs: for t in m.transforms: d = t.header.stamp - stamp secs = d.to_sec() if abs(secs) > 1.: if callerid in deltas: if abs(secs) > abs(deltas[callerid]): deltas[callerid] = secs else: deltas[callerid] = secs errors = [] for k, v in deltas.items(): errors.append("receiving transform from [{}] that differed from ROS time by {}s".format(k, v)) return errors def reparenting(ctx): errors = [] parent_id_map = {} for m, stamp, callerid in _msgs: for t in m.transforms: frame_id = t.child_frame_id parent_id = t.header.frame_id if frame_id in parent_id_map and parent_id_map[frame_id] != parent_id: msg = "reparenting of [{}] to [{}] by [{}]".format(frame_id, parent_id, callerid) parent_id_map[frame_id] = parent_id if msg not in errors: errors.append(msg) else: parent_id_map[frame_id] = parent_id return errors def cycle_detection(ctx): max_depth = 100 errors = [] parent_id_map = {} for m, stamp, callerid in _msgs: for t in m.transforms: frame_id = t.child_frame_id parent_id = t.header.frame_id parent_id_map[frame_id] = parent_id for frame in parent_id_map: frame_list = [] current_frame = frame count = 0 while count < max_depth + 2: count = count + 1 frame_list.append(current_frame) try: current_frame = parent_id_map[current_frame] except KeyError: break if current_frame == frame: errors.append("Frame {} is in a loop. It's loop has elements:\n{}".format(frame, " -> ".join(frame_list))) break return errors def multiple_authority(ctx): errors = [] authority_map = {} for m, stamp, callerid in _msgs: for t in m.transforms: frame_id = t.child_frame_id parent_id = t.header.frame_id if frame_id in authority_map and authority_map[frame_id] != callerid: msg = "node [{}] publishing transform [{}] with parent [{}] already published by node [{}]".format(callerid, frame_id, parent_id, authority_map[frame_id]) authority_map[frame_id] = callerid if msg not in errors: errors.append(msg) else: authority_map[frame_id] = callerid return errors def no_msgs(ctx): return not _msgs def not_normalized(ctx): errors = [] for m, stamp, callerid in _msgs: for t in m.transforms: q = t.transform.rotation length = math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) if math.fabs(length - 1) > 1e-6: errors.append("rotation from [{}] to [{}] is not unit length, {}".format(t.header.frame_id, t.child_frame_id, length)) return errors ################################################################################ # roswtf PLUGIN # tf_warnings and tf_errors declare the rules that we actually check tf_warnings = [ (no_msgs, "No tf messages"), (rostime_delta, "Received out-of-date/future transforms:"), (not_normalized, "Received non-normalized rotation in transforms:"), ] tf_errors = [ (reparenting, "TF re-parenting contention:"), (cycle_detection, "TF cycle detection::"), (multiple_authority, "TF multiple authority contention:"), ] # rospy subscriber callback for /tf def _tf_handle(msg): _msgs.append((msg, rospy.get_rostime(), msg._connection_header['callerid'])) # @return bool: True if /tf has a publisher def is_tf_active(): master = rosgraph.Master('/tfwtf') if master is not None: try: val = master.getPublishedTopics('/') if filter(lambda x: x[0] == '/tf', val): return True except: pass return False # roswtf entry point for online checks def roswtf_plugin_online(ctx): # don't run plugin if tf isn't active as these checks take awhile if not is_tf_active(): return print("running tf checks, this will take a second...") sub1 = rospy.Subscriber('/tf', tf.msg.tfMessage, _tf_handle) time.sleep(1.0) sub1.unregister() print("... tf checks complete") for r in tf_warnings: warning_rule(r, r[0](ctx), ctx) for r in tf_errors: error_rule(r, r[0](ctx), ctx) # currently no static checks for tf #def roswtf_plugin_static(ctx): # for r in tf_warnings: # warning_rule(r, r[0](ctx), ctx) # for r in tf_errors: # error_rule(r, r[0](ctx), ctx) geometry-1.13.2/tf/src/tf/transformations.py000066400000000000000000001605451366756125100210740ustar00rootroot00000000000000# -*- coding: utf-8 -*- # transformations.py # Copyright (c) 2006, Christoph Gohlke # Copyright (c) 2006-2009, The Regents of the University of California # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the copyright holders nor the names of any # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. """Homogeneous Transformation Matrices and Quaternions. A library for calculating 4x4 matrices for translating, rotating, reflecting, scaling, shearing, projecting, orthogonalizing, and superimposing arrays of 3D homogeneous coordinates as well as for converting between rotation matrices, Euler angles, and quaternions. Also includes an Arcball control object and functions to decompose transformation matrices. :Authors: `Christoph Gohlke `__, Laboratory for Fluorescence Dynamics, University of California, Irvine :Version: 20090418 Requirements ------------ * `Python 2.6 `__ * `Numpy 1.3 `__ * `transformations.c 20090418 `__ (optional implementation of some functions in C) Notes ----- Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using numpy.dot(M, v) for shape (4, \*) "point of arrays", respectively numpy.dot(v, M.T) for shape (\*, 4) "array of points". Calculations are carried out with numpy.float64 precision. This Python implementation is not optimized for speed. Vector, point, quaternion, and matrix function arguments are expected to be "array like", i.e. tuple, list, or numpy arrays. Return types are numpy arrays unless specified otherwise. Angles are in radians unless specified otherwise. Quaternions ix+jy+kz+w are represented as [x, y, z, w]. Use the transpose of transformation matrices for OpenGL glMultMatrixd(). A triple of Euler angles can be applied/interpreted in 24 ways, which can be specified using a 4 character string or encoded 4-tuple: *Axes 4-string*: e.g. 'sxyz' or 'ryxy' - first character : rotations are applied to 's'tatic or 'r'otating frame - remaining characters : successive rotation axis 'x', 'y', or 'z' *Axes 4-tuple*: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) - inner axis: code of axis ('x':0, 'y':1, 'z':2) of rightmost matrix. - parity : even (0) if inner axis 'x' is followed by 'y', 'y' is followed by 'z', or 'z' is followed by 'x'. Otherwise odd (1). - repetition : first and last axis are same (1) or different (0). - frame : rotations are applied to static (0) or rotating (1) frame. References ---------- (1) Matrices and transformations. Ronald Goldman. In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990. (2) More matrices and transformations: shear and pseudo-perspective. Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. (3) Decomposing a matrix into simple transformations. Spencer Thomas. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991. (4) Recovering the data from the transformation matrix. Ronald Goldman. In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991. (5) Euler angle conversion. Ken Shoemake. In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994. (6) Arcball rotation control. Ken Shoemake. In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994. (7) Representing attitude: Euler angles, unit quaternions, and rotation vectors. James Diebel. 2006. (8) A discussion of the solution for the best rotation to relate two sets of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828. (9) Closed-form solution of absolute orientation using unit quaternions. BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642. (10) Quaternions. Ken Shoemake. http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf (11) From quaternion to matrix and back. JMP van Waveren. 2005. http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm (12) Uniform random rotations. Ken Shoemake. In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992. Examples -------- >>> alpha, beta, gamma = 0.123, -1.234, 2.345 >>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1) >>> I = identity_matrix() >>> Rx = rotation_matrix(alpha, xaxis) >>> Ry = rotation_matrix(beta, yaxis) >>> Rz = rotation_matrix(gamma, zaxis) >>> R = concatenate_matrices(Rx, Ry, Rz) >>> euler = euler_from_matrix(R, 'rxyz') >>> numpy.allclose([alpha, beta, gamma], euler) True >>> Re = euler_matrix(alpha, beta, gamma, 'rxyz') >>> is_same_transform(R, Re) True >>> al, be, ga = euler_from_matrix(Re, 'rxyz') >>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz')) True >>> qx = quaternion_about_axis(alpha, xaxis) >>> qy = quaternion_about_axis(beta, yaxis) >>> qz = quaternion_about_axis(gamma, zaxis) >>> q = quaternion_multiply(qx, qy) >>> q = quaternion_multiply(q, qz) >>> Rq = quaternion_matrix(q) >>> is_same_transform(R, Rq) True >>> S = scale_matrix(1.23, origin) >>> T = translation_matrix((1, 2, 3)) >>> Z = shear_matrix(beta, xaxis, origin, zaxis) >>> R = random_rotation_matrix(numpy.random.rand(3)) >>> M = concatenate_matrices(T, R, Z, S) >>> scale, shear, angles, trans, persp = decompose_matrix(M) >>> numpy.allclose(scale, 1.23) True >>> numpy.allclose(trans, (1, 2, 3)) True >>> numpy.allclose(shear, (0, math.tan(beta), 0)) True >>> is_same_transform(R, euler_matrix(axes='sxyz', *angles)) True >>> M1 = compose_matrix(scale, shear, angles, trans, persp) >>> is_same_transform(M, M1) True """ from __future__ import division import warnings import math import numpy # Documentation in HTML format can be generated with Epydoc __docformat__ = "restructuredtext en" def identity_matrix(): """Return 4x4 identity/unit matrix. >>> I = identity_matrix() >>> numpy.allclose(I, numpy.dot(I, I)) True >>> numpy.sum(I), numpy.trace(I) (4.0, 4.0) >>> numpy.allclose(I, numpy.identity(4, dtype=numpy.float64)) True """ return numpy.identity(4, dtype=numpy.float64) def translation_matrix(direction): """Return matrix to translate by direction vector. >>> v = numpy.random.random(3) - 0.5 >>> numpy.allclose(v, translation_matrix(v)[:3, 3]) True """ M = numpy.identity(4) M[:3, 3] = direction[:3] return M def translation_from_matrix(matrix): """Return translation vector from translation matrix. >>> v0 = numpy.random.random(3) - 0.5 >>> v1 = translation_from_matrix(translation_matrix(v0)) >>> numpy.allclose(v0, v1) True """ return numpy.array(matrix, copy=False)[:3, 3].copy() def reflection_matrix(point, normal): """Return matrix to mirror at plane defined by point and normal vector. >>> v0 = numpy.random.random(4) - 0.5 >>> v0[3] = 1.0 >>> v1 = numpy.random.random(3) - 0.5 >>> R = reflection_matrix(v0, v1) >>> numpy.allclose(2., numpy.trace(R)) True >>> numpy.allclose(v0, numpy.dot(R, v0)) True >>> v2 = v0.copy() >>> v2[:3] += v1 >>> v3 = v0.copy() >>> v2[:3] -= v1 >>> numpy.allclose(v2, numpy.dot(R, v3)) True """ normal = unit_vector(normal[:3]) M = numpy.identity(4) M[:3, :3] -= 2.0 * numpy.outer(normal, normal) M[:3, 3] = (2.0 * numpy.dot(point[:3], normal)) * normal return M def reflection_from_matrix(matrix): """Return mirror plane point and normal vector from reflection matrix. >>> v0 = numpy.random.random(3) - 0.5 >>> v1 = numpy.random.random(3) - 0.5 >>> M0 = reflection_matrix(v0, v1) >>> point, normal = reflection_from_matrix(M0) >>> M1 = reflection_matrix(point, normal) >>> is_same_transform(M0, M1) True """ M = numpy.array(matrix, dtype=numpy.float64, copy=False) # normal: unit eigenvector corresponding to eigenvalue -1 l, V = numpy.linalg.eig(M[:3, :3]) i = numpy.where(abs(numpy.real(l) + 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue -1") normal = numpy.real(V[:, i[0]]).squeeze() # point: any unit eigenvector corresponding to eigenvalue 1 l, V = numpy.linalg.eig(M) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") point = numpy.real(V[:, i[-1]]).squeeze() point /= point[3] return point, normal def rotation_matrix(angle, direction, point=None): """Return matrix to rotate about axis defined by point and direction. >>> angle = (random.random() - 0.5) * (2*math.pi) >>> direc = numpy.random.random(3) - 0.5 >>> point = numpy.random.random(3) - 0.5 >>> R0 = rotation_matrix(angle, direc, point) >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) >>> is_same_transform(R0, R1) True >>> R0 = rotation_matrix(angle, direc, point) >>> R1 = rotation_matrix(-angle, -direc, point) >>> is_same_transform(R0, R1) True >>> I = numpy.identity(4, numpy.float64) >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) True >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, ... direc, point))) True """ sina = math.sin(angle) cosa = math.cos(angle) direction = unit_vector(direction[:3]) # rotation matrix around unit vector R = numpy.array(((cosa, 0.0, 0.0), (0.0, cosa, 0.0), (0.0, 0.0, cosa)), dtype=numpy.float64) R += numpy.outer(direction, direction) * (1.0 - cosa) direction *= sina R += numpy.array((( 0.0, -direction[2], direction[1]), ( direction[2], 0.0, -direction[0]), (-direction[1], direction[0], 0.0)), dtype=numpy.float64) M = numpy.identity(4) M[:3, :3] = R if point is not None: # rotation not around origin point = numpy.array(point[:3], dtype=numpy.float64, copy=False) M[:3, 3] = point - numpy.dot(R, point) return M def rotation_from_matrix(matrix): """Return rotation angle and axis from rotation matrix. >>> angle = (random.random() - 0.5) * (2*math.pi) >>> direc = numpy.random.random(3) - 0.5 >>> point = numpy.random.random(3) - 0.5 >>> R0 = rotation_matrix(angle, direc, point) >>> angle, direc, point = rotation_from_matrix(R0) >>> R1 = rotation_matrix(angle, direc, point) >>> is_same_transform(R0, R1) True """ R = numpy.array(matrix, dtype=numpy.float64, copy=False) R33 = R[:3, :3] # direction: unit eigenvector of R33 corresponding to eigenvalue of 1 l, W = numpy.linalg.eig(R33.T) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") direction = numpy.real(W[:, i[-1]]).squeeze() # point: unit eigenvector of R33 corresponding to eigenvalue of 1 l, Q = numpy.linalg.eig(R) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no unit eigenvector corresponding to eigenvalue 1") point = numpy.real(Q[:, i[-1]]).squeeze() point /= point[3] # rotation angle depending on direction cosa = (numpy.trace(R33) - 1.0) / 2.0 if abs(direction[2]) > 1e-8: sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] elif abs(direction[1]) > 1e-8: sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] else: sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] angle = math.atan2(sina, cosa) return angle, direction, point def scale_matrix(factor, origin=None, direction=None): """Return matrix to scale by factor around origin in direction. Use factor -1 for point symmetry. >>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0 >>> v[3] = 1.0 >>> S = scale_matrix(-1.234) >>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3]) True >>> factor = random.random() * 10 - 5 >>> origin = numpy.random.random(3) - 0.5 >>> direct = numpy.random.random(3) - 0.5 >>> S = scale_matrix(factor, origin) >>> S = scale_matrix(factor, origin, direct) """ if direction is None: # uniform scaling M = numpy.array(((factor, 0.0, 0.0, 0.0), (0.0, factor, 0.0, 0.0), (0.0, 0.0, factor, 0.0), (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) if origin is not None: M[:3, 3] = origin[:3] M[:3, 3] *= 1.0 - factor else: # nonuniform scaling direction = unit_vector(direction[:3]) factor = 1.0 - factor M = numpy.identity(4) M[:3, :3] -= factor * numpy.outer(direction, direction) if origin is not None: M[:3, 3] = (factor * numpy.dot(origin[:3], direction)) * direction return M def scale_from_matrix(matrix): """Return scaling factor, origin and direction from scaling matrix. >>> factor = random.random() * 10 - 5 >>> origin = numpy.random.random(3) - 0.5 >>> direct = numpy.random.random(3) - 0.5 >>> S0 = scale_matrix(factor, origin) >>> factor, origin, direction = scale_from_matrix(S0) >>> S1 = scale_matrix(factor, origin, direction) >>> is_same_transform(S0, S1) True >>> S0 = scale_matrix(factor, origin, direct) >>> factor, origin, direction = scale_from_matrix(S0) >>> S1 = scale_matrix(factor, origin, direction) >>> is_same_transform(S0, S1) True """ M = numpy.array(matrix, dtype=numpy.float64, copy=False) M33 = M[:3, :3] factor = numpy.trace(M33) - 2.0 try: # direction: unit eigenvector corresponding to eigenvalue factor l, V = numpy.linalg.eig(M33) i = numpy.where(abs(numpy.real(l) - factor) < 1e-8)[0][0] direction = numpy.real(V[:, i]).squeeze() direction /= vector_norm(direction) except IndexError: # uniform scaling factor = (factor + 2.0) / 3.0 direction = None # origin: any eigenvector corresponding to eigenvalue 1 l, V = numpy.linalg.eig(M) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no eigenvector corresponding to eigenvalue 1") origin = numpy.real(V[:, i[-1]]).squeeze() origin /= origin[3] return factor, origin, direction def projection_matrix(point, normal, direction=None, perspective=None, pseudo=False): """Return matrix to project onto plane defined by point and normal. Using either perspective point, projection direction, or none of both. If pseudo is True, perspective projections will preserve relative depth such that Perspective = dot(Orthogonal, PseudoPerspective). >>> P = projection_matrix((0, 0, 0), (1, 0, 0)) >>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:]) True >>> point = numpy.random.random(3) - 0.5 >>> normal = numpy.random.random(3) - 0.5 >>> direct = numpy.random.random(3) - 0.5 >>> persp = numpy.random.random(3) - 0.5 >>> P0 = projection_matrix(point, normal) >>> P1 = projection_matrix(point, normal, direction=direct) >>> P2 = projection_matrix(point, normal, perspective=persp) >>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True) >>> is_same_transform(P2, numpy.dot(P0, P3)) True >>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0)) >>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0 >>> v0[3] = 1.0 >>> v1 = numpy.dot(P, v0) >>> numpy.allclose(v1[1], v0[1]) True >>> numpy.allclose(v1[0], 3.0-v1[1]) True """ M = numpy.identity(4) point = numpy.array(point[:3], dtype=numpy.float64, copy=False) normal = unit_vector(normal[:3]) if perspective is not None: # perspective projection perspective = numpy.array(perspective[:3], dtype=numpy.float64, copy=False) M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) M[:3, :3] -= numpy.outer(perspective, normal) if pseudo: # preserve relative depth M[:3, :3] -= numpy.outer(normal, normal) M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) else: M[:3, 3] = numpy.dot(point, normal) * perspective M[3, :3] = -normal M[3, 3] = numpy.dot(perspective, normal) elif direction is not None: # parallel projection direction = numpy.array(direction[:3], dtype=numpy.float64, copy=False) scale = numpy.dot(direction, normal) M[:3, :3] -= numpy.outer(direction, normal) / scale M[:3, 3] = direction * (numpy.dot(point, normal) / scale) else: # orthogonal projection M[:3, :3] -= numpy.outer(normal, normal) M[:3, 3] = numpy.dot(point, normal) * normal return M def projection_from_matrix(matrix, pseudo=False): """Return projection plane and perspective point from projection matrix. Return values are same as arguments for projection_matrix function: point, normal, direction, perspective, and pseudo. >>> point = numpy.random.random(3) - 0.5 >>> normal = numpy.random.random(3) - 0.5 >>> direct = numpy.random.random(3) - 0.5 >>> persp = numpy.random.random(3) - 0.5 >>> P0 = projection_matrix(point, normal) >>> result = projection_from_matrix(P0) >>> P1 = projection_matrix(*result) >>> is_same_transform(P0, P1) True >>> P0 = projection_matrix(point, normal, direct) >>> result = projection_from_matrix(P0) >>> P1 = projection_matrix(*result) >>> is_same_transform(P0, P1) True >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False) >>> result = projection_from_matrix(P0, pseudo=False) >>> P1 = projection_matrix(*result) >>> is_same_transform(P0, P1) True >>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True) >>> result = projection_from_matrix(P0, pseudo=True) >>> P1 = projection_matrix(*result) >>> is_same_transform(P0, P1) True """ M = numpy.array(matrix, dtype=numpy.float64, copy=False) M33 = M[:3, :3] l, V = numpy.linalg.eig(M) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] if not pseudo and len(i): # point: any eigenvector corresponding to eigenvalue 1 point = numpy.real(V[:, i[-1]]).squeeze() point /= point[3] # direction: unit eigenvector corresponding to eigenvalue 0 l, V = numpy.linalg.eig(M33) i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] if not len(i): raise ValueError("no eigenvector corresponding to eigenvalue 0") direction = numpy.real(V[:, i[0]]).squeeze() direction /= vector_norm(direction) # normal: unit eigenvector of M33.T corresponding to eigenvalue 0 l, V = numpy.linalg.eig(M33.T) i = numpy.where(abs(numpy.real(l)) < 1e-8)[0] if len(i): # parallel projection normal = numpy.real(V[:, i[0]]).squeeze() normal /= vector_norm(normal) return point, normal, direction, None, False else: # orthogonal projection, where normal equals direction vector return point, direction, None, None, False else: # perspective projection i = numpy.where(abs(numpy.real(l)) > 1e-8)[0] if not len(i): raise ValueError( "no eigenvector not corresponding to eigenvalue 0") point = numpy.real(V[:, i[-1]]).squeeze() point /= point[3] normal = - M[3, :3] perspective = M[:3, 3] / numpy.dot(point[:3], normal) if pseudo: perspective -= normal return point, normal, None, perspective, pseudo def clip_matrix(left, right, bottom, top, near, far, perspective=False): """Return matrix to obtain normalized device coordinates from frustrum. The frustrum bounds are axis-aligned along x (left, right), y (bottom, top) and z (near, far). Normalized device coordinates are in range [-1, 1] if coordinates are inside the frustrum. If perspective is True the frustrum is a truncated pyramid with the perspective point at origin and direction along z axis, otherwise an orthographic canonical view volume (a box). Homogeneous coordinates transformed by the perspective clip matrix need to be dehomogenized (devided by w coordinate). >>> frustrum = numpy.random.rand(6) >>> frustrum[1] += frustrum[0] >>> frustrum[3] += frustrum[2] >>> frustrum[5] += frustrum[4] >>> M = clip_matrix(*frustrum, perspective=False) >>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) array([-1., -1., -1., 1.]) >>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0]) array([ 1., 1., 1., 1.]) >>> M = clip_matrix(*frustrum, perspective=True) >>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0]) >>> v / v[3] array([-1., -1., -1., 1.]) >>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0]) >>> v / v[3] array([ 1., 1., -1., 1.]) """ if left >= right or bottom >= top or near >= far: raise ValueError("invalid frustrum") if perspective: if near <= _EPS: raise ValueError("invalid frustrum: near <= 0") t = 2.0 * near M = ((-t/(right-left), 0.0, (right+left)/(right-left), 0.0), (0.0, -t/(top-bottom), (top+bottom)/(top-bottom), 0.0), (0.0, 0.0, -(far+near)/(far-near), t*far/(far-near)), (0.0, 0.0, -1.0, 0.0)) else: M = ((2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)), (0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)), (0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)), (0.0, 0.0, 0.0, 1.0)) return numpy.array(M, dtype=numpy.float64) def shear_matrix(angle, direction, point, normal): """Return matrix to shear by angle along direction vector on shear plane. The shear plane is defined by a point and normal vector. The direction vector must be orthogonal to the plane's normal vector. A point P is transformed by the shear matrix into P" such that the vector P-P" is parallel to the direction vector and its extent is given by the angle of P-P'-P", where P' is the orthogonal projection of P onto the shear plane. >>> angle = (random.random() - 0.5) * 4*math.pi >>> direct = numpy.random.random(3) - 0.5 >>> point = numpy.random.random(3) - 0.5 >>> normal = numpy.cross(direct, numpy.random.random(3)) >>> S = shear_matrix(angle, direct, point, normal) >>> numpy.allclose(1.0, numpy.linalg.det(S)) True """ normal = unit_vector(normal[:3]) direction = unit_vector(direction[:3]) if abs(numpy.dot(normal, direction)) > 1e-6: raise ValueError("direction and normal vectors are not orthogonal") angle = math.tan(angle) M = numpy.identity(4) M[:3, :3] += angle * numpy.outer(direction, normal) M[:3, 3] = -angle * numpy.dot(point[:3], normal) * direction return M def shear_from_matrix(matrix): """Return shear angle, direction and plane from shear matrix. >>> angle = (random.random() - 0.5) * 4*math.pi >>> direct = numpy.random.random(3) - 0.5 >>> point = numpy.random.random(3) - 0.5 >>> normal = numpy.cross(direct, numpy.random.random(3)) >>> S0 = shear_matrix(angle, direct, point, normal) >>> angle, direct, point, normal = shear_from_matrix(S0) >>> S1 = shear_matrix(angle, direct, point, normal) >>> is_same_transform(S0, S1) True """ M = numpy.array(matrix, dtype=numpy.float64, copy=False) M33 = M[:3, :3] # normal: cross independent eigenvectors corresponding to the eigenvalue 1 l, V = numpy.linalg.eig(M33) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-4)[0] if len(i) < 2: raise ValueError("No two linear independent eigenvectors found {}".format(l)) V = numpy.real(V[:, i]).squeeze().T lenorm = -1.0 for i0, i1 in ((0, 1), (0, 2), (1, 2)): n = numpy.cross(V[i0], V[i1]) l = vector_norm(n) if l > lenorm: lenorm = l normal = n normal /= lenorm # direction and angle direction = numpy.dot(M33 - numpy.identity(3), normal) angle = vector_norm(direction) direction /= angle angle = math.atan(angle) # point: eigenvector corresponding to eigenvalue 1 l, V = numpy.linalg.eig(M) i = numpy.where(abs(numpy.real(l) - 1.0) < 1e-8)[0] if not len(i): raise ValueError("no eigenvector corresponding to eigenvalue 1") point = numpy.real(V[:, i[-1]]).squeeze() point /= point[3] return angle, direction, point, normal def decompose_matrix(matrix): """Return sequence of transformations from transformation matrix. matrix : array_like Non-degenerative homogeneous transformation matrix Return tuple of: scale : vector of 3 scaling factors shear : list of shear factors for x-y, x-z, y-z axes angles : list of Euler angles about static x, y, z axes translate : translation vector along x, y, z axes perspective : perspective partition of matrix Raise ValueError if matrix is of wrong type or degenerative. >>> T0 = translation_matrix((1, 2, 3)) >>> scale, shear, angles, trans, persp = decompose_matrix(T0) >>> T1 = translation_matrix(trans) >>> numpy.allclose(T0, T1) True >>> S = scale_matrix(0.123) >>> scale, shear, angles, trans, persp = decompose_matrix(S) >>> scale[0] 0.123 >>> R0 = euler_matrix(1, 2, 3) >>> scale, shear, angles, trans, persp = decompose_matrix(R0) >>> R1 = euler_matrix(*angles) >>> numpy.allclose(R0, R1) True """ M = numpy.array(matrix, dtype=numpy.float64, copy=True).T if abs(M[3, 3]) < _EPS: raise ValueError("M[3, 3] is zero") M /= M[3, 3] P = M.copy() P[:, 3] = 0, 0, 0, 1 if not numpy.linalg.det(P): raise ValueError("Matrix is singular") scale = numpy.zeros((3, ), dtype=numpy.float64) shear = [0, 0, 0] angles = [0, 0, 0] if any(abs(M[:3, 3]) > _EPS): perspective = numpy.dot(M[:, 3], numpy.linalg.inv(P.T)) M[:, 3] = 0, 0, 0, 1 else: perspective = numpy.array((0, 0, 0, 1), dtype=numpy.float64) translate = M[3, :3].copy() M[3, :3] = 0 row = M[:3, :3].copy() scale[0] = vector_norm(row[0]) row[0] /= scale[0] shear[0] = numpy.dot(row[0], row[1]) row[1] -= row[0] * shear[0] scale[1] = vector_norm(row[1]) row[1] /= scale[1] shear[0] /= scale[1] shear[1] = numpy.dot(row[0], row[2]) row[2] -= row[0] * shear[1] shear[2] = numpy.dot(row[1], row[2]) row[2] -= row[1] * shear[2] scale[2] = vector_norm(row[2]) row[2] /= scale[2] shear[1:] /= scale[2] if numpy.dot(row[0], numpy.cross(row[1], row[2])) < 0: scale *= -1 row *= -1 angles[1] = math.asin(-row[0, 2]) if math.cos(angles[1]): angles[0] = math.atan2(row[1, 2], row[2, 2]) angles[2] = math.atan2(row[0, 1], row[0, 0]) else: #angles[0] = math.atan2(row[1, 0], row[1, 1]) angles[0] = math.atan2(-row[2, 1], row[1, 1]) angles[2] = 0.0 return scale, shear, angles, translate, perspective def compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None): """Return transformation matrix from sequence of transformations. This is the inverse of the decompose_matrix function. Sequence of transformations: scale : vector of 3 scaling factors shear : list of shear factors for x-y, x-z, y-z axes angles : list of Euler angles about static x, y, z axes translate : translation vector along x, y, z axes perspective : perspective partition of matrix >>> scale = numpy.random.random(3) - 0.5 >>> shear = numpy.random.random(3) - 0.5 >>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi) >>> trans = numpy.random.random(3) - 0.5 >>> persp = numpy.random.random(4) - 0.5 >>> M0 = compose_matrix(scale, shear, angles, trans, persp) >>> result = decompose_matrix(M0) >>> M1 = compose_matrix(*result) >>> is_same_transform(M0, M1) True """ M = numpy.identity(4) if perspective is not None: P = numpy.identity(4) P[3, :] = perspective[:4] M = numpy.dot(M, P) if translate is not None: T = numpy.identity(4) T[:3, 3] = translate[:3] M = numpy.dot(M, T) if angles is not None: R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') M = numpy.dot(M, R) if shear is not None: Z = numpy.identity(4) Z[1, 2] = shear[2] Z[0, 2] = shear[1] Z[0, 1] = shear[0] M = numpy.dot(M, Z) if scale is not None: S = numpy.identity(4) S[0, 0] = scale[0] S[1, 1] = scale[1] S[2, 2] = scale[2] M = numpy.dot(M, S) M /= M[3, 3] return M def orthogonalization_matrix(lengths, angles): """Return orthogonalization matrix for crystallographic cell coordinates. Angles are expected in degrees. The de-orthogonalization matrix is the inverse. >>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.)) >>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10) True >>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7]) >>> numpy.allclose(numpy.sum(O), 43.063229) True """ a, b, c = lengths angles = numpy.radians(angles) sina, sinb, _ = numpy.sin(angles) cosa, cosb, cosg = numpy.cos(angles) co = (cosa * cosb - cosg) / (sina * sinb) return numpy.array(( ( a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0), (-a*sinb*co, b*sina, 0.0, 0.0), ( a*cosb, b*cosa, c, 0.0), ( 0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) def superimposition_matrix(v0, v1, scaling=False, usesvd=True): """Return matrix to transform given vector set into second vector set. v0 and v1 are shape (3, \*) or (4, \*) arrays of at least 3 vectors. If usesvd is True, the weighted sum of squared deviations (RMSD) is minimized according to the algorithm by W. Kabsch [8]. Otherwise the quaternion based algorithm by B. Horn [9] is used (slower when using this Python implementation). The returned matrix performs rotation, translation and uniform scaling (if specified). >>> v0 = numpy.random.rand(3, 10) >>> M = superimposition_matrix(v0, v0) >>> numpy.allclose(M, numpy.identity(4)) True >>> R = random_rotation_matrix(numpy.random.random(3)) >>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1)) >>> v1 = numpy.dot(R, v0) >>> M = superimposition_matrix(v0, v1) >>> numpy.allclose(v1, numpy.dot(M, v0)) True >>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0 >>> v0[3] = 1.0 >>> v1 = numpy.dot(R, v0) >>> M = superimposition_matrix(v0, v1) >>> numpy.allclose(v1, numpy.dot(M, v0)) True >>> S = scale_matrix(random.random()) >>> T = translation_matrix(numpy.random.random(3)-0.5) >>> M = concatenate_matrices(T, R, S) >>> v1 = numpy.dot(M, v0) >>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1) >>> M = superimposition_matrix(v0, v1, scaling=True) >>> numpy.allclose(v1, numpy.dot(M, v0)) True >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) >>> numpy.allclose(v1, numpy.dot(M, v0)) True >>> v = numpy.empty((4, 100, 3), dtype=numpy.float64) >>> v[:, :, 0] = v0 >>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False) >>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0])) True """ v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] if v0.shape != v1.shape or v0.shape[1] < 3: raise ValueError("Vector sets are of wrong shape or type.") # move centroids to origin t0 = numpy.mean(v0, axis=1) t1 = numpy.mean(v1, axis=1) v0 = v0 - t0.reshape(3, 1) v1 = v1 - t1.reshape(3, 1) if usesvd: # Singular Value Decomposition of covariance matrix u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) # rotation matrix from SVD orthonormal bases R = numpy.dot(u, vh) if numpy.linalg.det(R) < 0.0: # R does not constitute right handed system R -= numpy.outer(u[:, 2], vh[2, :]*2.0) s[-1] *= -1.0 # homogeneous transformation matrix M = numpy.identity(4) M[:3, :3] = R else: # compute symmetric matrix N xx, yy, zz = numpy.sum(v0 * v1, axis=1) xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) N = ((xx+yy+zz, yz-zy, zx-xz, xy-yx), (yz-zy, xx-yy-zz, xy+yx, zx+xz), (zx-xz, xy+yx, -xx+yy-zz, yz+zy), (xy-yx, zx+xz, yz+zy, -xx-yy+zz)) # quaternion: eigenvector corresponding to most positive eigenvalue l, V = numpy.linalg.eig(N) q = V[:, numpy.argmax(l)] q /= vector_norm(q) # unit quaternion q = numpy.roll(q, -1) # move w component to end # homogeneous transformation matrix M = quaternion_matrix(q) # scale: ratio of rms deviations from centroid if scaling: v0 *= v0 v1 *= v1 M[:3, :3] *= math.sqrt(numpy.sum(v1) / numpy.sum(v0)) # translation M[:3, 3] = t1 T = numpy.identity(4) T[:3, 3] = -t0 M = numpy.dot(M, T) return M def euler_matrix(ai, aj, ak, axes='sxyz'): """Return homogeneous rotation matrix from Euler angles and axis sequence. ai, aj, ak : Euler's roll, pitch and yaw angles axes : One of 24 axis sequences as string or encoded tuple >>> R = euler_matrix(1, 2, 3, 'syxz') >>> numpy.allclose(numpy.sum(R[0]), -1.34786452) True >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1)) >>> numpy.allclose(numpy.sum(R[0]), -0.383436184) True >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5) >>> for axes in _AXES2TUPLE.keys(): ... R = euler_matrix(ai, aj, ak, axes) >>> for axes in _TUPLE2AXES.keys(): ... R = euler_matrix(ai, aj, ak, axes) """ try: firstaxis, parity, repetition, frame = _AXES2TUPLE[axes] except (AttributeError, KeyError): _ = _TUPLE2AXES[axes] firstaxis, parity, repetition, frame = axes i = firstaxis j = _NEXT_AXIS[i+parity] k = _NEXT_AXIS[i-parity+1] if frame: ai, ak = ak, ai if parity: ai, aj, ak = -ai, -aj, -ak si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) cc, cs = ci*ck, ci*sk sc, ss = si*ck, si*sk M = numpy.identity(4) if repetition: M[i, i] = cj M[i, j] = sj*si M[i, k] = sj*ci M[j, i] = sj*sk M[j, j] = -cj*ss+cc M[j, k] = -cj*cs-sc M[k, i] = -sj*ck M[k, j] = cj*sc+cs M[k, k] = cj*cc-ss else: M[i, i] = cj*ck M[i, j] = sj*sc-cs M[i, k] = sj*cc+ss M[j, i] = cj*sk M[j, j] = sj*ss+cc M[j, k] = sj*cs-sc M[k, i] = -sj M[k, j] = cj*si M[k, k] = cj*ci return M def euler_from_matrix(matrix, axes='sxyz'): """Return Euler angles from rotation matrix for specified axis sequence. axes : One of 24 axis sequences as string or encoded tuple Note that many Euler angle triplets can describe one matrix. >>> R0 = euler_matrix(1, 2, 3, 'syxz') >>> al, be, ga = euler_from_matrix(R0, 'syxz') >>> R1 = euler_matrix(al, be, ga, 'syxz') >>> numpy.allclose(R0, R1) True >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5) >>> for axes in _AXES2TUPLE.keys(): ... R0 = euler_matrix(axes=axes, *angles) ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes)) ... if not numpy.allclose(R0, R1): print axes, "failed" """ try: firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] except (AttributeError, KeyError): _ = _TUPLE2AXES[axes] firstaxis, parity, repetition, frame = axes i = firstaxis j = _NEXT_AXIS[i+parity] k = _NEXT_AXIS[i-parity+1] M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] if repetition: sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) if sy > _EPS: ax = math.atan2( M[i, j], M[i, k]) ay = math.atan2( sy, M[i, i]) az = math.atan2( M[j, i], -M[k, i]) else: ax = math.atan2(-M[j, k], M[j, j]) ay = math.atan2( sy, M[i, i]) az = 0.0 else: cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) if cy > _EPS: ax = math.atan2( M[k, j], M[k, k]) ay = math.atan2(-M[k, i], cy) az = math.atan2( M[j, i], M[i, i]) else: ax = math.atan2(-M[j, k], M[j, j]) ay = math.atan2(-M[k, i], cy) az = 0.0 if parity: ax, ay, az = -ax, -ay, -az if frame: ax, az = az, ax return ax, ay, az def euler_from_quaternion(quaternion, axes='sxyz'): """Return Euler angles from quaternion for specified axis sequence. >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) >>> numpy.allclose(angles, [0.123, 0, 0]) True """ return euler_from_matrix(quaternion_matrix(quaternion), axes) def quaternion_from_euler(ai, aj, ak, axes='sxyz'): """Return quaternion from Euler angles and axis sequence. ai, aj, ak : Euler's roll, pitch and yaw angles axes : One of 24 axis sequences as string or encoded tuple >>> q = quaternion_from_euler(1, 2, 3, 'ryxz') >>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) True """ try: firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] except (AttributeError, KeyError): _ = _TUPLE2AXES[axes] firstaxis, parity, repetition, frame = axes i = firstaxis j = _NEXT_AXIS[i+parity] k = _NEXT_AXIS[i-parity+1] if frame: ai, ak = ak, ai if parity: aj = -aj ai /= 2.0 aj /= 2.0 ak /= 2.0 ci = math.cos(ai) si = math.sin(ai) cj = math.cos(aj) sj = math.sin(aj) ck = math.cos(ak) sk = math.sin(ak) cc = ci*ck cs = ci*sk sc = si*ck ss = si*sk quaternion = numpy.empty((4, ), dtype=numpy.float64) if repetition: quaternion[i] = cj*(cs + sc) quaternion[j] = sj*(cc + ss) quaternion[k] = sj*(cs - sc) quaternion[3] = cj*(cc - ss) else: quaternion[i] = cj*sc - sj*cs quaternion[j] = cj*ss + sj*cc quaternion[k] = cj*cs - sj*sc quaternion[3] = cj*cc + sj*ss if parity: quaternion[j] *= -1 return quaternion def quaternion_about_axis(angle, axis): """Return quaternion for rotation about axis. >>> q = quaternion_about_axis(0.123, (1, 0, 0)) >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947]) True """ quaternion = numpy.zeros((4, ), dtype=numpy.float64) quaternion[:3] = axis[:3] qlen = vector_norm(quaternion) if qlen > _EPS: quaternion *= math.sin(angle/2.0) / qlen quaternion[3] = math.cos(angle/2.0) return quaternion def quaternion_matrix(quaternion): """Return homogeneous rotation matrix from quaternion. >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947]) >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0))) True """ q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True) nq = numpy.dot(q, q) if nq < _EPS: return numpy.identity(4) q *= math.sqrt(2.0 / nq) q = numpy.outer(q, q) return numpy.array(( (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], 0.0), ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], 0.0), ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0), ( 0.0, 0.0, 0.0, 1.0) ), dtype=numpy.float64) def quaternion_from_matrix(matrix): """Return quaternion from rotation matrix. >>> R = rotation_matrix(0.123, (1, 2, 3)) >>> q = quaternion_from_matrix(R) >>> numpy.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095]) True """ q = numpy.empty((4, ), dtype=numpy.float64) M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] t = numpy.trace(M) if t > M[3, 3]: q[3] = t q[2] = M[1, 0] - M[0, 1] q[1] = M[0, 2] - M[2, 0] q[0] = M[2, 1] - M[1, 2] else: i, j, k = 0, 1, 2 if M[1, 1] > M[0, 0]: i, j, k = 1, 2, 0 if M[2, 2] > M[i, i]: i, j, k = 2, 0, 1 t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] q[i] = t q[j] = M[i, j] + M[j, i] q[k] = M[k, i] + M[i, k] q[3] = M[k, j] - M[j, k] q *= 0.5 / math.sqrt(t * M[3, 3]) return q def quaternion_multiply(quaternion1, quaternion0): """Return multiplication of two quaternions. >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8]) >>> numpy.allclose(q, [-44, -14, 48, 28]) True """ x0, y0, z0, w0 = quaternion0 x1, y1, z1, w1 = quaternion1 return numpy.array(( x1*w0 + y1*z0 - z1*y0 + w1*x0, -x1*z0 + y1*w0 + z1*x0 + w1*y0, x1*y0 - y1*x0 + z1*w0 + w1*z0, -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64) def quaternion_conjugate(quaternion): """Return conjugate of quaternion. >>> q0 = random_quaternion() >>> q1 = quaternion_conjugate(q0) >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) True """ return numpy.array((-quaternion[0], -quaternion[1], -quaternion[2], quaternion[3]), dtype=numpy.float64) def quaternion_inverse(quaternion): """Return inverse of quaternion. >>> q0 = random_quaternion() >>> q1 = quaternion_inverse(q0) >>> numpy.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1]) True """ return quaternion_conjugate(quaternion) / numpy.dot(quaternion, quaternion) def quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True): """Return spherical linear interpolation between two quaternions. >>> q0 = random_quaternion() >>> q1 = random_quaternion() >>> q = quaternion_slerp(q0, q1, 0.0) >>> numpy.allclose(q, q0) True >>> q = quaternion_slerp(q0, q1, 1.0, 1) >>> numpy.allclose(q, q1) True >>> q = quaternion_slerp(q0, q1, 0.5) >>> angle = math.acos(numpy.dot(q0, q)) >>> numpy.allclose(2.0, math.acos(numpy.dot(q0, q1)) / angle) or \ numpy.allclose(2.0, math.acos(-numpy.dot(q0, q1)) / angle) True """ q0 = unit_vector(quat0[:4]) q1 = unit_vector(quat1[:4]) if fraction == 0.0: return q0 elif fraction == 1.0: return q1 d = numpy.dot(q0, q1) if abs(abs(d) - 1.0) < _EPS: return q0 if shortestpath and d < 0.0: # invert rotation d = -d q1 *= -1.0 angle = math.acos(d) + spin * math.pi if abs(angle) < _EPS: return q0 isin = 1.0 / math.sin(angle) q0 *= math.sin((1.0 - fraction) * angle) * isin q1 *= math.sin(fraction * angle) * isin q0 += q1 return q0 def random_quaternion(rand=None): """Return uniform random unit quaternion. rand: array like or None Three independent random variables that are uniformly distributed between 0 and 1. >>> q = random_quaternion() >>> numpy.allclose(1.0, vector_norm(q)) True >>> q = random_quaternion(numpy.random.random(3)) >>> q.shape (4,) """ if rand is None: rand = numpy.random.rand(3) else: assert len(rand) == 3 r1 = numpy.sqrt(1.0 - rand[0]) r2 = numpy.sqrt(rand[0]) pi2 = math.pi * 2.0 t1 = pi2 * rand[1] t2 = pi2 * rand[2] return numpy.array((numpy.sin(t1)*r1, numpy.cos(t1)*r1, numpy.sin(t2)*r2, numpy.cos(t2)*r2), dtype=numpy.float64) def random_rotation_matrix(rand=None): """Return uniform random rotation matrix. rnd: array like Three independent random variables that are uniformly distributed between 0 and 1 for each returned quaternion. >>> R = random_rotation_matrix() >>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4)) True """ return quaternion_matrix(random_quaternion(rand)) class Arcball(object): """Virtual Trackball Control. >>> ball = Arcball() >>> ball = Arcball(initial=numpy.identity(4)) >>> ball.place([320, 320], 320) >>> ball.down([500, 250]) >>> ball.drag([475, 275]) >>> R = ball.matrix() >>> numpy.allclose(numpy.sum(R), 3.90583455) True >>> ball = Arcball(initial=[0, 0, 0, 1]) >>> ball.place([320, 320], 320) >>> ball.setaxes([1,1,0], [-1, 1, 0]) >>> ball.setconstrain(True) >>> ball.down([400, 200]) >>> ball.drag([200, 400]) >>> R = ball.matrix() >>> numpy.allclose(numpy.sum(R), 0.2055924) True >>> ball.next() """ def __init__(self, initial=None): """Initialize virtual trackball control. initial : quaternion or rotation matrix """ self._axis = None self._axes = None self._radius = 1.0 self._center = [0.0, 0.0] self._vdown = numpy.array([0, 0, 1], dtype=numpy.float64) self._constrain = False if initial is None: self._qdown = numpy.array([0, 0, 0, 1], dtype=numpy.float64) else: initial = numpy.array(initial, dtype=numpy.float64) if initial.shape == (4, 4): self._qdown = quaternion_from_matrix(initial) elif initial.shape == (4, ): initial /= vector_norm(initial) self._qdown = initial else: raise ValueError("initial not a quaternion or matrix.") self._qnow = self._qpre = self._qdown def place(self, center, radius): """Place Arcball, e.g. when window size changes. center : sequence[2] Window coordinates of trackball center. radius : float Radius of trackball in window coordinates. """ self._radius = float(radius) self._center[0] = center[0] self._center[1] = center[1] def setaxes(self, *axes): """Set axes to constrain rotations.""" if axes is None: self._axes = None else: self._axes = [unit_vector(axis) for axis in axes] def setconstrain(self, constrain): """Set state of constrain to axis mode.""" self._constrain = constrain == True def getconstrain(self): """Return state of constrain to axis mode.""" return self._constrain def down(self, point): """Set initial cursor window coordinates and pick constrain-axis.""" self._vdown = arcball_map_to_sphere(point, self._center, self._radius) self._qdown = self._qpre = self._qnow if self._constrain and self._axes is not None: self._axis = arcball_nearest_axis(self._vdown, self._axes) self._vdown = arcball_constrain_to_axis(self._vdown, self._axis) else: self._axis = None def drag(self, point): """Update current cursor window coordinates.""" vnow = arcball_map_to_sphere(point, self._center, self._radius) if self._axis is not None: vnow = arcball_constrain_to_axis(vnow, self._axis) self._qpre = self._qnow t = numpy.cross(self._vdown, vnow) if numpy.dot(t, t) < _EPS: self._qnow = self._qdown else: q = [t[0], t[1], t[2], numpy.dot(self._vdown, vnow)] self._qnow = quaternion_multiply(q, self._qdown) def next(self, acceleration=0.0): """Continue rotation in direction of last drag.""" q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) self._qpre, self._qnow = self._qnow, q def matrix(self): """Return homogeneous rotation matrix.""" return quaternion_matrix(self._qnow) def arcball_map_to_sphere(point, center, radius): """Return unit sphere coordinates from window coordinates.""" v = numpy.array(((point[0] - center[0]) / radius, (center[1] - point[1]) / radius, 0.0), dtype=numpy.float64) n = v[0]*v[0] + v[1]*v[1] if n > 1.0: v /= math.sqrt(n) # position outside of sphere else: v[2] = math.sqrt(1.0 - n) return v def arcball_constrain_to_axis(point, axis): """Return sphere point perpendicular to axis.""" v = numpy.array(point, dtype=numpy.float64, copy=True) a = numpy.array(axis, dtype=numpy.float64, copy=True) v -= a * numpy.dot(a, v) # on plane n = vector_norm(v) if n > _EPS: if v[2] < 0.0: v *= -1.0 v /= n return v if a[2] == 1.0: return numpy.array([1, 0, 0], dtype=numpy.float64) return unit_vector([-a[1], a[0], 0]) def arcball_nearest_axis(point, axes): """Return axis, which arc is nearest to point.""" point = numpy.array(point, dtype=numpy.float64, copy=False) nearest = None mx = -1.0 for axis in axes: t = numpy.dot(arcball_constrain_to_axis(point, axis), point) if t > mx: nearest = axis mx = t return nearest # epsilon for testing whether a number is close to zero _EPS = numpy.finfo(float).eps * 4.0 # axis sequences for Euler angles _NEXT_AXIS = [1, 2, 0, 1] # map axes strings to/from tuples of inner axis, parity, repetition, frame _AXES2TUPLE = { 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) # helper functions def vector_norm(data, axis=None, out=None): """Return length, i.e. eucledian norm, of ndarray along axis. >>> v = numpy.random.random(3) >>> n = vector_norm(v) >>> numpy.allclose(n, numpy.linalg.norm(v)) True >>> v = numpy.random.rand(6, 5, 3) >>> n = vector_norm(v, axis=-1) >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2))) True >>> n = vector_norm(v, axis=1) >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) True >>> v = numpy.random.rand(5, 4, 3) >>> n = numpy.empty((5, 3), dtype=numpy.float64) >>> vector_norm(v, axis=1, out=n) >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1))) True >>> vector_norm([]) 0.0 >>> vector_norm([1.0]) 1.0 """ data = numpy.array(data, dtype=numpy.float64, copy=True) if out is None: if data.ndim == 1: return math.sqrt(numpy.dot(data, data)) data *= data out = numpy.atleast_1d(numpy.sum(data, axis=axis)) numpy.sqrt(out, out) return out else: data *= data numpy.sum(data, axis=axis, out=out) numpy.sqrt(out, out) def unit_vector(data, axis=None, out=None): """Return ndarray normalized by length, i.e. eucledian norm, along axis. >>> v0 = numpy.random.random(3) >>> v1 = unit_vector(v0) >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) True >>> v0 = numpy.random.rand(5, 4, 3) >>> v1 = unit_vector(v0, axis=-1) >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) >>> numpy.allclose(v1, v2) True >>> v1 = unit_vector(v0, axis=1) >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) >>> numpy.allclose(v1, v2) True >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float64) >>> unit_vector(v0, axis=1, out=v1) >>> numpy.allclose(v1, v2) True >>> list(unit_vector([])) [] >>> list(unit_vector([1.0])) [1.0] """ if out is None: data = numpy.array(data, dtype=numpy.float64, copy=True) if data.ndim == 1: data /= math.sqrt(numpy.dot(data, data)) return data else: if out is not data: out[:] = numpy.array(data, copy=False) data = out length = numpy.atleast_1d(numpy.sum(data*data, axis)) numpy.sqrt(length, length) if axis is not None: length = numpy.expand_dims(length, axis) data /= length if out is None: return data def random_vector(size): """Return array of random doubles in the half-open interval [0.0, 1.0). >>> v = random_vector(10000) >>> numpy.all(v >= 0.0) and numpy.all(v < 1.0) True >>> v0 = random_vector(10) >>> v1 = random_vector(10) >>> numpy.any(v0 == v1) False """ return numpy.random.random(size) def inverse_matrix(matrix): """Return inverse of square transformation matrix. >>> M0 = random_rotation_matrix() >>> M1 = inverse_matrix(M0.T) >>> numpy.allclose(M1, numpy.linalg.inv(M0.T)) True >>> for size in range(1, 7): ... M0 = numpy.random.rand(size, size) ... M1 = inverse_matrix(M0) ... if not numpy.allclose(M1, numpy.linalg.inv(M0)): print size """ return numpy.linalg.inv(matrix) def concatenate_matrices(*matrices): """Return concatenation of series of transformation matrices. >>> M = numpy.random.rand(16).reshape((4, 4)) - 0.5 >>> numpy.allclose(M, concatenate_matrices(M)) True >>> numpy.allclose(numpy.dot(M, M.T), concatenate_matrices(M, M.T)) True """ M = numpy.identity(4) for i in matrices: M = numpy.dot(M, i) return M def is_same_transform(matrix0, matrix1): """Return True if two matrices perform same transformation. >>> is_same_transform(numpy.identity(4), numpy.identity(4)) True >>> is_same_transform(numpy.identity(4), random_rotation_matrix()) False """ matrix0 = numpy.array(matrix0, dtype=numpy.float64, copy=True) matrix0 /= matrix0[3, 3] matrix1 = numpy.array(matrix1, dtype=numpy.float64, copy=True) matrix1 /= matrix1[3, 3] return numpy.allclose(matrix0, matrix1) def _import_module(module_name, warn=True, prefix='_py_', ignore='_'): """Try import all public attributes from module into global namespace. Existing attributes with name clashes are renamed with prefix. Attributes starting with underscore are ignored by default. Return True on successful import. """ try: module = __import__(module_name) except ImportError: if warn: warnings.warn("Failed to import module " + module_name) else: for attr in dir(module): if ignore and attr.startswith(ignore): continue if prefix: if attr in globals(): globals()[prefix + attr] = globals()[attr] elif warn: warnings.warn("No Python implementation of " + attr) globals()[attr] = getattr(module, attr) return True geometry-1.13.2/tf/src/tf_echo.cpp000066400000000000000000000117061366756125100167650ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include "tf/transform_listener.h" #include "ros/ros.h" #define _USE_MATH_DEFINES class echoListener { public: tf::TransformListener tf; //constructor with name echoListener() { } ~echoListener() { } private: }; int main(int argc, char ** argv) { //Initialize ROS ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName); // Allow 2 or 3 command line arguments if (argc < 3 || argc > 4) { printf("Usage: tf_echo source_frame target_frame [echo_rate]\n\n"); printf("This will echo the transform from the coordinate frame of the source_frame\n"); printf("to the coordinate frame of the target_frame. \n"); printf("Note: This is the transform to get data from target_frame into the source_frame.\n"); printf("Default echo rate is 1 if echo_rate is not given.\n"); return -1; } ros::NodeHandle nh("~"); double rate_hz; if (argc == 4) { // read rate from command line rate_hz = atof(argv[3]); } else { // read rate parameter nh.param("rate", rate_hz, 1.0); } if (rate_hz <= 0.0) { std::cerr << "Echo rate must be > 0.0\n"; return -1; } ros::Rate rate(rate_hz); int precision(3); if (nh.getParam("precision", precision)) { if (precision < 1) { std::cerr << "Precision must be > 0\n"; return -1; } printf("Precision default value was overriden, new value: %d\n", precision); } //Instantiate a local listener echoListener echoListener; std::string source_frameid = std::string(argv[1]); std::string target_frameid = std::string(argv[2]); // Wait for up to one second for the first transforms to become avaiable. echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0)); //Nothing needs to be done except wait for a quit //The callbacks withing the listener class //will take care of everything while(nh.ok()) { try { tf::StampedTransform echo_transform; echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform); std::cout.precision(precision); std::cout.setf(std::ios::fixed,std::ios::floatfield); std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl; double yaw, pitch, roll; echo_transform.getBasis().getRPY(roll, pitch, yaw); tf::Quaternion q = echo_transform.getRotation(); tf::Vector3 v = echo_transform.getOrigin(); std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl; std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", " << q.getZ() << ", " << q.getW() << "]" << std::endl << " in RPY (radian) [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl << " in RPY (degree) [" << roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl; //print transform } catch(tf::TransformException& ex) { std::cout << "Failure at "<< ros::Time::now() << std::endl; std::cout << "Exception thrown:" << ex.what()<< std::endl; std::cout << "The current list of frames is:" < #include #include #include "ros/ros.h" using namespace tf; using namespace ros; using namespace std; class TFMonitor { public: std::string framea_, frameb_; bool using_specific_chain_; ros::NodeHandle node_; ros::Subscriber subscriber_tf_, subscriber_tf_static_; std::vector chain_; std::map frame_authority_map; std::map > delay_map; std::map > authority_map; std::map > authority_frequency_map; TransformListener tf_; tf::tfMessage message_; boost::mutex map_lock_; void callback(const ros::MessageEvent& msg_evt) { const tf::tfMessage& message = *(msg_evt.getConstMessage()); std::string authority = msg_evt.getPublisherName(); // lookup the authority process_callback(message, authority, false); } void static_callback(const ros::MessageEvent& msg_evt) { const tf::tfMessage& message = *(msg_evt.getConstMessage()); std::string authority = msg_evt.getPublisherName() + std::string("(static)"); // lookup the authority process_callback(message, authority, true); } void process_callback(const tf::tfMessage& message, const std::string & authority, bool is_static) { double average_offset = 0; boost::mutex::scoped_lock my_lock(map_lock_); for (unsigned int i = 0; i < message.transforms.size(); i++) { frame_authority_map[message.transforms[i].child_frame_id] = authority; double offset; if (is_static) { offset = 0.0; } else { offset = (ros::Time::now() - message.transforms[i].header.stamp).toSec(); } average_offset += offset; std::map >::iterator it = delay_map.find(message.transforms[i].child_frame_id); if (it == delay_map.end()) { delay_map[message.transforms[i].child_frame_id] = std::vector(1,offset); } else { it->second.push_back(offset); if (it->second.size() > 1000) it->second.erase(it->second.begin()); } } average_offset /= max((size_t) 1, message.transforms.size()); //create the authority log std::map >::iterator it2 = authority_map.find(authority); if (it2 == authority_map.end()) { authority_map[authority] = std::vector(1,average_offset); } else { it2->second.push_back(average_offset); if (it2->second.size() > 1000) it2->second.erase(it2->second.begin()); } //create the authority frequency log std::map >::iterator it3 = authority_frequency_map.find(authority); if (it3 == authority_frequency_map.end()) { authority_frequency_map[authority] = std::vector(1,ros::Time::now().toSec()); } else { it3->second.push_back(ros::Time::now().toSec()); if (it3->second.size() > 1000) it3->second.erase(it3->second.begin()); } }; TFMonitor(bool using_specific_chain, std::string framea = "", std::string frameb = ""): framea_(framea), frameb_(frameb), using_specific_chain_(using_specific_chain) { if (using_specific_chain_) { cout << "Waiting for transform chain to become available between "<< framea_ << " and " << frameb_<< " " << flush; while (node_.ok() && !tf_.waitForTransform(framea_, frameb_, Time(), Duration(1.0))) cout << "." << flush; cout << endl; try{ tf_.chainAsVector(frameb_, ros::Time(), framea_, ros::Time(), frameb_, chain_); } catch(tf::TransformException& ex){ ROS_WARN("Transform Exception %s", ex.what()); return; } /* cout << "Chain currently is:" <("tf", 100, boost::bind(&TFMonitor::callback, this, _1)); subscriber_tf_static_ = node_.subscribe("tf_static", 100, boost::bind(&TFMonitor::static_callback, this, _1)); } std::string outputFrameInfo(const std::map >::iterator& it, const std::string& frame_authority) { std::stringstream ss; double average_delay = 0; double max_delay = 0; for (unsigned int i = 0; i < it->second.size(); i++) { average_delay += it->second[i]; max_delay = std::max(max_delay, it->second[i]); } average_delay /= it->second.size(); ss << "Frame: " << it->first <<" published by "<< frame_authority << " Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl; return ss.str(); } void spin() { // create tf listener double max_diff = 0; double avg_diff = 0; double lowpass = 0.01; unsigned int counter = 0; while (node_.ok()){ tf::StampedTransform tmp; counter++; // printf("looping %d\n", counter); if (using_specific_chain_) { tf_.lookupTransform(framea_, frameb_, Time(), tmp); double diff = (Time::now() - tmp.stamp_).toSec(); avg_diff = lowpass * diff + (1-lowpass)*avg_diff; if (diff > max_diff) max_diff = diff; } Duration(0.01).sleep(); if (counter > 20){ counter = 0; if (using_specific_chain_) { std::cout < "; } cout < >::iterator it = delay_map.begin(); for ( ; it != delay_map.end() ; ++it) { if (using_specific_chain_ ) { for ( unsigned int i = 0 ; i < chain_.size(); i++) { if (it->first != chain_[i]) continue; cout << outputFrameInfo(it, frame_authority_map[it->first]); } } else cout << outputFrameInfo(it, frame_authority_map[it->first]); } std::cerr < >::iterator it1 = authority_map.begin(); std::map >::iterator it2 = authority_frequency_map.begin(); for ( ; it1 != authority_map.end() ; ++it1, ++it2) { double average_delay = 0; double max_delay = 0; for (unsigned int i = 0; i < it1->second.size(); i++) { average_delay += it1->second[i]; max_delay = std::max(max_delay, it1->second[i]); } average_delay /= it1->second.size(); double frequency_out = (double)(it2->second.size())/std::max(0.00000001, (it2->second.back() - it2->second.front())); //cout << "output" <<&(*it2) <<" " << it2->second.back() <<" " << it2->second.front() <<" " << std::max((size_t)1, it2->second.size()) << " " << frequency_out << endl; cout << "Node: " <first << " " << frequency_out <<" Hz, Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl; } } } } }; int main(int argc, char ** argv) { //Initialize ROS init(argc, argv, "tf_monitor", ros::init_options::AnonymousName); string framea, frameb; bool using_specific_chain = true; if (argc == 3){ framea = argv[1]; frameb = argv[2]; } else if (argc == 1) using_specific_chain = false; else{ ROS_INFO("TF_Monitor: usage: tf_monitor framea frameb"); return -1; } std::string searched_param; std::string tf_prefix; ros::NodeHandle local_nh("~"); local_nh.searchParam("tf_prefix", searched_param); local_nh.getParam(searched_param, tf_prefix); //Make sure we don't start before recieving time when in simtime int iterations = 0; while (ros::Time::now() == ros::Time()) { if (++iterations > 10) { ROS_INFO("tf_monitor waiting for time to be published"); iterations = 0; } ros::WallDuration(0.1).sleep(); } ros::NodeHandle nh; boost::thread spinner( boost::bind( &ros::spin )); TFMonitor monitor(using_specific_chain, tf::resolve(tf_prefix, framea), tf::resolve(tf_prefix, frameb)); monitor.spin(); spinner.join(); return 0; } geometry-1.13.2/tf/src/transform_broadcaster.cpp000066400000000000000000000053561366756125100217460ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "ros/ros.h" #include "tf/transform_broadcaster.h" #include "tf/transform_listener.h" #include namespace tf { TransformBroadcaster::TransformBroadcaster(): tf2_broadcaster_() { } void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf) { tf2_broadcaster_.sendTransform(msgtf); } void TransformBroadcaster::sendTransform(const StampedTransform & transform) { geometry_msgs::TransformStamped msgtf; transformStampedTFToMsg(transform, msgtf); tf2_broadcaster_.sendTransform(msgtf); } void TransformBroadcaster::sendTransform(const std::vector & msgtf) { tf2_broadcaster_.sendTransform(msgtf); } void TransformBroadcaster::sendTransform(const std::vector & transforms) { std::vector msgtfs; for (std::vector::const_iterator it = transforms.begin(); it != transforms.end(); ++it) { geometry_msgs::TransformStamped msgtf; transformStampedTFToMsg(*it, msgtf); msgtfs.push_back(msgtf); } tf2_broadcaster_.sendTransform(msgtfs); } } geometry-1.13.2/tf/src/transform_listener.cpp000066400000000000000000000226671366756125100213060ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "tf/transform_listener.h" using namespace tf; std::string tf::remap(const std::string& frame_id) { ros::NodeHandle n("~"); return tf::resolve(getPrefixParam(n), frame_id); } TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread): Transformer(true, max_cache_time), tf2_listener_(*Transformer::tf2_buffer_ptr_, node_, spin_thread) { //Everything is done inside tf2 init } TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread): Transformer(true, max_cache_time), node_(nh), tf2_listener_(*Transformer::tf2_buffer_ptr_, nh, spin_thread) { //Everything is done inside tf2 init } TransformListener::~TransformListener() { //Everything is done inside tf2 init } //Override Transformer::ok() for ticket:4882 bool TransformListener::ok() const { return ros::ok(); } void TransformListener::transformQuaternion(const std::string& target_frame, const geometry_msgs::QuaternionStamped& msg_in, geometry_msgs::QuaternionStamped& msg_out) const { tf::assertQuaternionValid(msg_in.quaternion); Stamped pin, pout; quaternionStampedMsgToTF(msg_in, pin); transformQuaternion(target_frame, pin, pout); quaternionStampedTFToMsg(pout, msg_out); } void TransformListener::transformVector(const std::string& target_frame, const geometry_msgs::Vector3Stamped& msg_in, geometry_msgs::Vector3Stamped& msg_out) const { Stamped pin, pout; vector3StampedMsgToTF(msg_in, pin); transformVector(target_frame, pin, pout); vector3StampedTFToMsg(pout, msg_out); } void TransformListener::transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& msg_in, geometry_msgs::PointStamped& msg_out) const { Stamped pin, pout; pointStampedMsgToTF(msg_in, pin); transformPoint(target_frame, pin, pout); pointStampedTFToMsg(pout, msg_out); } void TransformListener::transformPose(const std::string& target_frame, const geometry_msgs::PoseStamped& msg_in, geometry_msgs::PoseStamped& msg_out) const { tf::assertQuaternionValid(msg_in.pose.orientation); Stamped pin, pout; poseStampedMsgToTF(msg_in, pin); transformPose(target_frame, pin, pout); poseStampedTFToMsg(pout, msg_out); } /* http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review void TransformListener::transformTwist(const std::string& target_frame, const geometry_msgs::TwistStamped& msg_in, geometry_msgs::TwistStamped& msg_out) const { tf::Vector3 twist_rot(msg_in.twist.angular.x, msg_in.twist.angular.y, msg_in.twist.angular.z); tf::Vector3 twist_vel(msg_in.twist.linear.x, msg_in.twist.linear.y, msg_in.twist.linear.z); tf::StampedTransform transform; lookupTransform(target_frame,msg_in.header.frame_id, msg_in.header.stamp, transform); tf::Vector3 out_rot = transform.getBasis() * twist_rot; tf::Vector3 out_vel = transform.getBasis()* twist_vel + transform.getOrigin().cross(out_rot); geometry_msgs::TwistStamped interframe_twist; lookupVelocity(target_frame, msg_in.header.frame_id, msg_in.header.stamp, ros::Duration(0.1), interframe_twist); //\todo get rid of hard coded number msg_out.header.stamp = msg_in.header.stamp; msg_out.header.frame_id = target_frame; msg_out.twist.linear.x = out_vel.x() + interframe_twist.twist.linear.x; msg_out.twist.linear.y = out_vel.y() + interframe_twist.twist.linear.y; msg_out.twist.linear.z = out_vel.z() + interframe_twist.twist.linear.z; msg_out.twist.angular.x = out_rot.x() + interframe_twist.twist.angular.x; msg_out.twist.angular.y = out_rot.y() + interframe_twist.twist.angular.y; msg_out.twist.angular.z = out_rot.z() + interframe_twist.twist.angular.z; }*/ void TransformListener::transformQuaternion(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::QuaternionStamped& msg_in, const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out) const { tf::assertQuaternionValid(msg_in.quaternion); Stamped pin, pout; quaternionStampedMsgToTF(msg_in, pin); transformQuaternion(target_frame, target_time, pin, fixed_frame, pout); quaternionStampedTFToMsg(pout, msg_out); } void TransformListener::transformVector(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::Vector3Stamped& msg_in, const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out) const { Stamped pin, pout; vector3StampedMsgToTF(msg_in, pin); transformVector(target_frame, target_time, pin, fixed_frame, pout); vector3StampedTFToMsg(pout, msg_out); } void TransformListener::transformPoint(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::PointStamped& msg_in, const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out) const { Stamped pin, pout; pointStampedMsgToTF(msg_in, pin); transformPoint(target_frame, target_time, pin, fixed_frame, pout); pointStampedTFToMsg(pout, msg_out); } void TransformListener::transformPose(const std::string& target_frame, const ros::Time& target_time, const geometry_msgs::PoseStamped& msg_in, const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out) const { tf::assertQuaternionValid(msg_in.pose.orientation); Stamped pin, pout; poseStampedMsgToTF(msg_in, pin); transformPose(target_frame, target_time, pin, fixed_frame, pout); poseStampedTFToMsg(pout, msg_out); } void TransformListener::transformPointCloud(const std::string & target_frame, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const { StampedTransform transform; lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform); transformPointCloud(target_frame, transform, cloudIn.header.stamp, cloudIn, cloudOut); } void TransformListener::transformPointCloud(const std::string& target_frame, const ros::Time& target_time, const sensor_msgs::PointCloud& cloudIn, const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut) const { StampedTransform transform; lookupTransform(target_frame, target_time, cloudIn.header.frame_id, cloudIn.header.stamp, fixed_frame, transform); transformPointCloud(target_frame, transform, target_time, cloudIn, cloudOut); } inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out) { // Use temporary variables in case &in == &out double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x(); double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y(); double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z(); out.x = x; out.y = y; out.z = z; } void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform, const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const { tf::Vector3 origin = net_transform.getOrigin(); tf::Matrix3x3 basis = net_transform.getBasis(); unsigned int length = cloudIn.points.size(); // Copy relevant data from cloudIn, if needed if (&cloudIn != &cloudOut) { cloudOut.header = cloudIn.header; cloudOut.points.resize(length); cloudOut.channels.resize(cloudIn.channels.size()); for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i) cloudOut.channels[i] = cloudIn.channels[i]; } // Transform points cloudOut.header.stamp = target_time; cloudOut.header.frame_id = target_frame; for (unsigned int i = 0; i < length ; i++) { transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]); } } geometry-1.13.2/tf/srv/000077500000000000000000000000001366756125100146705ustar00rootroot00000000000000geometry-1.13.2/tf/srv/FrameGraph.srv000066400000000000000000000000251366756125100174350ustar00rootroot00000000000000--- string dot_graph geometry-1.13.2/tf/test/000077500000000000000000000000001366756125100150355ustar00rootroot00000000000000geometry-1.13.2/tf/test/cache_unittest.cpp000066400000000000000000000310041366756125100205410ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include "tf/LinearMath/Vector3.h" #include "tf/LinearMath/Matrix3x3.h" void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); } using namespace tf; TEST(TimeCache, Repeatability) { unsigned int runs = 100; seed_rand(); tf::TimeCache cache; std::vector values(runs); StampedTransform t; t.setIdentity(); for ( uint64_t i = 1; i < runs ; i++ ) { values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; std::stringstream ss; ss << values[i]; t.frame_id_ = ss.str(); t.stamp_ = ros::Time().fromNSec(i); TransformStorage stor(t, i, 0); cache.insertData(stor); } TransformStorage out; for ( uint64_t i = 1; i < runs ; i++ ) { cache.getData(ros::Time().fromNSec(i), out); EXPECT_EQ(out.frame_id_, i); EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i)); } } TEST(TimeCache, RepeatabilityReverseInsertOrder) { unsigned int runs = 100; seed_rand(); tf::TimeCache cache; std::vector values(runs); StampedTransform t; t.setIdentity(); for ( int i = runs -1; i >= 0 ; i-- ) { values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; t.stamp_ = ros::Time().fromNSec(i); TransformStorage stor(t, i, 0); cache.insertData(stor); } TransformStorage out; for ( uint64_t i = 1; i < runs ; i++ ) { cache.getData(ros::Time().fromNSec(i), out); EXPECT_EQ(out.frame_id_, i); EXPECT_EQ(out.stamp_, ros::Time().fromNSec(i)); } } TEST(TimeCache, RepeatabilityRandomInsertOrder) { seed_rand(); tf::TimeCache cache; double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8}; std::vector values (my_vals, my_vals + sizeof(my_vals)/sizeof(double)); unsigned int runs = values.size(); StampedTransform t; t.setIdentity(); for ( uint64_t i = 0; i values(runs); StampedTransform t; t.setIdentity(); for ( uint64_t i = 0; i xvalues(2); std::vector yvalues(2); std::vector zvalues(2); uint64_t offset = 200; StampedTransform t; t.setIdentity(); for ( uint64_t i = 1; i < runs ; i++ ) { for (uint64_t step = 0; step < 2 ; step++) { xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step])); t.stamp_ = ros::Time().fromNSec(step * 100 + offset); TransformStorage stor(t, 2, 0); cache.insertData(stor); } TransformStorage out; for (int pos = 0; pos < 100 ; pos++) { cache.getData(ros::Time().fromNSec(offset + pos), out); double x_out = out.translation_.x(); double y_out = out.translation_.y(); double z_out = out.translation_.z(); EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon); EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon); EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon); } cache.clearList(); } } /* TEST IS BROKEN, NEED TO PREVENT THIS ///\brief Make sure we dont' interpolate across reparented data TEST(TimeCache, ReparentingInterpolationProtection) { double epsilon = 1e-6; uint64_t offset = 555; tf::TimeCache cache; std::vector xvalues(2); std::vector yvalues(2); std::vector zvalues(2); StampedTransform t; t.setIdentity(); for (uint64_t step = 0; step < 2 ; step++) { xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step])); t.stamp_ = ros::Time().fromNSec(step * 100 + offset); TransformStorage stor(t, step + 4, 0); cache.insertData(stor); } TransformStorage out; for (int pos = 0; pos < 100 ; pos ++) { EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), out)); double x_out = out.translation_.x(); double y_out = out.translation_.y(); double z_out = out.translation_.z(); EXPECT_NEAR(xvalues[0], x_out, epsilon); EXPECT_NEAR(yvalues[0], y_out, epsilon); EXPECT_NEAR(zvalues[0], z_out, epsilon); } for (int pos = 100; pos < 120 ; pos ++) { EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), out)); double x_out = out.translation_.x(); double y_out = out.translation_.y(); double z_out = out.translation_.z(); EXPECT_NEAR(xvalues[1], x_out, epsilon); EXPECT_NEAR(yvalues[1], y_out, epsilon); EXPECT_NEAR(zvalues[1], z_out, epsilon); } } // EXTRAPOLATION DISABLED TEST(TimeCache, CartesianExtrapolation) { uint64_t runs = 100; double epsilon = 1e-5; seed_rand(); tf::TimeCache cache; std::vector xvalues(2); std::vector yvalues(2); std::vector zvalues(2); uint64_t offset = 555; StampedTransform t; t.setIdentity(); for ( uint64_t i = 1; i < runs ; i++ ) { for (uint64_t step = 0; step < 2 ; step++) { xvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; t.setOrigin(tf::Vector3(xvalues[step], yvalues[step], zvalues[step])); t.stamp_ = ros::Time().fromNSec(step * 100 + offset); TransformStorage stor(t, 2, 0); cache.insertData(stor); } TransformStorage out; for (int pos = -200; pos < 300 ; pos ++) { cache.getData(ros::Time().fromNSec(offset + pos), out); double x_out = out.translation_.x(); double y_out = out.translation_.y(); double z_out = out.translation_.z(); EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon); EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon); EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon); } cache.clearList(); } } */ TEST(Bullet, Slerp) { uint64_t runs = 100; seed_rand(); tf::Quaternion q1, q2; q1.setEuler(0,0,0); for (uint64_t i = 0 ; i < runs ; i++) { q2.setEuler(1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX, 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX, 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX); tf::Quaternion q3 = slerp(q1,q2,0.5); EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5); } } TEST(TimeCache, AngularInterpolation) { uint64_t runs = 100; double epsilon = 1e-6; seed_rand(); tf::TimeCache cache; std::vector yawvalues(2); std::vector pitchvalues(2); std::vector rollvalues(2); uint64_t offset = 200; std::vector quats(2); StampedTransform t; t.setIdentity(); for ( uint64_t i = 1; i < runs ; i++ ) { for (uint64_t step = 0; step < 2 ; step++) { yawvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX / 100.0; pitchvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; rollvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]); t.setRotation(quats[step]); t.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1 TransformStorage stor(t, 3, 0); cache.insertData(stor); } TransformStorage out; for (int pos = 0; pos < 100 ; pos ++) { cache.getData(ros::Time().fromNSec(offset + pos), out); //get the transform for the position tf::Quaternion quat = out.rotation_; //get the quaternion out of the transform //Generate a ground truth quaternion directly calling slerp tf::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0); //Make sure the transformed one and the direct call match EXPECT_NEAR(0, angle(ground_truth, quat), epsilon); } cache.clearList(); } } TEST(TimeCache, DuplicateEntries) { TimeCache cache; StampedTransform t; t.setIdentity(); t.stamp_ = ros::Time().fromNSec(1); TransformStorage stor(t, 3, 0); cache.insertData(stor); cache.insertData(stor); cache.getData(ros::Time().fromNSec(1), stor); EXPECT_TRUE(!std::isnan(stor.translation_.x())); EXPECT_TRUE(!std::isnan(stor.translation_.y())); EXPECT_TRUE(!std::isnan(stor.translation_.z())); EXPECT_TRUE(!std::isnan(stor.rotation_.x())); EXPECT_TRUE(!std::isnan(stor.rotation_.y())); EXPECT_TRUE(!std::isnan(stor.rotation_.z())); EXPECT_TRUE(!std::isnan(stor.rotation_.w())); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/test/method_test.py000066400000000000000000000025621366756125100177330ustar00rootroot00000000000000from __future__ import print_function import rospy import tf import time import bullet import math try: transform_stamped = tf.TransformStamped() print("getting stamp") print(transform_stamped.stamp) # mytime = rospy.Time().now() mytime = rospy.Time(10,20) transform_stamped.stamp = mytime print(mytime) print("getting stamp", transform_stamped.stamp) print("transform:", transform_stamped.transform) transform_stamped.transform.setIdentity() print("after setIdentity()", transform_stamped.transform) # transform_stamped.transform.basis.setEulerZYX(0,0,0) quat = bullet.Quaternion(math.pi/2,0,0) print("quaternion ", quat) transform_stamped.transform.setRotation(quat) print("setting rotation to PI/2\n After setRotation") print(transform_stamped.transform) other_transform = bullet.Transform() other_transform.setIdentity() transform_stamped.transform = other_transform print("After assignment of Identity") print(transform_stamped.transform) other_transform = bullet.Transform() other_transform.setIdentity() other_transform.setRotation(quat) transform_stamped.transform = other_transform print("After assignment of Rotation Transformation") print(transform_stamped.transform) except ValueError as e: print("Exception {} Improperly thrown: {}".format(type(e), e)) geometry-1.13.2/tf/test/operator_overload.cpp000066400000000000000000000053661366756125100213010ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "tf/LinearMath/Transform.h" void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); }; int main(int argc, char **argv){ unsigned int runs = 400; seed_rand(); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; } //Useful Operator Overload for ( unsigned int i = 0; i < runs ; i++ ) { tf::Transform transform(tf::Quaternion(0,0,0), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])); tf::Quaternion initial(xvalues[i],yvalues[i],zvalues[i]); tf::Quaternion final(xvalues[i],yvalues[i],zvalues[i]); final = transform * initial; std::printf("Useful Operator Overload: %f, angle between quaternions\n", initial.angle(final)); } return 0; } geometry-1.13.2/tf/test/python_debug_test.py000066400000000000000000000114211366756125100211340ustar00rootroot00000000000000# Copyright (c) 2009, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from __future__ import print_function import rospy import tf import time import bullet import math try: rospy.init_node("test_node") tfl = tf.TransformListener() time.sleep(1) # View all frames print("All frames are:\n", tfl.all_frames_as_string()) # dot based introspection print("All frame graph is:\n", tfl.all_frames_as_dot()) # test transforming pose po = tf.PoseStamped() po.frame_id = "base_link" print("calling transform pose") po2 = tfl.transform_pose("/map", po) print("po2.pose.this", po2.pose.this) print("po.pose.this", po.pose.this) # test transforming point po = tf.PointStamped() po.frame_id = "base_link" po3 = tfl.transform_point("/map", po) print("po3") print(po3.this) # test transforming vector po = tf.VectorStamped() po.frame_id = "base_link" po4 = tfl.transform_vector("/map", po) print(po4.this) # test transforming quaternion po = tf.QuaternionStamped() po.frame_id = "base_link" po5 = tfl.transform_quaternion("/map", po) print("po5", po5.this) tr = tf.TransformStamped() lps = tf.PoseStamped() lps.pose.setIdentity() print("setting stamp") mytime = rospy.Time(10,20) lps.stamp = mytime print(mytime) print("getting stamp") output = lps.stamp print(output) print(lps.pose) print("setting pose.positon to 1,2,3") lps.pose.setOrigin( bullet.Vector3(1,2,3)) print(lps.pose.getOrigin()) print(lps.pose) transform_stamped = tf.TransformStamped() print("getting stamp") print(transform_stamped.stamp) # mytime = rospy.Time().now() mytime = rospy.Time(10,20) transform_stamped.stamp = mytime print(mytime) print("getting stamp", transform_stamped.stamp) print("transform:", transform_stamped.transform) transform_stamped.transform.setIdentity() print("after setIdentity()", transform_stamped.transform) # transform_stamped.transform.basis.setEulerZYX(0,0,0) quat = bullet.Quaternion(math.pi/2,0,0) print("quaternion ", quat) transform_stamped.transform.setRotation(quat) print("setting rotation to PI/2",transform_stamped.transform) pointstamped = tf.PointStamped() print("getting stamp") print(pointstamped.stamp) # mytime = rospy.Time().now() mytime = rospy.Time(10,20) pointstamped.stamp = mytime print(mytime) print("getting stamp") output = pointstamped.stamp print(output) print(pointstamped.point) print(transform_stamped.transform * pointstamped.point) pose_only = bullet.Transform(transform_stamped.transform) print("destructing pose_only", pose_only.this ) pose_only = [] print("Creating copy") po2_copy = tf.PoseStamped(po2) print("po2_copy.pose", po2_copy.pose.this) print("po2.pose", po2.pose.this) print("Creating copy2") po2_copy2 = tf.PoseStamped(po2) print("po2_copy2.pose", po2_copy2.pose.this) print("destructing po2 po2.pose is", po2.pose.this) po2 = [] print("destructing po2_copy po2_copy.pose is", po2_copy.pose.this) po2_copy = [] print("done") except ValueError as e: print("Exception {} Improperly thrown: {}".format(type(e), e)) geometry-1.13.2/tf/test/quaternion.cpp000066400000000000000000000100611366756125100177240ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include "tf/LinearMath/Transform.h" double epsilon = 10E-6; void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); } void testQuatRPY(tf::Quaternion q_baseline) { q_baseline.normalize(); tf::Matrix3x3 m(q_baseline); double roll, pitch, yaw; for (int solution = 1 ; solution < 2 ; ++solution) { m.getRPY(roll, pitch, yaw, solution); tf::Quaternion q_from_rpy; q_from_rpy.setRPY(roll, pitch, yaw); // use angleShortestPath() because angle() can return PI for equivalent // quaternions double angle1 = q_from_rpy.angleShortestPath(q_baseline); ASSERT_NEAR(0.0, angle1, epsilon); //std::printf("%f, angle between quaternions\n", angle1); tf::Matrix3x3 m2; m2.setRPY(roll, pitch, yaw); tf::Quaternion q_from_m_from_rpy; m2.getRotation(q_from_m_from_rpy); // use angleShortestPath() because angle() can return PI for equivalent // quaternions double angle2 = q_from_m_from_rpy.angleShortestPath(q_baseline); ASSERT_NEAR(0.0, angle2, epsilon); //std::printf("%f, angle between quaternions\n", angle2); } } TEST(tf, Quaternion) { unsigned int runs = 400; seed_rand(); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; } for ( unsigned int i = 0; i < runs ; i++ ) { tf::Matrix3x3 mat; tf::Quaternion q_baseline; q_baseline.setRPY(zvalues[i],yvalues[i],xvalues[i]); mat.setRotation(q_baseline); tf::Quaternion q_from_m; mat.getRotation(q_from_m); double angle = q_from_m.angle(q_baseline); ASSERT_NEAR(0.0, angle, epsilon); testQuatRPY(q_baseline); } // test some corner cases testQuatRPY(tf::Quaternion( 0.5, 0.5, 0.5, -0.5)); testQuatRPY(tf::Quaternion( 0.5, 0.5, 0.5, 0.5)); testQuatRPY(tf::Quaternion( 0.5, -0.5, 0.5, 0.5)); testQuatRPY(tf::Quaternion( 0.5, 0.5, -0.5, 0.5)); testQuatRPY(tf::Quaternion(-0.5, 0.5, 0.5, 0.5)); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/test/speed_test.cpp000066400000000000000000000154341366756125100177070ustar00rootroot00000000000000/* * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include using namespace tf; int main(int argc, char** argv) { uint32_t num_levels = 10; if (argc > 1) { num_levels = boost::lexical_cast(argv[1]); } tf::Transformer bc; StampedTransform t; t.stamp_ = ros::Time(1); t.frame_id_ = "root"; t.child_frame_id_ = "0"; t.setIdentity(); t.setOrigin(tf::Vector3(1,0,0)); bc.setTransform(t, "me"); t.stamp_ = ros::Time(2); bc.setTransform(t, "me"); for (uint32_t i = 1; i < num_levels/2; ++i) { for (uint32_t j = 1; j < 3; ++j) { std::stringstream parent_ss; parent_ss << (i - 1); std::stringstream child_ss; child_ss << i; t.stamp_ = ros::Time(j); t.frame_id_ = parent_ss.str(); t.child_frame_id_ = child_ss.str(); bc.setTransform(t, "me"); } } t.frame_id_ = "root"; std::stringstream ss; ss << num_levels/2; t.stamp_ = ros::Time(1); t.child_frame_id_ = ss.str(); bc.setTransform(t, "me"); t.stamp_ = ros::Time(2); bc.setTransform(t, "me"); for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i) { for (uint32_t j = 1; j < 3; ++j) { std::stringstream parent_ss; parent_ss << (i - 1); std::stringstream child_ss; child_ss << i; t.stamp_ = ros::Time(j); t.frame_id_ = parent_ss.str(); t.child_frame_id_ = child_ss.str(); bc.setTransform(t, "me"); } } //ROS_INFO_STREAM(bc.allFramesAsYAML()); std::string v_frame0 = boost::lexical_cast(num_levels - 1); std::string v_frame1 = boost::lexical_cast(num_levels/2 - 1); ROS_INFO("%s to %s", v_frame0.c_str(), v_frame1.c_str()); StampedTransform out_t; const uint32_t count = 1000000; ROS_INFO("Doing %d %d-level tests", count, num_levels); #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.lookupTransform(v_frame1, v_frame0, ros::Time(0), out_t); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); ROS_INFO("lookupTransform at Time(0) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.lookupTransform(v_frame1, v_frame0, ros::Time(1), out_t); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); ROS_INFO("lookupTransform at Time(1) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5), out_t); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); ROS_INFO("lookupTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.lookupTransform(v_frame1, v_frame0, ros::Time(2), out_t); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); ROS_INFO("lookupTransform at Time(2) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(0)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); ROS_INFO("canTransform at Time(0) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(1)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); ROS_INFO("canTransform at Time(1) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(1.5)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); ROS_INFO("canTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(2)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); ROS_INFO("canTransform at Time(2) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); } #endif } geometry-1.13.2/tf/test/testBroadcaster.cpp000066400000000000000000000065661366756125100207070ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "tf/transform_broadcaster.h" #include "ros/ros.h" class testBroadcaster { public: //constructor testBroadcaster() : count(0), count1(0){}; //Clean up ros connections ~testBroadcaster() { } //A pointer to the rosTFServer class tf::TransformBroadcaster broadcaster; // A function to call to send data periodically void test () { broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "frame2", "frame1")); if (count > 9000) { count = 0; std::cerr<<"Counter 0 rolledover at 9000"<< std::endl; } else count ++; //std::cerr< vec; vec.push_back(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "vframe2", "vframe1")); vec.push_back(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "vframe1", "vframe0")); broadcaster.sendTransform(vec); if (count1 > 9000) { count1 = 0; std::cerr<<"Counter 1 rolledover at 9000"<< std::endl; } else count1 ++; //std::cerr< TEST(TransformBroadcaster, single_frame) { tf::TransformListener tfl; EXPECT_TRUE(tfl.waitForTransform("frame1", "frame2", ros::Time(), ros::Duration(2.0))); } TEST(TransformBroadcaster, multi_frame) { tf::TransformListener tfl; EXPECT_TRUE(tfl.waitForTransform("vframe0", "vframe2", ros::Time(), ros::Duration(2.0))); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); //Initialize ROS ros::init(argc, argv, "listener"); ros::NodeHandle nh; int ret = RUN_ALL_TESTS(); return ret; } geometry-1.13.2/tf/test/testPython.py000066400000000000000000000303671366756125100176010ustar00rootroot00000000000000import rostest import rospy import unittest import time import tf.transformations import geometry_msgs.msg import sensor_msgs.msg import tf def process_time(): try: # Available in Python >= 3.3 required in Python >= 3.8 return time.process_time() except AttributeError: # Python < 3.3 compatibility return time.clock() class Mock: pass def setT(t, parent, frame, ti, x): m = Mock() m.parent_id = parent m.header = Mock() m.header.stamp = ti m.header.frame_id = frame m.transform = Mock() m.transform.translation = Mock() m.transform.translation.x = x m.transform.translation.y = 0 m.transform.translation.z = 0 m.transform.rotation = Mock() m.transform.rotation.x = 0 m.transform.rotation.y = 0 m.transform.rotation.z = 0 m.transform.rotation.w = 1 t.setTransform(m) class TestPython(unittest.TestCase): @classmethod def setUpClass(cls): super(TestPython, cls).setUpClass() rospy.rostime.set_rostime_initialized(True) def setUp(self): pass def common(self, t): m = geometry_msgs.msg.TransformStamped() m.header.frame_id = "PARENT" m.child_frame_id = "THISFRAME" m.transform.translation.y = 5.0 m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) t.setTransform(m) afs = t.allFramesAsString() self.assert_(len(afs) != 0) self.assert_("PARENT" in afs) self.assert_("THISFRAME" in afs) # Test getFrameStrings frames = t.getFrameStrings() self.assert_("THISFRAME" in frames) self.assert_("PARENT" not in frames) self.assert_(t.frameExists("THISFRAME")) self.assert_(not t.frameExists("PARENT")) self.assert_(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec() == 0) for ti in [3, 5, 10, 11, 19, 20, 21]: m.header.stamp.secs = ti t.setTransform(m) self.assert_(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec() == ti) # Verify that getLatestCommonTime with nonexistent frames raise exception self.assertRaises(tf.Exception, lambda: t.getLatestCommonTime("MANDALAY", "JUPITER")) self.assertRaises(tf.LookupException, lambda: t.lookupTransform("MANDALAY", "JUPITER", rospy.Time())) # Ask for transform for valid frames, but more than 10 seconds in the past. Should raise ExtrapolationException self.assertRaises(tf.ExtrapolationException, lambda: t.lookupTransform("THISFRAME", "PARENT", rospy.Time(2))) #### print t.lookupVelocity("THISFRAME", "PARENT", rospy.Time(15), rospy.Duration(5)) def test_smoke(self): t = tf.Transformer() self.common(t) def test_chain(self): t = tf.Transformer() self.common(t) m = geometry_msgs.msg.TransformStamped() m.header.frame_id = "A" m.child_frame_id = "B" m.transform.translation.y = 5.0 m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) t.setTransform(m) m.header.frame_id = "B" m.child_frame_id = "C" t.setTransform(m) m.header.frame_id = "B" m.child_frame_id = "C" t.setTransform(m) chain = t.chain("A", rospy.Time(0), "C", rospy.Time(0), "B") print("Chain is {}".format(chain)) self.assert_("C" in chain) self.assert_("B" in chain) def test_wait_for_transform(self): def elapsed_time_within_epsilon(t, delta, epsilon): self.assertLess( t - epsilon, delta) self.assertGreater( delta, t + epsilon) t = tf.Transformer() self.common(t) timeout = rospy.Duration(1) epsilon = 0.1 # Check for dedicated thread exception, existing frames self.assertRaises(tf.Exception, lambda: t.waitForTransform("PARENT", "THISFRAME", rospy.Time(), timeout)) # Check for dedicated thread exception, non-existing frames self.assertRaises(tf.Exception, lambda: t.waitForTransform("MANDALAY", "JUPITER", rospy.Time(), timeout)) t.setUsingDedicatedThread(True) # This will no longer thorw self.assertEqual(t.waitForTransform("PARENT", "THISFRAME", rospy.Time(), timeout), None) self.assertEqual(t.waitForTransform("PARENT", "THISFRAME", rospy.Time(15), timeout), None) # Verify exception still thrown with unavailable time near timeout start = process_time() self.assertRaises(tf.Exception, lambda: t.waitForTransform("PARENT", "THISFRAME", rospy.Time(25), timeout)) elapsed_time_within_epsilon(process_time() - start, timeout.to_sec(), epsilon) # Verify exception stil thrown with non-existing frames near timeout start = process_time() self.assertRaises(tf.Exception, lambda: t.waitForTransform("MANDALAY", "JUPITER", rospy.Time(), timeout)) elapsed_time_within_epsilon(process_time() - start, timeout.to_sec(), epsilon) def test_cache_time(self): # Vary cache_time and confirm its effect on ExtrapolationException from lookupTransform(). for cache_time in range(2, 98): t = tf.Transformer(True, rospy.Duration(cache_time)) m = geometry_msgs.msg.TransformStamped() m.header.frame_id = "PARENT" m.child_frame_id = "THISFRAME" m.transform.translation.y = 5.0 m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) t.setTransform(m) afs = t.allFramesAsString() self.assert_(len(afs) != 0) self.assert_("PARENT" in afs) self.assert_("THISFRAME" in afs) self.assert_(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec() == 0) # Set transforms for time 0..100 inclusive for ti in range(101): m.header.stamp = rospy.Time(ti) t.setTransform(m) self.assert_(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec() == ti) self.assertEqual(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec(), 100) # (avoid time of 0 because that means 'latest') for ti in range(1, 100 - cache_time): self.assertRaises(tf.ExtrapolationException, lambda: t.lookupTransform("THISFRAME", "PARENT", rospy.Time(ti))) for ti in range(100 - cache_time, 100): t.lookupTransform("THISFRAME", "PARENT", rospy.Time(ti)) def test_subclass(self): class TransformerSubclass(tf.Transformer): def extra(self): return 77 t = TransformerSubclass(True, rospy.Duration.from_sec(10.0)) self.assert_(t.extra() == 77) self.common(t) self.assert_(t.extra() == 77) def test_twist(self): t = tf.Transformer() vel = 3 for ti in range(5): m = geometry_msgs.msg.TransformStamped() m.header.frame_id = "PARENT" m.header.stamp = rospy.Time(ti) m.child_frame_id = "THISFRAME" m.transform.translation.x = ti * vel m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) t.setTransform(m) tw0 = t.lookupTwist("THISFRAME", "PARENT", rospy.Time(0.0), rospy.Duration(4.001)) self.assertAlmostEqual(tw0[0][0], vel, 2) tw1 = t.lookupTwistFull("THISFRAME", "PARENT", "PARENT", (0, 0, 0), "THISFRAME", rospy.Time(0.0), rospy.Duration(4.001)) self.assertEqual(tw0, tw1) def test_transformer_ros(self): tr = tf.TransformerROS() m = geometry_msgs.msg.TransformStamped() m.header.stamp = rospy.Time().from_sec(3.0) m.header.frame_id = "PARENT" m.child_frame_id = "THISFRAME" m.transform.translation.y = 5.0 m.transform.rotation.x = 0.04997917 m.transform.rotation.y = 0 m.transform.rotation.z = 0 m.transform.rotation.w = 0.99875026 tr.setTransform(m) m.header.stamp = rospy.Time().from_sec(5.0) tr.setTransform(m) # Smoke the various transform* methods types = [ "Point", "Pose", "Quaternion", "Vector3" ] for t in types: msg = getattr(geometry_msgs.msg, "{}Stamped".format(t))() msg.header.frame_id = "THISFRAME" msg_t = getattr(tr, "transform{}".format(t))("PARENT", msg) self.assertEqual(msg_t.header.frame_id, "PARENT") # PointCloud is a bit different, so smoke is different msg = sensor_msgs.msg.PointCloud() msg.header.frame_id = "THISFRAME" msg.points = [geometry_msgs.msg.Point32(1,2,3)] xmsg = tr.transformPointCloud("PARENT", msg) self.assertEqual(xmsg.header.frame_id, "PARENT") self.assertEqual(len(msg.points), len(xmsg.points)) self.assertNotEqual(msg.points[0], xmsg.points[0]) """ Two fixed quaternions, a small twist around X concatenated. >>> t.quaternion_from_euler(0.1, 0, 0) array([ 0.04997917, 0. , 0. , 0.99875026]) >>> t.quaternion_from_euler(0.2, 0, 0) array([ 0.09983342, 0. , 0. , 0.99500417]) """ # Specific test for quaternion types msg = geometry_msgs.msg.QuaternionStamped() q = [ 0.04997917, 0. , 0. , 0.99875026 ] msg.quaternion.x = q[0] msg.quaternion.y = q[1] msg.quaternion.z = q[2] msg.quaternion.w = q[3] msg.header.stamp = rospy.Time().from_sec(3.0) msg.header.frame_id = "THISFRAME" msg_t = tr.transformQuaternion("PARENT", msg) self.assertEqual(msg_t.header.frame_id, "PARENT") for a,v in zip("xyzw", [ 0.09983342, 0. , 0. , 0.99500417]): self.assertAlmostEqual(v, getattr(msg_t.quaternion, a), 4) def test_transformer_wait_for_transform_dedicated_thread(self): tr = tf.Transformer() try: tr.waitForTransform("PARENT", "THISFRAME", rospy.Time().from_sec(4.0), rospy.Duration(3.0)) self.assertFalse("This should throw") except tf.Exception as ex: print("successfully caught") pass def test_transformer_wait_for_transform(self): tr = tf.Transformer() tr.setUsingDedicatedThread(1) try: tr.waitForTransform("PARENT", "THISFRAME", rospy.Time().from_sec(4.0), rospy.Duration(3.0)) self.assertFalse("This should throw") except tf.Exception as ex: pass m = geometry_msgs.msg.TransformStamped() m.header.stamp = rospy.Time().from_sec(3.0) m.header.frame_id = "PARENT" m.child_frame_id = "THISFRAME" m.transform.translation.y = 5.0 m.transform.rotation.x = 0.04997917 m.transform.rotation.y = 0 m.transform.rotation.z = 0 m.transform.rotation.w = 0.99875026 tr.setTransform(m) m.header.stamp = rospy.Time().from_sec(5.0) tr.setTransform(m) try: tr.waitForTransform("PARENT", "THISFRAME", rospy.Time().from_sec(4.0), rospy.Duration(3.0)) except tf.Exception as ex: self.assertFalse("This should not throw") def test_getTFPrefix(self): t = tf.Transformer() self.assertEqual(t.getTFPrefix(), "") def disabled_random(self): import networkx as nx for (r,h) in [ (2,2), (2,5), (3,5) ]: G = nx.balanced_tree(r, h) t = tf.Transformer(True, rospy.Duration(10.0)) for n in G.nodes(): if n != 0: # n has parent p p = min(G.neighbors(n)) setT(t, str(p), str(n), rospy.Time(0), 1) for n in G.nodes(): ((x,_,_), _) = t.lookupTransform("0", str(n), rospy.Time(0)) self.assert_(x == nx.shortest_path_length(G, 0, n)) for i in G.nodes(): for j in G.nodes(): ((x,_,_), _) = t.lookupTransform(str(i), str(j), rospy.Time()) self.assert_(abs(x) == abs(nx.shortest_path_length(G, 0, i) - nx.shortest_path_length(G, 0, j))) if __name__ == '__main__': rostest.unitrun('tf', 'directed', TestPython) geometry-1.13.2/tf/test/test_broadcaster.launch000066400000000000000000000002411366756125100215560ustar00rootroot00000000000000 geometry-1.13.2/tf/test/test_datatype_conversion.py000077500000000000000000000222601366756125100225330ustar00rootroot00000000000000#!/usr/bin/env python import sys import unittest import tf import geometry_msgs.msg import rospy ## A sample python unit test class PoseConversions(unittest.TestCase): def setUp(self): #Setup the pose tests self.tfpose_stamped = tf.PoseStamped() self.tfpose_stamped.pose.setIdentity() self.tfpose_stamped.frame_id = "frame1" self.tfpose_stamped.stamp = roslib.rostime.Time(10,0) self.msgpose_stamped = geometry_msgs.msg.PoseStamped() self.msgpose_stamped.pose.position.x = 0 self.msgpose_stamped.pose.position.y = 0 self.msgpose_stamped.pose.position.z = 0 self.msgpose_stamped.pose.orientation.x = 0 self.msgpose_stamped.pose.orientation.y = 0 self.msgpose_stamped.pose.orientation.z = 0 self.msgpose_stamped.pose.orientation.w = 1 self.msgpose_stamped.header.frame_id = "frame1" self.msgpose_stamped.header.stamp = roslib.rostime.Time(10,0) ## Setup the point tests self.tfpoint_stamped = tf.PointStamped() self.tfpoint_stamped.point.x = 0 self.tfpoint_stamped.point.y = 0 self.tfpoint_stamped.point.z = 0 self.tfpoint_stamped.frame_id = "frame1" self.tfpoint_stamped.stamp = roslib.rostime.Time(10,0) self.msgpoint_stamped = geometry_msgs.msg.PointStamped() self.msgpoint_stamped.point.x = 0 self.msgpoint_stamped.point.y = 0 self.msgpoint_stamped.point.z = 0 self.msgpoint_stamped.header.frame_id = "frame1" self.msgpoint_stamped.header.stamp = roslib.rostime.Time(10,0) ## Setup the vector tests self.tfvector_stamped = tf.VectorStamped() self.tfvector_stamped.vector.x = 0 self.tfvector_stamped.vector.y = 0 self.tfvector_stamped.vector.z = 0 self.tfvector_stamped.frame_id = "frame1" self.tfvector_stamped.stamp = roslib.rostime.Time(10, 0) self.msgvector_stamped = geometry_msgs.msg.Vector3Stamped() self.msgvector_stamped.vector.x = 0 self.msgvector_stamped.vector.y = 0 self.msgvector_stamped.vector.z = 0 self.msgvector_stamped.header.frame_id = "frame1" self.msgvector_stamped.header.stamp = roslib.rostime.Time(10,0) ## Setup the quaternion tests self.tfquaternion_stamped = tf.QuaternionStamped() self.tfquaternion_stamped.quaternion.x = 0 self.tfquaternion_stamped.quaternion.y = 0 self.tfquaternion_stamped.quaternion.z = 0 self.tfquaternion_stamped.quaternion.w = 1 self.tfquaternion_stamped.frame_id = "frame1" self.tfquaternion_stamped.stamp = roslib.rostime.Time(10, 0) self.msgquaternion_stamped = geometry_msgs.msg.QuaternionStamped() self.msgquaternion_stamped.quaternion.x = 0 self.msgquaternion_stamped.quaternion.y = 0 self.msgquaternion_stamped.quaternion.z = 0 self.msgquaternion_stamped.quaternion.w = 1 self.msgquaternion_stamped.header.frame_id = "frame1" self.msgquaternion_stamped.header.stamp = roslib.rostime.Time(10,0) # Test Pose conversions def test_msg_operator_equals_pose(self): self.assertEquals(self.msgpose_stamped, self.msgpose_stamped, "pose msg test correctness") def test_bt_operator_equals_pose(self): self.assertEquals(self.tfpose_stamped, self.tfpose_stamped, "pose bt test correctness") def test_msg_operator_on_converted(self): self.assertEquals(tf.pose_stamped_bt_to_msg(self.tfpose_stamped), tf.pose_stamped_bt_to_msg(self.tfpose_stamped), "pose msg test correctness after conversion") def test_bt_operator_on_converted(self): self.assertEquals(tf.pose_stamped_msg_to_bt(self.msgpose_stamped), tf.pose_stamped_msg_to_bt(self.msgpose_stamped), "pose bt test correctness after conversion") def test_to_msg_pose(self): self.assertEquals(tf.pose_bt_to_msg(self.tfpose_stamped.pose), self.msgpose_stamped.pose, "pose tf to msg incorrect") def test_to_tf_pose(self): self.assertEquals(tf.pose_msg_to_bt(self.msgpose_stamped.pose), self.tfpose_stamped.pose, "pose stamped msg to tf incorrect") def test_stamped_to_msg_pose(self): self.assertEquals(tf.pose_stamped_bt_to_msg(self.tfpose_stamped), self.msgpose_stamped, "pose stamped tf to msg incorrect") def test_stamped_to_tf_pose(self): self.assertEquals(tf.pose_stamped_msg_to_bt(self.msgpose_stamped), self.tfpose_stamped, "pose stamped msg to tf incorrect") # Test Point conversions def test_bt_operator_equal_point(self): self.assertEquals(self.tfpoint_stamped, self.tfpoint_stamped, "point tf test correctness") def test_msg_operator_equal_point(self): self.assertEquals(self.msgpoint_stamped, self.msgpoint_stamped, "point msg test correctness") def test_msg_operator_equal_point_converted(self): self.assertEquals(tf.point_stamped_bt_to_msg(self.tfpoint_stamped), tf.point_stamped_bt_to_msg(self.tfpoint_stamped), "point msg test correctness after conversion") def test_bt_operator_equal_point_converted(self): self.assertEquals(tf.point_stamped_msg_to_bt(self.msgpoint_stamped), tf.point_stamped_msg_to_bt(self.msgpoint_stamped), "point bt test correctness after conversion") def test_to_msg_point(self): self.assertEquals(tf.point_bt_to_msg(self.tfpoint_stamped.point), self.msgpoint_stamped.point, "point tf to msg incorrect") def test_to_tf_point(self): self.assertEquals(tf.point_msg_to_bt(self.msgpoint_stamped.point), self.tfpoint_stamped.point, "point stamped msg to tf incorrect") def test_stamped_to_msg_point(self): self.assertEquals(tf.point_stamped_bt_to_msg(self.tfpoint_stamped), self.msgpoint_stamped, "point stamped tf to msg incorrect") def test_stamped_to_tf_point(self): self.assertEquals(tf.point_stamped_msg_to_bt(self.msgpoint_stamped), self.tfpoint_stamped, "point stamped msg to tf incorrect") # Test Vector conversions def test_msg_operator_equal_vector(self): self.assertEquals(self.tfvector_stamped, self.tfvector_stamped, "vector bt test correctness") def test_msg_operator_equal_vector(self): self.assertEquals(self.msgvector_stamped, self.msgvector_stamped, "vector msg test correctness") def test_msg_operator_equal_vector_converted(self): self.assertEquals(tf.vector_stamped_bt_to_msg(self.tfvector_stamped), tf.vector_stamped_bt_to_msg(self.tfvector_stamped), "vector msg test correctness after conversion") def test_bt_operator_equal_vector_converted(self): self.assertEquals(tf.vector_stamped_msg_to_bt(self.msgvector_stamped), tf.vector_stamped_msg_to_bt(self.msgvector_stamped), "vector bt test correctness after conversion") def test_to_msg_vector(self): self.assertEquals(tf.vector_bt_to_msg(self.tfvector_stamped.vector), self.msgvector_stamped.vector, "vector tf to msg incorrect") def test_to_tf_vector(self): self.assertEquals(tf.vector_msg_to_bt(self.msgvector_stamped.vector), self.tfvector_stamped.vector, "vector stamped msg to tf incorrect") def test_stamped_to_msg_vector(self): self.assertEquals(tf.vector_stamped_bt_to_msg(self.tfvector_stamped), self.msgvector_stamped, "vector stamped tf to msg incorrect") def test_stamped_to_tf_vector(self): self.assertEquals(tf.vector_stamped_msg_to_bt(self.msgvector_stamped), self.tfvector_stamped, "vector stamped msg to tf incorrect") # Test Quaternion conversions def test_bt_operator_equal_quaternion(self): self.assertEquals(self.tfquaternion_stamped, self.tfquaternion_stamped, "quaternion bt test correctness") def test_msg_operator_equal_quaternion(self): self.assertEquals(self.msgquaternion_stamped, self.msgquaternion_stamped, "quaternion msg test correctness") def test_msg_operator_equal_quaternion_converted(self): self.assertEquals(tf.quaternion_stamped_bt_to_msg(self.tfquaternion_stamped), tf.quaternion_stamped_bt_to_msg(self.tfquaternion_stamped), "quaternion msg test correctness after conversion") def test_bt_operator_equal_quaternion_converted(self): self.assertEquals(tf.quaternion_stamped_msg_to_bt(self.msgquaternion_stamped), tf.quaternion_stamped_msg_to_bt(self.msgquaternion_stamped), "quaternion bt test correctness after conversion") def test_to_msg_quaternion(self): self.assertEquals(tf.quaternion_bt_to_msg(self.tfquaternion_stamped.quaternion), self.msgquaternion_stamped.quaternion, "quaternion tf to msg incorrect") def test_to_tf_quaternion(self): self.assertEquals(tf.quaternion_msg_to_bt(self.msgquaternion_stamped.quaternion), self.tfquaternion_stamped.quaternion, "quaternion stamped msg to tf incorrect") def test_stamped_to_msg_quaternion(self): self.assertEquals(tf.quaternion_stamped_bt_to_msg(self.tfquaternion_stamped), self.msgquaternion_stamped, "quaternion stamped tf to msg incorrect") def test_stamped_to_tf_quaternion(self): self.assertEquals(tf.quaternion_stamped_msg_to_bt(self.msgquaternion_stamped), self.tfquaternion_stamped, "quaternion stamped msg to tf incorrect") if __name__ == '__main__': import rostest rostest.unitrun('tf', 'test_tf_data_conversions', PoseConversions) geometry-1.13.2/tf/test/test_message_filter.cpp000066400000000000000000000270061366756125100215760ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Josh Faust */ #include #include #include #include #include #include #include "ros/ros.h" #include using namespace tf; class Notification { public: Notification(int expected_count) : count_(0) , expected_count_(expected_count) , failure_count_(0) { } void notify(const geometry_msgs::PointStamped::ConstPtr& message) { ++count_; } void failure(const geometry_msgs::PointStamped::ConstPtr& message, FilterFailureReason reason) { ++failure_count_; } int count_; int expected_count_; int failure_count_; }; TEST(MessageFilter, noTransforms) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = ros::Time::now(); msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); } TEST(MessageFilter, noTransformsSameFrame) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = ros::Time::now(); msg->header.frame_id = "frame1"; filter.add(msg); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, preexistingTransforms) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); ros::Time stamp = ros::Time::now(); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, postTransforms) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); ros::Time stamp = ros::Time::now(); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, queueSize) { tf::TransformListener tf_client; Notification n(10); MessageFilter filter(tf_client, "frame1", 10); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp = ros::Time::now(); for (int i = 0; i < 20; ++i) { geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); } EXPECT_EQ(0, n.count_); EXPECT_EQ(10, n.failure_count_); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(10, n.count_); } TEST(MessageFilter, setTargetFrame) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.setTargetFrame("frame1000"); ros::Time stamp = ros::Time::now(); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2"); tf_client.setTransform(transform); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, multipleTargetFrames) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); std::vector target_frames; target_frames.push_back("frame1"); target_frames.push_back("frame2"); filter.setTargetFrames(target_frames); ros::Time stamp = ros::Time::now(); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3"); tf_client.setTransform(transform); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame3"; filter.add(msg); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet) //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0)); transform.child_frame_id_ = "frame2"; tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(1, n.count_); // frame2->frame3 now exists } TEST(MessageFilter, tolerance) { ros::Duration offset(0.2); tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.setTolerance(offset); ros::Time stamp = ros::Time::now(); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); //No return due to lack of space for offset //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1)); transform.stamp_ += offset*1.1; tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(1, n.count_); // Now have data for the message published earlier msg->header.stamp = stamp + offset; filter.add(msg); EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset } // TODO: re-enable once ROS 0.7.3 is out and the Timer issues have been fixed #if 0 TEST(MessageFilter, maxRate) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1, ros::NodeHandle(), ros::Duration(1.0)); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); ros::Time stamp = ros::Time::now(); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); stamp += ros::Duration(0.1); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); transform.stamp_ = stamp; tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(0, n.count_); //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0)); tf_client.setTransform(transform); ros::WallDuration(0.1).sleep(); ros::spinOnce(); EXPECT_EQ(1, n.count_); } #endif TEST(MessageFilter, outTheBackFailure) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp = ros::Time::now(); tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2"); tf_client.setTransform(transform); transform.stamp_ = stamp + ros::Duration(10000); tf_client.setTransform(transform); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.failure_count_); } TEST(MessageFilter, emptyFrameIDFailure) { tf::TransformListener tf_client; Notification n(1); MessageFilter filter(tf_client, "frame1", 1); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.frame_id = ""; filter.add(msg); EXPECT_EQ(1, n.failure_count_); } TEST(MessageFilter, removeCallback) { // Callback queue in separate thread ros::CallbackQueue cbqueue; ros::AsyncSpinner spinner(1, &cbqueue); ros::NodeHandle threaded_nh; threaded_nh.setCallbackQueue(&cbqueue); // TF filters; no data needs to be published boost::scoped_ptr tf_listener; boost::scoped_ptr > tf_filter; spinner.start(); for (int i = 0; i < 3; ++i) { tf_listener.reset(new tf::TransformListener()); // Have callback fire at high rate to increase chances of race condition tf_filter.reset( new tf::MessageFilter(*tf_listener, "map", 5, threaded_nh, ros::Duration(0.000001))); // Sleep and reset; sleeping increases chances of race condition ros::Duration(0.001).sleep(); tf_filter.reset(); tf_listener.reset(); } spinner.stop(); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::Time::setNow(ros::Time()); ros::init(argc, argv, "test_message_filter"); ros::NodeHandle nh; int ret = RUN_ALL_TESTS(); return ret; } geometry-1.13.2/tf/test/test_message_filter.xml000066400000000000000000000001411366756125100216030ustar00rootroot00000000000000 geometry-1.13.2/tf/test/test_transform_datatypes.cpp000066400000000000000000000036601366756125100226760ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include "tf/LinearMath/Vector3.h" using namespace tf; TEST(tf, set) { tf::Quaternion q = tf::createQuaternionFromRPY(0, 0, 0); EXPECT_EQ(q, tf::createIdentityQuaternion()); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/test/tf_benchmark.cpp000066400000000000000000000206431366756125100201710ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include "tf/LinearMath/Vector3.h" void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); }; void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) { seed_rand(); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; } } using namespace tf; TEST(tf_benchmark, canTransformCacheLength0) { tf::Transformer mTR(true); uint64_t runs = 100000; ros::WallTime start_time = ros::WallTime::now(); start_time = ros::WallTime::now(); for (uint64_t i = 0 ; i < runs; i++) { EXPECT_FALSE(mTR.canTransform("target","source_frame", ros::Time::now())); } ros::WallDuration run_duration = ros::WallTime::now() - start_time; double frequency = (double)runs / run_duration.toSec() ; printf("can frequency %.2f KHz\n", frequency / 1000.0); EXPECT_GT( frequency, 10000.0); } TEST(tf_benchmark, canTransformCacheLength10000) { tf::Transformer mTR(true); unsigned int cache_length = 10000; for (unsigned int i = 0; i < cache_length; i++) { StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child"); mTR.setTransform(tranStamped); } uint64_t runs = 100000; ros::WallTime start_time = ros::WallTime::now(); //Worst case start_time = ros::WallTime::now(); for (uint64_t i = 0 ; i < runs; i++) { EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10))); } ros::WallDuration run_duration = ros::WallTime::now() - start_time; double frequency = (double)runs / run_duration.toSec() ; printf("Worst Case Frequency %.2f KHz\n", frequency / 1000.0); EXPECT_GT( frequency, 10000.0); //Worst case start_time = ros::WallTime::now(); for (uint64_t i = 0 ; i < runs; i++) { EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2))); } run_duration = ros::WallTime::now() - start_time; frequency = (double)runs / run_duration.toSec() ; printf("Intermediate Case Frequency %.2f KHz\n", frequency / 1000.0); EXPECT_GT( frequency, 10000.0); //Best case start_time = ros::WallTime::now(); for (uint64_t i = 0 ; i < runs; i++) { EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1))); } run_duration = ros::WallTime::now() - start_time; frequency = (double)runs / run_duration.toSec() ; printf("Best Case Frequency %.2f KHz\n", frequency / 1000.0); EXPECT_GT( frequency, 10000.0); } TEST(tf_benchmark, lookupTransformCacheLength10000) { tf::Transformer mTR(true); unsigned int cache_length = 10000; for (unsigned int i = 0; i < cache_length; i++) { StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child"); mTR.setTransform(tranStamped); } uint64_t runs = 100000; ros::WallTime start_time = ros::WallTime::now(); StampedTransform rv; //Worst case start_time = ros::WallTime::now(); for (uint64_t i = 0 ; i < runs; i++) { mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10), rv); } ros::WallDuration run_duration = ros::WallTime::now() - start_time; double frequency = (double)runs / run_duration.toSec() ; printf("Worst Case Lookup Frequency %.2f KHz\n", frequency / 1000.0); EXPECT_GT( frequency, 10000.0); //Worst case start_time = ros::WallTime::now(); for (uint64_t i = 0 ; i < runs; i++) { mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2), rv); } run_duration = ros::WallTime::now() - start_time; frequency = (double)runs / run_duration.toSec() ; printf("Intermediate Case Lookup Frequency %.2f KHz\n", frequency / 1000.0); EXPECT_GT( frequency, 10000.0); //Best case start_time = ros::WallTime::now(); for (uint64_t i = 0 ; i < runs; i++) { mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1), rv); } run_duration = ros::WallTime::now() - start_time; frequency = (double)runs / run_duration.toSec() ; printf("Best Case Lookup Frequency %.2f KHz\n", frequency / 1000.0); EXPECT_GT( frequency, 10000.0); } TEST(tf_benchmark, benchmarkExhaustiveSearch) { uint64_t runs = 40000; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10.0 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } ros::WallTime start_time = ros::WallTime::now(); for ( uint64_t i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0 + i), "child"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("my_parent",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(runs), "child"); Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("child",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon); ros::WallDuration run_duration = ros::WallTime::now() - start_time; double frequency = (double)runs / run_duration.toSec() ; printf("exhaustive search frequency %.2f KHz\n", frequency / 1000.0); EXPECT_GT( frequency, 500.0); } int main(int argc, char **argv){ ros::Time::init(); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/test/tf_unittest.cpp000066400000000000000000002605051366756125100201210ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include "tf/LinearMath/Vector3.h" #include "rostest/permuter.h" using namespace tf; void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); } void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) { seed_rand(); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; } } void push_back_i(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { /* "a" v (1,0) "b" v (1,0) "c" */ children.push_back("b"); parents.push_back("a"); dx.push_back(1.0); dy.push_back(0.0); children.push_back("c"); parents.push_back("b"); dx.push_back(1.0); dy.push_back(0.0); } void push_back_y(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { /* "a" v (1,0) "b" ------(0,1)-----> "d" v (1,0) v (0,1) "c" "e" */ // a>b children.push_back("b"); parents.push_back("a"); dx.push_back(1.0); dy.push_back(0.0); // b>c children.push_back("c"); parents.push_back("b"); dx.push_back(1.0); dy.push_back(0.0); // b>d children.push_back("d"); parents.push_back("b"); dx.push_back(0.0); dy.push_back(1.0); // d>e children.push_back("e"); parents.push_back("d"); dx.push_back(0.0); dy.push_back(1.0); } void push_back_v(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { /* "a" ------(0,1)-----> "f" v (1,0) v (0,1) "b" "g" v (1,0) "c" */ // a>b children.push_back("b"); parents.push_back("a"); dx.push_back(1.0); dy.push_back(0.0); // b>c children.push_back("c"); parents.push_back("b"); dx.push_back(1.0); dy.push_back(0.0); // a>f children.push_back("f"); parents.push_back("a"); dx.push_back(0.0); dy.push_back(1.0); // f>g children.push_back("g"); parents.push_back("f"); dx.push_back(0.0); dy.push_back(1.0); } void push_back_1(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { children.push_back("2"); parents.push_back("1"); dx.push_back(1.0); dy.push_back(0.0); } void setupTree(tf::Transformer& mTR, const std::string& mode, const ros::Time & time, const ros::Duration& interpolation_space = ros::Duration()) { ROS_DEBUG("Clearing Buffer Core for new test setup"); mTR.clear(); ROS_DEBUG("Setting up test tree for formation %s", mode.c_str()); std::vector children; std::vector parents; std::vector dx, dy; if (mode == "i") { push_back_i(children, parents, dx, dy); } else if (mode == "y") { push_back_y(children, parents, dx, dy); } else if (mode == "v") { push_back_v(children, parents, dx, dy); } else if (mode == "ring_45") { /* Form a ring of transforms at every 45 degrees on the unit circle. */ std::vector frames; frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); frames.push_back("d"); frames.push_back("e"); frames.push_back("f"); frames.push_back("g"); frames.push_back("h"); frames.push_back("i"); for (uint8_t iteration = 0; iteration < 2; ++iteration) { double direction = 1; std::string frame_prefix; if (iteration == 0) { frame_prefix = "inverse_"; direction = -1; } else frame_prefix =""; for (uint64_t i = 1; i < frames.size(); i++) { StampedTransform ts; ts.setIdentity(); ts.setOrigin(tf::Vector3(direction * ( sqrt(2)/2 - 1), direction * sqrt(2)/2, 0)); ts.setRotation(tf::Quaternion(0, 0, sin(direction * M_PI/8), cos(direction * M_PI/8))); if (time > ros::Time() + (interpolation_space * .5)) ts.stamp_ = time - (interpolation_space * .5); else ts.stamp_ = ros::Time(); ts.child_frame_id_ = frame_prefix + frames[i]; if (i > 1) ts.frame_id_ = frame_prefix + frames[i-1]; else ts.frame_id_ = frames[i-1]; // connect first frame EXPECT_TRUE(mTR.setTransform(ts, "authority")); if (interpolation_space > ros::Duration()) ts.stamp_ = time + interpolation_space * .5; } } return; // nonstandard setup return before standard executinog } else if (mode == "1") { push_back_1(children, parents, dx, dy); } else if (mode =="1_v") { push_back_1(children, parents, dx, dy); push_back_v(children, parents, dx, dy); } else EXPECT_FALSE("Undefined mode for tree setup. Test harness improperly setup."); /// Standard for (uint64_t i = 0; i < children.size(); i++) { StampedTransform ts; ts.setIdentity(); ts.setOrigin(tf::Vector3(dx[i], dy[i], 0)); if (time > ros::Time() + (interpolation_space * .5)) ts.stamp_ = time - (interpolation_space * .5); else ts.stamp_ = ros::Time(); ts.frame_id_ = parents[i]; ts.child_frame_id_ = children[i]; EXPECT_TRUE(mTR.setTransform(ts, "authority")); if (interpolation_space > ros::Duration()) { ts.stamp_ = time + interpolation_space * .5; EXPECT_TRUE(mTR.setTransform(ts, "authority")); } } } #define CHECK_QUATERNION_NEAR(_q1, _q2, _epsilon) \ EXPECT_NEAR(_q1.angle(_q2), 0, _epsilon); \ #define CHECK_TRANSFORMS_NEAR(_out, _expected, _eps) \ EXPECT_NEAR(_out.getOrigin().x(), _expected.getOrigin().x(), epsilon); \ EXPECT_NEAR(_out.getOrigin().y(), _expected.getOrigin().y(), epsilon); \ EXPECT_NEAR(_out.getOrigin().z(), _expected.getOrigin().z(), epsilon); \ CHECK_QUATERNION_NEAR(_out.getRotation(), _expected.getRotation(), _eps); // Simple test with compound transform TEST(tf, lookupTransform_compount) { /* * Frames * * root->a * * root->b->c->d * */ double epsilon = 2e-5; // Larger epsilon for interpolation values tf::Transformer mTR; StampedTransform tsa; tsa.frame_id_ = "root"; tsa.child_frame_id_ = "a"; tsa.setOrigin(tf::Vector3(1,1,1)); tf::Quaternion q1; q1.setRPY(0.25, .5, .75); tsa.setRotation(q1); EXPECT_TRUE(mTR.setTransform(tsa, "authority")); StampedTransform tsb; tsb.frame_id_ = "root"; tsb.child_frame_id_ = "b"; tsb.setOrigin(tf::Vector3(-1, 0, -1)); tf::Quaternion q2; q2.setRPY(1.0, 0.25, 0.5); tsb.setRotation(q2); EXPECT_TRUE(mTR.setTransform(tsb, "authority")); StampedTransform tsc; tsc.frame_id_ = "b"; tsc.child_frame_id_ = "c"; tsc.setOrigin(tf::Vector3(0.0, 2.0, 0.5)); tf::Quaternion q3; q3.setRPY(0.25, 0.75, 1.25); tsc.setRotation(q3); EXPECT_TRUE(mTR.setTransform(tsc, "authority")); StampedTransform tsd; tsd.frame_id_ = "c"; tsd.child_frame_id_ = "d"; tsd.setOrigin(tf::Vector3(0.5, -1, 1.5)); tf::Quaternion q4; q4.setRPY(-0.5, 1.0, -0.75); tsd.setRotation(q4); EXPECT_TRUE(mTR.setTransform(tsd, "authority")); tf::Transform expected_ab, expected_bc, expected_cb, expected_ac, expected_ba, expected_ca, expected_ad, expected_da, expected_bd, expected_db, expected_rootd, expected_rootc; expected_ab = tsa.inverse() * tsb; expected_ac = tsa.inverse() * tsb * tsc; expected_ad = tsa.inverse() * tsb * tsc * tsd; expected_cb = tsc.inverse(); expected_bc = tsc; expected_bd = tsc * tsd; expected_db = expected_bd.inverse(); expected_ba = tsb.inverse() * tsa; expected_ca = tsc.inverse() * tsb.inverse() * tsa; expected_da = tsd.inverse() * tsc.inverse() * tsb.inverse() * tsa; expected_rootd = tsb * tsc * tsd; expected_rootc = tsb * tsc; // root -> b -> c StampedTransform out_rootc; mTR.lookupTransform("root", "c", ros::Time(), out_rootc); CHECK_TRANSFORMS_NEAR(out_rootc, expected_rootc, epsilon); // root -> b -> c -> d StampedTransform out_rootd; mTR.lookupTransform("root", "d", ros::Time(), out_rootd); CHECK_TRANSFORMS_NEAR(out_rootd, expected_rootd, epsilon); // a <- root -> b StampedTransform out_ab; mTR.lookupTransform("a", "b", ros::Time(), out_ab); CHECK_TRANSFORMS_NEAR(out_ab, expected_ab, epsilon); StampedTransform out_ba; mTR.lookupTransform("b", "a", ros::Time(), out_ba); CHECK_TRANSFORMS_NEAR(out_ba, expected_ba, epsilon); // a <- root -> b -> c StampedTransform out_ac; mTR.lookupTransform("a", "c", ros::Time(), out_ac); CHECK_TRANSFORMS_NEAR(out_ac, expected_ac, epsilon); StampedTransform out_ca; mTR.lookupTransform("c", "a", ros::Time(), out_ca); CHECK_TRANSFORMS_NEAR(out_ca, expected_ca, epsilon); // a <- root -> b -> c -> d StampedTransform out_ad; mTR.lookupTransform("a", "d", ros::Time(), out_ad); CHECK_TRANSFORMS_NEAR(out_ad, expected_ad, epsilon); StampedTransform out_da; mTR.lookupTransform("d", "a", ros::Time(), out_da); CHECK_TRANSFORMS_NEAR(out_da, expected_da, epsilon); // b -> c StampedTransform out_cb; mTR.lookupTransform("c", "b", ros::Time(), out_cb); CHECK_TRANSFORMS_NEAR(out_cb, expected_cb, epsilon); StampedTransform out_bc; mTR.lookupTransform("b", "c", ros::Time(), out_bc); CHECK_TRANSFORMS_NEAR(out_bc, expected_bc, epsilon); // b -> c -> d StampedTransform out_bd; mTR.lookupTransform("b", "d", ros::Time(), out_bd); CHECK_TRANSFORMS_NEAR(out_bd, expected_bd, epsilon); StampedTransform out_db; mTR.lookupTransform("d", "b", ros::Time(), out_db); CHECK_TRANSFORMS_NEAR(out_db, expected_db, epsilon); } // Time varying transforms, testing interpolation TEST(tf, lookupTransform_helix_configuration) { double epsilon = 2e-5; // Larger epsilon for interpolation values tf::Transformer mTR; ros::Time t0 = ros::Time() + ros::Duration(10); ros::Duration step = ros::Duration(0.05); ros::Duration half_step = ros::Duration(0.025); ros::Time t1 = t0 + ros::Duration(5.0); /* * a->b->c * * b.z = vel * (t - t0) * c.x = cos(theta * (t - t0)) * c.y = sin(theta * (t - t0)) * * a->d * * d.z = 2 * cos(theta * (t - t0)) * a->d transforms are at half-step between a->b->c transforms */ double theta = 0.25; double vel = 1.0; for (ros::Time t = t0; t <= t1; t += step) { ros::Time t2 = t + half_step; double dt = (t - t0).toSec(); double dt2 = (t2 - t0).toSec(); StampedTransform ts; ts.setIdentity(); ts.frame_id_ = "a"; ts.stamp_ = t; ts.child_frame_id_ = "b"; ts.setOrigin(tf::Vector3(0.0, 0.0, vel * dt)); EXPECT_TRUE(mTR.setTransform(ts, "authority")); StampedTransform ts2; ts2.setIdentity(); ts2.frame_id_ = "b"; ts2.stamp_ = t; ts2.child_frame_id_ = "c"; ts2.setOrigin(tf::Vector3(cos(theta*dt), sin(theta*dt),0)); tf::Quaternion q; q.setRPY(0,0,theta*dt); ts2.setRotation(q); EXPECT_TRUE(mTR.setTransform(ts2, "authority")); StampedTransform ts3; ts3.setIdentity(); ts3.frame_id_ = "a"; ts3.stamp_ = t2; ts3.child_frame_id_ = "d"; ts3.setOrigin(tf::Vector3(0, 0, cos(theta*dt2))); EXPECT_TRUE(mTR.setTransform(ts3, "authority")); } for (ros::Time t = t0 + half_step; t < t1; t += step) { ros::Time t2 = t + half_step; double dt = (t - t0).toSec(); double dt2 = (t2 - t0).toSec(); StampedTransform out_ab; mTR.lookupTransform("a", "b", t, out_ab); tf::Transform expected_ab; expected_ab.setIdentity(); expected_ab.setOrigin(tf::Vector3(0.0, 0.0, vel*dt)); CHECK_TRANSFORMS_NEAR(out_ab, expected_ab, epsilon); StampedTransform out_ac; mTR.lookupTransform("a", "c", t, out_ac); tf::Transform expected_ac; expected_ac.setOrigin(tf::Vector3(cos(theta*dt), sin(theta*dt), vel*dt)); tf::Quaternion q; q.setRPY(0,0,theta*dt); expected_ac.setRotation(q); CHECK_TRANSFORMS_NEAR(out_ac, expected_ac, epsilon); StampedTransform out_ad; mTR.lookupTransform("a", "d", t, out_ad); EXPECT_NEAR(out_ad.getOrigin().z(), cos(theta*dt), epsilon); StampedTransform out_cd; mTR.lookupTransform("c", "d", t2, out_cd); tf::Transform expected_cd; expected_cd.setOrigin(tf::Vector3(-1, 0, cos(theta * dt2) - vel * dt2)); tf::Quaternion q2; q2.setRPY(0,0,-theta*dt2); expected_cd.setRotation(q2); CHECK_TRANSFORMS_NEAR(out_cd, expected_cd, epsilon); } // Advanced API for (ros::Time t = t0 + half_step; t < t1; t += (step + step)) { ros::Time t2 = t + step; double dt = (t - t0).toSec(); double dt2 = (t2 - t0).toSec(); StampedTransform out_cd2; mTR.lookupTransform("c", t, "d", t2, "a", out_cd2); tf::Transform expected_cd2; expected_cd2.setOrigin(tf::Vector3(-1, 0, cos(theta*dt2) - vel*dt)); tf::Quaternion mq2; mq2.setRPY(0,0,-theta*dt); expected_cd2.setRotation(mq2); CHECK_TRANSFORMS_NEAR(out_cd2, expected_cd2, epsilon); } } TEST(tf, lookupTransform_ring45) { double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; times.push_back(ros::Time(1.0)); times.push_back(ros::Time(10.0)); times.push_back(ros::Time(0.01)); ros::Time eval_time; permuter.addOptionSet(times, &eval_time); std::vector durations; durations.push_back(ros::Duration(1.0)); durations.push_back(ros::Duration(0.001)); durations.push_back(ros::Duration(0.1)); ros::Duration interpolation_space; // permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); frames.push_back("d"); frames.push_back("e"); frames.push_back("f"); frames.push_back("g"); frames.push_back("h"); frames.push_back("i"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); std::string target_frame; permuter.addOptionSet(frames, &target_frame); while (permuter.step()) { tf::Transformer mTR; setupTree(mTR, "ring_45", eval_time, interpolation_space); StampedTransform out_xfm; mTR.lookupTransform(source_frame, target_frame, eval_time, out_xfm); //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); if (source_frame != target_frame) EXPECT_EQ(out_xfm.stamp_, eval_time); EXPECT_TRUE(out_xfm.frame_id_ == source_frame || out_xfm.frame_id_ == "/" + source_frame) << "Expected frame_id_ to equal source_frame: " << out_xfm.frame_id_ << ", " << source_frame << std::endl; EXPECT_TRUE(out_xfm.child_frame_id_ == target_frame || out_xfm.child_frame_id_ == "/" + target_frame) << "Expected child_frame_id_ to equal target_frame: " << out_xfm.child_frame_id_ << ", " << target_frame << std::endl; //Zero distance or all the way if (source_frame == target_frame || (source_frame == "a" && target_frame == "i") || (source_frame == "i" && target_frame == "a") || (source_frame == "a" && target_frame == "inverse_i") || (source_frame == "inverse_i" && target_frame == "a") ) { tf::Transform expected; expected.setIdentity(); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Chaining 1 else if ((source_frame == "a" && target_frame =="b") || (source_frame == "b" && target_frame =="c") || (source_frame == "c" && target_frame =="d") || (source_frame == "d" && target_frame =="e") || (source_frame == "e" && target_frame =="f") || (source_frame == "f" && target_frame =="g") || (source_frame == "g" && target_frame =="h") || (source_frame == "h" && target_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(M_PI/8),cos(M_PI/8)), tf::Vector3(sqrt(2)/2 - 1, sqrt(2)/2, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Inverse Chaining 1 else if ((source_frame == "b" && target_frame =="a") || (source_frame == "c" && target_frame =="b") || (source_frame == "d" && target_frame =="c") || (source_frame == "e" && target_frame =="d") || (source_frame == "f" && target_frame =="e") || (source_frame == "g" && target_frame =="f") || (source_frame == "h" && target_frame =="g") || (source_frame == "i" && target_frame =="h") ) { tf::Transform expected(tf::Quaternion(0,0,sin(-M_PI/8),cos(-M_PI/8)), tf::Vector3(sqrt(2)/2 - 1, -sqrt(2)/2, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Chaining 2 else if ((source_frame == "a" && target_frame =="c") || (source_frame == "b" && target_frame =="d") || (source_frame == "c" && target_frame =="e") || (source_frame == "d" && target_frame =="f") || (source_frame == "e" && target_frame =="g") || (source_frame == "f" && target_frame =="h") || (source_frame == "g" && target_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(M_PI/4),cos(M_PI/4)), tf::Vector3(-1, 1, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Inverse Chaining 2 else if ((source_frame == "c" && target_frame =="a") || (source_frame == "d" && target_frame =="b") || (source_frame == "e" && target_frame =="c") || (source_frame == "f" && target_frame =="d") || (source_frame == "g" && target_frame =="e") || (source_frame == "h" && target_frame =="f") || (source_frame == "i" && target_frame =="g") ) { tf::Transform expected(tf::Quaternion(0,0,sin(-M_PI/4),cos(-M_PI/4)), tf::Vector3(-1, -1, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Chaining 3 else if ((source_frame == "a" && target_frame =="d") || (source_frame == "b" && target_frame =="e") || (source_frame == "c" && target_frame =="f") || (source_frame == "d" && target_frame =="g") || (source_frame == "e" && target_frame =="h") || (source_frame == "f" && target_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(M_PI*3/8),cos(M_PI*3/8)), tf::Vector3(-1 - sqrt(2)/2, sqrt(2)/2, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Inverse Chaining 3 else if ((target_frame == "a" && source_frame =="d") || (target_frame == "b" && source_frame =="e") || (target_frame == "c" && source_frame =="f") || (target_frame == "d" && source_frame =="g") || (target_frame == "e" && source_frame =="h") || (target_frame == "f" && source_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(-M_PI*3/8),cos(-M_PI*3/8)), tf::Vector3(-1 - sqrt(2)/2, -sqrt(2)/2, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Chaining 4 else if ((source_frame == "a" && target_frame =="e") || (source_frame == "b" && target_frame =="f") || (source_frame == "c" && target_frame =="g") || (source_frame == "d" && target_frame =="h") || (source_frame == "e" && target_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(M_PI/2),cos(M_PI/2)), tf::Vector3(-2, 0, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Inverse Chaining 4 else if ((target_frame == "a" && source_frame =="e") || (target_frame == "b" && source_frame =="f") || (target_frame == "c" && source_frame =="g") || (target_frame == "d" && source_frame =="h") || (target_frame == "e" && source_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(-M_PI/2),cos(-M_PI/2)), tf::Vector3(-2, 0, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Chaining 5 else if ((source_frame == "a" && target_frame =="f") || (source_frame == "b" && target_frame =="g") || (source_frame == "c" && target_frame =="h") || (source_frame == "d" && target_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(M_PI*5/8),cos(M_PI*5/8)), tf::Vector3(-1-sqrt(2)/2, -sqrt(2)/2, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Inverse Chaining 5 else if ((target_frame == "a" && source_frame =="f") || (target_frame == "b" && source_frame =="g") || (target_frame == "c" && source_frame =="h") || (target_frame == "d" && source_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(-M_PI*5/8),cos(-M_PI*5/8)), tf::Vector3(-1-sqrt(2)/2, sqrt(2)/2, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Chaining 6 else if ((source_frame == "a" && target_frame =="g") || (source_frame == "b" && target_frame =="h") || (source_frame == "c" && target_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(M_PI*3/4),cos(M_PI*3/4)), tf::Vector3(-1, -1, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Inverse Chaining 6 else if ((target_frame == "a" && source_frame =="g") || (target_frame == "b" && source_frame =="h") || (target_frame == "c" && source_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(-M_PI*3/4),cos(-M_PI*3/4)), tf::Vector3(-1, 1, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Chaining 7 else if ((source_frame == "a" && target_frame =="h") || (source_frame == "b" && target_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(M_PI*7/8),cos(M_PI*7/8)), tf::Vector3(sqrt(2)/2-1, -sqrt(2)/2, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } // Inverse Chaining 7 else if ((target_frame == "a" && source_frame =="h") || (target_frame == "b" && source_frame =="i") ) { tf::Transform expected(tf::Quaternion(0,0,sin(-M_PI*7/8),cos(-M_PI*7/8)), tf::Vector3(sqrt(2)/2-1, sqrt(2)/2, 0)); CHECK_TRANSFORMS_NEAR(out_xfm, expected, epsilon); } else { EXPECT_FALSE("Ring_45 testing Shouldn't get here"); printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); } } } TEST(tf, setTransformNoInsertOnSelfTransform) { tf::Transformer mTR(true); StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0), "same_frame", "same_frame"); EXPECT_FALSE(mTR.setTransform(tranStamped)); } TEST(tf, setTransformNoInsertWithNan) { tf::Transformer mTR(true); StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0), "same_frame", "other_frame"); EXPECT_TRUE(mTR.setTransform(tranStamped)); tranStamped.setOrigin(tf::Point(1.0,1.0,0.0/0.0)); EXPECT_TRUE(std::isnan(tranStamped.getOrigin().z())); EXPECT_FALSE(mTR.setTransform(tranStamped)); } TEST(tf, setTransformNoInsertWithNoFrameID) { tf::Transformer mTR(true); StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0), "parent_frame", ""); EXPECT_FALSE(mTR.setTransform(tranStamped)); } TEST(tf, setTransformNoInsertWithNoParentID) { tf::Transformer mTR(true); StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0), "", "my_frame"); EXPECT_FALSE(mTR.setTransform(tranStamped)); } TEST(tf, TransformTransformsCartesian) { uint64_t runs = 400; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10.0 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } //std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl for ( uint64_t i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0 + i), "child"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("my_parent",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(runs), "child"); Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("child",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon); } /** Make sure that the edge cases of transform the top of the tree to the top of the tree and * the leaf of the tree can transform to the leaf of the tree without a lookup exception and accurately */ TEST(tf, TransformTransformToOwnFrame) { uint64_t runs = 400; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs), yawvalues(runs), pitchvalues(runs), rollvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yawvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; pitchvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; rollvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; tf::Quaternion qt; qt.setRPY(yawvalues[i],pitchvalues[i],rollvalues[i]); StampedTransform tranStamped(tf::Transform(qt, tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "parent", "child"); mTR.setTransform(tranStamped); } //std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( uint64_t i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10 + i), "child"); Stamped inpose2 (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10 + i), "parent"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("child",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon); EXPECT_NEAR(outpose.getRotation().w(), 1, epsilon); //Identity is 0,0,0,1 outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("parent",inpose2, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon); EXPECT_NEAR(outpose.getRotation().w(), 1, epsilon); //Identity is 0,0,0,1 } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(runs), "child"); Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("child",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon); } TEST(tf, TransformPointCartesian) { uint64_t runs = 400; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( uint64_t i = 0; i < runs ; i++ ) { double x =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; double y =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; double z =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; Stamped invec (tf::Vector3(x,y,z), ros::Time().fromNSec(10 + i), "child"); try{ Stamped outvec(tf::Vector3(0,0,0), ros::Time().fromNSec(10 + i), "child"); // outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPoint("my_parent",invec, outvec); EXPECT_NEAR(outvec.x(), xvalues[i]+x, epsilon); EXPECT_NEAR(outvec.y(), yvalues[i]+y, epsilon); EXPECT_NEAR(outvec.z(), zvalues[i]+z, epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, TransformVectorCartesian) { uint64_t runs = 400; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( uint64_t i = 0; i < runs ; i++ ) { double x =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; double y =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; double z =10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; Stamped invec (tf::Vector3(x,y,z), ros::Time().fromNSec(10 + i), "child"); try{ Stamped outvec(tf::Vector3(0,0,0), ros::Time().fromNSec(10 + i), "child"); // outpose.setIdentity(); //to make sure things are getting mutated mTR.transformVector("my_parent",invec, outvec); EXPECT_NEAR(outvec.x(), x, epsilon); EXPECT_NEAR(outvec.y(), y, epsilon); EXPECT_NEAR(outvec.z(), z, epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, TransformQuaternionCartesian) { uint64_t runs = 400; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( uint64_t i = 0; i < runs ; i++ ) { tf::Quaternion qt; qt.setRPY(xvalues[i],yvalues[i],zvalues[i]); Stamped invec (qt, ros::Time().fromNSec(10 + i), "child"); // printf("%f, %f, %f\n", xvalues[i],yvalues[i], zvalues[i]); try{ Stamped outvec(qt, ros::Time().fromNSec(10 + i), "child"); mTR.transformQuaternion("my_parent",invec, outvec); EXPECT_NEAR(outvec.angle(invec) , 0, epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(data, Vector3Conversions) { uint64_t runs = 400; double epsilon = 1e-6; std::vector xvalues(runs), yvalues(runs), zvalues(runs); generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues); for ( uint64_t i = 0; i < runs ; i++ ) { tf::Vector3 btv = tf::Vector3(xvalues[i], yvalues[i], zvalues[i]); tf::Vector3 btv_out = tf::Vector3(0,0,0); geometry_msgs::Vector3 msgv; vector3TFToMsg(btv, msgv); vector3MsgToTF(msgv, btv_out); EXPECT_NEAR(btv.x(), btv_out.x(), epsilon); EXPECT_NEAR(btv.y(), btv_out.y(), epsilon); EXPECT_NEAR(btv.z(), btv_out.z(), epsilon); } } TEST(data, Vector3StampedConversions) { uint64_t runs = 400; double epsilon = 1e-6; std::vector xvalues(runs), yvalues(runs), zvalues(runs); generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues); for ( uint64_t i = 0; i < runs ; i++ ) { Stamped btv = Stamped(tf::Vector3(xvalues[i], yvalues[i], zvalues[i]), ros::Time().fromNSec(1), "no frame"); Stamped btv_out; geometry_msgs::Vector3Stamped msgv; vector3StampedTFToMsg(btv, msgv); vector3StampedMsgToTF(msgv, btv_out); EXPECT_NEAR(btv.x(), btv_out.x(), epsilon); EXPECT_NEAR(btv.y(), btv_out.y(), epsilon); EXPECT_NEAR(btv.z(), btv_out.z(), epsilon); EXPECT_STREQ(btv.frame_id_.c_str(), btv_out.frame_id_.c_str()); EXPECT_EQ(btv.stamp_, btv_out.stamp_); } } TEST(data, QuaternionConversions) { uint64_t runs = 400; double epsilon = 1e-6; std::vector xvalues(runs), yvalues(runs), zvalues(runs); generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues); for ( uint64_t i = 0; i < runs ; i++ ) { tf::Quaternion btv; btv.setRPY(xvalues[i], yvalues[i], zvalues[i]); tf::Quaternion btv_out = tf::Quaternion(0,0,0,1); geometry_msgs::Quaternion msgv; quaternionTFToMsg(btv, msgv); quaternionMsgToTF(msgv, btv_out); EXPECT_NEAR(btv.x(), btv_out.x(), epsilon); EXPECT_NEAR(btv.y(), btv_out.y(), epsilon); EXPECT_NEAR(btv.z(), btv_out.z(), epsilon); EXPECT_NEAR(btv.w(), btv_out.w(), epsilon); } } TEST(data, QuaternionStampedConversions) { uint64_t runs = 400; double epsilon = 1e-6; std::vector xvalues(runs), yvalues(runs), zvalues(runs); generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues); for ( uint64_t i = 0; i < runs ; i++ ) { Stamped btv = Stamped(tf::Quaternion(), ros::Time().fromNSec(1), "no frame"); btv.setRPY(xvalues[i], yvalues[i], zvalues[i]); Stamped btv_out; geometry_msgs::QuaternionStamped msgv; quaternionStampedTFToMsg(btv, msgv); quaternionStampedMsgToTF(msgv, btv_out); EXPECT_NEAR(btv.x(), btv_out.x(), epsilon); EXPECT_NEAR(btv.y(), btv_out.y(), epsilon); EXPECT_NEAR(btv.z(), btv_out.z(), epsilon); EXPECT_NEAR(btv.w(), btv_out.w(), epsilon); EXPECT_STREQ(btv.frame_id_.c_str(), btv_out.frame_id_.c_str()); EXPECT_EQ(btv.stamp_, btv_out.stamp_); } } TEST(data, TransformConversions) { uint64_t runs = 400; double epsilon = 1e-6; std::vector xvalues(runs), yvalues(runs), zvalues(runs); generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues); std::vector xvalues2(runs), yvalues2(runs), zvalues2(runs); generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues); for ( uint64_t i = 0; i < runs ; i++ ) { tf::Quaternion qt; qt.setRPY(xvalues2[i], yvalues2[i], zvalues2[i]); tf::Transform btv = tf::Transform(qt, tf::Vector3(xvalues[i], yvalues[i], zvalues[i])); tf::Transform btv_out; geometry_msgs::Transform msgv; transformTFToMsg(btv, msgv); transformMsgToTF(msgv, btv_out); EXPECT_NEAR(btv.getOrigin().x(), btv_out.getOrigin().x(), epsilon); EXPECT_NEAR(btv.getOrigin().y(), btv_out.getOrigin().y(), epsilon); EXPECT_NEAR(btv.getOrigin().z(), btv_out.getOrigin().z(), epsilon); EXPECT_NEAR(btv.getRotation().x(), btv_out.getRotation().x(), epsilon); EXPECT_NEAR(btv.getRotation().y(), btv_out.getRotation().y(), epsilon); EXPECT_NEAR(btv.getRotation().z(), btv_out.getRotation().z(), epsilon); EXPECT_NEAR(btv.getRotation().w(), btv_out.getRotation().w(), epsilon); } } TEST(data, PoseStampedConversions) { uint64_t runs = 400; double epsilon = 1e-6; std::vector xvalues(runs), yvalues(runs), zvalues(runs); generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues); std::vector xvalues2(runs), yvalues2(runs), zvalues2(runs); generate_rand_vectors(1.0, runs, xvalues, yvalues,zvalues); for ( uint64_t i = 0; i < runs ; i++ ) { tf::Quaternion qt; qt.setRPY(xvalues2[i], yvalues2[i], zvalues2[i]); Stamped btv = Stamped(tf::Transform(qt, tf::Vector3(xvalues[i], yvalues[i], zvalues[i])), ros::Time().fromNSec(1), "no frame"); Stamped btv_out; geometry_msgs::PoseStamped msgv; poseStampedTFToMsg(btv, msgv); poseStampedMsgToTF(msgv, btv_out); EXPECT_NEAR(btv.getOrigin().x(), btv_out.getOrigin().x(), epsilon); EXPECT_NEAR(btv.getOrigin().y(), btv_out.getOrigin().y(), epsilon); EXPECT_NEAR(btv.getOrigin().z(), btv_out.getOrigin().z(), epsilon); EXPECT_NEAR(btv.getRotation().x(), btv_out.getRotation().x(), epsilon); EXPECT_NEAR(btv.getRotation().y(), btv_out.getRotation().y(), epsilon); EXPECT_NEAR(btv.getRotation().z(), btv_out.getRotation().z(), epsilon); EXPECT_NEAR(btv.getRotation().w(), btv_out.getRotation().w(), epsilon); EXPECT_STREQ(btv.frame_id_.c_str(), btv_out.frame_id_.c_str()); EXPECT_EQ(btv.stamp_, btv_out.stamp_); } } TEST(tf, ListOneInverse) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( uint64_t i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10 + i), "child"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("my_parent",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, ListTwoInverse) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); StampedTransform tranStamped2(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "child", "grandchild"); mTR.setTransform(tranStamped2); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( unsigned int i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10 + i), "grandchild"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("my_parent",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 2*xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 2*yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 2*zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, ListOneForward) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( uint64_t i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("child",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), -xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), -yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), -zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, ListTwoForward) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); StampedTransform tranStamped2(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "child", "grandchild"); mTR.setTransform(tranStamped2); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( unsigned int i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("grandchild",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), -2*xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), -2*yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), -2*zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, TransformThrougRoot) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100), "my_parent", "childA"); mTR.setTransform(tranStamped); StampedTransform tranStamped2(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100), "my_parent", "childB"); mTR.setTransform(tranStamped2); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( unsigned int i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000 + i*100), "childA"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("childB",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, TransformThroughNO_PARENT) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parentA", "childA"); mTR.setTransform(tranStamped); StampedTransform tranStamped2(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parentB", "childB"); mTR.setTransform(tranStamped2); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( unsigned int i = 0; i < runs ; i++ ) { Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10 + i), "childA"); bool exception_thrown = false; try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("childB",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon); } catch (tf::TransformException & ex) { exception_thrown = true; } EXPECT_TRUE(exception_thrown); } } TEST(tf, getParent) { std::vector children; std::vector parents; children.push_back("a"); parents.push_back("c"); children.push_back("b"); parents.push_back("c"); children.push_back("c"); parents.push_back("e"); children.push_back("d"); parents.push_back("e"); children.push_back("e"); parents.push_back("f"); children.push_back("f"); parents.push_back("j"); // Issue #74. children.push_back("/k"); parents.push_back("l"); tf::Transformer mTR(true); for (uint64_t i = 0; i < children.size(); i++) { StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10), parents[i], children[i]); mTR.setTransform(tranStamped); } //std::cout << mTR.allFramesAsString() << std::endl; std::string output; for (uint64_t i = 0; i < children.size(); i++) { EXPECT_TRUE(mTR.getParent(children[i], ros::Time().fromNSec(10), output)); EXPECT_STREQ(parents[i].c_str(), output.c_str()); } EXPECT_FALSE(mTR.getParent("j", ros::Time().fromNSec(10), output)); EXPECT_FALSE(mTR.getParent("no_value", ros::Time().fromNSec(10), output)); } TEST(tf, NO_PARENT_SET) { double epsilon = 1e-6; std::vector children; std::vector parents; children.push_back("b"); parents.push_back("a"); children.push_back("a"); parents.push_back("NO_PARENT"); tf::Transformer mTR(true); for (uint64_t i = 0; i < children.size(); i++) { StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10), parents[i], children[i]); mTR.setTransform(tranStamped); } //std::cout << mTR.allFramesAsString() << std::endl; Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10), "a"); Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("a",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon); } TEST(tf, waitForTransform) { EXPECT_TRUE(ros::ok()); tf::Transformer mTR(true); // Check assertion of extra string std::string error_str; EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), ros::Duration().fromSec(1.0), ros::Duration().fromSec(0.01), &error_str)); EXPECT_STREQ(tf2_ros::threading_error.c_str(), error_str.c_str()); // check that it doesn't segfault if NULL EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), ros::Duration().fromSec(1.0), ros::Duration().fromSec(0.01))); //A seperate thread is required to use the blocking call for normal usage // This isn't actually using it, but it will not affect this direct usage case. mTR.setUsingDedicatedThread(true); // make sure timeout is resonably lengthed ros::Duration timeout = ros::Duration().fromSec(1.0); ros::Duration poll_freq = ros::Duration().fromSec(0.1); double eps = 0.2; // Default polling freq ros::Time start_time = ros::Time::now(); EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), timeout)); ros::Time stop_time = ros::Time::now(); EXPECT_TRUE(fabs(((stop_time-start_time)-timeout).toSec()) < eps); // 10Hz polling start_time = ros::Time::now(); EXPECT_FALSE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000), timeout, poll_freq)); stop_time = ros::Time::now(); EXPECT_TRUE(fabs(((stop_time-start_time)-timeout).toSec()) < eps); //Now it should be able to transform mTR.setTransform( StampedTransform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000000), "parent", "me")); start_time = ros::Time::now(); EXPECT_TRUE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000),timeout)); stop_time = ros::Time::now(); EXPECT_TRUE(fabs(((stop_time-start_time)).toSec()) < eps); start_time = ros::Time::now(); EXPECT_TRUE(mTR.waitForTransform("parent", "me", ros::Time().fromNSec(10000000),timeout, poll_freq)); stop_time = ros::Time::now(); EXPECT_TRUE(fabs(((stop_time-start_time)).toSec()) < eps); } TEST(tf, Exceptions) { tf::Transformer mTR(true); Stamped outpose; //connectivity when no data EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(10000000))); try { mTR.transformPose("parent",Stamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000000) , "me"), outpose); EXPECT_FALSE("ConnectivityException Not Thrown"); } catch ( tf::LookupException &ex) { EXPECT_TRUE("Lookupgh Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } mTR.setTransform( StampedTransform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(100000), "parent", "me")); //Extrapolation not valid with one value EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000))); try { mTR.transformPose("parent",Stamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(300000), "parent", "me")); //NO Extration when Interpolating //inverse list EXPECT_TRUE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000))); try { mTR.transformPose("parent",Stamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_FALSE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } //forward list EXPECT_TRUE(mTR.canTransform("me", "parent", ros::Time().fromNSec(200000))); try { mTR.transformPose("me",Stamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(200000) , "parent"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_FALSE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } //Extrapolating backwards //inverse list EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(1000))); try { mTR.transformPose("parent",Stamped (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000) , "me"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } //forwards list EXPECT_FALSE(mTR.canTransform("me", "parent", ros::Time().fromNSec(1000))); try { mTR.transformPose("me",Stamped (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000) , "parent"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } // Test extrapolation inverse and forward linkages FORWARD //inverse list EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000))); try { mTR.transformPose("parent", Stamped (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(350000) , "me"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } //forward list EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000))); try { mTR.transformPose("me", Stamped (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(350000) , "parent"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } } TEST(tf, NoExtrapolationExceptionFromParent) { tf::Transformer mTR(true, ros::Duration().fromNSec(1000000)); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a")); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "a")); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "b")); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "b")); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent")); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent's parent", "parent's parent")); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent", "parent")); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent's parent", "parent's parent")); Stamped output; try { mTR.transformPoint( "b", Stamped(Point(1,1,1), ros::Time().fromNSec(2000), "a"), output); } catch (ExtrapolationException &ex) { EXPECT_FALSE("Shouldn't have gotten this exception"); } } TEST(tf, ExtrapolationFromOneValue) { tf::Transformer mTR(true, ros::Duration().fromNSec(1000000)); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a")); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent")); Stamped output; bool excepted = false; //Past time try { mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_TRUE(excepted); excepted = false; //Future one element try { mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(100000), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_TRUE(excepted); //Past multi link excepted = false; try { mTR.transformPoint( "parent's parent", Stamped(Point(1,1,1), ros::Time().fromNSec(1), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_TRUE(excepted); //Future case multi link excepted = false; try { mTR.transformPoint( "parent's parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_TRUE(excepted); mTR.setTransform( StampedTransform (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(20000), "parent", "a")); excepted = false; try { mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_FALSE(excepted); } TEST(tf, RepeatedTimes) { Transformer mTR; tf::Quaternion qt1, qt2; qt1.setRPY(1,0,0); qt2.setRPY(1,1,0); mTR.setTransform( StampedTransform (tf::Transform(qt1, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b")); mTR.setTransform( StampedTransform (tf::Transform(qt2, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b")); tf::StampedTransform output; try{ mTR.lookupTransform("parent", "b" , ros::Time().fromNSec(4000), output); EXPECT_TRUE(!std::isnan(output.getOrigin().x())); EXPECT_TRUE(!std::isnan(output.getOrigin().y())); EXPECT_TRUE(!std::isnan(output.getOrigin().z())); EXPECT_TRUE(!std::isnan(output.getRotation().x())); EXPECT_TRUE(!std::isnan(output.getRotation().y())); EXPECT_TRUE(!std::isnan(output.getRotation().z())); EXPECT_TRUE(!std::isnan(output.getRotation().w())); } catch (...) { EXPECT_FALSE("Excetion improperly thrown"); } } TEST(tf, frameExists) { Transformer mTR; // test with fully qualified name EXPECT_FALSE(mTR.frameExists("/b"));; EXPECT_FALSE(mTR.frameExists("/parent")); EXPECT_FALSE(mTR.frameExists("/other")); EXPECT_FALSE(mTR.frameExists("/frame")); //test with resolveping EXPECT_FALSE(mTR.frameExists("b"));; EXPECT_FALSE(mTR.frameExists("parent")); EXPECT_FALSE(mTR.frameExists("other")); EXPECT_FALSE(mTR.frameExists("frame")); tf::Quaternion qt1; qt1.setRPY(1,0,0); mTR.setTransform( StampedTransform (tf::Transform(qt1, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b")); // test with fully qualified name EXPECT_TRUE(mTR.frameExists("/b")); EXPECT_TRUE(mTR.frameExists("/parent")); EXPECT_FALSE(mTR.frameExists("/other")); EXPECT_FALSE(mTR.frameExists("/frame")); //Test with resolveping EXPECT_TRUE(mTR.frameExists("b")); EXPECT_TRUE(mTR.frameExists("parent")); EXPECT_FALSE(mTR.frameExists("other")); EXPECT_FALSE(mTR.frameExists("frame")); tf::Quaternion qt2; qt2.setRPY(1,1,0); mTR.setTransform( StampedTransform (tf::Transform(qt2, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000), "/frame", "/other")); // test with fully qualified name EXPECT_TRUE(mTR.frameExists("/b")); EXPECT_TRUE(mTR.frameExists("/parent")); EXPECT_TRUE(mTR.frameExists("/other")); EXPECT_TRUE(mTR.frameExists("/frame")); //Test with resolveping EXPECT_TRUE(mTR.frameExists("b")); EXPECT_TRUE(mTR.frameExists("parent")); EXPECT_TRUE(mTR.frameExists("other")); EXPECT_TRUE(mTR.frameExists("frame")); } TEST(tf, canTransform) { Transformer mTR; //confirm zero length list disconnected will return true EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time())); EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time::now())); //Create a two link tree between times 10 and 20 for (int i = 10; i < 20; i++) { tf::Quaternion qt; qt.setRPY(1,0,0); mTR.setTransform( StampedTransform (tf::Transform(qt, tf::Vector3(0,0,0)), ros::Time().fromSec(i), "parent", "child")); mTR.setTransform( StampedTransform (tf::Transform(qt, tf::Vector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child")); } // four different timestamps related to tf state ros::Time zero_time = ros::Time().fromSec(0); ros::Time old_time = ros::Time().fromSec(5); ros::Time valid_time = ros::Time().fromSec(15); ros::Time future_time = ros::Time().fromSec(25); //confirm zero length list disconnected will return true EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", zero_time)); EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", old_time)); EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", valid_time)); EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", future_time)); // Basic API Tests //Valid data should pass EXPECT_TRUE(mTR.canTransform("child", "parent", valid_time)); EXPECT_TRUE(mTR.canTransform("child", "other_child", valid_time)); //zero data should pass EXPECT_TRUE(mTR.canTransform("child", "parent", zero_time)); EXPECT_TRUE(mTR.canTransform("child", "other_child", zero_time)); //Old data should fail EXPECT_FALSE(mTR.canTransform("child", "parent", old_time)); EXPECT_FALSE(mTR.canTransform("child", "other_child", old_time)); //Future data should fail EXPECT_FALSE(mTR.canTransform("child", "parent", future_time)); EXPECT_FALSE(mTR.canTransform("child", "other_child", future_time)); //Same Frame should pass for all times EXPECT_TRUE(mTR.canTransform("child", "child", zero_time)); EXPECT_TRUE(mTR.canTransform("child", "child", old_time)); EXPECT_TRUE(mTR.canTransform("child", "child", valid_time)); EXPECT_TRUE(mTR.canTransform("child", "child", future_time)); // Advanced API Tests // Source = Fixed //zero data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "child")); EXPECT_TRUE(mTR.canTransform("child", zero_time, "other_child", valid_time, "child")); //Old data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", old_time, "parent", valid_time, "child")); EXPECT_TRUE(mTR.canTransform("child", old_time, "other_child", valid_time, "child")); //valid data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "child")); EXPECT_TRUE(mTR.canTransform("child", valid_time, "other_child", valid_time, "child")); //future data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", future_time, "parent", valid_time, "child")); EXPECT_TRUE(mTR.canTransform("child", future_time, "other_child", valid_time, "child")); //transforming through fixed into the past EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", old_time, "child")); EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", old_time, "child")); //transforming through fixed into the future EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", future_time, "child")); EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", future_time, "child")); // Target = Fixed //zero data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "parent")); //Old data in fixed frame should pass EXPECT_FALSE(mTR.canTransform("child", old_time, "parent", valid_time, "parent")); //valid data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent")); //future data in fixed frame should pass EXPECT_FALSE(mTR.canTransform("child", future_time, "parent", valid_time, "parent")); //transforming through fixed into the zero EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", zero_time, "parent")); //transforming through fixed into the past EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", old_time, "parent")); //transforming through fixed into the valid EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent")); //transforming through fixed into the future EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", future_time, "parent")); } TEST(tf, lookupTransform) { Transformer mTR; //Create a two link tree between times 10 and 20 for (int i = 10; i < 20; i++) { tf::Quaternion qt; qt.setRPY(1,0,0); mTR.setTransform( StampedTransform (tf::Transform(qt, tf::Vector3(0,0,0)), ros::Time().fromSec(i), "parent", "child")); mTR.setTransform( StampedTransform (tf::Transform(qt, tf::Vector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child")); } // four different timestamps related to tf state ros::Time zero_time = ros::Time().fromSec(0); ros::Time old_time = ros::Time().fromSec(5); ros::Time valid_time = ros::Time().fromSec(15); ros::Time future_time = ros::Time().fromSec(25); //output tf::StampedTransform output; // Basic API Tests try { //confirm zero length list disconnected will return true mTR.lookupTransform("some_frame1","some_frame1", zero_time, output); mTR.lookupTransform("some_frame2","some_frame2", old_time, output); mTR.lookupTransform("some_frame3","some_frame3", valid_time, output); mTR.lookupTransform("some_frame4","some_frame4", future_time, output); mTR.lookupTransform("child","child", future_time, output); mTR.lookupTransform("other_child","other_child", future_time, output); //Valid data should pass mTR.lookupTransform("child", "parent", valid_time, output); mTR.lookupTransform("child", "other_child", valid_time, output); //zero data should pass mTR.lookupTransform("child", "parent", zero_time, output); mTR.lookupTransform("child", "other_child", zero_time, output); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception thrown"); } try { //Old data should fail mTR.lookupTransform("child", "parent", old_time, output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //Future data should fail mTR.lookupTransform("child", "parent", future_time, output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //Same Frame should pass for all times mTR.lookupTransform("child", "child", zero_time, output); mTR.lookupTransform("child", "child", old_time, output); mTR.lookupTransform("child", "child", valid_time, output); mTR.lookupTransform("child", "child", future_time, output); // Advanced API Tests // Source = Fixed //zero data in fixed frame should pass mTR.lookupTransform("child", zero_time, "parent", valid_time, "child", output); mTR.lookupTransform("child", zero_time, "other_child", valid_time, "child", output); //Old data in fixed frame should pass mTR.lookupTransform("child", old_time, "parent", valid_time, "child", output); mTR.lookupTransform("child", old_time, "other_child", valid_time, "child", output); //valid data in fixed frame should pass mTR.lookupTransform("child", valid_time, "parent", valid_time, "child", output); mTR.lookupTransform("child", valid_time, "other_child", valid_time, "child", output); //future data in fixed frame should pass mTR.lookupTransform("child", future_time, "parent", valid_time, "child", output); mTR.lookupTransform("child", future_time, "other_child", valid_time, "child", output); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception incorrectly thrown"); } try { //transforming through fixed into the past mTR.lookupTransform("child", valid_time, "parent", old_time, "child", output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //transforming through fixed into the future mTR.lookupTransform("child", valid_time, "parent", future_time, "child", output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { // Target = Fixed //zero data in fixed frame should pass mTR.lookupTransform("child", zero_time, "parent", valid_time, "parent", output); //valid data in fixed frame should pass mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception incorrectly thrown"); } try { //Old data in fixed frame should pass mTR.lookupTransform("child", old_time, "parent", valid_time, "parent", output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //future data in fixed frame should pass mTR.lookupTransform("child", future_time, "parent", valid_time, "parent", output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //transforming through fixed into the zero mTR.lookupTransform("child", valid_time, "parent", zero_time, "parent", output); //transforming through fixed into the past mTR.lookupTransform("child", valid_time, "parent", old_time, "parent", output); //transforming through fixed into the valid mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output); //transforming through fixed into the future mTR.lookupTransform("child", valid_time, "parent", future_time, "parent", output); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception improperly thrown"); } //zero time goes to latest known value for frames try { double epsilon = 1e-6; mTR.lookupTransform("a", "a", ros::Time(),output); EXPECT_NEAR(ros::Time().toSec(), output.stamp_.toSec(), epsilon); mTR.lookupTransform("child", "child", ros::Time().fromSec(15),output); EXPECT_NEAR(15.0, output.stamp_.toSec(), epsilon); mTR.lookupTransform("child", "child", ros::Time(),output); EXPECT_NEAR(19.0, output.stamp_.toSec(), epsilon); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception improperly thrown"); } } TEST(tf, getFrameStrings) { Transformer mTR; tf::Quaternion qt1, qt2; qt1.setRPY(1,0,0); qt2.setRPY(1,1,0); mTR.setTransform( StampedTransform (tf::Transform(qt1, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b")); std::vector frames_string; mTR.getFrameStrings(frames_string); ASSERT_EQ(frames_string.size(), (unsigned)2); EXPECT_STREQ(frames_string[0].c_str(), std::string("b").c_str()); EXPECT_STREQ(frames_string[1].c_str(), std::string("parent").c_str()); mTR.setTransform( StampedTransform (tf::Transform(qt2, tf::Vector3(0,0,0)), ros::Time().fromNSec(4000), "/frame", "/other")); mTR.getFrameStrings(frames_string); ASSERT_EQ(frames_string.size(), (unsigned)4); EXPECT_STREQ(frames_string[0].c_str(), std::string("b").c_str()); EXPECT_STREQ(frames_string[1].c_str(), std::string("parent").c_str()); EXPECT_STREQ(frames_string[2].c_str(), std::string("other").c_str()); EXPECT_STREQ(frames_string[3].c_str(), std::string("frame").c_str()); } bool expectInvalidQuaternion(tf::Quaternion q) { try { tf::assertQuaternionValid(q); printf("this should have thrown\n"); return false; } catch (tf::InvalidArgument &ex) { return true; } catch (...) { printf("A different type of exception was expected\n"); return false; } return false; } bool expectValidQuaternion(tf::Quaternion q) { try { tf::assertQuaternionValid(q); } catch (tf::TransformException &ex) { return false; } return true; } bool expectInvalidQuaternion(geometry_msgs::Quaternion q) { try { tf::assertQuaternionValid(q); printf("this should have thrown\n"); return false; } catch (tf::InvalidArgument &ex) { return true; } catch (...) { printf("A different type of exception was expected\n"); return false; } return false; } bool expectValidQuaternion(geometry_msgs::Quaternion q) { try { tf::assertQuaternionValid(q); } catch (tf::TransformException &ex) { return false; } return true; } TEST(tf, assertQuaternionValid) { tf::Quaternion q(1,0,0,0); EXPECT_TRUE(expectValidQuaternion(q)); q.setX(0); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setY(1); EXPECT_TRUE(expectValidQuaternion(q)); q.setZ(1); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setY(0); EXPECT_TRUE(expectValidQuaternion(q)); q.setW(1); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setZ(0); EXPECT_TRUE(expectValidQuaternion(q)); q.setZ(sqrt(2.0)/2.0); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setW(sqrt(2.0)/2.0); EXPECT_TRUE(expectValidQuaternion(q)); q.setZ(sqrt(2.0)/2.0 + 0.01); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setZ(sqrt(2.0)/2.0 - 0.01); EXPECT_TRUE(expectInvalidQuaternion(q)); // check NaNs q.setValue(1,0,0,0); q.setX(0.0/0.0); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setX(1.0); q.setY(NAN); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setY(0.0); q.setZ(0.0/0.0); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setZ(0.0); q.setW(0.0/0.0); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setW(0.0); /* Waiting for gtest 1.1 or later EXPECT_NO_THROW(tf::assertQuaternionValid(q)); q.setX(0); EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument); q.setY(1); EXPECT_NO_THROW(tf::assertQuaternionValid(q)); */ } TEST(tf, assertQuaternionMsgValid) { geometry_msgs::Quaternion q; q.x = 1;//others zeroed to start EXPECT_TRUE(expectValidQuaternion(q)); q.x = 0; EXPECT_TRUE(expectInvalidQuaternion(q)); q.y = 1; EXPECT_TRUE(expectValidQuaternion(q)); q.z = 1; EXPECT_TRUE(expectInvalidQuaternion(q)); q.y = 0; EXPECT_TRUE(expectValidQuaternion(q)); q.w = 1; EXPECT_TRUE(expectInvalidQuaternion(q)); q.z = 0; EXPECT_TRUE(expectValidQuaternion(q)); q.z = sqrt(2.0)/2.0; EXPECT_TRUE(expectInvalidQuaternion(q)); q.w = sqrt(2.0)/2.0; EXPECT_TRUE(expectValidQuaternion(q)); q.z = sqrt(2.0)/2.0 + 0.01; EXPECT_TRUE(expectInvalidQuaternion(q)); q.z = sqrt(2.0)/2.0 - 0.01; EXPECT_TRUE(expectInvalidQuaternion(q)); // check NaNs q.x = 1.0; q.y = 0.0; q.z = 0.0; q.w = 0.0; q.x = 0.0/0.0; EXPECT_TRUE(expectInvalidQuaternion(q)); q.x = 1.0; q.y = NAN; EXPECT_TRUE(expectInvalidQuaternion(q)); q.y = 0.0; q.z = 0.0/0.0; EXPECT_TRUE(expectInvalidQuaternion(q)); q.z = 0.0; q.w = 0.0/0.0; EXPECT_TRUE(expectInvalidQuaternion(q)); q.w = 0.0; /* Waiting for gtest 1.1 or later EXPECT_NO_THROW(tf::assertQuaternionValid(q)); q.x = 0); EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument); q.y = 1); EXPECT_NO_THROW(tf::assertQuaternionValid(q)); */ } TEST(data, StampedOperatorEqualEqual) { tf::Pose pose0, pose1, pose0a; pose0.setIdentity(); pose0a.setIdentity(); pose1.setIdentity(); pose1.setOrigin(tf::Vector3(1, 0, 0)); tf::Stamped stamped_pose_reference(pose0a, ros::Time(), "frame_id"); tf::Stamped stamped_pose0A(pose0, ros::Time(), "frame_id"); EXPECT_TRUE(stamped_pose0A == stamped_pose_reference); // Equal tf::Stamped stamped_pose0B(pose0, ros::Time(), "frame_id_not_equal"); EXPECT_FALSE(stamped_pose0B == stamped_pose_reference); // Different Frame id tf::Stamped stamped_pose0C(pose0, ros::Time(1.0), "frame_id"); EXPECT_FALSE(stamped_pose0C == stamped_pose_reference); // Different Time tf::Stamped stamped_pose0D(pose0, ros::Time(1.0), "frame_id_not_equal"); EXPECT_FALSE(stamped_pose0D == stamped_pose_reference); // Different frame id and time tf::Stamped stamped_pose0E(pose1, ros::Time(), "frame_id_not_equal"); EXPECT_FALSE(stamped_pose0E == stamped_pose_reference); // Different pose, frame id tf::Stamped stamped_pose0F(pose1, ros::Time(1.0), "frame_id"); EXPECT_FALSE(stamped_pose0F == stamped_pose_reference); // Different pose, time tf::Stamped stamped_pose0G(pose1, ros::Time(1.0), "frame_id_not_equal"); EXPECT_FALSE(stamped_pose0G == stamped_pose_reference); // Different pose, frame id and time tf::Stamped stamped_pose0H(pose1, ros::Time(), "frame_id"); EXPECT_FALSE(stamped_pose0H == stamped_pose_reference); // Different pose } TEST(data, StampedTransformOperatorEqualEqual) { tf::Transform transform0, transform1, transform0a; transform0.setIdentity(); transform0a.setIdentity(); transform1.setIdentity(); transform1.setOrigin(tf::Vector3(1, 0, 0)); tf::StampedTransform stamped_transform_reference(transform0a, ros::Time(), "frame_id", "child_frame_id"); tf::StampedTransform stamped_transform0A(transform0, ros::Time(), "frame_id", "child_frame_id"); EXPECT_TRUE(stamped_transform0A == stamped_transform_reference); // Equal tf::StampedTransform stamped_transform0B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id"); EXPECT_FALSE(stamped_transform0B == stamped_transform_reference); // Different Frame id tf::StampedTransform stamped_transform0C(transform0, ros::Time(1.0), "frame_id", "child_frame_id"); EXPECT_FALSE(stamped_transform0C == stamped_transform_reference); // Different Time tf::StampedTransform stamped_transform0D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id"); EXPECT_FALSE(stamped_transform0D == stamped_transform_reference); // Different frame id and time tf::StampedTransform stamped_transform0E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id"); EXPECT_FALSE(stamped_transform0E == stamped_transform_reference); // Different transform, frame id tf::StampedTransform stamped_transform0F(transform1, ros::Time(1.0), "frame_id", "child_frame_id"); EXPECT_FALSE(stamped_transform0F == stamped_transform_reference); // Different transform, time tf::StampedTransform stamped_transform0G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id"); EXPECT_FALSE(stamped_transform0G == stamped_transform_reference); // Different transform, frame id and time tf::StampedTransform stamped_transform0H(transform1, ros::Time(), "frame_id", "child_frame_id"); EXPECT_FALSE(stamped_transform0H == stamped_transform_reference); // Different transform //Different child_frame_id tf::StampedTransform stamped_transform1A(transform0, ros::Time(), "frame_id", "child_frame_id2"); EXPECT_FALSE(stamped_transform1A == stamped_transform_reference); // Equal tf::StampedTransform stamped_transform1B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id2"); EXPECT_FALSE(stamped_transform1B == stamped_transform_reference); // Different Frame id tf::StampedTransform stamped_transform1C(transform0, ros::Time(1.0), "frame_id", "child_frame_id2"); EXPECT_FALSE(stamped_transform1C == stamped_transform_reference); // Different Time tf::StampedTransform stamped_transform1D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2"); EXPECT_FALSE(stamped_transform1D == stamped_transform_reference); // Different frame id and time tf::StampedTransform stamped_transform1E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id2"); EXPECT_FALSE(stamped_transform1E == stamped_transform_reference); // Different transform, frame id tf::StampedTransform stamped_transform1F(transform1, ros::Time(1.0), "frame_id", "child_frame_id2"); EXPECT_FALSE(stamped_transform1F == stamped_transform_reference); // Different transform, time tf::StampedTransform stamped_transform1G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2"); EXPECT_FALSE(stamped_transform1G == stamped_transform_reference); // Different transform, frame id and time tf::StampedTransform stamped_transform1H(transform1, ros::Time(), "frame_id", "child_frame_id2"); EXPECT_FALSE(stamped_transform1H == stamped_transform_reference); // Different transform } TEST(data, StampedOperatorEqual) { tf::Pose pose0, pose1, pose0a; pose0.setIdentity(); pose1.setIdentity(); pose1.setOrigin(tf::Vector3(1, 0, 0)); tf::Stamped stamped_pose0(pose0, ros::Time(), "frame_id"); tf::Stamped stamped_pose1(pose1, ros::Time(1.0), "frame_id_not_equal"); EXPECT_FALSE(stamped_pose1 == stamped_pose0); stamped_pose1 = stamped_pose0; EXPECT_TRUE(stamped_pose1 == stamped_pose0); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::Time::init(); //needed for ros::TIme::now() ros::init(argc, argv, "tf_unittest"); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/test/tf_unittest_future.cpp000066400000000000000000000047071366756125100215130ustar00rootroot00000000000000#include #include #include #include "tf/LinearMath/Vector3.h" using namespace tf; void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); }; TEST(tf, SignFlipExtrapolate) { double epsilon = 1e-3; double truex, truey, trueyaw1, trueyaw2; truex = 5.220; truey = 1.193; trueyaw1 = 2.094; trueyaw2 = 2.199; ros::Time ts0; ts0.fromSec(46.6); ros::Time ts1; ts1.fromSec(46.7); ros::Time ts2; ts2.fromSec(46.8); TransformStorage tout; double yaw, pitch, roll; TransformStorage t0(StampedTransform (tf::Transform(tf::Quaternion(0.000, 0.000, -0.8386707128751809, 0.5446388118427071), tf::Vector3(1.0330764266905630, 5.2545257423922198, -0.000)), ts0, "odom", "other0"), 3); TransformStorage t1(StampedTransform (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8660255375641606, -0.4999997682866531), tf::Vector3(1.5766646418987809, 5.1177550046707436, -0.000)), ts1, "odom", "other1"), 3); TransformStorage t2(StampedTransform (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8910066733792211, -0.4539902069358919), tf::Vector3(2.1029791754869160, 4.9249128183465967, -0.000)), ts2, "odom", "other2"), 3); tf::TimeCache tc; tf::Transform res; tc.interpolate(t0, t1, ts1, tout); res = tout.inverse(); res.getBasis().getEulerZYX(yaw,pitch,roll); EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); EXPECT_NEAR(yaw, trueyaw1, epsilon); tc.interpolate(t0, t1, ts2, tout); res = tout.inverse(); res.getBasis().getEulerZYX(yaw,pitch,roll); EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); EXPECT_NEAR(yaw, trueyaw2, epsilon); tc.interpolate(t1, t2, ts2, tout); res = tout.inverse(); res.getBasis().getEulerZYX(yaw,pitch,roll); EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); EXPECT_NEAR(yaw, trueyaw2, epsilon); } /** @todo Make this actually Assert something */ int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/test/transform_listener_unittest.cpp000066400000000000000000000057011366756125100234230ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); } void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) { seed_rand(); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; } } using namespace tf; TEST(transform_listener, resolve) { ros::NodeHandle n("~"); tf::TransformListener tl; //no prefix EXPECT_STREQ("id", tl.resolve("id").c_str()); n.setParam("tf_prefix", "a_tf_prefix"); tf::TransformListener tp; std::string prefix_str = tf::getPrefixParam(n); EXPECT_STREQ("a_tf_prefix", prefix_str.c_str()); EXPECT_STREQ("a_tf_prefix/id", tf::resolve(prefix_str, "id").c_str()); EXPECT_STREQ("a_tf_prefix/id", tp.resolve("id").c_str()); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "transform_listener_unittest"); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/test/transform_listener_unittest.launch000066400000000000000000000001611366756125100241060ustar00rootroot00000000000000 geometry-1.13.2/tf/test/transform_twist_test.cpp000066400000000000000000000363261366756125100220570ustar00rootroot00000000000000#include #include #include #include "tf/LinearMath/Vector3.h" using namespace tf; // The fixture for testing class Foo. class TransformTwistLinearTest : public ::testing::Test { protected: // You can remove any or all of the following functions if its body // is empty. TransformTwistLinearTest() { double x = -.1; double y = 0; double z = 0; for (double t = 0.1; t <= 6; t += 0.1) { if (t < 1) x += 1; else if (t < 2) y += 1; else if (t < 3) x -= 1; else if (t < 4) y -= 1; else if (t < 5) z += 1; else z -= 1; tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(x, y, z)), ros::Time(t), "foo", "bar")); tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child")); tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,1)), ros::Time(t), "stationary_offset_parent", "foo")); } // You can do set-up work for each test here. tw_.header.stamp = ros::Time(); tw_.header.frame_id = "bar"; tw_.twist.linear.x = 1; tw_.twist.linear.y = 2; tw_.twist.linear.z = 3; tw_.twist.angular.x = 0; tw_.twist.angular.y = 0; tw_.twist.angular.z = 0; } virtual ~TransformTwistLinearTest() { // You can do clean-up work that doesn't throw exceptions here. } // If the constructor and destructor are not enough for setting up // and cleaning up each test, you can define the following methods: virtual void SetUp() { // Code here will be called immediately after the constructor (right // before each test). } virtual void TearDown() { // Code here will be called immediately after each test (right // before the destructor). } // Objects declared here can be used by all tests in the test case for LinearVelocity. tf::TransformListener tf_; geometry_msgs::TwistStamped tw_; }; // The fixture for testing class Foo. class TransformTwistAngularTest : public ::testing::Test { protected: // You can remove any or all of the following functions if its body // is empty. TransformTwistAngularTest() { double x = -.1; double y = 0; double z = 0; for (double t = 0.1; t < 6; t += 0.1) { if (t < 1) x += .1; else if (t < 2) x -= .1; else if (t < 3) y += .1; else if (t < 4) y -= .1; else if (t < 5) z += .1; else z -= .1; tf_.setTransform(StampedTransform(tf::Transform(tf::createQuaternionFromRPY(x, y, z), tf::Vector3(0,0,0)), ros::Time(t), "foo", "bar")); tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child")); tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,-1)), ros::Time(t), "stationary_offset_parent", "foo")); } // You can do set-up work for each test here. // You can do set-up work for each test here. tw_x.header.stamp = ros::Time(); tw_x.header.frame_id = "bar"; tw_x.twist.angular.x = 1; tw_y.header.stamp = ros::Time(); tw_y.header.frame_id = "bar"; tw_y.twist.angular.y = 1; tw_z.header.stamp = ros::Time(); tw_z.header.frame_id = "bar"; tw_z.twist.angular.z = 1; } virtual ~TransformTwistAngularTest() { // You can do clean-up work that doesn't throw exceptions here. } // If the constructor and destructor are not enough for setting up // and cleaning up each test, you can define the following methods: virtual void SetUp() { // Code here will be called immediately after the constructor (right // before each test). } virtual void TearDown() { // Code here will be called immediately after each test (right // before the destructor). } // Objects declared here can be used by all tests in the test case for AngularVelocity. tf::TransformListener tf_; geometry_msgs::TwistStamped tw_x, tw_y, tw_z; }; TEST_F(TransformTwistLinearTest, LinearVelocityToThreeFrames) { geometry_msgs::TwistStamped tw; geometry_msgs::TwistStamped tw_in =tw_; std::vector offset_frames; offset_frames.push_back("foo"); offset_frames.push_back("stationary_offset_child"); offset_frames.push_back("stationary_offset_parent"); for (std::vector::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it) { try { tw_in.header.stamp = ros::Time(0.5); tf_.transformTwist(*it, tw_in, tw); EXPECT_FLOAT_EQ(11.0, tw.twist.linear.x); EXPECT_FLOAT_EQ(2.0 , tw.twist.linear.y); EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z); EXPECT_FLOAT_EQ(0.0, tw.twist.angular.x); EXPECT_FLOAT_EQ(0.0, tw.twist.angular.y); EXPECT_FLOAT_EQ(0.0, tw.twist.angular.z); tw_in.header.stamp = ros::Time(1.5); tf_.transformTwist(*it, tw_in, tw); EXPECT_FLOAT_EQ(1.0, tw.twist.linear.x); EXPECT_FLOAT_EQ(12.0, tw.twist.linear.y); EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z); EXPECT_FLOAT_EQ(0.0, tw.twist.angular.x); EXPECT_FLOAT_EQ(0.0, tw.twist.angular.y); EXPECT_FLOAT_EQ(0.0, tw.twist.angular.z); tw_in.header.stamp = ros::Time(2.5); tf_.transformTwist(*it, tw_in, tw); EXPECT_FLOAT_EQ(-9.0, tw.twist.linear.x); EXPECT_FLOAT_EQ(2.0, tw.twist.linear.y); EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z); EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0); EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0); EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0); tw_in.header.stamp = ros::Time(3.5); tf_.transformTwist(*it, tw_in, tw); EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0); EXPECT_FLOAT_EQ(tw.twist.linear.y, -8.0); EXPECT_FLOAT_EQ(tw.twist.linear.z, 3.0); EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0); EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0); EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0); tw_in.header.stamp = ros::Time(4.5); tf_.transformTwist(*it, tw_in, tw); EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0); EXPECT_FLOAT_EQ(tw.twist.linear.y, 2.0); EXPECT_FLOAT_EQ(tw.twist.linear.z, 13.0); EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0); EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0); EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0); tw_in.header.stamp = ros::Time(5.5); tf_.transformTwist(*it, tw_in, tw); EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0); EXPECT_FLOAT_EQ(tw.twist.linear.y, 2.0); EXPECT_FLOAT_EQ(tw.twist.linear.z, -7.0); EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0); EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0); EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0); } catch(tf::TransformException &ex) { EXPECT_STREQ("", ex.what()); } } }; TEST_F(TransformTwistAngularTest, AngularVelocityAlone) { double epsilon = 1e-14; geometry_msgs::TwistStamped tw; geometry_msgs::TwistStamped tw_in; try { //tf_.lookupVelocity("foo", "bar", ros::Time(0.5), ros::Duration(0.1), tw); tw_in =tw_x; tw_in.header.stamp = ros::Time(0.5); tf_.transformTwist("foo", tw_in, tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 2.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); //tf_.lookupVelocity("foo", "bar", ros::Time(1.5), ros::Duration(0.1), tw); tw_in =tw_x; tw_in.header.stamp = ros::Time(1.5); tf_.transformTwist("foo", tw_in, tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); // tf_.lookupVelocity("foo", "bar", ros::Time(2.5), ros::Duration(0.1), tw); tw_in =tw_y; tw_in.header.stamp = ros::Time(2.5); tf_.transformTwist("foo", tw_in, tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 2.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); //tf_.lookupVelocity("foo", "bar", ros::Time(3.5), ros::Duration(0.1), tw); tw_in =tw_y; tw_in.header.stamp = ros::Time(3.5); tf_.transformTwist("foo", tw_in, tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); //tf_.lookupVelocity("foo", "bar", ros::Time(4.5), ros::Duration(0.1), tw); tw_in =tw_z; tw_in.header.stamp = ros::Time(4.5); tf_.transformTwist("foo", tw_in, tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 2.0, epsilon); // tf_.lookupVelocity("foo", "bar", ros::Time(5.5), ros::Duration(0.1), tw); tw_in =tw_z; tw_in.header.stamp = ros::Time(5.5); tf_.transformTwist("foo", tw_in, tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); } catch(tf::TransformException &ex) { EXPECT_STREQ("", ex.what()); } }; /* TEST_F(TransformTwistAngularTest, AngularVelocityOffsetChildFrameInX) { double epsilon = 1e-14; geometry_msgs::TwistStamped tw; try { tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(0.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 1.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(1.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, -1.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(2.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, -1.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 1.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(3.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 1.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, -1.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(4.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 1.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 1.0, epsilon); tf_.lookupVelocity("stationary_offset_child", "bar", ros::Time(5.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, -1.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, -1.0, epsilon); } catch(tf::TransformException &ex) { EXPECT_STREQ("", ex.what()); } }; TEST_F(TransformTwistAngularTest, AngularVelocityOffsetParentFrameInZ) { double epsilon = 1e-14; geometry_msgs::TwistStamped tw; try { tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(0.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, -1.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 1.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(1.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 1.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, -1.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(2.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 1.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 1.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(3.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, -1.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, -1.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon); tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(4.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, 1.0, epsilon); tf_.lookupVelocity("stationary_offset_parent", "bar", ros::Time(5.5), ros::Duration(0.1), tw); EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon); EXPECT_NEAR(tw.twist.angular.z, -1.0, epsilon); } catch(tf::TransformException &ex) { EXPECT_STREQ("", ex.what()); } }; */ int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "testing_transform_twist"); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/test/transform_twist_test.launch000066400000000000000000000001421366756125100225320ustar00rootroot00000000000000 geometry-1.13.2/tf/test/velocity_test.cpp000066400000000000000000000324471366756125100204500ustar00rootroot00000000000000#include #include #include #include "tf/LinearMath/Vector3.h" using namespace tf; // The fixture for testing class Foo. class LinearVelocitySquareTest : public ::testing::Test { protected: // You can remove any or all of the following functions if its body // is empty. LinearVelocitySquareTest() { double x = -.1; double y = 0; double z = 0; for (double t = 0.1; t <= 6; t += 0.1) { if (t < 1) x += .1; else if (t < 2) y += .1; else if (t < 3) x -= .1; else if (t < 4) y -= .1; else if (t < 5) z += .1; else z -= .1; tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(x, y, z)), ros::Time(t), "foo", "bar")); tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child")); tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,1)), ros::Time(t), "stationary_offset_parent", "foo")); } // You can do set-up work for each test here. } virtual ~LinearVelocitySquareTest() { // You can do clean-up work that doesn't throw exceptions here. } // If the constructor and destructor are not enough for setting up // and cleaning up each test, you can define the following methods: virtual void SetUp() { // Code here will be called immediately after the constructor (right // before each test). } virtual void TearDown() { // Code here will be called immediately after each test (right // before the destructor). } // Objects declared here can be used by all tests in the test case for LinearVelocity. tf::Transformer tf_; }; // The fixture for testing class Foo. class AngularVelocitySquareTest : public ::testing::Test { protected: // You can remove any or all of the following functions if its body // is empty. AngularVelocitySquareTest() { double x = -.1; double y = 0; double z = 0; for (double t = 0.1; t < 6; t += 0.1) { if (t < 1) x += .1; else if (t < 2) x -= .1; else if (t < 3) y += .1; else if (t < 4) y -= .1; else if (t < 5) z += .1; else z -= .1; tf_.setTransform(StampedTransform(tf::Transform(tf::createQuaternionFromRPY(x, y, z), tf::Vector3(0,0,0)), ros::Time(t), "foo", "bar")); tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,0,0)), ros::Time(t), "foo", "stationary_offset_child")); tf_.setTransform(StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(0,0,-1)), ros::Time(t), "stationary_offset_parent", "foo")); } // You can do set-up work for each test here. } virtual ~AngularVelocitySquareTest() { // You can do clean-up work that doesn't throw exceptions here. } // If the constructor and destructor are not enough for setting up // and cleaning up each test, you can define the following methods: virtual void SetUp() { // Code here will be called immediately after the constructor (right // before each test). } virtual void TearDown() { // Code here will be called immediately after each test (right // before the destructor). } // Objects declared here can be used by all tests in the test case for AngularVelocity. tf::Transformer tf_; }; TEST_F(LinearVelocitySquareTest, LinearVelocityToThreeFrames) { geometry_msgs::Twist twist; std::vector offset_frames; offset_frames.push_back("foo"); offset_frames.push_back("stationary_offset_child"); offset_frames.push_back("stationary_offset_parent"); for (std::vector::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it) { try { tf_.lookupTwist("bar", *it, ros::Time(0.5), ros::Duration(0.1), twist); EXPECT_FLOAT_EQ(twist.linear.x, 1.0); EXPECT_FLOAT_EQ(twist.linear.y, 0.0); EXPECT_FLOAT_EQ(twist.linear.z, 0.0); EXPECT_FLOAT_EQ(twist.angular.x, 0.0); EXPECT_FLOAT_EQ(twist.angular.y, 0.0); EXPECT_FLOAT_EQ(twist.angular.z, 0.0); tf_.lookupTwist("bar", *it, ros::Time(1.5), ros::Duration(0.1), twist); EXPECT_FLOAT_EQ(twist.linear.x, 0.0); EXPECT_FLOAT_EQ(twist.linear.y, 1.0); EXPECT_FLOAT_EQ(twist.linear.z, 0.0); EXPECT_FLOAT_EQ(twist.angular.x, 0.0); EXPECT_FLOAT_EQ(twist.angular.y, 0.0); EXPECT_FLOAT_EQ(twist.angular.z, 0.0); tf_.lookupTwist("bar", *it, ros::Time(2.5), ros::Duration(0.1), twist); EXPECT_FLOAT_EQ(twist.linear.x, -1.0); EXPECT_FLOAT_EQ(twist.linear.y, 0.0); EXPECT_FLOAT_EQ(twist.linear.z, 0.0); EXPECT_FLOAT_EQ(twist.angular.x, 0.0); EXPECT_FLOAT_EQ(twist.angular.y, 0.0); EXPECT_FLOAT_EQ(twist.angular.z, 0.0); tf_.lookupTwist("bar", *it, ros::Time(3.5), ros::Duration(0.1), twist); EXPECT_FLOAT_EQ(twist.linear.x, 0.0); EXPECT_FLOAT_EQ(twist.linear.y, -1.0); EXPECT_FLOAT_EQ(twist.linear.z, 0.0); EXPECT_FLOAT_EQ(twist.angular.x, 0.0); EXPECT_FLOAT_EQ(twist.angular.y, 0.0); EXPECT_FLOAT_EQ(twist.angular.z, 0.0); tf_.lookupTwist("bar", *it, ros::Time(4.5), ros::Duration(0.1), twist); EXPECT_FLOAT_EQ(twist.linear.x, 0.0); EXPECT_FLOAT_EQ(twist.linear.y, 0.0); EXPECT_FLOAT_EQ(twist.linear.z, 1.0); EXPECT_FLOAT_EQ(twist.angular.x, 0.0); EXPECT_FLOAT_EQ(twist.angular.y, 0.0); EXPECT_FLOAT_EQ(twist.angular.z, 0.0); tf_.lookupTwist("bar", *it, ros::Time(5.5), ros::Duration(0.1), twist); EXPECT_FLOAT_EQ(twist.linear.x, 0.0); EXPECT_FLOAT_EQ(twist.linear.y, 0.0); EXPECT_FLOAT_EQ(twist.linear.z, -1.0); EXPECT_FLOAT_EQ(twist.angular.x, 0.0); EXPECT_FLOAT_EQ(twist.angular.y, 0.0); EXPECT_FLOAT_EQ(twist.angular.z, 0.0); } catch(tf::TransformException &ex) { EXPECT_STREQ("", ex.what()); } } } TEST_F(AngularVelocitySquareTest, AngularVelocityAlone) { double epsilon = 1e-6; geometry_msgs::Twist twist; try { tf_.lookupTwist("bar", "foo", ros::Time(0.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 1.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "foo", ros::Time(1.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, -1.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "foo", ros::Time(2.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 1.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "foo", ros::Time(3.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, -1.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "foo", ros::Time(4.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 1.0, epsilon); tf_.lookupTwist("bar", "foo", ros::Time(5.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, -1.0, epsilon); } catch(tf::TransformException &ex) { EXPECT_STREQ("", ex.what()); } } TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetChildFrameInX) { double epsilon = 1e-6; geometry_msgs::Twist twist; try { tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(0.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 1.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(1.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, -1.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(2.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, -1.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 1.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(3.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 1.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, -1.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(4.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 1.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 1.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_child", ros::Time(5.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, -1.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, -1.0, epsilon); } catch(tf::TransformException &ex) { EXPECT_STREQ("", ex.what()); } } TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetParentFrameInZ) { double epsilon = 1e-6; geometry_msgs::Twist twist; try { tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(0.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, -1.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 1.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(1.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 1.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, -1.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(2.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 1.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 1.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(3.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, -1.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, -1.0, epsilon); EXPECT_NEAR(twist.angular.z, 0.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(4.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, 1.0, epsilon); tf_.lookupTwist("bar", "stationary_offset_parent", ros::Time(5.5), ros::Duration(0.1), twist); EXPECT_NEAR(twist.linear.x, 0.0, epsilon); EXPECT_NEAR(twist.linear.y, 0.0, epsilon); EXPECT_NEAR(twist.linear.z, 0.0, epsilon); EXPECT_NEAR(twist.angular.x, 0.0, epsilon); EXPECT_NEAR(twist.angular.y, 0.0, epsilon); EXPECT_NEAR(twist.angular.z, -1.0, epsilon); } catch(tf::TransformException &ex) { EXPECT_STREQ("", ex.what()); } } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry-1.13.2/tf/tf_python.rst000066400000000000000000000274201366756125100166270ustar00rootroot00000000000000tf (Python) =========== .. exception:: Exception base class for tf exceptions. Because :exc:`tf.Exception` is the base class for other exceptions, you can catch all tf exceptions by writing:: try: # do some tf work except tf.Exception: print "some tf exception happened" .. exception:: ConnectivityException subclass of :exc:`Exception`. Raised when that the fixed_frame tree is not connected between the frames requested. .. exception:: LookupException subclass of :exc:`Exception`. Raised when a tf method has attempted to access a frame, but the frame is not in the graph. The most common reason for this is that the frame is not being published, or a parent frame was not set correctly causing the tree to be broken. .. exception:: ExtrapolationException subclass of :exc:`Exception` Raised when a tf method would have required extrapolation beyond current limits. Transformer ----------- .. class:: tf.Transformer(interpolating, cache_time = rospy.Duration(10)) :param interpolating: Whether to interpolate transformations. :param cache_time: how long tf should retain transformation information in the past. The Transformer object is the core of tf. It maintains a time-varying graph of transforms, and permits asynchronous graph modification and queries: .. doctest:: >>> import rospy >>> import tf >>> import geometry_msgs.msg >>> t = tf.Transformer(True, rospy.Duration(10.0)) >>> t.getFrameStrings() [] >>> m = geometry_msgs.msg.TransformStamped() >>> m.header.frame_id = 'THISFRAME' >>> m.child_frame_id = 'CHILD' >>> m.transform.translation.x = 2.71828183 >>> m.transform.rotation.w = 1.0 >>> t.setTransform(m) >>> t.getFrameStrings() ['/CHILD', '/THISFRAME'] >>> t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0)) ((2.71828183, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) >>> t.lookupTransform('CHILD', 'THISFRAME', rospy.Time(0)) ((-2.71828183, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) The transformer refers to frames using strings, and represents transformations using translation (x, y, z) and quaternions (x, y, z, w) expressed as Python a :class:`tuple`. Transformer also does not mandate any particular linear algebra library. Transformer does not handle ROS messages directly; the only ROS type it uses is `rospy.Time() `_. To use tf with ROS messages, see :class:`TransformerROS` and :class:`TransformListener`. .. method:: allFramesAsDot() -> string Returns a string representing all frames, intended for use with `Graphviz `_. .. method:: allFramesAsString() -> string Returns a human-readable string representing all frames .. method:: setTransform(transform, authority = "") :param transform: transform object, see below :param authority: string giving authority for this transformation. Adds a new transform to the Transformer graph. transform is an object with the following structure:: header stamp time stamp, rospy.Time frame_id string, parent frame child_frame_id string, child frame transform translation x float y float z float rotation x float y float z float w float These members exactly match those of a ROS TransformStamped message. .. method:: waitForTransform(target_frame, source_frame, time, timeout, polling_sleep_duration = rospy.Duration(0.01)) :param target_frame: transformation target frame in tf, string :param source_frame: transformation source frame in tf, string :param time: time of the transformation, use ``rospy.Time()`` to indicate present time. :param timeout: how long this call should block while waiting for the transform, as a :class:`rospy.Duration` :raises: :exc:`tf.Exception` Waits for the given transformation to become available. If the timeout occurs before the transformation becomes available, raises :exc:`tf.Exception`. .. method:: waitForTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame) :param target_frame: transformation target frame in tf, string :param target_time: time of transformation in target_frame, a :class:`rospy.Time` :param source_frame: transformation source frame in tf, string :param source_time: time of transformation in target_frame, a :class:`rospy.Time` :param fixed_frame: reference frame common to both target_frame and source_frame. :param timeout: how long this call should block while waiting for the transform, as a :class:`rospy.Duration` :raises: :exc:`tf.Exception` Extended version of :meth:`waitForTransform`. .. method:: canTransform(target_frame, source_frame, time) -> bool :param target_frame: transformation target frame in tf, string :param source_frame: transformation source frame in tf, string :param time: time of the transformation, use ``rospy.Time()`` to indicate present time. Returns True if the Transformer can determine the transform from source_frame to target_frame at time. .. method:: canTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame) -> bool Extended version of :meth:`canTransform`. .. method:: chain(target_frame, target_time, source_frame, source_time, fixed_frame) -> list :param target_frame: transformation target frame in tf, string :param target_time: time of transformation in target_frame, a :class:`rospy.Time` :param source_frame: transformation source frame in tf, string :param source_time: time of transformation in target_frame, a :class:`rospy.Time` :param fixed_frame: reference frame common to both target_frame and source_frame. :returns: list of tf frames :raises: :exc:`tf.ConnectivityException`, :exc:`tf.LookupException` returns the chain of frames connecting source_frame to target_frame. .. method:: clear() -> None Clear all transformations. .. method:: frameExists(frame_id) -> Bool :param frame_id: a tf frame, string returns True if frame frame_id exists in the Transformer. .. method:: getFrameStrings -> list returns all frame names in the Transformer as a list. .. method:: getLatestCommonTime(source_frame, target_frame) -> time :param target_frame: transformation target frame in tf, string :param source_frame: transformation source frame in tf, string :returns: a :class:`rospy.Time` for the most recent time at which the transform is available :raises: :exc:`tf.Exception` Determines the most recent time for which Transformer can compute the transform between the two given frames. Raises :exc:`tf.Exception` if transformation is not possible. .. method:: lookupTransform(target_frame, source_frame, time) -> (position, quaternion) :param target_frame: transformation target frame in tf, string :param source_frame: transformation source frame in tf, string :param time: time of the transformation, use ``rospy.Time()`` to indicate most recent common time. :returns: position as a translation (x, y, z) and orientation as a quaternion (x, y, z, w) :raises: :exc:`tf.ConnectivityException`, :exc:`tf.LookupException`, or :exc:`tf.ExtrapolationException` Returns the transform from source_frame to target_frame at time. Raises one of the exceptions if the transformation is not possible. Note that a time of zero means latest common time, so:: t.lookupTransform("a", "b", rospy.Time()) is equivalent to:: t.lookupTransform("a", "b", t.getLatestCommonTime("a", "b")) .. method:: lookupTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame) -> position, quaternion :param target_frame: transformation target frame in tf, string :param target_time: time of transformation in target_frame, a :class:`rospy.Time` :param source_frame: transformation source frame in tf, string :param source_time: time of transformation in target_frame, a :class:`rospy.Time` :param fixed_frame: reference frame common to both target_frame and source_frame. :raises: :exc:`tf.ConnectivityException`, :exc:`tf.LookupException`, or :exc:`tf.ExtrapolationException` Extended version of :meth:`lookupTransform`. .. method:: lookupTwist(tracking_frame, observation_frame, time, averaging_interval) -> linear, angular :param tracking_frame: The frame to track :type tracking_frame: str :param observation_frame: The frame from which to measure the twist :type observation_frame: str :param time: The time at which to get the velocity :type time: :class:`rospy.Time` :param duration: The period over which to average :type duration: :class:`rospy.Duration` :returns: a tuple with linear velocity as (x, y, z) and angular velocity as (x, y, z) :raises: :exc:`tf.ConnectivityException`, :exc:`tf.LookupException`, or :exc:`tf.ExtrapolationException` Simplified version of :meth:`tf.lookupTwistFull`. Return the linear and angular velocity of the moving_frame in the reference_frame. tf considers :math:`time - duration / 2` to :math:`time + duration / 2` as the initial interval, and will shift by up to :math:`duration / 2` to avoid no data. .. versionadded:: 1.1 .. method:: lookupTwistFull(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame, time, averaging_interval) -> linear, angular :param tracking_frame: The frame to track :type tracking_frame: str :param observation_frame: The frame from which to measure the twist :type observation_frame: str :param reference_frame: The reference frame in which to express the twist :type reference_frame: str :param reference_point: The reference point with which to express the twist :type reference_point: x, y, z :param reference_point_frame: The frame_id in which the reference point is expressed :type reference_point_frame: str :param time: The time at which to get the velocity :type time: :class:`rospy.Time` :param duration: The period over which to average :type duration: :class:`rospy.Duration` :returns: a tuple with linear velocity as (x, y, z) and angular velocity as (x, y, z) :raises: :exc:`tf.ConnectivityException`, :exc:`tf.LookupException`, or :exc:`tf.ExtrapolationException` Return the linear and angular velocity of the moving_frame in the reference_frame. tf considers :math:`time - duration / 2` to :math:`time + duration / 2` as the initial interval, and will shift by up to :math:`duration / 2` to avoid no data. .. versionadded:: 1.1 .. method:: getTFPrefix() -> str :returns: the TF Prefix that the transformer is running with Returns the tf_prefix this transformer is running with. TransformerROS -------------- .. autoclass:: tf.TransformerROS :members: TransformListener ----------------- .. autoclass:: tf.TransformListener TransformBroadcaster -------------------- .. autoclass:: tf.TransformBroadcaster :members: geometry-1.13.2/tf/transformations.rst000066400000000000000000000001161366756125100200370ustar00rootroot00000000000000transformations ============== .. automodule:: tf.transformations :members: geometry-1.13.2/tf_conversions/000077500000000000000000000000001366756125100165065ustar00rootroot00000000000000geometry-1.13.2/tf_conversions/CHANGELOG.rst000066400000000000000000000162401366756125100205320ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf_conversions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.13.2 (2020-06-08) ------------------- 1.13.1 (2020-05-15) ------------------- * Format 2 and build_export_depend orocos kdl (`#210 `_) * import setup from setuptools instead of distutils-core (`#209 `_) * Contributors: Alejandro Hernández Cordero, Shane Loretz 1.13.0 (2020-03-10) ------------------- * Replace kdl packages with rosdep keys (`#206 `_) * Contributors: Shane Loretz 1.12.1 (2020-03-10) ------------------- * Bump CMake version to avoid CMP0048 warning (`#204 `_) * initialize the test values (`#194 `_) * windows bring up, use ROS_DEPRECATED * Contributors: James Xu, Shane Loretz, Tully Foote 1.12.0 (2018-05-02) ------------------- * Remove dependency on cmake_modules (`#157 `_) This is not needed anymore with the move from Eigen to Eigen3 in 707eb41. * Make python scripts Python3 compatible. (`#151 `_) * Python 3 fixes * Prefer str.format over operator % and small python3 fixes. * Contributors: Jochen Sprickerhof, Maarten de Vries 1.11.9 (2017-07-14) ------------------- * Fix cmake dependency export usage * address gcc6 build error (`#143 `_) * Contributors: Lukas Bulwahn, Tully Foote, Timo Röhling 1.11.8 (2016-03-04) ------------------- * tf_conversions: Add conversion functions for Eigen::Isometry3d * Remove outdated manifest loading in python files * Contributors: Maarten de Vries, Michael Hwang 1.11.7 (2015-04-21) ------------------- * Fixed Vector3 documentation * Contributors: Jackie Kay 1.11.6 (2015-03-25) ------------------- 1.11.5 (2015-03-17) ------------------- 1.11.4 (2014-12-23) ------------------- * Update package.xml * Contributors: David Lu!! 1.11.3 (2014-05-07) ------------------- 1.11.2 (2014-02-25) ------------------- * fixing eigen usage in tf_conversions * Contributors: Tully Foote 1.11.1 (2014-02-23) ------------------- 1.11.0 (2014-02-14) ------------------- 1.10.8 (2014-02-01) ------------------- * add missing runtime dependency on python_orocos_kdl for the pyKDL * Contributors: Tully Foote 1.10.7 (2013-12-27) ------------------- * Fix build error in tf_conversions with orocos_kdl dependency Fixes `#45 `_ * Contributors: Tessa Lau 1.10.6 (2013-08-28) ------------------- 1.10.5 (2013-07-19) ------------------- 1.10.4 (2013-07-11) ------------------- 1.10.3 (2013-07-09) ------------------- * calling out versioned catkin requirement for CATKIN_ENABLE_TESTING 1.10.2 (2013-07-09) ------------------- * adding unit test if CATKIN_ENABLE_TESTING statements for tf_conversions to make the whole repo compliant. 1.10.1 (2013-07-05) ------------------- 1.10.0 (2013-07-05) ------------------- * fixing unit tests * fixing kdl linking 1.9.31 (2013-04-18 18:16) ------------------------- 1.9.30 (2013-04-18 16:26) ------------------------- 1.9.29 (2013-01-13) ------------------- 1.9.28 (2013-01-02) ------------------- * add missing setup.py 1.9.27 (2012-12-21) ------------------- * added license headers to various files 1.9.26 (2012-12-14) ------------------- * add missing dep to catkin 1.9.25 (2012-12-13) ------------------- 1.9.24 (2012-12-11) ------------------- * Version 1.9.24 * Fixes to CMakeLists.txt's while building from source 1.9.23 (2012-11-22) ------------------- * Releaseing version 1.9.23 1.9.22 (2012-11-04 09:14) ------------------------- * more backwards compatible conversions and an include to make sure it gets into the old header location 1.9.21 (2012-11-04 01:19) ------------------------- * filling out deprecated functions for backwards compatability 1.9.20 (2012-11-02) ------------------- 1.9.19 (2012-10-31) ------------------- * Removed deprecated 'brief' attribute from tags. 1.9.18 (2012-10-16) ------------------- 1.9.17 (2012-10-02) ------------------- * fix several dependency issues 1.9.16 (2012-09-29) ------------------- * adding geometry metapackage and updating to 1.9.16 1.9.15 (2012-09-30) ------------------- * fix a few dependency/catkin problems * remove old API files * comply to the new catkin API 1.9.14 (2012-09-18) ------------------- 1.9.13 (2012-09-17) ------------------- * update manifests 1.9.12 (2012-09-16) ------------------- 1.9.11 (2012-09-14 22:49) ------------------------- 1.9.10 (2012-09-14 22:30) ------------------------- 1.9.9 (2012-09-11) ------------------ * update depends * minor patches for new build system 1.9.8 (2012-09-03) ------------------ 1.9.7 (2012-08-10 12:19) ------------------------ * minor build fixes * fixed some minor errors from last commit * completed set of eigen conversions; added KDL conversions * adding additional conversion functions 1.9.6 (2012-08-02 19:59) ------------------------ 1.9.5 (2012-08-02 19:48) ------------------------ 1.9.4 (2012-08-02 18:29) ------------------------ 1.9.3 (2012-08-02 18:28) ------------------------ * forgot to install some things * also using DEPENDS 1.9.2 (2012-08-01 21:05) ------------------------ * make sure the tf target depends on the messages (and clean some include_directories too) 1.9.1 (2012-08-01 19:16) ------------------------ * install manifest.xml 1.9.0 (2012-08-01 18:52) ------------------------ * catkin build system * successfully running rosrun tf bullet_migration_sed.py and testing afterwords * eigen to rosdep from dependency * removing eigen dependency as it's now system installed * add missing empty_listener.cpp file * compiling with eigen3 * more extensive search * applying patch from sed script for eigen3 compatability * tests for tf_kdl and fixes for tf_kdl based on tests * add pykdl to example * link to kdl pages * Added VectorEigenToTF and RotationEigenToTF to tf_conversions * returning to camelCase for consistency with tf and pykdl * converting from camelCase to under_scored methods for python style * Added Ubuntu platform tags * removing pykdl finishing series of commits for `#4039 `_ * promoting pykdl index.rst * removing index.rst for replacing * posemath using kdl promoted * reverting change in test * passing test with kdl_posemath.py copied to src/posemath.py * Corrected module to tf_conversions * Improved pose comparison in test_roundtrip * `#4039 `_ original posemath now in tf_conversions * Enable posemath unit test, `#4039 `_ * Moved PoseMath from tf to tf_conversions, `#4039 `_ * PyKDL based PoseMath, `#4039 `_ * fixes for `#3915 `_ into trunk * Remove use of deprecated rosbuild macros * tf conversions is doc reviewed * api cleared * add list of supported data types * deprecate addDelta function because it is not a conversion * add api doc to tf_conversions * update documentation * migration part 1 geometry-1.13.2/tf_conversions/CMakeLists.txt000066400000000000000000000026371366756125100212560ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf_conversions) find_package(orocos_kdl REQUIRED) find_package(catkin REQUIRED geometry_msgs kdl_conversions tf) find_package(Eigen3 REQUIRED) catkin_python_setup() catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} DEPENDS EIGEN3 orocos_kdl CATKIN_DEPENDS geometry_msgs kdl_conversions tf ) include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) # Needed due to no full filename in orocos_kdl pkg-config export link_directories(${orocos_kdl_LIBRARY_DIRS}) add_library(${PROJECT_NAME} src/tf_kdl.cpp src/tf_eigen.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) # Tests if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_eigen_tf test/test_eigen_tf.cpp) target_link_libraries(test_eigen_tf ${PROJECT_NAME} ${orocos_kdl_LIBRARIES}) catkin_add_gtest(test_kdl_tf test/test_kdl_tf.cpp) if(CMAKE_COMPILER_IS_GNUCXX) set_target_properties(test_kdl_tf PROPERTIES COMPILE_FLAGS "-Wno-deprecated-declarations") endif() target_link_libraries(test_kdl_tf ${PROJECT_NAME} ${orocos_kdl_LIBRARIES}) catkin_add_nosetests(test/posemath.py) endif() install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) install(TARGETS ${PROJECT_NAME} DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) geometry-1.13.2/tf_conversions/conf.py000066400000000000000000000151471366756125100200150ustar00rootroot00000000000000# -*- coding: utf-8 -*- # # tf documentation build configuration file, created by # sphinx-quickstart on Mon Jun 1 14:21:53 2009. # # This file is execfile()d with the current directory set to its containing dir. # # Note that not all possible configuration values are present in this # autogenerated file. # # All configuration values have a default; values that are commented out # serve to show the default. import sys, os # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. #sys.path.append(os.path.abspath('.')) # -- General configuration ----------------------------------------------------- # Add any Sphinx extension module names here, as strings. They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] # The suffix of source filenames. source_suffix = '.rst' # The encoding of source files. #source_encoding = 'utf-8' # The master toctree document. master_doc = 'index' # General information about the project. project = u'tf_conversions' copyright = u'2010, Willow Garage, Inc.' # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. # # The short X.Y version. version = '0.1' # The full version, including alpha/beta/rc tags. release = '0.1.0' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. #language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: #today = '' # Else, today_fmt is used as the format for a strftime call. #today_fmt = '%B %d, %Y' # List of documents that shouldn't be included in the build. #unused_docs = [] # List of directories, relative to source directory, that shouldn't be searched # for source files. exclude_trees = ['_build'] # The reST default role (used for this markup: `text`) to use for all documents. #default_role = None # If true, '()' will be appended to :func: etc. cross-reference text. #add_function_parentheses = True # If true, the current module name will be prepended to all description # unit titles (such as .. function::). #add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. #show_authors = False # The name of the Pygments (syntax highlighting) style to use. pygments_style = 'sphinx' # A list of ignored prefixes for module index sorting. #modindex_common_prefix = [] # -- Options for HTML output --------------------------------------------------- # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. html_theme = 'default' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the # documentation. #html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. #html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". #html_title = None # A shorter title for the navigation bar. Default is the same as html_title. #html_short_title = None # The name of an image file (relative to this directory) to place at the top # of the sidebar. #html_logo = None # The name of an image file (within the static path) to use as favicon of the # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. #html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ['_static'] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. #html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. #html_use_smartypants = True # Custom sidebar templates, maps document names to template names. #html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. #html_additional_pages = {} # If false, no module index is generated. #html_use_modindex = True # If false, no index is generated. #html_use_index = True # If true, the index is split into individual pages for each letter. #html_split_index = False # If true, links to the reST sources are added to the pages. #html_show_sourcelink = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. #html_use_opensearch = '' # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). #html_file_suffix = '' # Output file base name for HTML help builder. htmlhelp_basename = 'tfdoc' # -- Options for LaTeX output -------------------------------------------------- # The paper size ('letter' or 'a4'). #latex_paper_size = 'letter' # The font size ('10pt', '11pt' or '12pt'). #latex_font_size = '10pt' # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, author, documentclass [howto/manual]). latex_documents = [ ('index', 'tf_conversions.tex', u'stereo\\_utils Documentation', u'James Bowman', 'manual'), ] # The name of an image file (relative to this directory) to place at the top of # the title page. #latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. #latex_use_parts = False # Additional stuff for the LaTeX preamble. #latex_preamble = '' # Documents to append as an appendix to all manuals. #latex_appendices = [] # If false, no module index is generated. #latex_use_modindex = True # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = { 'http://docs.python.org/': None, 'http://docs.opencv.org/3.0-last-rst/': None, 'http://www.ros.org/doc/api/tf/html/python/' : None, 'http://docs.scipy.org/doc/numpy' : None, 'http://www.ros.org/doc/api/kdl/html/python/' : None } geometry-1.13.2/tf_conversions/include/000077500000000000000000000000001366756125100201315ustar00rootroot00000000000000geometry-1.13.2/tf_conversions/include/tf_conversions/000077500000000000000000000000001366756125100231725ustar00rootroot00000000000000geometry-1.13.2/tf_conversions/include/tf_conversions/mainpage.dox000066400000000000000000000007251366756125100254730ustar00rootroot00000000000000/** \mainpage @htmlinclude manifest.html The tf conversions package provide methods to convert between datatypes used by tf, and semantically identical data types of other libraries. The package now supports KDL and Eigen. The supported KDL data types are: \li Frame \li Vector \li Rotation \li Twist The supported Eigen data types are: \li Affine3d \li Vector3d \li Quaterniond For a complete list of available conversion methods, follow this link: tf */ geometry-1.13.2/tf_conversions/include/tf_conversions/tf_eigen.h000066400000000000000000000066601366756125100251330ustar00rootroot00000000000000/* * Copyright (c) 2009-2012, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ // Author: Adam Leeper (and others) #ifndef CONVERSIONS_TF_EIGEN_H #define CONVERSIONS_TF_EIGEN_H #include "tf/transform_datatypes.h" #include "Eigen/Core" #include "Eigen/Geometry" namespace tf { /// Converts a tf Matrix3x3 into an Eigen Quaternion void matrixTFToEigen(const tf::Matrix3x3 &t, Eigen::Matrix3d &e); /// Converts an Eigen Quaternion into a tf Matrix3x3 void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t); /// Converts a tf Pose into an Eigen Affine3d void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e); /// Converts a tf Pose into an Eigen Isometry3d void poseTFToEigen(const tf::Pose &t, Eigen::Isometry3d &e); /// Converts an Eigen Affine3d into a tf Transform void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t); /// Converts an Eigen Isometry3d into a tf Transform void poseEigenToTF(const Eigen::Isometry3d &e, tf::Pose &t); /// Converts a tf Quaternion into an Eigen Quaternion void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e); /// Converts an Eigen Quaternion into a tf Quaternion void quaternionEigenToTF(const Eigen::Quaterniond &e, tf::Quaternion &t); /// Converts a tf Transform into an Eigen Affine3d void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e); /// Converts a tf Transform into an Eigen Isometry3d void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e); /// Converts an Eigen Affine3d into a tf Transform void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t); /// Converts an Eigen Isometry3d into a tf Transform void transformEigenToTF(const Eigen::Isometry3d &e, tf::Transform &t); /// Converts a tf Vector3 into an Eigen Vector3d void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3d &e); /// Converts an Eigen Vector3d into a tf Vector3 void vectorEigenToTF(const Eigen::Vector3d &e, tf::Vector3 &t); } // namespace tf #endif geometry-1.13.2/tf_conversions/include/tf_conversions/tf_kdl.h000066400000000000000000000112311366756125100246040ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef CONVERSIONS_TF_KDL_H #define CONVERSIONS_TF_KDL_H #include "kdl_conversions/kdl_msg.h" //backwards compatability for deprecated methods #include "tf/transform_datatypes.h" #include "kdl/frames.hpp" #include #include #include namespace tf { /// Converts a tf Pose into a KDL Frame void poseTFToKDL(const tf::Pose &t, KDL::Frame &k); /// Converts a KDL Frame into a tf Pose void poseKDLToTF(const KDL::Frame &k, tf::Pose &t); /// Converts a tf Quaternion into a KDL Rotation void quaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k); /// Converts a tf Quaternion into a KDL Rotation void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t); /// Converts a tf Transform into a KDL Frame void transformTFToKDL(const tf::Transform &t, KDL::Frame &k); /// Converts a KDL Frame into a tf Transform void transformKDLToTF(const KDL::Frame &k, tf::Transform &t); /// Converts a tf Vector3 into a KDL Vector void vectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k); /// Converts a tf Vector3 into a KDL Vector void vectorKDLToTF(const KDL::Vector &k, tf::Vector3 &t); /* DEPRECATED FUNCTIONS */ /// Starting from a Pose from A to B, apply a Twist with reference frame A and reference point B, during a time t. ROS_DEPRECATED geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t); /// Converts a tf Pose into a KDL Frame ROS_DEPRECATED void PoseTFToKDL(const tf::Pose &t, KDL::Frame &k); void inline PoseTFToKDL(const tf::Pose &t, KDL::Frame &k) {poseTFToKDL(t, k);}; /// Converts a KDL Frame into a tf Pose ROS_DEPRECATED void PoseKDLToTF(const KDL::Frame &k, tf::Pose &t); void inline PoseKDLToTF(const KDL::Frame &k, tf::Pose &t) {poseKDLToTF(k, t);}; /// Converts a tf Quaternion into a KDL Rotation ROS_DEPRECATED void QuaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k); void inline QuaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k) {quaternionTFToKDL(t, k);}; /// Converts a tf Quaternion into a KDL Rotation ROS_DEPRECATED void QuaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t); void inline QuaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t) {quaternionKDLToTF(k, t);}; /// Converts a tf Transform into a KDL Frame ROS_DEPRECATED void TransformTFToKDL(const tf::Transform &t, KDL::Frame &k); void inline TransformTFToKDL(const tf::Transform &t, KDL::Frame &k) {transformTFToKDL(t, k);}; /// Converts a KDL Frame into a tf Transform ROS_DEPRECATED void TransformKDLToTF(const KDL::Frame &k, tf::Transform &t); void inline TransformKDLToTF(const KDL::Frame &k, tf::Transform &t) {transformKDLToTF(k, t);}; /// Converts a tf Vector3 into a KDL Vector ROS_DEPRECATED void VectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k); void inline VectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k) {vectorTFToKDL(t, k);}; /// Converts a tf Vector3 into a KDL Vector ROS_DEPRECATED void VectorKDLToTF(const KDL::Vector &k, tf::Vector3 &t); void inline VectorKDLToTF(const KDL::Vector &k, tf::Vector3 &t) {vectorKDLToTF(k, t);}; } // namespace tf #endif geometry-1.13.2/tf_conversions/index.rst000066400000000000000000000021531366756125100203500ustar00rootroot00000000000000tf_conversions ============== .. toctree:: :maxdepth: 2 PoseMath -------- PoseMath is a utility that makes it easy to work with :class:`PyKDL.Frame`'s. It can work with poses from a variety of sources: :meth:`tf.Transformer.lookupTransform`, :mod:`opencv` and ROS messages. It has utility functions to convert between these types and the :class:`PyKDL.Frame` pose representation. .. doctest:: :options: -ELLIPSIS, +NORMALIZE_WHITESPACE >>> from geometry_msgs.msg import Pose >>> import tf_conversions.posemath as pm >>> import PyKDL >>> >>> msg = Pose() >>> msg.position.x = 7.0 >>> msg.orientation.w = 1.0 >>> >>> frame = PyKDL.Frame(PyKDL.Rotation.RPY(2, 0, 1), PyKDL.Vector(1,2,4)) >>> >>> res = pm.toTf(pm.fromMsg(msg) * frame) >>> print res ((8.0, 2.0, 4.0), (0.73846026260412856, 0.40342268011133486, 0.25903472399992566, 0.4741598817790379)) .. automodule:: tf_conversions.posemath :members: fromTf, fromMsg, toMsg, fromMatrix, toMatrix, fromCameraParams Indices and tables ================== * :ref:`genindex` * :ref:`modindex` * :ref:`search` geometry-1.13.2/tf_conversions/package.xml000066400000000000000000000026721366756125100206320ustar00rootroot00000000000000 tf_conversions 1.13.2 This package contains a set of conversion functions to convert common tf datatypes (point, vector, pose, etc) into semantically identical datatypes used by other libraries. The conversion functions make it easier for users of the transform library (tf) to work with the datatype of their choice. Currently this package has support for the Kinematics and Dynamics Library (KDL) and the Eigen matrix library. This package is stable, and will get integrated into tf in the next major release cycle (see roadmap). Tully Foote Tully Foote BSD http://www.ros.org/wiki/tf_conversions catkin eigen geometry_msgs kdl_conversions liborocos-kdl-dev tf liborocos-kdl-dev eigen geometry_msgs kdl_conversions liborocos-kdl python3-pykdl tf geometry-1.13.2/tf_conversions/rosdoc.yaml000066400000000000000000000002731366756125100206650ustar00rootroot00000000000000 - builder: rosmake - builder: sphinx name: Python API output_dir: python - builder: doxygen name: C++ API output_dir: c++ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' geometry-1.13.2/tf_conversions/setup.py000066400000000000000000000004121366756125100202150ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['tf_conversions'], package_dir={'': 'src'}, requires=['geometry_msgs', 'rospy', 'tf'] ) setup(**d) geometry-1.13.2/tf_conversions/src/000077500000000000000000000000001366756125100172755ustar00rootroot00000000000000geometry-1.13.2/tf_conversions/src/tf_conversions/000077500000000000000000000000001366756125100223365ustar00rootroot00000000000000geometry-1.13.2/tf_conversions/src/tf_conversions/__init__.py000066400000000000000000000031561366756125100244540ustar00rootroot00000000000000# Copyright (c) 2010, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from __future__ import absolute_import from .posemath import * geometry-1.13.2/tf_conversions/src/tf_conversions/posemath.py000066400000000000000000000134351366756125100245360ustar00rootroot00000000000000# Copyright (c) 2010, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from geometry_msgs.msg import Pose, Point, Quaternion from tf import transformations import tf import rospy import numpy from PyKDL import * def fromTf(tf): """ :param tf: :class:`tf.Transformer` transform :type tf: tuple (translation, quaternion) :return: New :class:`PyKDL.Frame` object Convert a pose returned by :meth:`tf.Transformer.lookupTransform` to a :class:`PyKDL.Frame`. .. doctest:: >>> import rospy >>> import tf >>> import geometry_msgs.msg >>> t = tf.Transformer(True, rospy.Duration(10.0)) >>> m = geometry_msgs.msg.TransformStamped() >>> m.header.frame_id = 'THISFRAME' >>> m.child_frame_id = 'CHILD' >>> m.transform.translation.x = 668.5 >>> m.transform.rotation.w = 1.0 >>> t.setTransform(m) >>> t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0)) ((668.5, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) >>> import tf_conversions.posemath as pm >>> p = pm.fromTf(t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0))) >>> print(pm.toMsg(p * p)) position: x: 1337.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 """ position, quaternion = tf x, y, z = position Qx, Qy, Qz, Qw = quaternion return Frame(Rotation.Quaternion(Qx, Qy, Qz, Qw), Vector(x, y, z)) def toTf(f): """ :param f: input pose :type f: :class:`PyKDL.Frame` Return a tuple (position, quaternion) for the pose. """ return ((f.p[0], f.p[1], f.p[2]), f.M.GetQuaternion()) # to and from pose message def fromMsg(p): """ :param p: input pose :type p: :class:`geometry_msgs.msg.Pose` :return: New :class:`PyKDL.Frame` object Convert a pose represented as a ROS Pose message to a :class:`PyKDL.Frame`. """ return Frame(Rotation.Quaternion(p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w), Vector(p.position.x, p.position.y, p.position.z)) def toMsg(f): """ :param f: input pose :type f: :class:`PyKDL.Frame` Return a ROS Pose message for the Frame f. """ p = Pose() p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w = f.M.GetQuaternion() p.position.x = f.p[0] p.position.y = f.p[1] p.position.z = f.p[2] return p # to and from matrix def fromMatrix(m): """ :param m: input 4x4 matrix :type m: :func:`numpy.array` :return: New :class:`PyKDL.Frame` object Convert a pose represented as a 4x4 numpy array to a :class:`PyKDL.Frame`. """ return Frame(Rotation(m[0,0], m[0,1], m[0,2], m[1,0], m[1,1], m[1,2], m[2,0], m[2,1], m[2,2]), Vector(m[0,3], m[1, 3], m[2, 3])) def toMatrix(f): """ :param f: input pose :type f: :class:`PyKDL.Frame` Return a numpy 4x4 array for the Frame F. """ return numpy.array([[f.M[0,0], f.M[0,1], f.M[0,2], f.p[0]], [f.M[1,0], f.M[1,1], f.M[1,2], f.p[1]], [f.M[2,0], f.M[2,1], f.M[2,2], f.p[2]], [0,0,0,1]]) # from camera parameters def fromCameraParams(cv, rvec, tvec): """ :param cv: OpenCV module :param rvec: A Rodrigues rotation vector - see :func:`Rodrigues2` :type rvec: 3x1 :class:`CvMat` :param tvec: A translation vector :type tvec: 3x1 :class:`CvMat` :return: New :class:`PyKDL.Frame` object For use with :func:`FindExtrinsicCameraParams2`:: import cv import tf_conversions.posemath as pm ... rvec = cv.CreateMat(3, 1, cv.CV_32FC1) tvec = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rvec, tvec) pose = pm.fromCameraParams(cv, rvec, tvec) """ m = numpy.array([ [ 0, 0, 0, tvec[0,0] ], [ 0, 0, 0, tvec[1,0] ], [ 0, 0, 0, tvec[2,0] ], [ 0, 0, 0, 1.0 ] ], dtype = numpy.float32) cv.Rodrigues2(rvec, m[:3,:3]) return fromMatrix(m) geometry-1.13.2/tf_conversions/src/tf_eigen.cpp000066400000000000000000000103641366756125100215650ustar00rootroot00000000000000/* * Copyright (c) 2009-2012, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ // Author: Adam Leeper, Stuart Glaser #include "tf_conversions/tf_eigen.h" namespace tf { void matrixTFToEigen(const tf::Matrix3x3 &t, Eigen::Matrix3d &e) { for(int i=0; i<3; i++) for(int j=0; j<3; j++) e(i,j) = t[i][j]; } void matrixEigenToTF(const Eigen::Matrix3d &e, tf::Matrix3x3 &t) { for(int i=0; i<3; i++) for(int j=0; j<3; j++) t[i][j] = e(i,j); } void poseTFToEigen(const tf::Pose &t, Eigen::Affine3d &e) { transformTFToEigen(t, e); } void poseTFToEigen(const tf::Pose &t, Eigen::Isometry3d &e) { transformTFToEigen(t, e); } void poseEigenToTF(const Eigen::Affine3d &e, tf::Pose &t) { transformEigenToTF(e, t); } void poseEigenToTF(const Eigen::Isometry3d &e, tf::Pose &t) { transformEigenToTF(e, t); } void quaternionTFToEigen(const tf::Quaternion& t, Eigen::Quaterniond& e) { e = Eigen::Quaterniond(t[3],t[0],t[1],t[2]); } void quaternionEigenToTF(const Eigen::Quaterniond& e, tf::Quaternion& t) { t[0] = e.x(); t[1] = e.y(); t[2] = e.z(); t[3] = e.w(); } namespace { template void transformTFToEigenImpl(const tf::Transform &t, Transform & e) { for(int i=0; i<3; i++) { e.matrix()(i,3) = t.getOrigin()[i]; for(int j=0; j<3; j++) { e.matrix()(i,j) = t.getBasis()[i][j]; } } // Fill in identity in last row for (int col = 0 ; col < 3; col ++) e.matrix()(3, col) = 0; e.matrix()(3,3) = 1; } template void transformEigenToTFImpl(const T &e, tf::Transform &t) { t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3))); t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1), e.matrix()(0,2), e.matrix()(1,0), e.matrix()(1,1), e.matrix()(1,2), e.matrix()(2,0), e.matrix()(2,1), e.matrix()(2,2))); } } void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e) { transformTFToEigenImpl(t, e); }; void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e) { transformTFToEigenImpl(t, e); }; void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t) { transformEigenToTFImpl(e, t); } void transformEigenToTF(const Eigen::Isometry3d &e, tf::Transform &t) { transformEigenToTFImpl(e, t); } void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3d& e) { e(0) = t[0]; e(1) = t[1]; e(2) = t[2]; } void vectorEigenToTF(const Eigen::Vector3d& e, tf::Vector3& t) { t[0] = e(0); t[1] = e(1); t[2] = e(2); } } // namespace tf geometry-1.13.2/tf_conversions/src/tf_kdl.cpp000066400000000000000000000073331366756125100212520ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include "tf_conversions/tf_kdl.h" #include "kdl_conversions/kdl_msg.h" namespace tf { void poseTFToKDL(const tf::Pose& t, KDL::Frame& k) { for (unsigned int i = 0; i < 3; ++i) k.p[i] = t.getOrigin()[i]; for (unsigned int i = 0; i < 9; ++i) k.M.data[i] = t.getBasis()[i/3][i%3]; } void poseKDLToTF(const KDL::Frame& k, tf::Pose& t) { t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2])); t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2], k.M.data[3], k.M.data[4], k.M.data[5], k.M.data[6], k.M.data[7], k.M.data[8])); } void quaternionTFToKDL(const tf::Quaternion& t, KDL::Rotation& k) { k = KDL::Rotation::Quaternion(t[0], t[1], t[2], t[3]); } void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t) { double x, y, z, w; k.GetQuaternion(x, y, z, w); t = tf::Quaternion(x, y, z, w); } void transformTFToKDL(const tf::Transform &t, KDL::Frame &k) { for (unsigned int i = 0; i < 3; ++i) k.p[i] = t.getOrigin()[i]; for (unsigned int i = 0; i < 9; ++i) k.M.data[i] = t.getBasis()[i/3][i%3]; } void transformKDLToTF(const KDL::Frame &k, tf::Transform &t) { t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2])); t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2], k.M.data[3], k.M.data[4], k.M.data[5], k.M.data[6], k.M.data[7], k.M.data[8])); } void vectorTFToKDL(const tf::Vector3& t, KDL::Vector& k) { k[0] = t[0]; k[1] = t[1]; k[2] = t[2]; } void vectorKDLToTF(const KDL::Vector& k, tf::Vector3& t) { t[0] = k[0]; t[1] = k[1]; t[2] = k[2]; } /* DEPRECATED FUNCTIONS */ geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t) { geometry_msgs::Pose result; KDL::Twist kdl_twist; KDL::Frame kdl_pose_id, kdl_pose; poseMsgToKDL(pose,kdl_pose); twistMsgToKDL(twist,kdl_twist); kdl_pose = KDL::addDelta(kdl_pose_id,kdl_twist,t)*kdl_pose; poseKDLToMsg(kdl_pose,result); return result; } } // namespace tf geometry-1.13.2/tf_conversions/test/000077500000000000000000000000001366756125100174655ustar00rootroot00000000000000geometry-1.13.2/tf_conversions/test/posemath.py000066400000000000000000000024021366756125100216550ustar00rootroot00000000000000import rostest import rospy import numpy import unittest import sys from tf import Transformer import tf_conversions.posemath as pm from geometry_msgs.msg import TransformStamped from PyKDL import Frame class TestPoseMath(unittest.TestCase): def setUp(self): pass def test_fromTf(self): transformer = Transformer(True, rospy.Duration(10.0)) m = TransformStamped() m.header.frame_id = 'wim' m.child_frame_id = 'james' m.transform.translation.x = 2.71828183 m.transform.rotation.w = 1.0 transformer.setTransform(m) b = pm.fromTf(transformer.lookupTransform('wim', 'james', rospy.Time(0))) def test_roundtrip(self): c = Frame() d = pm.fromMsg(pm.toMsg(c)) self.assertEqual(repr(c), repr(d)) d = pm.fromMatrix(pm.toMatrix(c)) self.assertEqual(repr(c), repr(d)) d = pm.fromTf(pm.toTf(c)) self.assertEqual(repr(c), repr(d)) if __name__ == '__main__': if len(sys.argv) == 1 or sys.argv[1].startswith('--gtest_output'): rostest.unitrun('tf', 'directed', TestPoseMath) else: suite = unittest.TestSuite() suite.addTest(TestPoseMath(sys.argv[1])) unittest.TextTestRunner(verbosity=2).run(suite) geometry-1.13.2/tf_conversions/test/test_eigen_tf.cpp000066400000000000000000000111361366756125100230120ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include using namespace tf; double gen_rand(double min, double max) { int rand_num = rand()%100+1; double result = min + (double)((max-min)*rand_num)/101.0; return result; } TEST(TFEigenConversions, tf_eigen_vector) { tf::Vector3 t; t[0] = gen_rand(-10,10); t[1] = gen_rand(-10,10); t[2] = gen_rand(-10,10); Eigen::Vector3d k; vectorTFToEigen(t,k); ASSERT_NEAR(t[0],k[0],1e-6); ASSERT_NEAR(t[1],k[1],1e-6); ASSERT_NEAR(t[2],k[2],1e-6); } TEST(TFEigenConversions, tf_eigen_quaternion) { tf::Quaternion t; t[0] = gen_rand(-1.0,1.0); t[1] = gen_rand(-1.0,1.0); t[2] = gen_rand(-1.0,1.0); t[3] = gen_rand(-1.0,1.0); t.normalize(); Eigen::Quaterniond k; quaternionTFToEigen(t,k); ASSERT_NEAR(t[0],k.coeffs()(0),1e-6); ASSERT_NEAR(t[1],k.coeffs()(1),1e-6); ASSERT_NEAR(t[2],k.coeffs()(2),1e-6); ASSERT_NEAR(t[3],k.coeffs()(3),1e-6); ASSERT_NEAR(k.norm(),1.0,1e-10); } TEST(TFEigenConversions, tf_eigen_transform) { tf::Transform t; tf::Quaternion tq; tq[0] = gen_rand(-1.0,1.0); tq[1] = gen_rand(-1.0,1.0); tq[2] = gen_rand(-1.0,1.0); tq[3] = gen_rand(-1.0,1.0); tq.normalize(); t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); t.setRotation(tq); Eigen::Affine3d affine; Eigen::Isometry3d isometry; transformTFToEigen(t, affine); transformTFToEigen(t, isometry); for(int i=0; i < 3; i++) { ASSERT_NEAR(t.getOrigin()[i],affine.matrix()(i,3),1e-6); ASSERT_NEAR(t.getOrigin()[i],isometry.matrix()(i,3),1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t.getBasis()[i][j],affine.matrix()(i,j),1e-6); ASSERT_NEAR(t.getBasis()[i][j],isometry.matrix()(i,j),1e-6); } } for (int col = 0 ; col < 3; col ++) { ASSERT_NEAR(affine.matrix()(3, col), 0, 1e-6); ASSERT_NEAR(isometry.matrix()(3, col), 0, 1e-6); } ASSERT_NEAR(affine.matrix()(3,3), 1, 1e-6); ASSERT_NEAR(isometry.matrix()(3,3), 1, 1e-6); } TEST(TFEigenConversions, eigen_tf_transform) { tf::Transform t1; tf::Transform t2; Eigen::Affine3d affine; Eigen::Isometry3d isometry; Eigen::Quaterniond kq; kq.coeffs()(0) = gen_rand(-1.0,1.0); kq.coeffs()(1) = gen_rand(-1.0,1.0); kq.coeffs()(2) = gen_rand(-1.0,1.0); kq.coeffs()(3) = gen_rand(-1.0,1.0); kq.normalize(); isometry.setIdentity(); isometry.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); isometry.rotate(kq); affine = isometry; transformEigenToTF(affine,t1); transformEigenToTF(isometry,t2); for(int i=0; i < 3; i++) { ASSERT_NEAR(t1.getOrigin()[i],affine.matrix()(i,3),1e-6); ASSERT_NEAR(t2.getOrigin()[i],isometry.matrix()(i,3),1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t1.getBasis()[i][j],affine.matrix()(i,j),1e-6); ASSERT_NEAR(t2.getBasis()[i][j],isometry.matrix()(i,j),1e-6); } } } int main(int argc, char **argv){ /* initialize random seed: */ srand ( time(NULL) ); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry-1.13.2/tf_conversions/test/test_kdl_tf.cpp000066400000000000000000000130221366756125100224710ustar00rootroot00000000000000/* * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include using namespace tf; double gen_rand(double min, double max) { int rand_num = rand()%100+1; double result = min + (double)((max-min)*rand_num)/101.0; return result; } TEST(TFKDLConversions, tf_kdl_vector) { tf::Vector3 t; t[0] = gen_rand(-10,10); t[1] = gen_rand(-10,10); t[2] = gen_rand(-10,10); KDL::Vector k; VectorTFToKDL(t,k); ASSERT_NEAR(t[0],k[0],1e-6); ASSERT_NEAR(t[1],k[1],1e-6); ASSERT_NEAR(t[2],k[2],1e-6); } TEST(TFKDLConversions, tf_kdl_rotation) { tf::Quaternion t; t[0] = gen_rand(-1.0,1.0); t[1] = gen_rand(-1.0,1.0); t[2] = gen_rand(-1.0,1.0); t[3] = gen_rand(-1.0,1.0); t.normalize(); KDL::Rotation k; QuaternionTFToKDL(t,k); double x,y,z,w; k.GetQuaternion(x,y,z,w); ASSERT_NEAR(fabs(t[0]),fabs(x),1e-6); ASSERT_NEAR(fabs(t[1]),fabs(y),1e-6); ASSERT_NEAR(fabs(t[2]),fabs(z),1e-6); ASSERT_NEAR(fabs(t[3]),fabs(w),1e-6); ASSERT_NEAR(t[0]*t[1]*t[2]*t[3],x*y*z*w,1e-6); } TEST(TFKDLConversions, tf_kdl_transform) { tf::Transform t; tf::Quaternion tq; tq[0] = gen_rand(-1.0,1.0); tq[1] = gen_rand(-1.0,1.0); tq[2] = gen_rand(-1.0,1.0); tq[3] = gen_rand(-1.0,1.0); tq.normalize(); t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); t.setRotation(tq); //test tf->kdl KDL::Frame k; TransformTFToKDL(t,k); for(int i=0; i < 3; i++) { ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6); } } //test kdl->tf tf::Transform r; TransformKDLToTF(k,r); for(int i=0; i< 3; i++) { ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6); } } } TEST(TFKDLConversions, tf_kdl_pose) { tf::Pose t; tf::Quaternion tq; tq[0] = gen_rand(-1.0,1.0); tq[1] = gen_rand(-1.0,1.0); tq[2] = gen_rand(-1.0,1.0); tq[3] = gen_rand(-1.0,1.0); tq.normalize(); t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); t.setRotation(tq); //test tf->kdl KDL::Frame k; PoseTFToKDL(t,k); for(int i=0; i < 3; i++) { ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6); } } //test kdl->tf tf::Pose r; PoseKDLToTF(k,r); for(int i=0; i< 3; i++) { ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6); } } } TEST(TFKDLConversions, msg_kdl_twist) { geometry_msgs::Twist m; m.linear.x = gen_rand(-10,10); m.linear.y = gen_rand(-10,10); m.linear.z = gen_rand(-10,10); m.angular.x = gen_rand(-10,10); m.angular.y = gen_rand(-10,10); m.angular.y = gen_rand(-10,10); //test msg->kdl KDL::Twist t; TwistMsgToKDL(m,t); ASSERT_NEAR(m.linear.x,t.vel[0],1e-6); ASSERT_NEAR(m.linear.y,t.vel[1],1e-6); ASSERT_NEAR(m.linear.z,t.vel[2],1e-6); ASSERT_NEAR(m.angular.x,t.rot[0],1e-6); ASSERT_NEAR(m.angular.y,t.rot[1],1e-6); ASSERT_NEAR(m.angular.z,t.rot[2],1e-6); //test msg->kdl geometry_msgs::Twist r; TwistKDLToMsg(t,r); ASSERT_NEAR(r.linear.x,t.vel[0],1e-6); ASSERT_NEAR(r.linear.y,t.vel[1],1e-6); ASSERT_NEAR(r.linear.z,t.vel[2],1e-6); ASSERT_NEAR(r.angular.x,t.rot[0],1e-6); ASSERT_NEAR(r.angular.y,t.rot[1],1e-6); ASSERT_NEAR(r.angular.z,t.rot[2],1e-6); } int main(int argc, char **argv){ /* initialize random seed: */ srand ( time(NULL) ); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }