├── .gitignore ├── srv └── FrameGraph.srv ├── msg └── tfMessage.msg ├── doc ├── bifrucation.pdf └── bifrucation.gv ├── test ├── test_message_filter.xml ├── transform_twist_test.launch ├── transform_listener_unittest.launch ├── test_broadcaster.launch ├── method_test.py ├── test_transform_datatypes.cpp ├── testListener.cpp ├── tf_unittest_future.cpp ├── operator_overload.cpp ├── transform_listener_unittest.cpp ├── testBroadcaster.cpp ├── quaternion.cpp ├── python_debug_test.py ├── speed_test.cpp ├── tf_benchmark.cpp ├── test_datatype_conversion.py └── testPython.py ├── index.rst ├── rosdoc.yaml ├── scripts ├── groovy_compatibility │ ├── tf_remap │ └── view_frames ├── tf_remap ├── bullet_migration_sed.py ├── python_benchmark.py └── view_frames ├── remap_tf.launch ├── setup.py ├── README.md ├── include └── tf │ ├── LinearMath │ ├── MinMax.h │ ├── QuadWord.h │ └── Transform.h │ ├── exceptions.h │ ├── transform_broadcaster.h │ ├── time_cache.h │ ├── transform_listener.h │ └── transform_datatypes.h ├── src ├── tf │ ├── __init__.py │ ├── broadcaster.py │ └── tfwtf.py ├── empty_listener.cpp ├── transform_broadcaster.cpp ├── tf_echo.cpp ├── static_transform_publisher.cpp ├── change_notifier.cpp ├── cache.cpp ├── transform_listener.cpp └── tf_monitor.cpp ├── package.xml ├── mainpage.dox ├── CMakeLists.txt └── conf.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | 3 | -------------------------------------------------------------------------------- /srv/FrameGraph.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string dot_graph 3 | -------------------------------------------------------------------------------- /msg/tfMessage.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/TransformStamped[] transforms 2 | -------------------------------------------------------------------------------- /doc/bifrucation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davheld/tf/HEAD/doc/bifrucation.pdf -------------------------------------------------------------------------------- /test/test_message_filter.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /test/transform_twist_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /test/transform_listener_unittest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /test/test_broadcaster.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /index.rst: -------------------------------------------------------------------------------- 1 | tf 2 | == 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | tf_python 8 | 9 | Indices and tables 10 | ================== 11 | 12 | * :ref:`genindex` 13 | * :ref:`modindex` 14 | * :ref:`search` 15 | 16 | -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: rosmake 2 | - builder: sphinx 3 | name: Python API 4 | output_dir: python 5 | - builder: doxygen 6 | name: C++ API 7 | output_dir: c++ 8 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' 9 | -------------------------------------------------------------------------------- /scripts/groovy_compatibility/tf_remap: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | echo "Please use the package local script inside tf not the global one, this is deprecated." 4 | echo "Running [rosrun tf tf_remap] for you" 5 | exec rosrun tf tf_remap "$@" -------------------------------------------------------------------------------- /scripts/groovy_compatibility/view_frames: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | echo "Please use the package local script inside tf not the global one, this is deprecated." 4 | echo "Running [rosrun tf view_frames] for you" 5 | exec rosrun tf view_frames "$@" -------------------------------------------------------------------------------- /remap_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | - {old: "/asdf", 5 | new: "/a"} 6 | - {old: "/fdsa", 7 | new: "/b"} 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['tf'], 8 | package_dir={'': 'src'}, 9 | requires=['genmsg', 'genpy', 'roslib', 'rospkg', 'geometry_msgs', 'sensor_msgs', 'std_msgs'], 10 | scripts=['scripts/groovy_compatibility/tf_remap', 11 | 'scripts/groovy_compatibility/view_frames'] 12 | ) 13 | 14 | setup(**d) 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | tf 2 | == 3 | 4 | A custom version of the ros/tf package, with small API changes. Used by some the our code, until we migrate to TF2. 5 | 6 | Inserted a class in the class hierarchy between Transformer and 7 | TransformListener. This class serves to decouple the 2 functions 8 | of the original TransformListener class: 9 | - a bunch of helper functions to apply a transform to some data structures 10 | - listen to tf messages and update the buffer 11 | 12 | With the new class hierarchy, TransformerHelper provides the helper functions, 13 | and TransformListener updates the buffer. 14 | 15 | The same decoupling was implemented in TF2, so as soon as we migrate our code 16 | to TF2 we don't need this custom implementation anymore. 17 | 18 | I submitted the changes to the upstream TF package, but Tully Foote does not want 19 | to modify the stable TF API at this point. 20 | -------------------------------------------------------------------------------- /doc/bifrucation.gv: -------------------------------------------------------------------------------- 1 | digraph G{ 2 | label = "Effect of publishing Inverted Parent Child Relationship on TF tree"; 3 | subgraph cluster0{ 4 | a [label="A"]; 5 | aa [label="AA"]; 6 | b [label="B"]; 7 | bb [label="BB"]; 8 | c [ label="C"]; 9 | d [ label="D"]; 10 | a -> b; 11 | aa -> b; 12 | bb -> c; 13 | b -> c; 14 | c -> d; 15 | label = "a) Example Tree"; 16 | color=black; 17 | } 18 | subgraph cluster1{ 19 | a1 [label="A"]; 20 | aa1 [label="AA"]; 21 | b1 [label="B"]; 22 | bb1 [label="BB"]; 23 | c1 [ label="C"]; 24 | d1 [ label="D"]; 25 | a1 -> b1; 26 | aa1 -> b1; 27 | bb1 -> c1; 28 | c1 -> b1; 29 | label = "b) C-B Inverted Case1 (Part of the time)"; 30 | color=black; 31 | } 32 | subgraph cluster2{ 33 | a2 [label="A"]; 34 | aa2 [label="AA"]; 35 | b2 [label="B"]; 36 | bb2 [label="BB"]; 37 | c2 [ label="C"]; 38 | d2 [ label="D"]; 39 | 40 | a2 -> b2; 41 | aa2 -> b2; 42 | bb2 -> c2; 43 | c2 -> d2; 44 | label = "c) C-B Inverted Case2 (part of the time)"; 45 | color=black; 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /test/method_test.py: -------------------------------------------------------------------------------- 1 | import roslib; roslib.load_manifest("tf") 2 | 3 | import rospy 4 | import tf 5 | import time 6 | import bullet 7 | import math 8 | 9 | 10 | try: 11 | transform_stamped = tf.TransformStamped() 12 | print "getting stamp" 13 | print transform_stamped.stamp 14 | # mytime = rospy.Time().now() 15 | mytime = rospy.Time(10,20) 16 | transform_stamped.stamp = mytime 17 | print mytime 18 | print "getting stamp", transform_stamped.stamp 19 | print "transform:", transform_stamped.transform 20 | transform_stamped.transform.setIdentity() 21 | print "after setIdentity()", transform_stamped.transform 22 | # transform_stamped.transform.basis.setEulerZYX(0,0,0) 23 | quat = bullet.Quaternion(math.pi/2,0,0) 24 | print "quaternion ", quat 25 | transform_stamped.transform.setRotation(quat) 26 | print "setting rotation to PI/2\n After setRotation" 27 | print transform_stamped.transform 28 | 29 | other_transform = bullet.Transform() 30 | other_transform.setIdentity() 31 | transform_stamped.transform = other_transform 32 | print "After assignment of Identity" 33 | print transform_stamped.transform 34 | 35 | other_transform = bullet.Transform() 36 | other_transform.setIdentity() 37 | other_transform.setRotation(quat) 38 | transform_stamped.transform = other_transform 39 | print "After assignment of Rotation Transformation" 40 | print transform_stamped.transform 41 | 42 | except ValueError, e: 43 | print "Exception %s Improperly thrown: %s"%(type(e), e) 44 | -------------------------------------------------------------------------------- /include/tf/LinearMath/MinMax.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 3 | 4 | This software is provided 'as-is', without any express or implied warranty. 5 | In no event will the authors be held liable for any damages arising from the use of this software. 6 | Permission is granted to anyone to use this software for any purpose, 7 | including commercial applications, and to alter it and redistribute it freely, 8 | subject to the following restrictions: 9 | 10 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 | 3. This notice may not be removed or altered from any source distribution. 13 | */ 14 | 15 | 16 | 17 | #ifndef TF_MINMAX_H 18 | #define TF_MINMAX_H 19 | 20 | template 21 | TFSIMD_FORCE_INLINE const T& tfMin(const T& a, const T& b) 22 | { 23 | return a < b ? a : b ; 24 | } 25 | 26 | template 27 | TFSIMD_FORCE_INLINE const T& tfMax(const T& a, const T& b) 28 | { 29 | return a > b ? a : b; 30 | } 31 | 32 | 33 | template 34 | TFSIMD_FORCE_INLINE void tfSetMin(T& a, const T& b) 35 | { 36 | if (b < a) 37 | { 38 | a = b; 39 | } 40 | } 41 | 42 | template 43 | TFSIMD_FORCE_INLINE void tfSetMax(T& a, const T& b) 44 | { 45 | if (a < b) 46 | { 47 | a = b; 48 | } 49 | } 50 | 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /src/tf/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | from _tf import * 29 | from listener import TransformListener, TransformerROS 30 | from broadcaster import TransformBroadcaster 31 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | tf 3 | 1.10.8 4 | 5 | 6 | tf is a package that lets the user keep track of multiple coordinate 7 | frames over time. tf maintains the relationship between coordinate 8 | frames in a tree structure buffered in time, and lets the user 9 | transform points, vectors, etc between any two coordinate frames at 10 | any desired point in time. 11 | 12 | 13 | Tully Foote 14 | Eitan Marder-Eppstein 15 | Wim Meeussen 16 | Tully Foote 17 | 18 | BSD 19 | http://www.ros.org/wiki/tf 20 | 21 | catkin 22 | 23 | angles 24 | geometry_msgs 25 | message_filters 26 | message_generation 27 | rosconsole 28 | roscpp 29 | rostest 30 | sensor_msgs 31 | std_msgs 32 | tf2 33 | tf2_ros 34 | 35 | geometry_msgs 36 | graphviz 37 | message_filters 38 | message_runtime 39 | rosconsole 40 | roscpp 41 | roswtf 42 | sensor_msgs 43 | std_msgs 44 | tf2 45 | tf2_ros 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /test/test_transform_datatypes.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "tf/LinearMath/Vector3.h" 35 | 36 | using namespace tf; 37 | 38 | TEST(tf, set) 39 | { 40 | tf::Quaternion q = tf::createQuaternionFromRPY(0, 0, 0); 41 | EXPECT_EQ(q, tf::createIdentityQuaternion()); 42 | } 43 | 44 | 45 | 46 | int main(int argc, char **argv){ 47 | testing::InitGoogleTest(&argc, argv); 48 | return RUN_ALL_TESTS(); 49 | } 50 | -------------------------------------------------------------------------------- /include/tf/exceptions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #ifndef TF_EXCEPTIONS_H 33 | #define TF_EXCEPTIONS_H 34 | 35 | #include 36 | #include 37 | 38 | namespace tf{ 39 | 40 | // Pass through exceptions from tf2 41 | typedef tf2::TransformException TransformException; 42 | typedef tf2::LookupException LookupException; 43 | typedef tf2::ConnectivityException ConnectivityException; 44 | typedef tf2::ExtrapolationException ExtrapolationException; 45 | typedef tf2::InvalidArgumentException InvalidArgument; 46 | 47 | 48 | } 49 | 50 | 51 | #endif //TF_EXCEPTIONS_H 52 | -------------------------------------------------------------------------------- /src/empty_listener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #include 30 | #include 31 | 32 | 33 | int main(int argc, char** argv) 34 | { 35 | ros::init(argc, argv, "my_tf_listener"); 36 | 37 | ros::NodeHandle node; 38 | 39 | tf::TransformListener listener; 40 | 41 | ros::Rate rate(10.0); 42 | while (node.ok()){ 43 | tf::StampedTransform transform; 44 | try 45 | { 46 | listener.lookupTransform("odom_combined", "base_link", ros::Time(0), transform); 47 | } 48 | catch (tf::TransformException ex) 49 | { 50 | ROS_ERROR("%s",ex.what()); 51 | } 52 | 53 | rate.sleep(); 54 | } 55 | return 0; 56 | }; 57 | -------------------------------------------------------------------------------- /mainpage.dox: -------------------------------------------------------------------------------- 1 | 2 | /** \mainpage 3 | 4 | @htmlinclude manifest.html 5 | 6 | @b tf is a library for keeping track of coordinate frames. There are both C++ and Python bindings. 7 | 8 | @section usage Common Usage 9 | For most ROS use cases, the basic tf::Transformer library is not used directly. 10 | 11 | There are two helper classes to provide sending and recieving of ROS transform 12 | messages. tf::TransformBroadcaster and tf::TransformListener. 13 | 14 | @subsection listener TransformListener 15 | The tf::TransformListener class inherits from tf::Transformer to provide all the functionality of the 16 | basic library. In addition, it provides methods to transform data ROS messages directly and 17 | it automatically listens for transforms published on ROS. 18 | 19 | @subsection message_filter MessageFilter 20 | The tf::MessageFilter is the recommended method for receiving almost any sensor data from ROS. 21 | Data in ROS can be published with respect to any known frame. 22 | The tf::MessageFilter class makes it easy to use this data 23 | by providing callbacks only when it is possible to transform it into your desired 24 | target frame. 25 | 26 | The tf::MessageFilter class can subscribe to any ROS datatype that has a ROS Header. 27 | 28 | @subsection broadcaster TransformBroadcaster 29 | The tf::TransformBroadcaster class is the complement to the tf::TransformListener class. The broadcaster class provides a 30 | simple API for broadcasting coordinate frame transforms to other ROS nodes. 31 | 32 | @subsection send_transform send_transform 33 | The send_transform command is the easiest way to report transforms for fixed offsets. 34 | It is a simple command-line utility that repeatedly publishes the fixed-offset transform to ROS. 35 | 36 | @subsection datatypes Data Types used in tf 37 | - Quaternion typedef of btQuaternion 38 | - Vector3 typedef of btVector3 39 | - Point typedef of btVector3 40 | - Transform typedef of btTransform 41 | - Pose typedef of btTransform 42 | 43 | -Stamped version of all of the above inherits from the data type and also has: 44 | - ros::Time stamp_ 45 | - std::string frame_id_ 46 | - std::string child_frame_id_ (only used for Stamped ) 47 | 48 | - There are analogous ROS messages in std_msgs to the Stamped data types. 49 | 50 | - Time represented by ros::Time and ros::Duration in ros/time.h in roscpp 51 | 52 | 53 | 54 | */ 55 | -------------------------------------------------------------------------------- /test/testListener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "tf/transform_listener.h" 31 | #include "ros/ros.h" 32 | 33 | #include 34 | 35 | TEST(TransformBroadcaster, single_frame) 36 | { 37 | tf::TransformListener tfl; 38 | EXPECT_TRUE(tfl.waitForTransform("frame1", "frame2", ros::Time(), ros::Duration(2.0))); 39 | 40 | } 41 | 42 | TEST(TransformBroadcaster, multi_frame) 43 | { 44 | tf::TransformListener tfl; 45 | EXPECT_TRUE(tfl.waitForTransform("vframe0", "vframe2", ros::Time(), ros::Duration(2.0))); 46 | 47 | } 48 | 49 | int main(int argc, char ** argv) 50 | { 51 | testing::InitGoogleTest(&argc, argv); 52 | 53 | //Initialize ROS 54 | ros::init(argc, argv, "listener"); 55 | ros::NodeHandle nh; 56 | 57 | int ret = RUN_ALL_TESTS(); 58 | 59 | return ret; 60 | }; 61 | 62 | -------------------------------------------------------------------------------- /test/tf_unittest_future.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "tf/LinearMath/Vector3.h" 6 | 7 | using namespace tf; 8 | 9 | void seed_rand() 10 | { 11 | //Seed random number generator with current microseond count 12 | timeval temp_time_struct; 13 | gettimeofday(&temp_time_struct,NULL); 14 | srand(temp_time_struct.tv_usec); 15 | }; 16 | 17 | 18 | 19 | TEST(tf, SignFlipExtrapolate) 20 | { 21 | double epsilon = 1e-3; 22 | 23 | double truex, truey, trueyaw1, trueyaw2; 24 | 25 | truex = 5.220; 26 | truey = 1.193; 27 | trueyaw1 = 2.094; 28 | trueyaw2 = 2.199; 29 | ros::Time ts0; 30 | ts0.fromSec(46.6); 31 | ros::Time ts1; 32 | ts1.fromSec(46.7); 33 | ros::Time ts2; 34 | ts2.fromSec(46.8); 35 | 36 | TransformStorage tout; 37 | double yaw, pitch, roll; 38 | 39 | TransformStorage t0(StampedTransform 40 | (tf::Transform(tf::Quaternion(0.000, 0.000, -0.8386707128751809, 0.5446388118427071), 41 | tf::Vector3(1.0330764266905630, 5.2545257423922198, -0.000)), 42 | ts0, "odom", "other0"), 3); 43 | TransformStorage t1(StampedTransform 44 | (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8660255375641606, -0.4999997682866531), 45 | tf::Vector3(1.5766646418987809, 5.1177550046707436, -0.000)), 46 | ts1, "odom", "other1"), 3); 47 | TransformStorage t2(StampedTransform 48 | (tf::Transform(tf::Quaternion(0.000, 0.000, 0.8910066733792211, -0.4539902069358919), 49 | tf::Vector3(2.1029791754869160, 4.9249128183465967, -0.000)), 50 | ts2, "odom", "other2"), 3); 51 | 52 | tf::TimeCache tc; 53 | tf::Transform res; 54 | 55 | tc.interpolate(t0, t1, ts1, tout); 56 | res = tout.inverse(); 57 | res.getBasis().getEulerZYX(yaw,pitch,roll); 58 | 59 | EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); 60 | EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); 61 | EXPECT_NEAR(yaw, trueyaw1, epsilon); 62 | 63 | tc.interpolate(t0, t1, ts2, tout); 64 | res = tout.inverse(); 65 | res.getBasis().getEulerZYX(yaw,pitch,roll); 66 | 67 | EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); 68 | EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); 69 | EXPECT_NEAR(yaw, trueyaw2, epsilon); 70 | 71 | tc.interpolate(t1, t2, ts2, tout); 72 | res = tout.inverse(); 73 | res.getBasis().getEulerZYX(yaw,pitch,roll); 74 | 75 | EXPECT_NEAR(res.getOrigin().x(), truex, epsilon); 76 | EXPECT_NEAR(res.getOrigin().y(), truey, epsilon); 77 | EXPECT_NEAR(yaw, trueyaw2, epsilon); 78 | } 79 | 80 | 81 | 82 | /** @todo Make this actually Assert something */ 83 | 84 | int main(int argc, char **argv){ 85 | testing::InitGoogleTest(&argc, argv); 86 | return RUN_ALL_TESTS(); 87 | } 88 | 89 | -------------------------------------------------------------------------------- /scripts/tf_remap: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Willow Garage, Inc. nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | 35 | 36 | ## remap a tf topic 37 | 38 | import roslib; roslib.load_manifest('tf') 39 | 40 | import rospy 41 | from tf.msg import tfMessage 42 | 43 | class TfRemapper: 44 | def __init__(self): 45 | self.pub = rospy.Publisher('/tf', tfMessage) 46 | mappings = rospy.get_param('~mappings', []) 47 | self.mappings = {} 48 | 49 | for i in mappings: 50 | if "old" in i and "new" in i: 51 | self.mappings[i["old"]] = i["new"] 52 | 53 | print "Applying the following mappings to incoming tf frame ids", self.mappings 54 | rospy.Subscriber("/tf_old", tfMessage, self.callback) 55 | 56 | def callback(self, tf_msg): 57 | for transform in tf_msg.transforms: 58 | if transform.header.frame_id in self.mappings: 59 | transform.header.frame_id = self.mappings[transform.header.frame_id] 60 | if transform.child_frame_id in self.mappings: 61 | transform.child_frame_id = self.mappings[transform.child_frame_id] 62 | 63 | self.pub.publish(tf_msg) 64 | 65 | def remap_tf(): 66 | 67 | pub.publish(Empty()) 68 | 69 | 70 | if __name__ == '__main__': 71 | rospy.init_node('tf_remapper') 72 | tfr = TfRemapper() 73 | rospy.spin() 74 | -------------------------------------------------------------------------------- /test/operator_overload.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | 33 | #include "tf/LinearMath/Transform.h" 34 | 35 | 36 | void seed_rand() 37 | { 38 | //Seed random number generator with current microseond count 39 | timeval temp_time_struct; 40 | gettimeofday(&temp_time_struct,NULL); 41 | srand(temp_time_struct.tv_usec); 42 | }; 43 | 44 | 45 | int main(int argc, char **argv){ 46 | 47 | unsigned int runs = 400; 48 | seed_rand(); 49 | 50 | std::vector xvalues(runs), yvalues(runs), zvalues(runs); 51 | for ( unsigned int i = 0; i < runs ; i++ ) 52 | { 53 | xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 54 | yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 55 | zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 56 | } 57 | 58 | //Useful Operator Overload 59 | for ( unsigned int i = 0; i < runs ; i++ ) 60 | { 61 | tf::Transform transform(tf::Quaternion(0,0,0), tf::Vector3(xvalues[i],yvalues[i],zvalues[i])); 62 | tf::Quaternion initial(xvalues[i],yvalues[i],zvalues[i]); 63 | tf::Quaternion final(xvalues[i],yvalues[i],zvalues[i]); 64 | final = transform * initial; 65 | std::printf("Useful Operator Overload: %f, angle between quaternions\n", initial.angle(final)); 66 | } 67 | 68 | 69 | return 0; 70 | } 71 | 72 | 73 | -------------------------------------------------------------------------------- /src/transform_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | /** \author Tully Foote */ 32 | 33 | 34 | #include "ros/ros.h" 35 | #include "tf/transform_broadcaster.h" 36 | #include "tf/transform_listener.h" 37 | 38 | #include 39 | 40 | 41 | 42 | namespace tf { 43 | 44 | TransformBroadcaster::TransformBroadcaster(): 45 | tf2_broadcaster_() 46 | { 47 | }; 48 | 49 | void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf) 50 | { 51 | tf2_broadcaster_.sendTransform(msgtf); 52 | } 53 | 54 | void TransformBroadcaster::sendTransform(const StampedTransform & transform) 55 | { 56 | geometry_msgs::TransformStamped msgtf; 57 | transformStampedTFToMsg(transform, msgtf); 58 | tf2_broadcaster_.sendTransform(msgtf); 59 | } 60 | 61 | void TransformBroadcaster::sendTransform(const std::vector & msgtf) 62 | { 63 | tf2_broadcaster_.sendTransform(msgtf); 64 | } 65 | 66 | void TransformBroadcaster::sendTransform(const std::vector & transforms) 67 | { 68 | std::vector msgtfs; 69 | for (std::vector::const_iterator it = transforms.begin(); it != transforms.end(); ++it) 70 | { 71 | geometry_msgs::TransformStamped msgtf; 72 | transformStampedTFToMsg(*it, msgtf); 73 | msgtfs.push_back(msgtf); 74 | 75 | } 76 | tf2_broadcaster_.sendTransform(msgtfs); 77 | } 78 | 79 | 80 | 81 | 82 | } 83 | 84 | 85 | -------------------------------------------------------------------------------- /test/transform_listener_unittest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | 35 | void seed_rand() 36 | { 37 | //Seed random number generator with current microseond count 38 | timeval temp_time_struct; 39 | gettimeofday(&temp_time_struct,NULL); 40 | srand(temp_time_struct.tv_usec); 41 | }; 42 | 43 | void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) 44 | { 45 | seed_rand(); 46 | for ( uint64_t i = 0; i < runs ; i++ ) 47 | { 48 | xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 49 | yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 50 | zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 51 | } 52 | } 53 | 54 | 55 | using namespace tf; 56 | 57 | 58 | TEST(transform_listener, resolve) 59 | { 60 | ros::NodeHandle n("~"); 61 | tf::TransformListener tl; 62 | 63 | //no prefix 64 | EXPECT_STREQ("id", tl.resolve("id").c_str()); 65 | 66 | n.setParam("tf_prefix", "a_tf_prefix"); 67 | tf::TransformListener tp; 68 | 69 | std::string prefix_str = tf::getPrefixParam(n); 70 | 71 | EXPECT_STREQ("a_tf_prefix", prefix_str.c_str()); 72 | 73 | EXPECT_STREQ("a_tf_prefix/id", tf::resolve(prefix_str, "id").c_str()); 74 | 75 | EXPECT_STREQ("a_tf_prefix/id", tp.resolve("id").c_str()); 76 | 77 | 78 | } 79 | 80 | 81 | int main(int argc, char **argv){ 82 | testing::InitGoogleTest(&argc, argv); 83 | ros::init(argc, argv, "transform_listener_unittest"); 84 | return RUN_ALL_TESTS(); 85 | } 86 | -------------------------------------------------------------------------------- /src/tf/broadcaster.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2008, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of the Willow Garage nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | import rospy 34 | import tf 35 | import tf.msg 36 | import geometry_msgs.msg 37 | import math 38 | 39 | class TransformBroadcaster: 40 | """ 41 | :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. 42 | """ 43 | 44 | def __init__(self): 45 | self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage) 46 | 47 | def sendTransform(self, translation, rotation, time, child, parent): 48 | """ 49 | :param translation: the translation of the transformtion as a tuple (x, y, z) 50 | :param rotation: the rotation of the transformation as a tuple (x, y, z, w) 51 | :param time: the time of the transformation, as a rospy.Time() 52 | :param child: child frame in tf, string 53 | :param parent: parent frame in tf, string 54 | 55 | Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``. 56 | """ 57 | 58 | t = geometry_msgs.msg.TransformStamped() 59 | t.header.frame_id = parent 60 | t.header.stamp = time 61 | t.child_frame_id = child 62 | t.transform.translation.x = translation[0] 63 | t.transform.translation.y = translation[1] 64 | t.transform.translation.z = translation[2] 65 | 66 | t.transform.rotation.x = rotation[0] 67 | t.transform.rotation.y = rotation[1] 68 | t.transform.rotation.z = rotation[2] 69 | t.transform.rotation.w = rotation[3] 70 | 71 | tfm = tf.msg.tfMessage([t]) 72 | self.pub_tf.publish(tfm) 73 | 74 | if __name__ == '__main__': 75 | rospy.init_node('tf_turtle') 76 | tfb = TurtleTFBroadcaster(rospy.get_param('~turtle')) 77 | rospy.spin() 78 | -------------------------------------------------------------------------------- /include/tf/transform_broadcaster.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | 31 | /** \author Tully Foote */ 32 | 33 | #ifndef TF_TRANSFORMBROADCASTER_H 34 | #define TF_TRANSFORMBROADCASTER_H 35 | 36 | #include "tf/tf.h" 37 | #include "tf/tfMessage.h" 38 | 39 | #include 40 | 41 | 42 | namespace tf 43 | { 44 | 45 | 46 | /** \brief This class provides an easy way to publish coordinate frame transform information. 47 | * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the 48 | * necessary data needed for each message. */ 49 | 50 | class TransformBroadcaster{ 51 | public: 52 | /** \brief Constructor (needs a ros::Node reference) */ 53 | TransformBroadcaster(); 54 | 55 | /** \brief Send a StampedTransform 56 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 57 | void sendTransform(const StampedTransform & transform); 58 | 59 | /** \brief Send a vector of StampedTransforms 60 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 61 | void sendTransform(const std::vector & transforms); 62 | 63 | /** \brief Send a TransformStamped message 64 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 65 | void sendTransform(const geometry_msgs::TransformStamped & transform); 66 | 67 | /** \brief Send a vector of TransformStamped messages 68 | * The stamped data structure includes frame_id, and time, and parent_id already. */ 69 | void sendTransform(const std::vector & transforms); 70 | 71 | private: 72 | 73 | tf2_ros::TransformBroadcaster tf2_broadcaster_; 74 | 75 | }; 76 | 77 | } 78 | 79 | #endif //TF_TRANSFORMBROADCASTER_H 80 | -------------------------------------------------------------------------------- /scripts/bullet_migration_sed.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2011, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | 35 | 36 | # Run this script to update bullet datatypes from tf in Electric to 37 | # Fuerte/Unstable or newer 38 | 39 | # by default this assumes your files are not using the tf namespace. 40 | # If they are change the line below with the for loop to use the 41 | # namespaced_rules 42 | 43 | import subprocess 44 | 45 | 46 | 47 | 48 | cmd = "find . -type f ! -name '*.svn-base' -a ! -name '*.hg' -a ! -name '*.git' -a $ -name '*.c*' -o -name '*.h*' $ -exec sed -i '%(rule)s' {} \;" 49 | 50 | rules = ['s|LinearMath/bt|tf/LinearMath/|g', # include path 51 | 's/btTransform\.h/Transform\.h/g', # include files 52 | 's/btMatrix3x3\.h/Matrix3x3\.h/g', 53 | 's/btScalar\.h/Scalar\.h/g', 54 | 's/btQuaternion\.h/Quaternion\.h/g', 55 | 's/btQuadWord\.h/QuadWord\.h/g', 56 | 's/btMinMax\.h/MinMax\.h/g', 57 | 's/btVector3\.h/Vector3\.h/g', 58 | 's/btScalar/tfScalar/g', 59 | ] 60 | 61 | unnamespaced_rules = [ 62 | 's/btTransform/tf::Transform/g', 63 | 's/btQuaternion/tf::Quaternion/g', 64 | 's/btVector3/tf::Vector3/g', 65 | 's/btMatrix3x3/tf::Matrix3x3/g', 66 | 's/btQuadWord/tf::QuadWord/g', 67 | 68 | ] 69 | 70 | namespaced_rules = [ 71 | 's/btTransform/Transform/g', 72 | 's/btQuaternion/Quaternion/g', 73 | 's/btVector3/Vector3/g', 74 | 's/btMatrix3x3/Matrix3x3/g', 75 | 's/btQuadWord/QuadWord/g', 76 | #'s/btScalar/Scalar/g', 77 | ] 78 | 79 | 80 | 81 | for rule in rules + unnamespaced_rules: #change me if using files with namespace tf set 82 | full_cmd = cmd%locals() 83 | print ("Running %s"%full_cmd) 84 | ret_code = subprocess.call(full_cmd, shell=True) 85 | if ret_code == 0: 86 | print ("success") 87 | else: 88 | print ("failure") 89 | -------------------------------------------------------------------------------- /test/testBroadcaster.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "tf/transform_broadcaster.h" 31 | #include "ros/ros.h" 32 | 33 | class testBroadcaster 34 | { 35 | public: 36 | //constructor 37 | testBroadcaster() : count(0), count1(0){}; 38 | //Clean up ros connections 39 | ~testBroadcaster() { } 40 | 41 | //A pointer to the rosTFServer class 42 | tf::TransformBroadcaster broadcaster; 43 | 44 | 45 | // A function to call to send data periodically 46 | void test () { 47 | broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "frame2", "frame1")); 48 | 49 | if (count > 9000) 50 | { 51 | count = 0; 52 | std::cerr<<"Counter 0 rolledover at 9000"<< std::endl; 53 | } 54 | else 55 | count ++; 56 | //std::cerr< vec; 62 | vec.push_back(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "vframe2", "vframe1")); 63 | vec.push_back(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "vframe1", "vframe0")); 64 | broadcaster.sendTransform(vec); 65 | 66 | if (count1 > 9000) 67 | { 68 | count1 = 0; 69 | std::cerr<<"Counter 1 rolledover at 9000"<< std::endl; 70 | } 71 | else 72 | count1 ++; 73 | //std::cerr< 31 | #include 32 | #include 33 | #include 34 | 35 | #include "tf/LinearMath/Transform.h" 36 | 37 | double epsilon = 10E-6; 38 | 39 | 40 | void seed_rand() 41 | { 42 | //Seed random number generator with current microseond count 43 | timeval temp_time_struct; 44 | gettimeofday(&temp_time_struct,NULL); 45 | srand(temp_time_struct.tv_usec); 46 | }; 47 | 48 | void testQuatRPY(tf::Quaternion q_baseline) 49 | { 50 | q_baseline.normalize(); 51 | tf::Matrix3x3 m(q_baseline); 52 | double roll, pitch, yaw; 53 | 54 | for (int solution = 1 ; solution < 2 ; ++solution) 55 | { 56 | m.getRPY(roll, pitch, yaw, solution); 57 | tf::Quaternion q_from_rpy; 58 | q_from_rpy.setRPY(roll, pitch, yaw); 59 | // use angleShortestPath() because angle() can return PI for equivalent 60 | // quaternions 61 | double angle1 = q_from_rpy.angleShortestPath(q_baseline); 62 | ASSERT_NEAR(0.0, angle1, epsilon); 63 | //std::printf("%f, angle between quaternions\n", angle1); 64 | 65 | tf::Matrix3x3 m2; 66 | m2.setRPY(roll, pitch, yaw); 67 | tf::Quaternion q_from_m_from_rpy; 68 | m2.getRotation(q_from_m_from_rpy); 69 | // use angleShortestPath() because angle() can return PI for equivalent 70 | // quaternions 71 | double angle2 = q_from_m_from_rpy.angleShortestPath(q_baseline); 72 | ASSERT_NEAR(0.0, angle2, epsilon); 73 | //std::printf("%f, angle between quaternions\n", angle2); 74 | } 75 | } 76 | 77 | TEST(tf, Quaternion) 78 | { 79 | unsigned int runs = 400; 80 | seed_rand(); 81 | 82 | std::vector xvalues(runs), yvalues(runs), zvalues(runs); 83 | for ( unsigned int i = 0; i < runs ; i++ ) 84 | { 85 | xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 86 | yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 87 | zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 88 | } 89 | 90 | 91 | for ( unsigned int i = 0; i < runs ; i++ ) 92 | { 93 | tf::Matrix3x3 mat; 94 | tf::Quaternion q_baseline; 95 | q_baseline.setRPY(zvalues[i],yvalues[i],xvalues[i]); 96 | mat.setRotation(q_baseline); 97 | tf::Quaternion q_from_m; 98 | mat.getRotation(q_from_m); 99 | double angle = q_from_m.angle(q_baseline); 100 | ASSERT_NEAR(0.0, angle, epsilon); 101 | testQuatRPY(q_baseline); 102 | } 103 | 104 | // test some corner cases 105 | testQuatRPY(tf::Quaternion( 0.5, 0.5, 0.5, -0.5)); 106 | testQuatRPY(tf::Quaternion( 0.5, 0.5, 0.5, 0.5)); 107 | testQuatRPY(tf::Quaternion( 0.5, -0.5, 0.5, 0.5)); 108 | testQuatRPY(tf::Quaternion( 0.5, 0.5, -0.5, 0.5)); 109 | testQuatRPY(tf::Quaternion(-0.5, 0.5, 0.5, 0.5)); 110 | } 111 | 112 | 113 | int main(int argc, char **argv){ 114 | testing::InitGoogleTest(&argc, argv); 115 | return RUN_ALL_TESTS(); 116 | } 117 | -------------------------------------------------------------------------------- /src/tf_echo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include "tf/transform_listener.h" 32 | #include "ros/ros.h" 33 | 34 | class echoListener 35 | { 36 | public: 37 | 38 | tf::TransformListener tf; 39 | 40 | //constructor with name 41 | echoListener() 42 | { 43 | 44 | }; 45 | 46 | ~echoListener() 47 | { 48 | 49 | }; 50 | 51 | private: 52 | 53 | }; 54 | 55 | 56 | int main(int argc, char ** argv) 57 | { 58 | //Initialize ROS 59 | ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName); 60 | 61 | if (argc != 3) 62 | { 63 | printf("Usage: tf_echo source_frame target_frame\n\n"); 64 | printf("This will echo the transform from the coordinate frame of the source_frame\n"); 65 | printf("to the coordinate frame of the target_frame. \n"); 66 | printf("Note: This is the transform to get data from target_frame into the source_frame.\n"); 67 | return -1; 68 | } 69 | 70 | ros::NodeHandle nh; 71 | //Instantiate a local listener 72 | echoListener echoListener; 73 | 74 | 75 | std::string source_frameid = std::string(argv[1]); 76 | std::string target_frameid = std::string(argv[2]); 77 | 78 | // Wait for up to one second for the first transforms to become avaiable. 79 | echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0)); 80 | 81 | //Nothing needs to be done except wait for a quit 82 | //The callbacks withing the listener class 83 | //will take care of everything 84 | while(nh.ok()) 85 | { 86 | try 87 | { 88 | tf::StampedTransform echo_transform; 89 | echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform); 90 | std::cout.precision(3); 91 | std::cout.setf(std::ios::fixed,std::ios::floatfield); 92 | std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl; 93 | double yaw, pitch, roll; 94 | echo_transform.getBasis().getRPY(roll, pitch, yaw); 95 | tf::Quaternion q = echo_transform.getRotation(); 96 | tf::Vector3 v = echo_transform.getOrigin(); 97 | std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl; 98 | std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", " 99 | << q.getZ() << ", " << q.getW() << "]" << std::endl 100 | << " in RPY [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl; 101 | 102 | //print transform 103 | } 104 | catch(tf::TransformException& ex) 105 | { 106 | std::cout << "Failure at "<< ros::Time::now() << std::endl; 107 | std::cout << "Exception thrown:" << ex.what()<< std::endl; 108 | std::cout << "The current list of frames is:" < distutils.version.StrictVersion('2.8'): 107 | subprocess.check_call(["dot", "-Tpdf", "frames.gv", "-o", "frames.pdf"]) 108 | print "frames.pdf generated" 109 | else: 110 | subprocess.check_call(["dot", "-Tps2", "frames.gv", "-o", "frames.ps"]) 111 | print "frames.ps generated" 112 | except subprocess.CalledProcessError: 113 | print >> sys.stderr, "failed to generate frames.pdf" 114 | 115 | 116 | if __name__ == '__main__': 117 | parser = OptionParser(usage="usage: %prog [options]", prog='viewFrames.py') 118 | parser.add_option("--node", metavar="node name", 119 | type="string", help="Node to get frames from", 120 | dest="node") 121 | options, args = parser.parse_args() 122 | 123 | dot_graph = '' 124 | if not options.node: 125 | dot_graph = listen(5.0) 126 | else: 127 | print "Generating graph for node: ", options.node 128 | dot_graph = poll(options.node) 129 | 130 | generate(dot_graph) 131 | -------------------------------------------------------------------------------- /include/tf/time_cache.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #ifndef TF_TIME_CACHE_H 33 | #define TF_TIME_CACHE_H 34 | 35 | #include 36 | #include 37 | 38 | #include "tf/transform_datatypes.h" 39 | #include "tf/exceptions.h" 40 | 41 | #include "tf/LinearMath/Transform.h" 42 | 43 | #include 44 | 45 | namespace tf 46 | { 47 | enum ExtrapolationMode { ONE_VALUE, INTERPOLATE, EXTRAPOLATE_BACK, EXTRAPOLATE_FORWARD }; 48 | 49 | 50 | typedef uint32_t CompactFrameID; 51 | typedef std::pair P_TimeAndFrameID; 52 | 53 | /** \brief Storage for transforms and their parent */ 54 | class TransformStorage 55 | { 56 | public: 57 | TransformStorage(); 58 | TransformStorage(const StampedTransform& data, CompactFrameID frame_id, CompactFrameID child_frame_id); 59 | 60 | TransformStorage(const TransformStorage& rhs) 61 | { 62 | *this = rhs; 63 | } 64 | 65 | TransformStorage& operator=(const TransformStorage& rhs) 66 | { 67 | #if 01 68 | rotation_ = rhs.rotation_; 69 | translation_ = rhs.translation_; 70 | stamp_ = rhs.stamp_; 71 | frame_id_ = rhs.frame_id_; 72 | child_frame_id_ = rhs.child_frame_id_; 73 | #endif 74 | return *this; 75 | } 76 | 77 | bool operator< (const TransformStorage &b) const 78 | { 79 | return this->stamp_ < b.stamp_; 80 | } 81 | 82 | 83 | tf::Quaternion rotation_; 84 | tf::Vector3 translation_; 85 | ros::Time stamp_; 86 | CompactFrameID frame_id_; 87 | CompactFrameID child_frame_id_; 88 | }; 89 | 90 | 91 | 92 | 93 | /** \brief A class to keep a sorted linked list in time 94 | * This builds and maintains a list of timestamped 95 | * data. And provides lookup functions to get 96 | * data out as a function of time. */ 97 | class TimeCache 98 | { 99 | public: 100 | static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below. 101 | static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; //!< Maximum length of linked list, to make sure not to be able to use unlimited memory. 102 | static const int64_t DEFAULT_MAX_STORAGE_TIME = 1ULL * 1000000000LL; //!< default value of 10 seconds storage 103 | 104 | TimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME)); 105 | 106 | bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); 107 | bool insertData(const TransformStorage& new_data); 108 | void clearList(); 109 | CompactFrameID getParent(ros::Time time, std::string* error_str); 110 | P_TimeAndFrameID getLatestTimeAndParent(); 111 | 112 | /// Debugging information methods 113 | unsigned int getListLength(); 114 | ros::Time getLatestTimestamp(); 115 | ros::Time getOldestTimestamp(); 116 | 117 | 118 | private: 119 | typedef std::set L_TransformStorage; 120 | L_TransformStorage storage_; 121 | 122 | ros::Duration max_storage_time_; 123 | 124 | 125 | /// A helper function for getData 126 | //Assumes storage is already locked for it 127 | inline uint8_t findClosest(const TransformStorage*& one, const TransformStorage*& two, ros::Time target_time, std::string* error_str); 128 | 129 | inline void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output); 130 | 131 | 132 | void pruneList(); 133 | 134 | 135 | 136 | }; 137 | 138 | 139 | } 140 | #endif // TF_TIME_CACHE_H 141 | -------------------------------------------------------------------------------- /test/python_debug_test.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2009, Willow Garage, Inc. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Willow Garage, Inc. nor the names of its 13 | # contributors may be used to endorse or promote products derived from 14 | # this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | # POSSIBILITY OF SUCH DAMAGE. 27 | 28 | import roslib; roslib.load_manifest("tf") 29 | 30 | import rospy 31 | import tf 32 | import time 33 | import bullet 34 | import math 35 | 36 | try: 37 | 38 | rospy.init_node("test_node") 39 | tfl = tf.TransformListener() 40 | time.sleep(1) 41 | 42 | # View all frames 43 | print "All frames are:\n", tfl.all_frames_as_string() 44 | 45 | # dot based introspection 46 | print "All frame graph is:\n", tfl.all_frames_as_dot() 47 | 48 | 49 | # test transforming pose 50 | po = tf.PoseStamped() 51 | po.frame_id = "base_link" 52 | print "calling transform pose" 53 | po2 = tfl.transform_pose("/map", po) 54 | 55 | print "po2.pose.this", po2.pose.this 56 | print "po.pose.this", po.pose.this 57 | 58 | # test transforming point 59 | po = tf.PointStamped() 60 | po.frame_id = "base_link" 61 | po3 = tfl.transform_point("/map", po) 62 | print "po3" 63 | print po3.this 64 | 65 | # test transforming vector 66 | po = tf.VectorStamped() 67 | po.frame_id = "base_link" 68 | po4 = tfl.transform_vector("/map", po) 69 | print po4.this 70 | 71 | # test transforming quaternion 72 | po = tf.QuaternionStamped() 73 | po.frame_id = "base_link" 74 | po5 = tfl.transform_quaternion("/map", po) 75 | print "po5", po5.this 76 | 77 | tr = tf.TransformStamped() 78 | 79 | lps = tf.PoseStamped() 80 | lps.pose.setIdentity() 81 | print "setting stamp" 82 | mytime = rospy.Time(10,20) 83 | lps.stamp = mytime 84 | print mytime 85 | print "getting stamp" 86 | output = lps.stamp 87 | print output 88 | print lps.pose 89 | print "setting pose.positon to 1,2,3" 90 | lps.pose.setOrigin( bullet.Vector3(1,2,3)) 91 | print lps.pose.getOrigin() 92 | print lps.pose 93 | 94 | transform_stamped = tf.TransformStamped() 95 | print "getting stamp" 96 | print transform_stamped.stamp 97 | # mytime = rospy.Time().now() 98 | mytime = rospy.Time(10,20) 99 | transform_stamped.stamp = mytime 100 | print mytime 101 | print "getting stamp", transform_stamped.stamp 102 | print "transform:", transform_stamped.transform 103 | transform_stamped.transform.setIdentity() 104 | print "after setIdentity()", transform_stamped.transform 105 | # transform_stamped.transform.basis.setEulerZYX(0,0,0) 106 | quat = bullet.Quaternion(math.pi/2,0,0) 107 | print "quaternion ", quat 108 | transform_stamped.transform.setRotation(quat) 109 | print "setting rotation to PI/2",transform_stamped.transform 110 | 111 | 112 | pointstamped = tf.PointStamped() 113 | print "getting stamp" 114 | print pointstamped.stamp 115 | # mytime = rospy.Time().now() 116 | mytime = rospy.Time(10,20) 117 | pointstamped.stamp = mytime 118 | print mytime 119 | print "getting stamp" 120 | output = pointstamped.stamp 121 | print output 122 | print pointstamped.point 123 | print transform_stamped.transform * pointstamped.point 124 | 125 | pose_only = bullet.Transform(transform_stamped.transform) 126 | print "destructing pose_only", pose_only.this 127 | pose_only = [] 128 | 129 | 130 | print "Creating copy" 131 | po2_copy = tf.PoseStamped(po2) 132 | print "po2_copy.pose", po2_copy.pose.this 133 | print "po2.pose", po2.pose.this 134 | 135 | print "Creating copy2" 136 | po2_copy2 = tf.PoseStamped(po2) 137 | print "po2_copy2.pose", po2_copy2.pose.this 138 | 139 | 140 | print "destructing po2 po2.pose is", po2.pose.this 141 | po2 = [] 142 | 143 | 144 | print "destructing po2_copy po2_copy.pose is", po2_copy.pose.this 145 | po2_copy = [] 146 | 147 | 148 | 149 | 150 | 151 | 152 | print "done" 153 | 154 | except ValueError, e: 155 | print "Exception %s Improperly thrown: %s"%(type(e), e) 156 | 157 | 158 | -------------------------------------------------------------------------------- /src/static_transform_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include "tf/transform_broadcaster.h" 32 | 33 | class TransformSender 34 | { 35 | public: 36 | ros::NodeHandle node_; 37 | //constructor 38 | TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string& frame_id, const std::string& child_frame_id) 39 | { 40 | tf::Quaternion q; 41 | q.setRPY(roll, pitch,yaw); 42 | transform_ = tf::StampedTransform(tf::Transform(q, tf::Vector3(x,y,z)), time, frame_id, child_frame_id ); 43 | }; 44 | TransformSender(double x, double y, double z, double qx, double qy, double qz, double qw, ros::Time time, const std::string& frame_id, const std::string& child_frame_id) : 45 | transform_(tf::Transform(tf::Quaternion(qx,qy,qz,qw), tf::Vector3(x,y,z)), time, frame_id, child_frame_id){}; 46 | //Clean up ros connections 47 | ~TransformSender() { } 48 | 49 | //A pointer to the rosTFServer class 50 | tf::TransformBroadcaster broadcaster; 51 | 52 | 53 | 54 | // A function to call to send data periodically 55 | void send (ros::Time time) { 56 | transform_.stamp_ = time; 57 | broadcaster.sendTransform(transform_); 58 | }; 59 | 60 | private: 61 | tf::StampedTransform transform_; 62 | 63 | }; 64 | 65 | int main(int argc, char ** argv) 66 | { 67 | //Initialize ROS 68 | ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName); 69 | 70 | if(argc == 11) 71 | { 72 | ros::Duration sleeper(atof(argv[10])/1000.0); 73 | 74 | if (strcmp(argv[8], argv[9]) == 0) 75 | ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]); 76 | 77 | TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]), 78 | atof(argv[4]), atof(argv[5]), atof(argv[6]), atof(argv[7]), 79 | ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout 80 | argv[8], argv[9]); 81 | 82 | 83 | 84 | while(tf_sender.node_.ok()) 85 | { 86 | tf_sender.send(ros::Time::now() + sleeper); 87 | ROS_DEBUG("Sending transform from %s with parent %s\n", argv[8], argv[9]); 88 | sleeper.sleep(); 89 | } 90 | 91 | return 0; 92 | } 93 | else if (argc == 10) 94 | { 95 | ros::Duration sleeper(atof(argv[9])/1000.0); 96 | 97 | if (strcmp(argv[7], argv[8]) == 0) 98 | ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[7], argv[8]); 99 | 100 | TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]), 101 | atof(argv[4]), atof(argv[5]), atof(argv[6]), 102 | ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout 103 | argv[7], argv[8]); 104 | 105 | 106 | 107 | while(tf_sender.node_.ok()) 108 | { 109 | tf_sender.send(ros::Time::now() + sleeper); 110 | ROS_DEBUG("Sending transform from %s with parent %s\n", argv[7], argv[8]); 111 | sleeper.sleep(); 112 | } 113 | 114 | return 0; 115 | 116 | } 117 | else 118 | { 119 | printf("A command line utility for manually sending a transform.\n"); 120 | printf("It will periodicaly republish the given transform. \n"); 121 | printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period(milliseconds) \n"); 122 | printf("OR \n"); 123 | printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period(milliseconds) \n"); 124 | printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n"); 125 | printf("of the child_frame_id. \n"); 126 | ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments"); 127 | return -1; 128 | } 129 | 130 | 131 | }; 132 | 133 | -------------------------------------------------------------------------------- /include/tf/LinearMath/QuadWord.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 3 | 4 | This software is provided 'as-is', without any express or implied warranty. 5 | In no event will the authors be held liable for any damages arising from the use of this software. 6 | Permission is granted to anyone to use this software for any purpose, 7 | including commercial applications, and to alter it and redistribute it freely, 8 | subject to the following restrictions: 9 | 10 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 | 3. This notice may not be removed or altered from any source distribution. 13 | */ 14 | 15 | 16 | #ifndef TF_QUADWORD_H 17 | #define TF_QUADWORD_H 18 | 19 | #include "Scalar.h" 20 | #include "MinMax.h" 21 | 22 | 23 | #if defined (__CELLOS_LV2) && defined (__SPU__) 24 | #include 25 | #endif 26 | 27 | 28 | namespace tf 29 | { 30 | /**@brief The QuadWord class is base class for Vector3 and Quaternion. 31 | * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. 32 | */ 33 | #ifndef USE_LIBSPE2 34 | ATTRIBUTE_ALIGNED16(class) QuadWord 35 | #else 36 | class QuadWord 37 | #endif 38 | { 39 | protected: 40 | 41 | #if defined (__SPU__) && defined (__CELLOS_LV2__) 42 | union { 43 | vec_float4 mVec128; 44 | tfScalar m_floats[4]; 45 | }; 46 | public: 47 | vec_float4 get128() const 48 | { 49 | return mVec128; 50 | } 51 | protected: 52 | #else //__CELLOS_LV2__ __SPU__ 53 | tfScalar m_floats[4]; 54 | #endif //__CELLOS_LV2__ __SPU__ 55 | 56 | public: 57 | 58 | 59 | /**@brief Return the x value */ 60 | TFSIMD_FORCE_INLINE const tfScalar& getX() const { return m_floats[0]; } 61 | /**@brief Return the y value */ 62 | TFSIMD_FORCE_INLINE const tfScalar& getY() const { return m_floats[1]; } 63 | /**@brief Return the z value */ 64 | TFSIMD_FORCE_INLINE const tfScalar& getZ() const { return m_floats[2]; } 65 | /**@brief Set the x value */ 66 | TFSIMD_FORCE_INLINE void setX(tfScalar x) { m_floats[0] = x;}; 67 | /**@brief Set the y value */ 68 | TFSIMD_FORCE_INLINE void setY(tfScalar y) { m_floats[1] = y;}; 69 | /**@brief Set the z value */ 70 | TFSIMD_FORCE_INLINE void setZ(tfScalar z) { m_floats[2] = z;}; 71 | /**@brief Set the w value */ 72 | TFSIMD_FORCE_INLINE void setW(tfScalar w) { m_floats[3] = w;}; 73 | /**@brief Return the x value */ 74 | TFSIMD_FORCE_INLINE const tfScalar& x() const { return m_floats[0]; } 75 | /**@brief Return the y value */ 76 | TFSIMD_FORCE_INLINE const tfScalar& y() const { return m_floats[1]; } 77 | /**@brief Return the z value */ 78 | TFSIMD_FORCE_INLINE const tfScalar& z() const { return m_floats[2]; } 79 | /**@brief Return the w value */ 80 | TFSIMD_FORCE_INLINE const tfScalar& w() const { return m_floats[3]; } 81 | 82 | //TFSIMD_FORCE_INLINE tfScalar& operator[](int i) { return (&m_floats[0])[i]; } 83 | //TFSIMD_FORCE_INLINE const tfScalar& operator[](int i) const { return (&m_floats[0])[i]; } 84 | ///operator tfScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. 85 | TFSIMD_FORCE_INLINE operator tfScalar *() { return &m_floats[0]; } 86 | TFSIMD_FORCE_INLINE operator const tfScalar *() const { return &m_floats[0]; } 87 | 88 | TFSIMD_FORCE_INLINE bool operator==(const QuadWord& other) const 89 | { 90 | return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); 91 | } 92 | 93 | TFSIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const 94 | { 95 | return !(*this == other); 96 | } 97 | 98 | /**@brief Set x,y,z and zero w 99 | * @param x Value of x 100 | * @param y Value of y 101 | * @param z Value of z 102 | */ 103 | TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z) 104 | { 105 | m_floats[0]=x; 106 | m_floats[1]=y; 107 | m_floats[2]=z; 108 | m_floats[3] = 0.f; 109 | } 110 | 111 | /* void getValue(tfScalar *m) const 112 | { 113 | m[0] = m_floats[0]; 114 | m[1] = m_floats[1]; 115 | m[2] = m_floats[2]; 116 | } 117 | */ 118 | /**@brief Set the values 119 | * @param x Value of x 120 | * @param y Value of y 121 | * @param z Value of z 122 | * @param w Value of w 123 | */ 124 | TFSIMD_FORCE_INLINE void setValue(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) 125 | { 126 | m_floats[0]=x; 127 | m_floats[1]=y; 128 | m_floats[2]=z; 129 | m_floats[3]=w; 130 | } 131 | /**@brief No initialization constructor */ 132 | TFSIMD_FORCE_INLINE QuadWord() 133 | // :m_floats[0](tfScalar(0.)),m_floats[1](tfScalar(0.)),m_floats[2](tfScalar(0.)),m_floats[3](tfScalar(0.)) 134 | { 135 | } 136 | 137 | /**@brief Three argument constructor (zeros w) 138 | * @param x Value of x 139 | * @param y Value of y 140 | * @param z Value of z 141 | */ 142 | TFSIMD_FORCE_INLINE QuadWord(const tfScalar& x, const tfScalar& y, const tfScalar& z) 143 | { 144 | m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; 145 | } 146 | 147 | /**@brief Initializing constructor 148 | * @param x Value of x 149 | * @param y Value of y 150 | * @param z Value of z 151 | * @param w Value of w 152 | */ 153 | TFSIMD_FORCE_INLINE QuadWord(const tfScalar& x, const tfScalar& y, const tfScalar& z,const tfScalar& w) 154 | { 155 | m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; 156 | } 157 | 158 | /**@brief Set each element to the max of the current values and the values of another QuadWord 159 | * @param other The other QuadWord to compare with 160 | */ 161 | TFSIMD_FORCE_INLINE void setMax(const QuadWord& other) 162 | { 163 | tfSetMax(m_floats[0], other.m_floats[0]); 164 | tfSetMax(m_floats[1], other.m_floats[1]); 165 | tfSetMax(m_floats[2], other.m_floats[2]); 166 | tfSetMax(m_floats[3], other.m_floats[3]); 167 | } 168 | /**@brief Set each element to the min of the current values and the values of another QuadWord 169 | * @param other The other QuadWord to compare with 170 | */ 171 | TFSIMD_FORCE_INLINE void setMin(const QuadWord& other) 172 | { 173 | tfSetMin(m_floats[0], other.m_floats[0]); 174 | tfSetMin(m_floats[1], other.m_floats[1]); 175 | tfSetMin(m_floats[2], other.m_floats[2]); 176 | tfSetMin(m_floats[3], other.m_floats[3]); 177 | } 178 | 179 | 180 | 181 | }; 182 | 183 | } 184 | 185 | #endif //TFSIMD_QUADWORD_H 186 | -------------------------------------------------------------------------------- /src/tf/tfwtf.py: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # 3 | # Copyright (c) 2009, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions 8 | # are met: 9 | # 10 | # * Redistributions of source code must retain the above copyright 11 | # notice, this list of conditions and the following disclaimer. 12 | # * Redistributions in binary form must reproduce the above 13 | # copyright notice, this list of conditions and the following 14 | # disclaimer in the documentation and/or other materials provided 15 | # with the distribution. 16 | # * Neither the name of Willow Garage, Inc. nor the names of its 17 | # contributors may be used to endorse or promote products derived 18 | # from this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | # 33 | # Revision $Id$ 34 | 35 | import time 36 | 37 | from roswtf.rules import warning_rule, error_rule 38 | import rosgraph 39 | import rospy 40 | 41 | import tf.msg 42 | 43 | 44 | # global list of messages received 45 | _msgs = [] 46 | 47 | ################################################################################ 48 | # RULES 49 | 50 | def rostime_delta(ctx): 51 | deltas = {} 52 | for m, stamp, callerid in _msgs: 53 | for t in m.transforms: 54 | d = t.header.stamp - stamp 55 | secs = d.to_sec() 56 | if abs(secs) > 1.: 57 | if callerid in deltas: 58 | if abs(secs) > abs(deltas[callerid]): 59 | deltas[callerid] = secs 60 | else: 61 | deltas[callerid] = secs 62 | 63 | errors = [] 64 | for k, v in deltas.iteritems(): 65 | errors.append("receiving transform from [%s] that differed from ROS time by %ss"%(k, v)) 66 | return errors 67 | 68 | def reparenting(ctx): 69 | errors = [] 70 | parent_id_map = {} 71 | for m, stamp, callerid in _msgs: 72 | for t in m.transforms: 73 | frame_id = t.child_frame_id 74 | parent_id = t.header.frame_id 75 | if frame_id in parent_id_map and parent_id_map[frame_id] != parent_id: 76 | msg = "reparenting of [%s] to [%s] by [%s]"%(frame_id, parent_id, callerid) 77 | parent_id_map[frame_id] = parent_id 78 | if msg not in errors: 79 | errors.append(msg) 80 | else: 81 | parent_id_map[frame_id] = parent_id 82 | return errors 83 | 84 | def cycle_detection(ctx): 85 | max_depth = 100 86 | errors = [] 87 | parent_id_map = {} 88 | for m, stamp, callerid in _msgs: 89 | for t in m.transforms: 90 | frame_id = t.child_frame_id 91 | parent_id = t.header.frame_id 92 | parent_id_map[frame_id] = parent_id 93 | 94 | for frame in parent_id_map: 95 | frame_list = [] 96 | current_frame = frame 97 | count = 0 98 | while count < max_depth + 2: 99 | count = count + 1 100 | frame_list.append(current_frame) 101 | try: 102 | current_frame = parent_id_map[current_frame] 103 | except KeyError: 104 | break 105 | if current_frame == frame: 106 | errors.append("Frame %s is in a loop. It's loop has elements:\n%s"% (frame, " -> ".join(frame_list))) 107 | break 108 | 109 | 110 | return errors 111 | 112 | def multiple_authority(ctx): 113 | errors = [] 114 | authority_map = {} 115 | for m, stamp, callerid in _msgs: 116 | for t in m.transforms: 117 | frame_id = t.child_frame_id 118 | parent_id = t.header.frame_id 119 | if frame_id in authority_map and authority_map[frame_id] != callerid: 120 | msg = "node [%s] publishing transform [%s] with parent [%s] already published by node [%s]"%(callerid, frame_id, parent_id, authority_map[frame_id]) 121 | authority_map[frame_id] = callerid 122 | if msg not in errors: 123 | errors.append(msg) 124 | else: 125 | authority_map[frame_id] = callerid 126 | return errors 127 | 128 | def no_msgs(ctx): 129 | return not _msgs 130 | 131 | ################################################################################ 132 | # roswtf PLUGIN 133 | 134 | # tf_warnings and tf_errors declare the rules that we actually check 135 | 136 | tf_warnings = [ 137 | (no_msgs, "No tf messages"), 138 | (rostime_delta, "Received out-of-date/future transforms:"), 139 | ] 140 | tf_errors = [ 141 | (reparenting, "TF re-parenting contention:"), 142 | (cycle_detection, "TF cycle detection::"), 143 | (multiple_authority, "TF multiple authority contention:"), 144 | ] 145 | 146 | # rospy subscriber callback for /tf 147 | def _tf_handle(msg): 148 | _msgs.append((msg, rospy.get_rostime(), msg._connection_header['callerid'])) 149 | 150 | # @return bool: True if /tf has a publisher 151 | def is_tf_active(): 152 | master = rosgraph.Master('/tfwtf') 153 | if master is not None: 154 | try: 155 | val = master.getPublishedTopics('/') 156 | if filter(lambda x: x[0] == '/tf', val): 157 | return True 158 | except: 159 | pass 160 | return False 161 | 162 | # roswtf entry point for online checks 163 | def roswtf_plugin_online(ctx): 164 | # don't run plugin if tf isn't active as these checks take awhile 165 | if not is_tf_active(): 166 | return 167 | 168 | print "running tf checks, this will take a second..." 169 | sub1 = rospy.Subscriber('/tf', tf.msg.tfMessage, _tf_handle) 170 | time.sleep(1.0) 171 | sub1.unregister() 172 | print "... tf checks complete" 173 | 174 | for r in tf_warnings: 175 | warning_rule(r, r[0](ctx), ctx) 176 | for r in tf_errors: 177 | error_rule(r, r[0](ctx), ctx) 178 | 179 | 180 | # currently no static checks for tf 181 | #def roswtf_plugin_static(ctx): 182 | # for r in tf_warnings: 183 | # warning_rule(r, r[0](ctx), ctx) 184 | # for r in tf_errors: 185 | # error_rule(r, r[0](ctx), ctx) 186 | 187 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(tf) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | find_package(Boost REQUIRED thread signals) 7 | find_package(catkin REQUIRED COMPONENTS 8 | angles geometry_msgs message_filters message_generation rosconsole 9 | roscpp rostest rostime sensor_msgs std_msgs tf2_ros 10 | ) 11 | 12 | catkin_python_setup() 13 | 14 | include_directories(SYSTEM ${Boost_INCLUDE_DIR} 15 | ${catkin_INCLUDE_DIRS} 16 | ) 17 | include_directories(include) 18 | link_directories(${catkin_LIBRARY_DIRS}) 19 | 20 | add_message_files(DIRECTORY msg FILES tfMessage.msg) 21 | add_service_files(DIRECTORY srv FILES FrameGraph.srv) 22 | 23 | generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs) 24 | 25 | catkin_package( 26 | INCLUDE_DIRS include 27 | LIBRARIES ${PROJECT_NAME} 28 | CATKIN_DEPENDS geometry_msgs message_filters message_runtime 29 | roscpp sensor_msgs std_msgs tf2_ros rosconsole 30 | ) 31 | 32 | add_library(${PROJECT_NAME} src/tf.cpp src/transform_listener.cpp src/cache.cpp src/transform_broadcaster.cpp) 33 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 34 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) 35 | 36 | # Debug 37 | add_executable(tf_empty_listener src/empty_listener.cpp) 38 | target_link_libraries(tf_empty_listener ${PROJECT_NAME}) 39 | 40 | add_executable(tf_echo src/tf_echo.cpp) 41 | target_link_libraries(tf_echo ${PROJECT_NAME}) 42 | 43 | add_executable(tf_change_notifier src/change_notifier.cpp) 44 | target_link_libraries(tf_change_notifier ${PROJECT_NAME}) 45 | 46 | add_executable(tf_monitor src/tf_monitor.cpp) 47 | target_link_libraries(tf_monitor ${PROJECT_NAME}) 48 | 49 | add_executable(static_transform_publisher src/static_transform_publisher.cpp) 50 | target_link_libraries(static_transform_publisher ${PROJECT_NAME}) 51 | 52 | # Tests 53 | if(CATKIN_ENABLE_TESTING) 54 | 55 | catkin_add_gtest(tf_unittest test/tf_unittest.cpp) 56 | target_link_libraries(tf_unittest ${PROJECT_NAME}) 57 | 58 | catkin_add_gtest(tf_quaternion_unittest test/quaternion.cpp) 59 | target_link_libraries(tf_quaternion_unittest ${PROJECT_NAME}) 60 | 61 | catkin_add_gtest(test_transform_datatypes test/test_transform_datatypes.cpp) 62 | target_link_libraries(test_transform_datatypes ${PROJECT_NAME}) 63 | 64 | add_executable(transform_listener_unittest test/transform_listener_unittest.cpp) 65 | target_link_libraries(transform_listener_unittest ${PROJECT_NAME} ${GTEST_LIBRARIES}) 66 | add_rostest(test/transform_listener_unittest.launch) 67 | 68 | # Disabled because of changes in TransformStorage 69 | #catkin_add_gtest_future(tf_unittest_future test/tf_unittest_future.cpp) 70 | #target_link_libraries(tf_unittest_future ${PROJECT_NAME}) 71 | 72 | catkin_add_gtest(test_velocity test/velocity_test.cpp) 73 | target_link_libraries(test_velocity ${PROJECT_NAME}) 74 | 75 | #add_executable(test_transform_twist test/transform_twist_test.cpp) 76 | #target_link_libraries(test_transform_twist ${PROJECT_NAME}) 77 | #catkin_add_gtest_build_flags(test_transform_twist) 78 | #add_rostest(test/transform_twist_test.launch) 79 | 80 | catkin_add_gtest(cache_unittest test/cache_unittest.cpp) 81 | target_link_libraries(cache_unittest ${PROJECT_NAME}) 82 | 83 | 84 | add_executable(test_message_filter EXCLUDE_FROM_ALL test/test_message_filter.cpp) 85 | target_link_libraries(test_message_filter tf ${Boost_LIBRARIES} ${GTEST_LIBRARIES}) 86 | add_rostest(test/test_message_filter.xml) 87 | 88 | 89 | ### Benchmarking 90 | #catkin_add_gtest_future(tf_benchmark test/tf_benchmark.cpp) 91 | #target_link_libraries(tf_benchmark ${PROJECT_NAME}) 92 | 93 | add_executable(testListener test/testListener.cpp) 94 | target_link_libraries(testListener ${PROJECT_NAME} ${GTEST_LIBRARIES}) 95 | add_rostest(test/test_broadcaster.launch) 96 | 97 | add_executable(testBroadcaster test/testBroadcaster.cpp) 98 | target_link_libraries(testBroadcaster ${PROJECT_NAME}) 99 | 100 | endif() 101 | 102 | 103 | #Python setup 104 | set(Python_ADDITIONAL_VERSIONS 2.7) 105 | find_package(PythonLibs REQUIRED) 106 | include_directories(${PYTHON_INCLUDE_PATH}) 107 | 108 | # # If on Darwin, create a symlink _foo.so -> _foo.dylib, because the 109 | # # MacPorts version of Python won't find _foo.dylib for 'import _foo' 110 | # include(CMakeDetermineSystem) 111 | # if(CMAKE_SYSTEM_NAME MATCHES "Darwin") 112 | # add_custom_command(OUTPUT ${LIBRARY_OUTPUT_PATH}/_${PROJECT_NAME}_swig.so 113 | # COMMAND cmake -E create_symlink ${LIBRARY_OUTPUT_PATH}/_${PROJECT_NAME}_swig.dylib ${LIBRARY_OUTPUT_PATH}/_${PROJECT_NAME}_swig.so 114 | # DEPENDS python_${PROJECT_NAME}) 115 | # add_custom_target(symlink_darwin_lib ALL 116 | # DEPENDS ${LIBRARY_OUTPUT_PATH}/_${PROJECT_NAME}_swig.so) 117 | # endif(CMAKE_SYSTEM_NAME MATCHES "Darwin") 118 | 119 | 120 | # Check for SSE 121 | #check_for_sse() 122 | 123 | # Dynamic linking with tf worked OK, except for exception propagation, which failed in the unit test. 124 | # so build with the objects directly instead. 125 | 126 | link_libraries(${PYTHON_LIBRARIES}) 127 | add_library(pytf_py src/pytf.cpp src/tf.cpp src/transform_listener.cpp src/cache.cpp) 128 | add_dependencies(pytf_py ${PROJECT_NAME}_gencpp) 129 | 130 | find_package(PythonLibs REQUIRED) 131 | set_target_properties(pytf_py PROPERTIES OUTPUT_NAME tf PREFIX "_" SUFFIX ".so" 132 | LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/tf) 133 | 134 | # target_link_libraries(pytf_py ${PROJECT_NAME}) 135 | # add_compile_flags(pytf_py -g -Wno-missing-field-initializers) 136 | #add_compile_flags(pytf_py ${SSE_FLAGS}) #conditionally adds sse flags if available 137 | # add_pyunit(test/testPython.py) 138 | # DOES PYUNIT WORK IN CATKIN? 139 | target_link_libraries(pytf_py ${Boost_LIBRARIES} ${catkin_LIBRARIES}) 140 | 141 | if(CATKIN_ENABLE_TESTING) 142 | add_executable(tf_speed_test EXCLUDE_FROM_ALL test/speed_test.cpp) 143 | target_link_libraries(tf_speed_test ${PROJECT_NAME}) 144 | endif() 145 | 146 | 147 | install(DIRECTORY include/${PROJECT_NAME}/ 148 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 149 | 150 | install(TARGETS ${PROJECT_NAME} tf_echo tf_empty_listener tf_change_notifier tf_monitor static_transform_publisher 151 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 153 | 154 | install(TARGETS pytf_py 155 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/${PYTHON_INSTALL_DIR}/tf) 156 | 157 | # Install rosrun-able scripts 158 | install(PROGRAMS 159 | scripts/bullet_migration_sed.py 160 | scripts/tf_remap 161 | scripts/view_frames 162 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) 163 | 164 | # Need to install python msg and srv directories manually because genmsg 165 | # specifically avoids installing python message code if catkin_python_setup() 166 | # has been used. See https://github.com/ros/genmsg/issues/10 167 | install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/tf/msg 168 | DESTINATION ${CMAKE_INSTALL_PREFIX}/${PYTHON_INSTALL_DIR}/tf) 169 | install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/tf/srv 170 | DESTINATION ${CMAKE_INSTALL_PREFIX}/${PYTHON_INSTALL_DIR}/tf) 171 | 172 | 173 | if(TARGET tests) 174 | add_dependencies(tests testBroadcaster testListener transform_listener_unittest test_message_filter ) 175 | endif() 176 | 177 | 178 | -------------------------------------------------------------------------------- /conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # tf documentation build configuration file, created by 4 | # sphinx-quickstart on Mon Jun 1 14:21:53 2009. 5 | # 6 | # This file is execfile()d with the current directory set to its containing dir. 7 | # 8 | # Note that not all possible configuration values are present in this 9 | # autogenerated file. 10 | # 11 | # All configuration values have a default; values that are commented out 12 | # serve to show the default. 13 | 14 | import roslib 15 | roslib.load_manifest('tf') 16 | import sys, os 17 | 18 | # If extensions (or modules to document with autodoc) are in another directory, 19 | # add these directories to sys.path here. If the directory is relative to the 20 | # documentation root, use os.path.abspath to make it absolute, like shown here. 21 | #sys.path.append(os.path.abspath('.')) 22 | 23 | # -- General configuration ----------------------------------------------------- 24 | 25 | # Add any Sphinx extension module names here, as strings. They can be extensions 26 | # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. 27 | extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] 28 | 29 | # Add any paths that contain templates here, relative to this directory. 30 | templates_path = ['_templates'] 31 | 32 | # The suffix of source filenames. 33 | source_suffix = '.rst' 34 | 35 | # The encoding of source files. 36 | #source_encoding = 'utf-8' 37 | 38 | # The master toctree document. 39 | master_doc = 'index' 40 | 41 | # General information about the project. 42 | project = u'tf' 43 | copyright = u'2009, Willow Garage, Inc.' 44 | 45 | # The version info for the project you're documenting, acts as replacement for 46 | # |version| and |release|, also used in various other places throughout the 47 | # built documents. 48 | # 49 | # The short X.Y version. 50 | version = '0.1' 51 | # The full version, including alpha/beta/rc tags. 52 | release = '0.1.0' 53 | 54 | # The language for content autogenerated by Sphinx. Refer to documentation 55 | # for a list of supported languages. 56 | #language = None 57 | 58 | # There are two options for replacing |today|: either, you set today to some 59 | # non-false value, then it is used: 60 | #today = '' 61 | # Else, today_fmt is used as the format for a strftime call. 62 | #today_fmt = '%B %d, %Y' 63 | 64 | # List of documents that shouldn't be included in the build. 65 | #unused_docs = [] 66 | 67 | # List of directories, relative to source directory, that shouldn't be searched 68 | # for source files. 69 | exclude_trees = ['_build'] 70 | 71 | # The reST default role (used for this markup: `text`) to use for all documents. 72 | #default_role = None 73 | 74 | # If true, '()' will be appended to :func: etc. cross-reference text. 75 | #add_function_parentheses = True 76 | 77 | # If true, the current module name will be prepended to all description 78 | # unit titles (such as .. function::). 79 | #add_module_names = True 80 | 81 | # If true, sectionauthor and moduleauthor directives will be shown in the 82 | # output. They are ignored by default. 83 | #show_authors = False 84 | 85 | # The name of the Pygments (syntax highlighting) style to use. 86 | pygments_style = 'sphinx' 87 | 88 | # A list of ignored prefixes for module index sorting. 89 | #modindex_common_prefix = [] 90 | 91 | 92 | # -- Options for HTML output --------------------------------------------------- 93 | 94 | # The theme to use for HTML and HTML Help pages. Major themes that come with 95 | # Sphinx are currently 'default' and 'sphinxdoc'. 96 | html_theme = 'default' 97 | 98 | # Theme options are theme-specific and customize the look and feel of a theme 99 | # further. For a list of options available for each theme, see the 100 | # documentation. 101 | #html_theme_options = {} 102 | 103 | # Add any paths that contain custom themes here, relative to this directory. 104 | #html_theme_path = [] 105 | 106 | # The name for this set of Sphinx documents. If None, it defaults to 107 | # " v documentation". 108 | #html_title = None 109 | 110 | # A shorter title for the navigation bar. Default is the same as html_title. 111 | #html_short_title = None 112 | 113 | # The name of an image file (relative to this directory) to place at the top 114 | # of the sidebar. 115 | #html_logo = None 116 | 117 | # The name of an image file (within the static path) to use as favicon of the 118 | # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 119 | # pixels large. 120 | #html_favicon = None 121 | 122 | # Add any paths that contain custom static files (such as style sheets) here, 123 | # relative to this directory. They are copied after the builtin static files, 124 | # so a file named "default.css" will overwrite the builtin "default.css". 125 | html_static_path = ['_static'] 126 | 127 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 128 | # using the given strftime format. 129 | #html_last_updated_fmt = '%b %d, %Y' 130 | 131 | # If true, SmartyPants will be used to convert quotes and dashes to 132 | # typographically correct entities. 133 | #html_use_smartypants = True 134 | 135 | # Custom sidebar templates, maps document names to template names. 136 | #html_sidebars = {} 137 | 138 | # Additional templates that should be rendered to pages, maps page names to 139 | # template names. 140 | #html_additional_pages = {} 141 | 142 | # If false, no module index is generated. 143 | #html_use_modindex = True 144 | 145 | # If false, no index is generated. 146 | #html_use_index = True 147 | 148 | # If true, the index is split into individual pages for each letter. 149 | #html_split_index = False 150 | 151 | # If true, links to the reST sources are added to the pages. 152 | #html_show_sourcelink = True 153 | 154 | # If true, an OpenSearch description file will be output, and all pages will 155 | # contain a tag referring to it. The value of this option must be the 156 | # base URL from which the finished HTML is served. 157 | #html_use_opensearch = '' 158 | 159 | # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). 160 | #html_file_suffix = '' 161 | 162 | # Output file base name for HTML help builder. 163 | htmlhelp_basename = 'tfdoc' 164 | 165 | 166 | # -- Options for LaTeX output -------------------------------------------------- 167 | 168 | # The paper size ('letter' or 'a4'). 169 | #latex_paper_size = 'letter' 170 | 171 | # The font size ('10pt', '11pt' or '12pt'). 172 | #latex_font_size = '10pt' 173 | 174 | # Grouping the document tree into LaTeX files. List of tuples 175 | # (source start file, target name, title, author, documentclass [howto/manual]). 176 | latex_documents = [ 177 | ('index', 'tf.tex', u'stereo\\_utils Documentation', 178 | u'James Bowman', 'manual'), 179 | ] 180 | 181 | # The name of an image file (relative to this directory) to place at the top of 182 | # the title page. 183 | #latex_logo = None 184 | 185 | # For "manual" documents, if this is true, then toplevel headings are parts, 186 | # not chapters. 187 | #latex_use_parts = False 188 | 189 | # Additional stuff for the LaTeX preamble. 190 | #latex_preamble = '' 191 | 192 | # Documents to append as an appendix to all manuals. 193 | #latex_appendices = [] 194 | 195 | # If false, no module index is generated. 196 | #latex_use_modindex = True 197 | 198 | 199 | # Example configuration for intersphinx: refer to the Python standard library. 200 | intersphinx_mapping = { 201 | 'http://docs.python.org/': None, 202 | 'http://opencv.willowgarage.com/documentation/': None, 203 | 'http://opencv.willowgarage.com/documentation/python/': None, 204 | 'http://docs.scipy.org/doc/numpy' : None 205 | } 206 | -------------------------------------------------------------------------------- /test/speed_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | using namespace tf; 38 | 39 | int main(int argc, char** argv) 40 | { 41 | uint32_t num_levels = 10; 42 | if (argc > 1) 43 | { 44 | num_levels = boost::lexical_cast(argv[1]); 45 | } 46 | 47 | tf::Transformer bc; 48 | StampedTransform t; 49 | t.stamp_ = ros::Time(1); 50 | t.frame_id_ = "root"; 51 | t.child_frame_id_ = "0"; 52 | t.setIdentity(); 53 | t.setOrigin(tf::Vector3(1,0,0)); 54 | bc.setTransform(t, "me"); 55 | t.stamp_ = ros::Time(2); 56 | bc.setTransform(t, "me"); 57 | 58 | for (uint32_t i = 1; i < num_levels/2; ++i) 59 | { 60 | for (uint32_t j = 1; j < 3; ++j) 61 | { 62 | std::stringstream parent_ss; 63 | parent_ss << (i - 1); 64 | std::stringstream child_ss; 65 | child_ss << i; 66 | 67 | t.stamp_ = ros::Time(j); 68 | t.frame_id_ = parent_ss.str(); 69 | t.child_frame_id_ = child_ss.str(); 70 | bc.setTransform(t, "me"); 71 | } 72 | } 73 | 74 | t.frame_id_ = "root"; 75 | std::stringstream ss; 76 | ss << num_levels/2; 77 | t.stamp_ = ros::Time(1); 78 | t.child_frame_id_ = ss.str(); 79 | bc.setTransform(t, "me"); 80 | t.stamp_ = ros::Time(2); 81 | bc.setTransform(t, "me"); 82 | 83 | for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i) 84 | { 85 | for (uint32_t j = 1; j < 3; ++j) 86 | { 87 | std::stringstream parent_ss; 88 | parent_ss << (i - 1); 89 | std::stringstream child_ss; 90 | child_ss << i; 91 | 92 | t.stamp_ = ros::Time(j); 93 | t.frame_id_ = parent_ss.str(); 94 | t.child_frame_id_ = child_ss.str(); 95 | bc.setTransform(t, "me"); 96 | } 97 | } 98 | 99 | //ROS_INFO_STREAM(bc.allFramesAsYAML()); 100 | 101 | std::string v_frame0 = boost::lexical_cast(num_levels - 1); 102 | std::string v_frame1 = boost::lexical_cast(num_levels/2 - 1); 103 | ROS_INFO("%s to %s", v_frame0.c_str(), v_frame1.c_str()); 104 | StampedTransform out_t; 105 | 106 | const uint32_t count = 1000000; 107 | ROS_INFO("Doing %d %d-level tests", count, num_levels); 108 | 109 | #if 01 110 | { 111 | ros::WallTime start = ros::WallTime::now(); 112 | for (uint32_t i = 0; i < count; ++i) 113 | { 114 | bc.lookupTransform(v_frame1, v_frame0, ros::Time(0), out_t); 115 | } 116 | ros::WallTime end = ros::WallTime::now(); 117 | ros::WallDuration dur = end - start; 118 | //ROS_INFO_STREAM(out_t); 119 | ROS_INFO("lookupTransform at Time(0) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); 120 | } 121 | #endif 122 | 123 | #if 01 124 | { 125 | ros::WallTime start = ros::WallTime::now(); 126 | for (uint32_t i = 0; i < count; ++i) 127 | { 128 | bc.lookupTransform(v_frame1, v_frame0, ros::Time(1), out_t); 129 | } 130 | ros::WallTime end = ros::WallTime::now(); 131 | ros::WallDuration dur = end - start; 132 | //ROS_INFO_STREAM(out_t); 133 | ROS_INFO("lookupTransform at Time(1) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); 134 | } 135 | #endif 136 | 137 | #if 01 138 | { 139 | ros::WallTime start = ros::WallTime::now(); 140 | for (uint32_t i = 0; i < count; ++i) 141 | { 142 | bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5), out_t); 143 | } 144 | ros::WallTime end = ros::WallTime::now(); 145 | ros::WallDuration dur = end - start; 146 | //ROS_INFO_STREAM(out_t); 147 | ROS_INFO("lookupTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); 148 | } 149 | #endif 150 | 151 | #if 01 152 | { 153 | ros::WallTime start = ros::WallTime::now(); 154 | for (uint32_t i = 0; i < count; ++i) 155 | { 156 | bc.lookupTransform(v_frame1, v_frame0, ros::Time(2), out_t); 157 | } 158 | ros::WallTime end = ros::WallTime::now(); 159 | ros::WallDuration dur = end - start; 160 | //ROS_INFO_STREAM(out_t); 161 | ROS_INFO("lookupTransform at Time(2) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); 162 | } 163 | #endif 164 | 165 | #if 01 166 | { 167 | ros::WallTime start = ros::WallTime::now(); 168 | for (uint32_t i = 0; i < count; ++i) 169 | { 170 | bc.canTransform(v_frame1, v_frame0, ros::Time(0)); 171 | } 172 | ros::WallTime end = ros::WallTime::now(); 173 | ros::WallDuration dur = end - start; 174 | //ROS_INFO_STREAM(out_t); 175 | ROS_INFO("canTransform at Time(0) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); 176 | } 177 | #endif 178 | 179 | #if 01 180 | { 181 | ros::WallTime start = ros::WallTime::now(); 182 | for (uint32_t i = 0; i < count; ++i) 183 | { 184 | bc.canTransform(v_frame1, v_frame0, ros::Time(1)); 185 | } 186 | ros::WallTime end = ros::WallTime::now(); 187 | ros::WallDuration dur = end - start; 188 | //ROS_INFO_STREAM(out_t); 189 | ROS_INFO("canTransform at Time(1) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); 190 | } 191 | #endif 192 | 193 | #if 01 194 | { 195 | ros::WallTime start = ros::WallTime::now(); 196 | for (uint32_t i = 0; i < count; ++i) 197 | { 198 | bc.canTransform(v_frame1, v_frame0, ros::Time(1.5)); 199 | } 200 | ros::WallTime end = ros::WallTime::now(); 201 | ros::WallDuration dur = end - start; 202 | //ROS_INFO_STREAM(out_t); 203 | ROS_INFO("canTransform at Time(1.5) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); 204 | } 205 | #endif 206 | 207 | #if 01 208 | { 209 | ros::WallTime start = ros::WallTime::now(); 210 | for (uint32_t i = 0; i < count; ++i) 211 | { 212 | bc.canTransform(v_frame1, v_frame0, ros::Time(2)); 213 | } 214 | ros::WallTime end = ros::WallTime::now(); 215 | ros::WallDuration dur = end - start; 216 | //ROS_INFO_STREAM(out_t); 217 | ROS_INFO("canTransform at Time(2) took %.3fs for an average of %.3f us", dur.toSec(), dur.toSec() * 1.0e6 / (double)count); 218 | } 219 | #endif 220 | } 221 | -------------------------------------------------------------------------------- /test/tf_benchmark.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "tf/LinearMath/Vector3.h" 35 | 36 | void seed_rand() 37 | { 38 | //Seed random number generator with current microseond count 39 | timeval temp_time_struct; 40 | gettimeofday(&temp_time_struct,NULL); 41 | srand(temp_time_struct.tv_usec); 42 | }; 43 | 44 | void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) 45 | { 46 | seed_rand(); 47 | for ( uint64_t i = 0; i < runs ; i++ ) 48 | { 49 | xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 50 | yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 51 | zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 52 | } 53 | } 54 | 55 | 56 | using namespace tf; 57 | 58 | 59 | TEST(tf_benchmark, canTransformCacheLength0) 60 | { 61 | tf::Transformer mTR(true); 62 | 63 | uint64_t runs = 100000; 64 | ros::WallTime start_time = ros::WallTime::now(); 65 | 66 | 67 | start_time = ros::WallTime::now(); 68 | for (uint64_t i = 0 ; i < runs; i++) 69 | { 70 | EXPECT_FALSE(mTR.canTransform("target","source_frame", ros::Time::now())); 71 | } 72 | ros::WallDuration run_duration = ros::WallTime::now() - start_time; 73 | double frequency = (double)runs / run_duration.toSec() ; 74 | printf("can frequency %.2f KHz\n", frequency / 1000.0); 75 | EXPECT_GT( frequency, 10000.0); 76 | } 77 | 78 | 79 | TEST(tf_benchmark, canTransformCacheLength10000) 80 | { 81 | tf::Transformer mTR(true); 82 | 83 | unsigned int cache_length = 10000; 84 | for (unsigned int i = 0; i < cache_length; i++) 85 | { 86 | StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child"); 87 | mTR.setTransform(tranStamped); 88 | } 89 | 90 | uint64_t runs = 100000; 91 | ros::WallTime start_time = ros::WallTime::now(); 92 | 93 | 94 | //Worst case 95 | start_time = ros::WallTime::now(); 96 | for (uint64_t i = 0 ; i < runs; i++) 97 | { 98 | EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10))); 99 | } 100 | ros::WallDuration run_duration = ros::WallTime::now() - start_time; 101 | double frequency = (double)runs / run_duration.toSec() ; 102 | printf("Worst Case Frequency %.2f KHz\n", frequency / 1000.0); 103 | EXPECT_GT( frequency, 10000.0); 104 | 105 | //Worst case 106 | start_time = ros::WallTime::now(); 107 | for (uint64_t i = 0 ; i < runs; i++) 108 | { 109 | EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2))); 110 | } 111 | run_duration = ros::WallTime::now() - start_time; 112 | frequency = (double)runs / run_duration.toSec() ; 113 | printf("Intermediate Case Frequency %.2f KHz\n", frequency / 1000.0); 114 | EXPECT_GT( frequency, 10000.0); 115 | 116 | //Best case 117 | start_time = ros::WallTime::now(); 118 | for (uint64_t i = 0 ; i < runs; i++) 119 | { 120 | EXPECT_TRUE(mTR.canTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1))); 121 | } 122 | run_duration = ros::WallTime::now() - start_time; 123 | frequency = (double)runs / run_duration.toSec() ; 124 | printf("Best Case Frequency %.2f KHz\n", frequency / 1000.0); 125 | EXPECT_GT( frequency, 10000.0); 126 | } 127 | 128 | TEST(tf_benchmark, lookupTransformCacheLength10000) 129 | { 130 | tf::Transformer mTR(true); 131 | 132 | unsigned int cache_length = 10000; 133 | for (unsigned int i = 0; i < cache_length; i++) 134 | { 135 | StampedTransform tranStamped(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0+i), "my_parent", "child"); 136 | mTR.setTransform(tranStamped); 137 | } 138 | 139 | uint64_t runs = 100000; 140 | ros::WallTime start_time = ros::WallTime::now(); 141 | 142 | StampedTransform rv; 143 | //Worst case 144 | start_time = ros::WallTime::now(); 145 | for (uint64_t i = 0 ; i < runs; i++) 146 | { 147 | mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10), rv); 148 | } 149 | ros::WallDuration run_duration = ros::WallTime::now() - start_time; 150 | double frequency = (double)runs / run_duration.toSec() ; 151 | printf("Worst Case Lookup Frequency %.2f KHz\n", frequency / 1000.0); 152 | EXPECT_GT( frequency, 10000.0); 153 | 154 | //Worst case 155 | start_time = ros::WallTime::now(); 156 | for (uint64_t i = 0 ; i < runs; i++) 157 | { 158 | mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10+cache_length/2), rv); 159 | } 160 | run_duration = ros::WallTime::now() - start_time; 161 | frequency = (double)runs / run_duration.toSec() ; 162 | printf("Intermediate Case Lookup Frequency %.2f KHz\n", frequency / 1000.0); 163 | EXPECT_GT( frequency, 10000.0); 164 | 165 | //Best case 166 | start_time = ros::WallTime::now(); 167 | for (uint64_t i = 0 ; i < runs; i++) 168 | { 169 | mTR.lookupTransform("child","my_parent", ros::Time().fromNSec(10+cache_length -1), rv); 170 | } 171 | run_duration = ros::WallTime::now() - start_time; 172 | frequency = (double)runs / run_duration.toSec() ; 173 | printf("Best Case Lookup Frequency %.2f KHz\n", frequency / 1000.0); 174 | EXPECT_GT( frequency, 10000.0); 175 | } 176 | 177 | TEST(tf_benchmark, benchmarkExhaustiveSearch) 178 | { 179 | uint64_t runs = 40000; 180 | double epsilon = 1e-6; 181 | seed_rand(); 182 | 183 | tf::Transformer mTR(true); 184 | std::vector xvalues(runs), yvalues(runs), zvalues(runs); 185 | for ( uint64_t i = 0; i < runs ; i++ ) 186 | { 187 | xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 188 | yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 189 | zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; 190 | 191 | 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"); 192 | mTR.setTransform(tranStamped); 193 | } 194 | 195 | ros::WallTime start_time = ros::WallTime::now(); 196 | for ( uint64_t i = 0; i < runs ; i++ ) 197 | 198 | { 199 | Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(10.0 + i), "child"); 200 | 201 | try{ 202 | Stamped outpose; 203 | outpose.setIdentity(); //to make sure things are getting mutated 204 | mTR.transformPose("my_parent",inpose, outpose); 205 | EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon); 206 | EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon); 207 | EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon); 208 | } 209 | catch (tf::TransformException & ex) 210 | { 211 | std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; 212 | bool exception_improperly_thrown = true; 213 | EXPECT_FALSE(exception_improperly_thrown); 214 | } 215 | } 216 | 217 | Stamped inpose (tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(0,0,0)), ros::Time().fromNSec(runs), "child"); 218 | Stamped outpose; 219 | outpose.setIdentity(); //to make sure things are getting mutated 220 | mTR.transformPose("child",inpose, outpose); 221 | EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon); 222 | EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon); 223 | EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon); 224 | 225 | ros::WallDuration run_duration = ros::WallTime::now() - start_time; 226 | double frequency = (double)runs / run_duration.toSec() ; 227 | printf("exhaustive search frequency %.2f KHz\n", frequency / 1000.0); 228 | EXPECT_GT( frequency, 500.0); 229 | 230 | } 231 | 232 | int main(int argc, char **argv){ 233 | ros::Time::init(); 234 | testing::InitGoogleTest(&argc, argv); 235 | return RUN_ALL_TESTS(); 236 | } 237 | -------------------------------------------------------------------------------- /include/tf/transform_listener.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #ifndef TF_TRANSFORMLISTENER_H 33 | #define TF_TRANSFORMLISTENER_H 34 | 35 | #include "sensor_msgs/PointCloud.h" 36 | #include "std_msgs/Empty.h" 37 | #include "tf/tfMessage.h" 38 | #include "tf/tf.h" 39 | #include "ros/ros.h" 40 | #include "ros/callback_queue.h" 41 | 42 | #include "tf/FrameGraph.h" //frame graph service 43 | #include "boost/thread.hpp" 44 | 45 | #include 46 | 47 | 48 | namespace tf{ 49 | 50 | /** \brief Get the tf_prefix from the parameter server 51 | * \param nh The node handle to use to lookup the parameter. 52 | * \return The tf_prefix value for this NodeHandle 53 | */ 54 | inline std::string getPrefixParam(ros::NodeHandle & nh) { 55 | std::string param; 56 | if (!nh.searchParam("tf_prefix", param)) 57 | return ""; 58 | 59 | std::string return_val; 60 | nh.getParam(param, return_val); 61 | return return_val; 62 | } 63 | 64 | /** \brief resolve names 65 | * \deprecated Use TransformListener::remap instead */ 66 | std::string remap(const std::string& frame_id) __attribute__((deprecated)); 67 | 68 | 69 | class TransformerHelper: public Transformer { 70 | 71 | public: 72 | TransformerHelper(ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME)); 73 | 74 | virtual ~TransformerHelper(); 75 | 76 | /* Methods from transformer unhiding them here */ 77 | using Transformer::transformQuaternion; 78 | using Transformer::transformVector; 79 | using Transformer::transformPoint; 80 | using Transformer::transformPose; 81 | 82 | 83 | /** \brief Transform a Stamped Quaternion Message into the target frame 84 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 85 | void transformQuaternion(const std::string& target_frame, const geometry_msgs::QuaternionStamped& stamped_in, geometry_msgs::QuaternionStamped& stamped_out) const; 86 | /** \brief Transform a Stamped Vector Message into the target frame 87 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 88 | void transformVector(const std::string& target_frame, const geometry_msgs::Vector3Stamped& stamped_in, geometry_msgs::Vector3Stamped& stamped_out) const; 89 | /** \brief Transform a Stamped Point Message into the target frame 90 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 91 | void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const; 92 | /** \brief Transform a Stamped Pose Message into the target frame 93 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 94 | void transformPose(const std::string& target_frame, const geometry_msgs::PoseStamped& stamped_in, geometry_msgs::PoseStamped& stamped_out) const; 95 | 96 | /** \brief Transform a Stamped Twist Message into the target frame 97 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 98 | // http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review 99 | // void transformTwist(const std::string& target_frame, const geometry_msgs::TwistStamped& stamped_in, geometry_msgs::TwistStamped& stamped_out) const; 100 | 101 | /** \brief Transform a Stamped Quaternion Message into the target frame 102 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 103 | void transformQuaternion(const std::string& target_frame, const ros::Time& target_time, 104 | const geometry_msgs::QuaternionStamped& qin, 105 | const std::string& fixed_frame, geometry_msgs::QuaternionStamped& qout) const; 106 | /** \brief Transform a Stamped Vector Message into the target frame and time 107 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 108 | void transformVector(const std::string& target_frame, const ros::Time& target_time, 109 | const geometry_msgs::Vector3Stamped& vin, 110 | const std::string& fixed_frame, geometry_msgs::Vector3Stamped& vout) const; 111 | /** \brief Transform a Stamped Point Message into the target frame and time 112 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 113 | void transformPoint(const std::string& target_frame, const ros::Time& target_time, 114 | const geometry_msgs::PointStamped& pin, 115 | const std::string& fixed_frame, geometry_msgs::PointStamped& pout) const; 116 | /** \brief Transform a Stamped Pose Message into the target frame and time 117 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 118 | void transformPose(const std::string& target_frame, const ros::Time& target_time, 119 | const geometry_msgs::PoseStamped& pin, 120 | const std::string& fixed_frame, geometry_msgs::PoseStamped& pout) const; 121 | 122 | 123 | /** \brief Transform a sensor_msgs::PointCloud natively 124 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 125 | void transformPointCloud(const std::string& target_frame, const sensor_msgs::PointCloud& pcin, sensor_msgs::PointCloud& pcout) const; 126 | 127 | /** @brief Transform a sensor_msgs::PointCloud in space and time 128 | * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */ 129 | void transformPointCloud(const std::string& target_frame, const ros::Time& target_time, 130 | const sensor_msgs::PointCloud& pcin, 131 | const std::string& fixed_frame, sensor_msgs::PointCloud& pcout) const; 132 | 133 | 134 | 135 | ///\todo move to high precision laser projector class void projectAndTransformLaserScan(const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& pcout); 136 | 137 | bool getFrames(tf::FrameGraph::Request& req, tf::FrameGraph::Response& res) 138 | { 139 | res.dot_graph = allFramesAsDot(); 140 | return true; 141 | } 142 | 143 | /* \brief Resolve frame_name into a frame_id using tf_prefix parameter */ 144 | std::string resolve(const std::string& frame_name) 145 | { 146 | ros::NodeHandle n("~"); 147 | std::string prefix = tf::getPrefixParam(n); 148 | return tf::resolve(prefix, frame_name); 149 | } 150 | 151 | private: 152 | /** @brief a helper function to be used for both transfrom pointCloud methods */ 153 | 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; 154 | }; 155 | 156 | 157 | /** \brief This class inherits from Transformer and automatically subscribes to ROS transform messages */ 158 | class TransformListener : public TransformerHelper { //subscribes to message and automatically stores incoming data 159 | 160 | public: 161 | /**@brief Constructor for transform listener 162 | * \param max_cache_time How long to store transform information */ 163 | TransformListener(ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true); 164 | 165 | /** 166 | * \brief Alternate constructor for transform listener 167 | * \param nh The NodeHandle to use for any ROS interaction 168 | * \param max_cache_time How long to store transform information 169 | */ 170 | TransformListener(const ros::NodeHandle& nh, 171 | ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true); 172 | 173 | ~TransformListener(); 174 | 175 | protected: 176 | bool ok() const; 177 | 178 | private: 179 | 180 | // Must be above the listener 181 | ros::NodeHandle node_; 182 | 183 | /// replacing implementation with tf2_ros' 184 | tf2_ros::TransformListener tf2_listener_; 185 | }; 186 | } 187 | 188 | #endif //TF_TRANSFORMLISTENER_H 189 | -------------------------------------------------------------------------------- /src/change_notifier.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2009, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #include "ros/ros.h" 33 | #include "tf/transform_listener.h" 34 | #include "XmlRpcValue.h" 35 | 36 | class FramePair 37 | { 38 | public: 39 | FramePair(const std::string& source_frame, const std::string& target_frame, double translational_update_distance, double angular_update_distance) : 40 | source_frame_(source_frame), 41 | target_frame_(target_frame), 42 | translational_update_distance_(translational_update_distance), 43 | angular_update_distance_(angular_update_distance) 44 | { 45 | pose_in_ = tf::Stamped(tf::Pose(tf::createIdentityQuaternion(), tf::Vector3(0, 0, 0)), ros::Time(), source_frame_); 46 | } 47 | 48 | public: 49 | std::string source_frame_; 50 | std::string target_frame_; 51 | 52 | tf::Stamped pose_in_; 53 | tf::Stamped pose_out_; 54 | tf::Stamped last_sent_pose_; 55 | 56 | double translational_update_distance_; 57 | double angular_update_distance_; 58 | }; 59 | 60 | bool getFramePairs(const ros::NodeHandle& local_node, std::vector& frame_pairs, double default_translational_update_distance, double default_angular_update_distance) 61 | { 62 | XmlRpc::XmlRpcValue frame_pairs_param; 63 | if (!local_node.getParam("frame_pairs", frame_pairs_param)) 64 | { 65 | // No frame_pairs parameter provided. Default to base_link->map. 66 | frame_pairs.push_back(FramePair("base_link", "map", default_translational_update_distance, default_angular_update_distance)); 67 | return true; 68 | } 69 | 70 | if (frame_pairs_param.getType() != XmlRpc::XmlRpcValue::TypeArray) 71 | { 72 | ROS_ERROR("Expecting a list for frame_pairs parameter"); 73 | return false; 74 | } 75 | for (int i = 0; i < frame_pairs_param.size(); i++) 76 | { 77 | XmlRpc::XmlRpcValue frame_pair_param = frame_pairs_param[i]; 78 | if (frame_pair_param.getType() != XmlRpc::XmlRpcValue::TypeStruct) 79 | { 80 | ROS_ERROR("frame_pairs must be specified as maps, but they are XmlRpcType: %d", frame_pair_param.getType()); 81 | return false; 82 | } 83 | 84 | // Get the source_frame 85 | if (!frame_pair_param.hasMember("source_frame")) 86 | { 87 | ROS_ERROR("frame_pair does not specified source_frame"); 88 | return false; 89 | } 90 | XmlRpc::XmlRpcValue source_frame_param = frame_pair_param["source_frame"]; 91 | if (source_frame_param.getType() != XmlRpc::XmlRpcValue::TypeString) 92 | { 93 | ROS_ERROR("source_frame must be a string, but it is XmlRpcType: %d", source_frame_param.getType()); 94 | return false; 95 | } 96 | std::string source_frame = source_frame_param; 97 | 98 | // Get the target_frame 99 | if (!frame_pair_param.hasMember("target_frame")) 100 | { 101 | ROS_ERROR("frame_pair does not specified target_frame"); 102 | return false; 103 | } 104 | XmlRpc::XmlRpcValue target_frame_param = frame_pair_param["target_frame"]; 105 | if (target_frame_param.getType() != XmlRpc::XmlRpcValue::TypeString) 106 | { 107 | ROS_ERROR("target_frame must be a string, but it is XmlRpcType: %d", target_frame_param.getType()); 108 | return false; 109 | } 110 | std::string target_frame = target_frame_param; 111 | 112 | // Get the (optional) translational_update_distance 113 | double translational_update_distance = default_translational_update_distance; 114 | if (frame_pair_param.hasMember("translational_update_distance")) 115 | { 116 | XmlRpc::XmlRpcValue translational_update_distance_param = frame_pair_param["translational_update_distance"]; 117 | if (translational_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeDouble && 118 | translational_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeInt) 119 | { 120 | ROS_ERROR("translational_update_distance must be either an integer or a double, but it is XmlRpcType: %d", translational_update_distance_param.getType()); 121 | return false; 122 | } 123 | translational_update_distance = translational_update_distance_param; 124 | } 125 | 126 | // Get the (optional) angular_update_distance 127 | double angular_update_distance = default_angular_update_distance; 128 | if (frame_pair_param.hasMember("angular_update_distance")) 129 | { 130 | XmlRpc::XmlRpcValue angular_update_distance_param = frame_pair_param["angular_update_distance"]; 131 | if (angular_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeDouble && 132 | angular_update_distance_param.getType() != XmlRpc::XmlRpcValue::TypeInt) 133 | { 134 | ROS_ERROR("angular_update_distance must be either an integer or a double, but it is XmlRpcType: %d", angular_update_distance_param.getType()); 135 | return false; 136 | } 137 | angular_update_distance = angular_update_distance_param; 138 | } 139 | 140 | 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); 141 | 142 | frame_pairs.push_back(FramePair(source_frame, target_frame, translational_update_distance, angular_update_distance)); 143 | } 144 | 145 | return true; 146 | } 147 | 148 | /** This is a program to provide notifications of changes of state within tf 149 | * It was written for providing an easy way to on demand update a web graphic of 150 | * where the robot is located. It's not designed or recommended for use in live 151 | * operation for feedback. */ 152 | int main(int argc, char** argv) 153 | { 154 | ros::init(argc, argv, "change_notifier", ros::init_options::AnonymousName); 155 | ros::NodeHandle node; 156 | ros::NodeHandle local_node("~"); 157 | 158 | double polling_frequency, translational_update_distance, angular_update_distance; 159 | local_node.param(std::string("polling_frequency"), polling_frequency, 10.0); 160 | local_node.param(std::string("translational_update_distance"), translational_update_distance, 0.10); 161 | local_node.param(std::string("angular_update_distance"), angular_update_distance, 0.10); 162 | 163 | std::vector frame_pairs; 164 | if (!getFramePairs(local_node, frame_pairs, translational_update_distance, angular_update_distance)) 165 | return 1; 166 | 167 | tf::TransformListener tfl(node); 168 | 169 | // Advertise the service 170 | ros::Publisher pub = node.advertise("tf_changes", 1, true); 171 | 172 | while (node.ok()) 173 | { 174 | try 175 | { 176 | tf::tfMessage msg; 177 | 178 | for (std::vector::iterator i = frame_pairs.begin(); i != frame_pairs.end(); i++) 179 | { 180 | FramePair& fp = *i; 181 | 182 | tfl.transformPose(fp.target_frame_, fp.pose_in_, fp.pose_out_); 183 | 184 | const tf::Vector3& origin = fp.pose_out_.getOrigin(); 185 | const tf::Quaternion& rotation = fp.pose_out_.getRotation(); 186 | 187 | if (origin.distance(fp.last_sent_pose_.getOrigin()) > fp.translational_update_distance_ || 188 | rotation.angle(fp.last_sent_pose_.getRotation()) > fp.angular_update_distance_) 189 | { 190 | fp.last_sent_pose_ = fp.pose_out_; 191 | 192 | tf::StampedTransform stampedTf(tf::Transform(rotation, origin), fp.pose_out_.stamp_, "/" + fp.target_frame_, "/" + fp.source_frame_); 193 | geometry_msgs::TransformStamped msgtf; 194 | transformStampedTFToMsg(stampedTf, msgtf); 195 | msg.transforms.push_back(msgtf); 196 | } 197 | } 198 | 199 | if (msg.transforms.size() > 0) 200 | pub.publish(msg); 201 | } 202 | catch (tf::TransformException& ex) 203 | { 204 | ROS_DEBUG("Exception: %s\n", ex.what()); 205 | } 206 | 207 | // Sleep until next polling 208 | if (polling_frequency > 0) 209 | ros::Duration().fromSec(1.0 / polling_frequency).sleep(); 210 | } 211 | 212 | return 0; 213 | } 214 | -------------------------------------------------------------------------------- /src/cache.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #include "tf/time_cache.h" 33 | #include "tf/exceptions.h" 34 | 35 | #include "tf/LinearMath/Transform.h" 36 | #include 37 | 38 | #include "ros/assert.h" 39 | 40 | using namespace tf; 41 | 42 | TransformStorage::TransformStorage() 43 | { 44 | } 45 | 46 | TransformStorage::TransformStorage(const StampedTransform& data, CompactFrameID frame_id, 47 | CompactFrameID child_frame_id) 48 | : rotation_(data.getRotation()) 49 | , translation_(data.getOrigin()) 50 | , stamp_(data.stamp_) 51 | , frame_id_(frame_id) 52 | , child_frame_id_(child_frame_id) 53 | { } 54 | 55 | TimeCache::TimeCache(ros::Duration max_storage_time) 56 | : max_storage_time_(max_storage_time) 57 | {} 58 | 59 | // hoisting these into separate functions causes an ~8% speedup. Removing calling them altogether adds another ~10% 60 | void createEmptyException(std::string *error_str) 61 | { 62 | if (error_str) 63 | { 64 | *error_str = "Unable to lookup transform, cache is empty"; 65 | } 66 | } 67 | 68 | void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* error_str) 69 | { 70 | if (error_str) 71 | { 72 | std::stringstream ss; 73 | ss << "Lookup would require extrapolation at time " << t0 << ", but only time " << t1 << " is in the buffer"; 74 | *error_str = ss.str(); 75 | } 76 | } 77 | 78 | void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str) 79 | { 80 | if (error_str) 81 | { 82 | std::stringstream ss; 83 | ss << "Lookup would require extrapolation into the future. Requested time " << t0 << " but the latest data is at time " << t1; 84 | *error_str = ss.str(); 85 | } 86 | } 87 | 88 | void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str) 89 | { 90 | if (error_str) 91 | { 92 | std::stringstream ss; 93 | ss << "Lookup would require extrapolation into the past. Requested time " << t0 << " but the earliest data is at time " << t1; 94 | *error_str = ss.str(); 95 | } 96 | } 97 | 98 | uint8_t TimeCache::findClosest(const TransformStorage*& one, const TransformStorage*& two, ros::Time target_time, std::string* error_str) 99 | { 100 | //No values stored 101 | if (storage_.empty()) 102 | { 103 | createEmptyException(error_str); 104 | return 0; 105 | } 106 | 107 | //If time == 0 return the latest 108 | if (target_time.isZero()) 109 | { 110 | one = &(*storage_.rbegin()); 111 | return 1; 112 | } 113 | 114 | // One value stored 115 | if (++storage_.begin() == storage_.end()) 116 | { 117 | const TransformStorage& ts = *storage_.begin(); 118 | if (ts.stamp_ == target_time) 119 | { 120 | one = &ts; 121 | return 1; 122 | } 123 | else 124 | { 125 | createExtrapolationException1(target_time, ts.stamp_, error_str); 126 | return 0; 127 | } 128 | } 129 | 130 | ros::Time latest_time = (*storage_.rbegin()).stamp_; 131 | ros::Time earliest_time = (*(storage_.begin())).stamp_; 132 | 133 | if (target_time == latest_time) 134 | { 135 | one = &(*storage_.rbegin()); 136 | return 1; 137 | } 138 | else if (target_time == earliest_time) 139 | { 140 | one = &(*storage_.begin()); 141 | return 1; 142 | } 143 | // Catch cases that would require extrapolation 144 | else if (target_time > latest_time) 145 | { 146 | createExtrapolationException2(target_time, latest_time, error_str); 147 | return 0; 148 | } 149 | else if (target_time < earliest_time) 150 | { 151 | createExtrapolationException3(target_time, earliest_time, error_str); 152 | return 0; 153 | } 154 | 155 | //Create a temporary object to compare to when searching the lower bound via std::set 156 | TransformStorage tmp; 157 | tmp.stamp_ = target_time; 158 | 159 | //Find the first value equal or higher than the target value 160 | L_TransformStorage::iterator storage_it = storage_.upper_bound(tmp); 161 | 162 | //Finally the case were somewhere in the middle Guarenteed no extrapolation :-) 163 | two = &*(storage_it); //Newer 164 | one = &*(--storage_it); //Older 165 | 166 | return 2; 167 | 168 | } 169 | 170 | void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output) 171 | { 172 | // Check for zero distance case 173 | if( two.stamp_ == one.stamp_ ) 174 | { 175 | output = two; 176 | return; 177 | } 178 | //Calculate the ratio 179 | tfScalar ratio = (time.toSec() - one.stamp_.toSec()) / (two.stamp_.toSec() - one.stamp_.toSec()); 180 | 181 | //Interpolate translation 182 | output.translation_.setInterpolate3(one.translation_, two.translation_, ratio); 183 | 184 | //Interpolate rotation 185 | output.rotation_ = slerp( one.rotation_, two.rotation_, ratio); 186 | 187 | output.stamp_ = one.stamp_; 188 | output.frame_id_ = one.frame_id_; 189 | output.child_frame_id_ = one.child_frame_id_; 190 | } 191 | 192 | bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available 193 | { 194 | const TransformStorage* p_temp_1 = NULL; 195 | const TransformStorage* p_temp_2 = NULL; 196 | 197 | int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); 198 | if (num_nodes == 0) 199 | { 200 | return false; 201 | } 202 | else if (num_nodes == 1) 203 | { 204 | data_out = *p_temp_1; 205 | } 206 | else if (num_nodes == 2) 207 | { 208 | if( p_temp_1->frame_id_ == p_temp_2->frame_id_) 209 | { 210 | interpolate(*p_temp_1, *p_temp_2, time, data_out); 211 | } 212 | else 213 | { 214 | data_out = *p_temp_1; 215 | } 216 | } 217 | else 218 | { 219 | ROS_BREAK(); 220 | } 221 | 222 | return true; 223 | } 224 | 225 | CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str) 226 | { 227 | const TransformStorage* p_temp_1 = NULL; 228 | const TransformStorage* p_temp_2 = NULL; 229 | 230 | int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); 231 | if (num_nodes == 0) 232 | { 233 | return 0; 234 | } 235 | 236 | return p_temp_1->frame_id_; 237 | } 238 | 239 | bool TimeCache::insertData(const TransformStorage& new_data) 240 | { 241 | 242 | if (storage_.begin() != storage_.end()) 243 | { 244 | // trying to add data that dates back longer than we want to keep history 245 | if (storage_.rbegin()->stamp_ > new_data.stamp_ + max_storage_time_) 246 | return false; 247 | 248 | // if we already have data at that exact time, delete it to ensure the latest data is stored 249 | if (storage_.rbegin()->stamp_ >= new_data.stamp_) 250 | { 251 | L_TransformStorage::iterator storage_it = storage_.find(new_data); 252 | if (storage_it != storage_.end()) 253 | storage_.erase(storage_it); 254 | } 255 | } 256 | 257 | storage_.insert(storage_.end(), new_data); 258 | 259 | pruneList(); 260 | 261 | return true; 262 | } 263 | 264 | void TimeCache::clearList() 265 | { 266 | storage_.clear(); 267 | } 268 | 269 | unsigned int TimeCache::getListLength() 270 | { 271 | return storage_.size(); 272 | } 273 | 274 | P_TimeAndFrameID TimeCache::getLatestTimeAndParent() 275 | { 276 | if (storage_.empty()) 277 | { 278 | return std::make_pair(ros::Time(), 0); 279 | } 280 | 281 | const TransformStorage& ts = *storage_.rbegin(); 282 | return std::make_pair(ts.stamp_, ts.frame_id_); 283 | } 284 | 285 | ros::Time TimeCache::getLatestTimestamp() 286 | { 287 | if (storage_.empty()) return ros::Time(); //empty list case 288 | return storage_.rbegin()->stamp_; 289 | } 290 | 291 | ros::Time TimeCache::getOldestTimestamp() 292 | { 293 | if (storage_.empty()) return ros::Time(); //empty list case 294 | return storage_.begin()->stamp_; 295 | } 296 | 297 | void TimeCache::pruneList() 298 | { 299 | ros::Time latest_time = storage_.rbegin()->stamp_; 300 | 301 | while(!storage_.empty() && storage_.begin()->stamp_ + max_storage_time_ < latest_time) 302 | { 303 | storage_.erase(storage_.begin()); 304 | } 305 | 306 | } 307 | -------------------------------------------------------------------------------- /include/tf/LinearMath/Transform.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 3 | 4 | This software is provided 'as-is', without any express or implied warranty. 5 | In no event will the authors be held liable for any damages arising from the use of this software. 6 | Permission is granted to anyone to use this software for any purpose, 7 | including commercial applications, and to alter it and redistribute it freely, 8 | subject to the following restrictions: 9 | 10 | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 11 | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 12 | 3. This notice may not be removed or altered from any source distribution. 13 | */ 14 | 15 | 16 | 17 | #ifndef tfTransform_H 18 | #define tfTransform_H 19 | 20 | 21 | #include "Matrix3x3.h" 22 | 23 | namespace tf 24 | { 25 | 26 | #define TransformData TransformDoubleData 27 | 28 | 29 | /**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear. 30 | *It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */ 31 | class Transform { 32 | 33 | ///Storage for the rotation 34 | Matrix3x3 m_basis; 35 | ///Storage for the translation 36 | Vector3 m_origin; 37 | 38 | public: 39 | 40 | /**@brief No initialization constructor */ 41 | Transform() {} 42 | /**@brief Constructor from Quaternion (optional Vector3 ) 43 | * @param q Rotation from quaternion 44 | * @param c Translation from Vector (default 0,0,0) */ 45 | explicit TFSIMD_FORCE_INLINE Transform(const Quaternion& q, 46 | const Vector3& c = Vector3(tfScalar(0), tfScalar(0), tfScalar(0))) 47 | : m_basis(q), 48 | m_origin(c) 49 | {} 50 | 51 | /**@brief Constructor from Matrix3x3 (optional Vector3) 52 | * @param b Rotation from Matrix 53 | * @param c Translation from Vector default (0,0,0)*/ 54 | explicit TFSIMD_FORCE_INLINE Transform(const Matrix3x3& b, 55 | const Vector3& c = Vector3(tfScalar(0), tfScalar(0), tfScalar(0))) 56 | : m_basis(b), 57 | m_origin(c) 58 | {} 59 | /**@brief Copy constructor */ 60 | TFSIMD_FORCE_INLINE Transform (const Transform& other) 61 | : m_basis(other.m_basis), 62 | m_origin(other.m_origin) 63 | { 64 | } 65 | /**@brief Assignment Operator */ 66 | TFSIMD_FORCE_INLINE Transform& operator=(const Transform& other) 67 | { 68 | m_basis = other.m_basis; 69 | m_origin = other.m_origin; 70 | return *this; 71 | } 72 | 73 | /**@brief Set the current transform as the value of the product of two transforms 74 | * @param t1 Transform 1 75 | * @param t2 Transform 2 76 | * This = Transform1 * Transform2 */ 77 | TFSIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) { 78 | m_basis = t1.m_basis * t2.m_basis; 79 | m_origin = t1(t2.m_origin); 80 | } 81 | 82 | /* void multInverseLeft(const Transform& t1, const Transform& t2) { 83 | Vector3 v = t2.m_origin - t1.m_origin; 84 | m_basis = tfMultTransposeLeft(t1.m_basis, t2.m_basis); 85 | m_origin = v * t1.m_basis; 86 | } 87 | */ 88 | 89 | /**@brief Return the transform of the vector */ 90 | TFSIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const 91 | { 92 | return Vector3(m_basis[0].dot(x) + m_origin.x(), 93 | m_basis[1].dot(x) + m_origin.y(), 94 | m_basis[2].dot(x) + m_origin.z()); 95 | } 96 | 97 | /**@brief Return the transform of the vector */ 98 | TFSIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const 99 | { 100 | return (*this)(x); 101 | } 102 | 103 | /**@brief Return the transform of the Quaternion */ 104 | TFSIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const 105 | { 106 | return getRotation() * q; 107 | } 108 | 109 | /**@brief Return the basis matrix for the rotation */ 110 | TFSIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; } 111 | /**@brief Return the basis matrix for the rotation */ 112 | TFSIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; } 113 | 114 | /**@brief Return the origin vector translation */ 115 | TFSIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; } 116 | /**@brief Return the origin vector translation */ 117 | TFSIMD_FORCE_INLINE const Vector3& getOrigin() const { return m_origin; } 118 | 119 | /**@brief Return a quaternion representing the rotation */ 120 | Quaternion getRotation() const { 121 | Quaternion q; 122 | m_basis.getRotation(q); 123 | return q; 124 | } 125 | 126 | 127 | /**@brief Set from an array 128 | * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ 129 | void setFromOpenGLMatrix(const tfScalar *m) 130 | { 131 | m_basis.setFromOpenGLSubMatrix(m); 132 | m_origin.setValue(m[12],m[13],m[14]); 133 | } 134 | 135 | /**@brief Fill an array representation 136 | * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ 137 | void getOpenGLMatrix(tfScalar *m) const 138 | { 139 | m_basis.getOpenGLSubMatrix(m); 140 | m[12] = m_origin.x(); 141 | m[13] = m_origin.y(); 142 | m[14] = m_origin.z(); 143 | m[15] = tfScalar(1.0); 144 | } 145 | 146 | /**@brief Set the translational element 147 | * @param origin The vector to set the translation to */ 148 | TFSIMD_FORCE_INLINE void setOrigin(const Vector3& origin) 149 | { 150 | m_origin = origin; 151 | } 152 | 153 | TFSIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const; 154 | 155 | 156 | /**@brief Set the rotational element by Matrix3x3 */ 157 | TFSIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis) 158 | { 159 | m_basis = basis; 160 | } 161 | 162 | /**@brief Set the rotational element by Quaternion */ 163 | TFSIMD_FORCE_INLINE void setRotation(const Quaternion& q) 164 | { 165 | m_basis.setRotation(q); 166 | } 167 | 168 | 169 | /**@brief Set this transformation to the identity */ 170 | void setIdentity() 171 | { 172 | m_basis.setIdentity(); 173 | m_origin.setValue(tfScalar(0.0), tfScalar(0.0), tfScalar(0.0)); 174 | } 175 | 176 | /**@brief Multiply this Transform by another(this = this * another) 177 | * @param t The other transform */ 178 | Transform& operator*=(const Transform& t) 179 | { 180 | m_origin += m_basis * t.m_origin; 181 | m_basis *= t.m_basis; 182 | return *this; 183 | } 184 | 185 | /**@brief Return the inverse of this transform */ 186 | Transform inverse() const 187 | { 188 | Matrix3x3 inv = m_basis.transpose(); 189 | return Transform(inv, inv * -m_origin); 190 | } 191 | 192 | /**@brief Return the inverse of this transform times the other transform 193 | * @param t The other transform 194 | * return this.inverse() * the other */ 195 | Transform inverseTimes(const Transform& t) const; 196 | 197 | /**@brief Return the product of this transform and the other */ 198 | Transform operator*(const Transform& t) const; 199 | 200 | /**@brief Return an identity transform */ 201 | static const Transform& getIdentity() 202 | { 203 | static const Transform identityTransform(Matrix3x3::getIdentity()); 204 | return identityTransform; 205 | } 206 | 207 | void serialize(struct TransformData& dataOut) const; 208 | 209 | void serializeFloat(struct TransformFloatData& dataOut) const; 210 | 211 | void deSerialize(const struct TransformData& dataIn); 212 | 213 | void deSerializeDouble(const struct TransformDoubleData& dataIn); 214 | 215 | void deSerializeFloat(const struct TransformFloatData& dataIn); 216 | 217 | }; 218 | 219 | 220 | TFSIMD_FORCE_INLINE Vector3 221 | Transform::invXform(const Vector3& inVec) const 222 | { 223 | Vector3 v = inVec - m_origin; 224 | return (m_basis.transpose() * v); 225 | } 226 | 227 | TFSIMD_FORCE_INLINE Transform 228 | Transform::inverseTimes(const Transform& t) const 229 | { 230 | Vector3 v = t.getOrigin() - m_origin; 231 | return Transform(m_basis.transposeTimes(t.m_basis), 232 | v * m_basis); 233 | } 234 | 235 | TFSIMD_FORCE_INLINE Transform 236 | Transform::operator*(const Transform& t) const 237 | { 238 | return Transform(m_basis * t.m_basis, 239 | (*this)(t.m_origin)); 240 | } 241 | 242 | /**@brief Test if two transforms have all elements equal */ 243 | TFSIMD_FORCE_INLINE bool operator==(const Transform& t1, const Transform& t2) 244 | { 245 | return ( t1.getBasis() == t2.getBasis() && 246 | t1.getOrigin() == t2.getOrigin() ); 247 | } 248 | 249 | 250 | ///for serialization 251 | struct TransformFloatData 252 | { 253 | Matrix3x3FloatData m_basis; 254 | Vector3FloatData m_origin; 255 | }; 256 | 257 | struct TransformDoubleData 258 | { 259 | Matrix3x3DoubleData m_basis; 260 | Vector3DoubleData m_origin; 261 | }; 262 | 263 | 264 | 265 | TFSIMD_FORCE_INLINE void Transform::serialize(TransformData& dataOut) const 266 | { 267 | m_basis.serialize(dataOut.m_basis); 268 | m_origin.serialize(dataOut.m_origin); 269 | } 270 | 271 | TFSIMD_FORCE_INLINE void Transform::serializeFloat(TransformFloatData& dataOut) const 272 | { 273 | m_basis.serializeFloat(dataOut.m_basis); 274 | m_origin.serializeFloat(dataOut.m_origin); 275 | } 276 | 277 | 278 | TFSIMD_FORCE_INLINE void Transform::deSerialize(const TransformData& dataIn) 279 | { 280 | m_basis.deSerialize(dataIn.m_basis); 281 | m_origin.deSerialize(dataIn.m_origin); 282 | } 283 | 284 | TFSIMD_FORCE_INLINE void Transform::deSerializeFloat(const TransformFloatData& dataIn) 285 | { 286 | m_basis.deSerializeFloat(dataIn.m_basis); 287 | m_origin.deSerializeFloat(dataIn.m_origin); 288 | } 289 | 290 | TFSIMD_FORCE_INLINE void Transform::deSerializeDouble(const TransformDoubleData& dataIn) 291 | { 292 | m_basis.deSerializeDouble(dataIn.m_basis); 293 | m_origin.deSerializeDouble(dataIn.m_origin); 294 | } 295 | 296 | } 297 | 298 | #endif 299 | 300 | 301 | 302 | 303 | 304 | 305 | -------------------------------------------------------------------------------- /test/test_datatype_conversion.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib; roslib.load_manifest('tf') 3 | 4 | import sys 5 | import unittest 6 | import tf 7 | import geometry_msgs.msg 8 | import rospy 9 | 10 | ## A sample python unit test 11 | class PoseConversions(unittest.TestCase): 12 | def setUp(self): 13 | #Setup the pose tests 14 | self.tfpose_stamped = tf.PoseStamped() 15 | self.tfpose_stamped.pose.setIdentity() 16 | self.tfpose_stamped.frame_id = "frame1" 17 | self.tfpose_stamped.stamp = roslib.rostime.Time(10,0) 18 | 19 | self.msgpose_stamped = geometry_msgs.msg.PoseStamped() 20 | self.msgpose_stamped.pose.position.x = 0 21 | self.msgpose_stamped.pose.position.y = 0 22 | self.msgpose_stamped.pose.position.z = 0 23 | self.msgpose_stamped.pose.orientation.x = 0 24 | self.msgpose_stamped.pose.orientation.y = 0 25 | self.msgpose_stamped.pose.orientation.z = 0 26 | self.msgpose_stamped.pose.orientation.w = 1 27 | self.msgpose_stamped.header.frame_id = "frame1" 28 | self.msgpose_stamped.header.stamp = roslib.rostime.Time(10,0) 29 | 30 | ## Setup the point tests 31 | self.tfpoint_stamped = tf.PointStamped() 32 | self.tfpoint_stamped.point.x = 0 33 | self.tfpoint_stamped.point.y = 0 34 | self.tfpoint_stamped.point.z = 0 35 | self.tfpoint_stamped.frame_id = "frame1" 36 | self.tfpoint_stamped.stamp = roslib.rostime.Time(10,0) 37 | 38 | self.msgpoint_stamped = geometry_msgs.msg.PointStamped() 39 | self.msgpoint_stamped.point.x = 0 40 | self.msgpoint_stamped.point.y = 0 41 | self.msgpoint_stamped.point.z = 0 42 | self.msgpoint_stamped.header.frame_id = "frame1" 43 | self.msgpoint_stamped.header.stamp = roslib.rostime.Time(10,0) 44 | 45 | ## Setup the vector tests 46 | self.tfvector_stamped = tf.VectorStamped() 47 | self.tfvector_stamped.vector.x = 0 48 | self.tfvector_stamped.vector.y = 0 49 | self.tfvector_stamped.vector.z = 0 50 | self.tfvector_stamped.frame_id = "frame1" 51 | self.tfvector_stamped.stamp = roslib.rostime.Time(10, 0) 52 | 53 | self.msgvector_stamped = geometry_msgs.msg.Vector3Stamped() 54 | self.msgvector_stamped.vector.x = 0 55 | self.msgvector_stamped.vector.y = 0 56 | self.msgvector_stamped.vector.z = 0 57 | self.msgvector_stamped.header.frame_id = "frame1" 58 | self.msgvector_stamped.header.stamp = roslib.rostime.Time(10,0) 59 | 60 | ## Setup the quaternion tests 61 | self.tfquaternion_stamped = tf.QuaternionStamped() 62 | self.tfquaternion_stamped.quaternion.x = 0 63 | self.tfquaternion_stamped.quaternion.y = 0 64 | self.tfquaternion_stamped.quaternion.z = 0 65 | self.tfquaternion_stamped.quaternion.w = 1 66 | self.tfquaternion_stamped.frame_id = "frame1" 67 | self.tfquaternion_stamped.stamp = roslib.rostime.Time(10, 0) 68 | 69 | self.msgquaternion_stamped = geometry_msgs.msg.QuaternionStamped() 70 | self.msgquaternion_stamped.quaternion.x = 0 71 | self.msgquaternion_stamped.quaternion.y = 0 72 | self.msgquaternion_stamped.quaternion.z = 0 73 | self.msgquaternion_stamped.quaternion.w = 1 74 | self.msgquaternion_stamped.header.frame_id = "frame1" 75 | self.msgquaternion_stamped.header.stamp = roslib.rostime.Time(10,0) 76 | 77 | # Test Pose conversions 78 | def test_msg_operator_equals_pose(self): 79 | self.assertEquals(self.msgpose_stamped, self.msgpose_stamped, "pose msg test correctness") 80 | 81 | def test_bt_operator_equals_pose(self): 82 | self.assertEquals(self.tfpose_stamped, self.tfpose_stamped, "pose bt test correctness") 83 | 84 | def test_msg_operator_on_converted(self): 85 | 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") 86 | 87 | def test_bt_operator_on_converted(self): 88 | 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") 89 | 90 | def test_to_msg_pose(self): 91 | self.assertEquals(tf.pose_bt_to_msg(self.tfpose_stamped.pose), self.msgpose_stamped.pose, "pose tf to msg incorrect") 92 | def test_to_tf_pose(self): 93 | self.assertEquals(tf.pose_msg_to_bt(self.msgpose_stamped.pose), self.tfpose_stamped.pose, "pose stamped msg to tf incorrect") 94 | 95 | def test_stamped_to_msg_pose(self): 96 | self.assertEquals(tf.pose_stamped_bt_to_msg(self.tfpose_stamped), self.msgpose_stamped, "pose stamped tf to msg incorrect") 97 | def test_stamped_to_tf_pose(self): 98 | self.assertEquals(tf.pose_stamped_msg_to_bt(self.msgpose_stamped), self.tfpose_stamped, "pose stamped msg to tf incorrect") 99 | 100 | 101 | 102 | # Test Point conversions 103 | def test_bt_operator_equal_point(self): 104 | self.assertEquals(self.tfpoint_stamped, self.tfpoint_stamped, "point tf test correctness") 105 | 106 | def test_msg_operator_equal_point(self): 107 | self.assertEquals(self.msgpoint_stamped, self.msgpoint_stamped, "point msg test correctness") 108 | 109 | def test_msg_operator_equal_point_converted(self): 110 | 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") 111 | 112 | def test_bt_operator_equal_point_converted(self): 113 | 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") 114 | 115 | def test_to_msg_point(self): 116 | self.assertEquals(tf.point_bt_to_msg(self.tfpoint_stamped.point), self.msgpoint_stamped.point, "point tf to msg incorrect") 117 | def test_to_tf_point(self): 118 | self.assertEquals(tf.point_msg_to_bt(self.msgpoint_stamped.point), self.tfpoint_stamped.point, "point stamped msg to tf incorrect") 119 | 120 | def test_stamped_to_msg_point(self): 121 | self.assertEquals(tf.point_stamped_bt_to_msg(self.tfpoint_stamped), self.msgpoint_stamped, "point stamped tf to msg incorrect") 122 | def test_stamped_to_tf_point(self): 123 | self.assertEquals(tf.point_stamped_msg_to_bt(self.msgpoint_stamped), self.tfpoint_stamped, "point stamped msg to tf incorrect") 124 | 125 | # Test Vector conversions 126 | def test_msg_operator_equal_vector(self): 127 | self.assertEquals(self.tfvector_stamped, self.tfvector_stamped, "vector bt test correctness") 128 | 129 | def test_msg_operator_equal_vector(self): 130 | self.assertEquals(self.msgvector_stamped, self.msgvector_stamped, "vector msg test correctness") 131 | 132 | def test_msg_operator_equal_vector_converted(self): 133 | 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") 134 | 135 | def test_bt_operator_equal_vector_converted(self): 136 | 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") 137 | 138 | def test_to_msg_vector(self): 139 | self.assertEquals(tf.vector_bt_to_msg(self.tfvector_stamped.vector), self.msgvector_stamped.vector, "vector tf to msg incorrect") 140 | def test_to_tf_vector(self): 141 | self.assertEquals(tf.vector_msg_to_bt(self.msgvector_stamped.vector), self.tfvector_stamped.vector, "vector stamped msg to tf incorrect") 142 | 143 | def test_stamped_to_msg_vector(self): 144 | self.assertEquals(tf.vector_stamped_bt_to_msg(self.tfvector_stamped), self.msgvector_stamped, "vector stamped tf to msg incorrect") 145 | def test_stamped_to_tf_vector(self): 146 | self.assertEquals(tf.vector_stamped_msg_to_bt(self.msgvector_stamped), self.tfvector_stamped, "vector stamped msg to tf incorrect") 147 | 148 | # Test Quaternion conversions 149 | def test_bt_operator_equal_quaternion(self): 150 | self.assertEquals(self.tfquaternion_stamped, self.tfquaternion_stamped, "quaternion bt test correctness") 151 | 152 | def test_msg_operator_equal_quaternion(self): 153 | self.assertEquals(self.msgquaternion_stamped, self.msgquaternion_stamped, "quaternion msg test correctness") 154 | 155 | def test_msg_operator_equal_quaternion_converted(self): 156 | 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") 157 | 158 | def test_bt_operator_equal_quaternion_converted(self): 159 | 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") 160 | 161 | def test_to_msg_quaternion(self): 162 | self.assertEquals(tf.quaternion_bt_to_msg(self.tfquaternion_stamped.quaternion), self.msgquaternion_stamped.quaternion, "quaternion tf to msg incorrect") 163 | def test_to_tf_quaternion(self): 164 | self.assertEquals(tf.quaternion_msg_to_bt(self.msgquaternion_stamped.quaternion), self.tfquaternion_stamped.quaternion, "quaternion stamped msg to tf incorrect") 165 | 166 | def test_stamped_to_msg_quaternion(self): 167 | self.assertEquals(tf.quaternion_stamped_bt_to_msg(self.tfquaternion_stamped), self.msgquaternion_stamped, "quaternion stamped tf to msg incorrect") 168 | def test_stamped_to_tf_quaternion(self): 169 | self.assertEquals(tf.quaternion_stamped_msg_to_bt(self.msgquaternion_stamped), self.tfquaternion_stamped, "quaternion stamped msg to tf incorrect") 170 | if __name__ == '__main__': 171 | import rostest 172 | rostest.unitrun('tf', 'test_tf_data_conversions', PoseConversions) 173 | 174 | -------------------------------------------------------------------------------- /test/testPython.py: -------------------------------------------------------------------------------- 1 | import roslib 2 | roslib.load_manifest('tf') 3 | import rostest 4 | import rospy 5 | import numpy 6 | import unittest 7 | import sys 8 | 9 | import tf.transformations 10 | import geometry_msgs.msg 11 | import sensor_msgs.msg 12 | 13 | import tf 14 | 15 | class Mock: 16 | pass 17 | 18 | def setT(t, parent, frame, ti, x): 19 | m = Mock() 20 | m.parent_id = parent 21 | m.header = Mock() 22 | m.header.stamp = ti 23 | m.header.frame_id = frame 24 | m.transform = Mock() 25 | m.transform.translation = Mock() 26 | m.transform.translation.x = x 27 | m.transform.translation.y = 0 28 | m.transform.translation.z = 0 29 | m.transform.rotation = Mock() 30 | m.transform.rotation.x = 0 31 | m.transform.rotation.y = 0 32 | m.transform.rotation.z = 0 33 | m.transform.rotation.w = 1 34 | t.setTransform(m) 35 | 36 | class TestPython(unittest.TestCase): 37 | 38 | def setUp(self): 39 | pass 40 | 41 | def common(self, t): 42 | m = geometry_msgs.msg.TransformStamped() 43 | m.header.frame_id = "PARENT" 44 | m.child_frame_id = "THISFRAME" 45 | m.transform.translation.y = 5.0 46 | m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) 47 | t.setTransform(m) 48 | afs = t.allFramesAsString() 49 | self.assert_(len(afs) != 0) 50 | self.assert_("PARENT" in afs) 51 | self.assert_("THISFRAME" in afs) 52 | self.assert_(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec() == 0) 53 | for ti in [3, 5, 10, 11, 19, 20, 21]: 54 | m.header.stamp.secs = ti 55 | t.setTransform(m) 56 | self.assert_(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec() == ti) 57 | 58 | # Verify that getLatestCommonTime with nonexistent frames raise exception 59 | self.assertRaises(tf.Exception, lambda: t.getLatestCommonTime("MANDALAY", "JUPITER")) 60 | self.assertRaises(tf.LookupException, lambda: t.lookupTransform("MANDALAY", "JUPITER", rospy.Time())) 61 | 62 | # Ask for transform for valid frames, but more than 10 seconds in the past. Should raise ExtrapolationException 63 | self.assertRaises(tf.ExtrapolationException, lambda: t.lookupTransform("THISFRAME", "PARENT", rospy.Time(2))) 64 | 65 | #### print t.lookupVelocity("THISFRAME", "PARENT", rospy.Time(15), rospy.Duration(5)) 66 | 67 | def test_smoke(self): 68 | t = tf.Transformer() 69 | self.common(t) 70 | 71 | def test_cache_time(self): 72 | # Vary cache_time and confirm its effect on ExtrapolationException from lookupTransform(). 73 | 74 | for cache_time in range(2, 98): 75 | t = tf.Transformer(True, rospy.Duration(cache_time)) 76 | m = geometry_msgs.msg.TransformStamped() 77 | m.header.frame_id = "PARENT" 78 | m.child_frame_id = "THISFRAME" 79 | m.transform.translation.y = 5.0 80 | m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) 81 | t.setTransform(m) 82 | afs = t.allFramesAsString() 83 | self.assert_(len(afs) != 0) 84 | self.assert_("PARENT" in afs) 85 | self.assert_("THISFRAME" in afs) 86 | self.assert_(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec() == 0) 87 | 88 | # Set transforms for time 0..100 inclusive 89 | for ti in range(101): 90 | m.header.stamp = rospy.Time(ti) 91 | t.setTransform(m) 92 | self.assert_(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec() == ti) 93 | self.assertEqual(t.getLatestCommonTime("THISFRAME", "PARENT").to_sec(), 100) 94 | 95 | # (avoid time of 0 because that means 'latest') 96 | 97 | for ti in range(1, 100 - cache_time): 98 | self.assertRaises(tf.ExtrapolationException, lambda: t.lookupTransform("THISFRAME", "PARENT", rospy.Time(ti))) 99 | for ti in range(100 - cache_time, 100): 100 | t.lookupTransform("THISFRAME", "PARENT", rospy.Time(ti)) 101 | 102 | def test_subclass(self): 103 | class TransformerSubclass(tf.Transformer): 104 | def extra(self): 105 | return 77 106 | t = TransformerSubclass(True, rospy.Duration.from_sec(10.0)) 107 | self.assert_(t.extra() == 77) 108 | self.common(t) 109 | self.assert_(t.extra() == 77) 110 | 111 | def test_twist(self): 112 | t = tf.Transformer() 113 | 114 | vel = 3 115 | for ti in range(5): 116 | m = geometry_msgs.msg.TransformStamped() 117 | m.header.frame_id = "PARENT" 118 | m.header.stamp = rospy.Time(ti) 119 | m.child_frame_id = "THISFRAME" 120 | m.transform.translation.x = ti * vel 121 | m.transform.rotation = geometry_msgs.msg.Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0)) 122 | t.setTransform(m) 123 | 124 | tw0 = t.lookupTwist("THISFRAME", "PARENT", rospy.Time(0.0), rospy.Duration(4.001)) 125 | self.assertAlmostEqual(tw0[0][0], vel, 2) 126 | tw1 = t.lookupTwistFull("THISFRAME", "PARENT", "PARENT", (0, 0, 0), "THISFRAME", rospy.Time(0.0), rospy.Duration(4.001)) 127 | self.assertEqual(tw0, tw1) 128 | 129 | def test_transformer_ros(self): 130 | tr = tf.TransformerROS() 131 | 132 | m = geometry_msgs.msg.TransformStamped() 133 | m.header.stamp = rospy.Time().from_sec(3.0) 134 | m.header.frame_id = "PARENT" 135 | m.child_frame_id = "THISFRAME" 136 | m.transform.translation.y = 5.0 137 | m.transform.rotation.x = 0.04997917 138 | m.transform.rotation.y = 0 139 | m.transform.rotation.z = 0 140 | m.transform.rotation.w = 0.99875026 141 | tr.setTransform(m) 142 | m.header.stamp = rospy.Time().from_sec(5.0) 143 | tr.setTransform(m) 144 | 145 | # Smoke the various transform* methods 146 | 147 | types = [ "Point", "Pose", "Quaternion", "Vector3" ] 148 | for t in types: 149 | msg = getattr(geometry_msgs.msg, "%sStamped" % t)() 150 | msg.header.frame_id = "THISFRAME" 151 | msg_t = getattr(tr, "transform%s" % t)("PARENT", msg) 152 | self.assertEqual(msg_t.header.frame_id, "PARENT") 153 | 154 | # PointCloud is a bit different, so smoke is different 155 | 156 | msg = sensor_msgs.msg.PointCloud() 157 | msg.header.frame_id = "THISFRAME" 158 | msg.points = [geometry_msgs.msg.Point32(1,2,3)] 159 | xmsg = tr.transformPointCloud("PARENT", msg) 160 | self.assertEqual(xmsg.header.frame_id, "PARENT") 161 | self.assertEqual(len(msg.points), len(xmsg.points)) 162 | self.assertNotEqual(msg.points[0], xmsg.points[0]) 163 | 164 | """ 165 | Two fixed quaternions, a small twist around X concatenated. 166 | 167 | >>> t.quaternion_from_euler(0.1, 0, 0) 168 | array([ 0.04997917, 0. , 0. , 0.99875026]) 169 | >>> t.quaternion_from_euler(0.2, 0, 0) 170 | array([ 0.09983342, 0. , 0. , 0.99500417]) 171 | """ 172 | 173 | # Specific test for quaternion types 174 | 175 | msg = geometry_msgs.msg.QuaternionStamped() 176 | q = [ 0.04997917, 0. , 0. , 0.99875026 ] 177 | msg.quaternion.x = q[0] 178 | msg.quaternion.y = q[1] 179 | msg.quaternion.z = q[2] 180 | msg.quaternion.w = q[3] 181 | msg.header.stamp = rospy.Time().from_sec(3.0) 182 | msg.header.frame_id = "THISFRAME" 183 | msg_t = tr.transformQuaternion("PARENT", msg) 184 | self.assertEqual(msg_t.header.frame_id, "PARENT") 185 | for a,v in zip("xyzw", [ 0.09983342, 0. , 0. , 0.99500417]): 186 | self.assertAlmostEqual(v, 187 | getattr(msg_t.quaternion, a), 188 | 4) 189 | 190 | def test_transformer_wait_for_transform_dedicated_thread(self): 191 | tr = tf.Transformer() 192 | try: 193 | tr.waitForTransform("PARENT", "THISFRAME", rospy.Time().from_sec(4.0), rospy.Duration(3.0)) 194 | self.assertFalse("This should throw") 195 | except tf.Exception, ex: 196 | print "successfully caught" 197 | pass 198 | 199 | 200 | def test_transformer_wait_for_transform(self): 201 | tr = tf.Transformer() 202 | tr.setUsingDedicatedThread(1) 203 | 204 | try: 205 | tr.waitForTransform("PARENT", "THISFRAME", rospy.Time().from_sec(4.0), rospy.Duration(3.0)) 206 | self.assertFalse("This should throw") 207 | except tf.Exception, ex: 208 | pass 209 | 210 | m = geometry_msgs.msg.TransformStamped() 211 | m.header.stamp = rospy.Time().from_sec(3.0) 212 | m.header.frame_id = "PARENT" 213 | m.child_frame_id = "THISFRAME" 214 | m.transform.translation.y = 5.0 215 | m.transform.rotation.x = 0.04997917 216 | m.transform.rotation.y = 0 217 | m.transform.rotation.z = 0 218 | m.transform.rotation.w = 0.99875026 219 | tr.setTransform(m) 220 | m.header.stamp = rospy.Time().from_sec(5.0) 221 | tr.setTransform(m) 222 | 223 | try: 224 | tr.waitForTransform("PARENT", "THISFRAME", rospy.Time().from_sec(4.0), rospy.Duration(3.0)) 225 | except tf.Exception, ex: 226 | self.assertFalse("This should not throw") 227 | 228 | 229 | def test_getTFPrefix(self): 230 | t = tf.Transformer() 231 | self.assertEqual(t.getTFPrefix(), "") 232 | 233 | def no_test_random(self): 234 | import networkx as nx 235 | for (r,h) in [ (2,2), (2,5), (3,5) ]: 236 | G = nx.balanced_tree(r, h) 237 | t = tf.Transformer(True, rospy.Duration(10.0)) 238 | 239 | for n in G.nodes(): 240 | if n != 0: 241 | # n has parent p 242 | p = min(G.neighbors(n)) 243 | setT(t, str(p), str(n), rospy.Time(0), 1) 244 | for n in G.nodes(): 245 | ((x,_,_), _) = t.lookupTransform("0", str(n), rospy.Time(0)) 246 | self.assert_(x == nx.shortest_path_length(G, 0, n)) 247 | for i in G.nodes(): 248 | for j in G.nodes(): 249 | ((x,_,_), _) = t.lookupTransform(str(i), str(j), rospy.Time()) 250 | self.assert_(abs(x) == abs(nx.shortest_path_length(G, 0, i) - nx.shortest_path_length(G, 0, j))) 251 | 252 | if __name__ == '__main__': 253 | rostest.unitrun('tf', 'directed', TestPython) 254 | -------------------------------------------------------------------------------- /src/transform_listener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #include "tf/transform_listener.h" 33 | 34 | #include 35 | #include 36 | 37 | 38 | using namespace tf; 39 | std::string tf::remap(const std::string& frame_id) 40 | { 41 | ros::NodeHandle n("~"); 42 | return tf::resolve(getPrefixParam(n), frame_id); 43 | } 44 | 45 | 46 | TransformerHelper::TransformerHelper(ros::Duration max_cache_time) 47 | : Transformer(true, max_cache_time) 48 | { 49 | 50 | } 51 | 52 | TransformerHelper::~TransformerHelper() 53 | { 54 | 55 | } 56 | 57 | void TransformerHelper::transformQuaternion(const std::string& target_frame, 58 | const geometry_msgs::QuaternionStamped& msg_in, 59 | geometry_msgs::QuaternionStamped& msg_out) const 60 | { 61 | tf::assertQuaternionValid(msg_in.quaternion); 62 | 63 | Stamped pin, pout; 64 | quaternionStampedMsgToTF(msg_in, pin); 65 | transformQuaternion(target_frame, pin, pout); 66 | quaternionStampedTFToMsg(pout, msg_out); 67 | } 68 | 69 | void TransformerHelper::transformVector(const std::string& target_frame, 70 | const geometry_msgs::Vector3Stamped& msg_in, 71 | geometry_msgs::Vector3Stamped& msg_out) const 72 | { 73 | Stamped pin, pout; 74 | vector3StampedMsgToTF(msg_in, pin); 75 | transformVector(target_frame, pin, pout); 76 | vector3StampedTFToMsg(pout, msg_out); 77 | } 78 | 79 | void TransformerHelper::transformPoint(const std::string& target_frame, 80 | const geometry_msgs::PointStamped& msg_in, 81 | geometry_msgs::PointStamped& msg_out) const 82 | { 83 | Stamped pin, pout; 84 | pointStampedMsgToTF(msg_in, pin); 85 | transformPoint(target_frame, pin, pout); 86 | pointStampedTFToMsg(pout, msg_out); 87 | } 88 | 89 | void TransformerHelper::transformPose(const std::string& target_frame, 90 | const geometry_msgs::PoseStamped& msg_in, 91 | geometry_msgs::PoseStamped& msg_out) const 92 | { 93 | tf::assertQuaternionValid(msg_in.pose.orientation); 94 | 95 | Stamped pin, pout; 96 | poseStampedMsgToTF(msg_in, pin); 97 | transformPose(target_frame, pin, pout); 98 | poseStampedTFToMsg(pout, msg_out); 99 | } 100 | /* http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review 101 | void TransformerHelper::transformTwist(const std::string& target_frame, 102 | const geometry_msgs::TwistStamped& msg_in, 103 | geometry_msgs::TwistStamped& msg_out) const 104 | { 105 | tf::Vector3 twist_rot(msg_in.twist.angular.x, 106 | msg_in.twist.angular.y, 107 | msg_in.twist.angular.z); 108 | tf::Vector3 twist_vel(msg_in.twist.linear.x, 109 | msg_in.twist.linear.y, 110 | msg_in.twist.linear.z); 111 | 112 | tf::StampedTransform transform; 113 | lookupTransform(target_frame,msg_in.header.frame_id, msg_in.header.stamp, transform); 114 | 115 | 116 | tf::Vector3 out_rot = transform.getBasis() * twist_rot; 117 | tf::Vector3 out_vel = transform.getBasis()* twist_vel + transform.getOrigin().cross(out_rot); 118 | 119 | geometry_msgs::TwistStamped interframe_twist; 120 | 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 121 | 122 | msg_out.header.stamp = msg_in.header.stamp; 123 | msg_out.header.frame_id = target_frame; 124 | msg_out.twist.linear.x = out_vel.x() + interframe_twist.twist.linear.x; 125 | msg_out.twist.linear.y = out_vel.y() + interframe_twist.twist.linear.y; 126 | msg_out.twist.linear.z = out_vel.z() + interframe_twist.twist.linear.z; 127 | msg_out.twist.angular.x = out_rot.x() + interframe_twist.twist.angular.x; 128 | msg_out.twist.angular.y = out_rot.y() + interframe_twist.twist.angular.y; 129 | msg_out.twist.angular.z = out_rot.z() + interframe_twist.twist.angular.z; 130 | 131 | }*/ 132 | 133 | void TransformerHelper::transformQuaternion(const std::string& target_frame, const ros::Time& target_time, 134 | const geometry_msgs::QuaternionStamped& msg_in, 135 | const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out) const 136 | { 137 | tf::assertQuaternionValid(msg_in.quaternion); 138 | Stamped pin, pout; 139 | quaternionStampedMsgToTF(msg_in, pin); 140 | transformQuaternion(target_frame, target_time, pin, fixed_frame, pout); 141 | quaternionStampedTFToMsg(pout, msg_out); 142 | } 143 | 144 | void TransformerHelper::transformVector(const std::string& target_frame, const ros::Time& target_time, 145 | const geometry_msgs::Vector3Stamped& msg_in, 146 | const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out) const 147 | { 148 | Stamped pin, pout; 149 | vector3StampedMsgToTF(msg_in, pin); 150 | transformVector(target_frame, target_time, pin, fixed_frame, pout); 151 | vector3StampedTFToMsg(pout, msg_out); 152 | } 153 | 154 | void TransformerHelper::transformPoint(const std::string& target_frame, const ros::Time& target_time, 155 | const geometry_msgs::PointStamped& msg_in, 156 | const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out) const 157 | { 158 | Stamped pin, pout; 159 | pointStampedMsgToTF(msg_in, pin); 160 | transformPoint(target_frame, target_time, pin, fixed_frame, pout); 161 | pointStampedTFToMsg(pout, msg_out); 162 | } 163 | 164 | void TransformerHelper::transformPose(const std::string& target_frame, const ros::Time& target_time, 165 | const geometry_msgs::PoseStamped& msg_in, 166 | const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out) const 167 | { 168 | tf::assertQuaternionValid(msg_in.pose.orientation); 169 | 170 | Stamped pin, pout; 171 | poseStampedMsgToTF(msg_in, pin); 172 | transformPose(target_frame, target_time, pin, fixed_frame, pout); 173 | poseStampedTFToMsg(pout, msg_out); 174 | } 175 | 176 | void TransformerHelper::transformPointCloud(const std::string & target_frame, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const 177 | { 178 | StampedTransform transform; 179 | lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform); 180 | 181 | transformPointCloud(target_frame, transform, cloudIn.header.stamp, cloudIn, cloudOut); 182 | } 183 | void TransformerHelper::transformPointCloud(const std::string& target_frame, const ros::Time& target_time, 184 | const sensor_msgs::PointCloud& cloudIn, 185 | const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut) const 186 | { 187 | StampedTransform transform; 188 | lookupTransform(target_frame, target_time, 189 | cloudIn.header.frame_id, cloudIn.header.stamp, 190 | fixed_frame, 191 | transform); 192 | 193 | transformPointCloud(target_frame, transform, target_time, cloudIn, cloudOut); 194 | 195 | 196 | } 197 | 198 | inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out) 199 | { 200 | // Use temporary variables in case &in == &out 201 | double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x(); 202 | double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y(); 203 | double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z(); 204 | 205 | out.x = x; out.y = y; out.z = z; 206 | } 207 | 208 | 209 | void TransformerHelper::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform, 210 | const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn, 211 | sensor_msgs::PointCloud & cloudOut) const 212 | { 213 | tf::Vector3 origin = net_transform.getOrigin(); 214 | tf::Matrix3x3 basis = net_transform.getBasis(); 215 | 216 | unsigned int length = cloudIn.points.size(); 217 | 218 | // Copy relevant data from cloudIn, if needed 219 | if (&cloudIn != &cloudOut) 220 | { 221 | cloudOut.header = cloudIn.header; 222 | cloudOut.points.resize(length); 223 | cloudOut.channels.resize(cloudIn.channels.size()); 224 | for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i) 225 | cloudOut.channels[i] = cloudIn.channels[i]; 226 | } 227 | 228 | // Transform points 229 | cloudOut.header.stamp = target_time; 230 | cloudOut.header.frame_id = target_frame; 231 | for (unsigned int i = 0; i < length ; i++) { 232 | transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]); 233 | } 234 | } 235 | 236 | 237 | 238 | 239 | TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread): 240 | TransformerHelper(max_cache_time), tf2_listener_(Transformer::tf2_buffer_, node_, spin_thread) 241 | { 242 | //Everything is done inside tf2 init 243 | } 244 | 245 | TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread): 246 | TransformerHelper(max_cache_time), node_(nh), tf2_listener_(Transformer::tf2_buffer_, nh, spin_thread) 247 | { 248 | //Everything is done inside tf2 init 249 | } 250 | 251 | TransformListener::~TransformListener() 252 | { 253 | //Everything is done inside tf2 init 254 | } 255 | 256 | //Override Transformer::ok() for ticket:4882 257 | bool TransformListener::ok() const { return ros::ok(); } 258 | -------------------------------------------------------------------------------- /src/tf_monitor.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | /* 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Willow Garage, Inc. nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | /** \author Wim Meeussen */ 33 | 34 | 35 | #include "tf/tf.h" 36 | #include "tf/transform_listener.h" 37 | #include 38 | #include 39 | #include 40 | #include "ros/ros.h" 41 | 42 | using namespace tf; 43 | using namespace ros; 44 | using namespace std; 45 | 46 | 47 | class TFMonitor 48 | { 49 | public: 50 | std::string framea_, frameb_; 51 | bool using_specific_chain_; 52 | 53 | ros::NodeHandle node_; 54 | ros::Subscriber subscriber_tf_, subscriber_tf_message_; 55 | std::vector chain_; 56 | std::map frame_authority_map; 57 | std::map > delay_map; 58 | std::map > authority_map; 59 | std::map > authority_frequency_map; 60 | 61 | TransformListener tf_; 62 | 63 | tf::tfMessage message_; 64 | 65 | boost::mutex map_lock_; 66 | void callback(const tf::tfMessageConstPtr& msg_ptr) 67 | { 68 | const tf::tfMessage& message = *msg_ptr; 69 | //Lookup the authority 70 | std::string authority; 71 | std::map* msg_header_map = message.__connection_header.get(); 72 | std::map::iterator it = msg_header_map->find("callerid"); 73 | if (it == msg_header_map->end()) 74 | { 75 | ROS_WARN("Message recieved without callerid"); 76 | authority = "no callerid"; 77 | } 78 | else 79 | { 80 | authority = it->second; 81 | } 82 | 83 | 84 | double average_offset = 0; 85 | boost::mutex::scoped_lock my_lock(map_lock_); 86 | for (unsigned int i = 0; i < message.transforms.size(); i++) 87 | { 88 | frame_authority_map[message.transforms[i].child_frame_id] = authority; 89 | 90 | double offset = (ros::Time::now() - message.transforms[i].header.stamp).toSec(); 91 | average_offset += offset; 92 | 93 | std::map >::iterator it = delay_map.find(message.transforms[i].child_frame_id); 94 | if (it == delay_map.end()) 95 | { 96 | delay_map[message.transforms[i].child_frame_id] = std::vector(1,offset); 97 | } 98 | else 99 | { 100 | it->second.push_back(offset); 101 | if (it->second.size() > 1000) 102 | it->second.erase(it->second.begin()); 103 | } 104 | 105 | } 106 | 107 | average_offset /= max((size_t) 1, message.transforms.size()); 108 | 109 | //create the authority log 110 | std::map >::iterator it2 = authority_map.find(authority); 111 | if (it2 == authority_map.end()) 112 | { 113 | authority_map[authority] = std::vector(1,average_offset); 114 | } 115 | else 116 | { 117 | it2->second.push_back(average_offset); 118 | if (it2->second.size() > 1000) 119 | it2->second.erase(it2->second.begin()); 120 | } 121 | 122 | //create the authority frequency log 123 | std::map >::iterator it3 = authority_frequency_map.find(authority); 124 | if (it3 == authority_frequency_map.end()) 125 | { 126 | authority_frequency_map[authority] = std::vector(1,ros::Time::now().toSec()); 127 | } 128 | else 129 | { 130 | it3->second.push_back(ros::Time::now().toSec()); 131 | if (it3->second.size() > 1000) 132 | it3->second.erase(it3->second.begin()); 133 | } 134 | 135 | }; 136 | 137 | TFMonitor(bool using_specific_chain, std::string framea = "", std::string frameb = ""): 138 | framea_(framea), frameb_(frameb), 139 | using_specific_chain_(using_specific_chain) 140 | { 141 | 142 | if (using_specific_chain_) 143 | { 144 | cout << "Waiting for transform chain to become available between "<< framea_ << " and " << frameb_<< " " << flush; 145 | while (node_.ok() && !tf_.waitForTransform(framea_, frameb_, Time(), Duration(1.0))) 146 | cout << "." << flush; 147 | cout << endl; 148 | 149 | try{ 150 | tf_.chainAsVector(frameb_, ros::Time(), framea_, ros::Time(), frameb_, chain_); 151 | } 152 | catch(tf::TransformException& ex){ 153 | ROS_WARN("Transform Exception %s", ex.what()); 154 | return; 155 | } 156 | 157 | /* cout << "Chain currently is:" <("tf", 100, boost::bind(&TFMonitor::callback, this, _1)); 165 | subscriber_tf_message_ = node_.subscribe("tf_message", 100, boost::bind(&TFMonitor::callback, this, _1)); 166 | 167 | } 168 | 169 | std::string outputFrameInfo(const std::map >::iterator& it, const std::string& frame_authority) 170 | { 171 | std::stringstream ss; 172 | double average_delay = 0; 173 | double max_delay = 0; 174 | for (unsigned int i = 0; i < it->second.size(); i++) 175 | { 176 | average_delay += it->second[i]; 177 | max_delay = std::max(max_delay, it->second[i]); 178 | } 179 | average_delay /= it->second.size(); 180 | ss << "Frame: " << it->first <<" published by "<< frame_authority << " Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl; 181 | return ss.str(); 182 | } 183 | 184 | void spin() 185 | { 186 | 187 | // create tf listener 188 | double max_diff = 0; 189 | double avg_diff = 0; 190 | double lowpass = 0.01; 191 | unsigned int counter = 0; 192 | 193 | 194 | 195 | while (node_.ok()){ 196 | tf::StampedTransform tmp; 197 | counter++; 198 | // printf("looping %d\n", counter); 199 | if (using_specific_chain_) 200 | { 201 | tf_.lookupTransform(framea_, frameb_, Time(), tmp); 202 | double diff = (Time::now() - tmp.stamp_).toSec(); 203 | avg_diff = lowpass * diff + (1-lowpass)*avg_diff; 204 | if (diff > max_diff) max_diff = diff; 205 | } 206 | Duration(0.01).sleep(); 207 | if (counter > 20){ 208 | counter = 0; 209 | 210 | if (using_specific_chain_) 211 | { 212 | std::cout < "; 219 | } 220 | cout < >::iterator it = delay_map.begin(); 228 | for ( ; it != delay_map.end() ; ++it) 229 | { 230 | if (using_specific_chain_ ) 231 | { 232 | for ( unsigned int i = 0 ; i < chain_.size(); i++) 233 | { 234 | if (it->first != chain_[i]) 235 | continue; 236 | 237 | cout << outputFrameInfo(it, frame_authority_map[it->first]); 238 | } 239 | } 240 | else 241 | cout << outputFrameInfo(it, frame_authority_map[it->first]); 242 | } 243 | std::cerr < >::iterator it1 = authority_map.begin(); 245 | std::map >::iterator it2 = authority_frequency_map.begin(); 246 | for ( ; it1 != authority_map.end() ; ++it1, ++it2) 247 | { 248 | double average_delay = 0; 249 | double max_delay = 0; 250 | for (unsigned int i = 0; i < it1->second.size(); i++) 251 | { 252 | average_delay += it1->second[i]; 253 | max_delay = std::max(max_delay, it1->second[i]); 254 | } 255 | average_delay /= it1->second.size(); 256 | double frequency_out = (double)(it2->second.size())/std::max(0.00000001, (it2->second.back() - it2->second.front())); 257 | //cout << "output" <<&(*it2) <<" " << it2->second.back() <<" " << it2->second.front() <<" " << std::max((size_t)1, it2->second.size()) << " " << frequency_out << endl; 258 | cout << "Node: " <first << " " << frequency_out <<" Hz, Average Delay: " << average_delay << " Max Delay: " << max_delay << std::endl; 259 | } 260 | 261 | } 262 | } 263 | 264 | } 265 | }; 266 | 267 | 268 | int main(int argc, char ** argv) 269 | { 270 | //Initialize ROS 271 | init(argc, argv, "tf_monitor", ros::init_options::AnonymousName); 272 | 273 | 274 | string framea, frameb; 275 | bool using_specific_chain = true; 276 | if (argc == 3){ 277 | framea = argv[1]; 278 | frameb = argv[2]; 279 | } 280 | else if (argc == 1) 281 | using_specific_chain = false; 282 | else{ 283 | ROS_INFO("TF_Monitor: usage: tf_monitor framea frameb"); 284 | return -1; 285 | } 286 | 287 | std::string searched_param; 288 | std::string tf_prefix; 289 | ros::NodeHandle local_nh("~"); 290 | local_nh.searchParam("tf_prefix", searched_param); 291 | local_nh.getParam(searched_param, tf_prefix); 292 | 293 | 294 | //Make sure we don't start before recieving time when in simtime 295 | int iterations = 0; 296 | while (ros::Time::now() == ros::Time()) 297 | { 298 | if (++iterations > 10) 299 | { 300 | ROS_INFO("tf_monitor waiting for time to be published"); 301 | iterations = 0; 302 | } 303 | ros::WallDuration(0.1).sleep(); 304 | } 305 | 306 | ros::NodeHandle nh; 307 | boost::thread spinner( boost::bind( &ros::spin )); 308 | TFMonitor monitor(using_specific_chain, tf::resolve(tf_prefix, framea), tf::resolve(tf_prefix, frameb)); 309 | monitor.spin(); 310 | spinner.join(); 311 | return 0; 312 | 313 | } 314 | -------------------------------------------------------------------------------- /include/tf/transform_datatypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /** \author Tully Foote */ 31 | 32 | #ifndef TF_TRANSFORM_DATATYPES_H 33 | #define TF_TRANSFORM_DATATYPES_H 34 | 35 | #include 36 | #include "geometry_msgs/PointStamped.h" 37 | #include "geometry_msgs/Vector3Stamped.h" 38 | #include "geometry_msgs/QuaternionStamped.h" 39 | #include "geometry_msgs/TransformStamped.h" 40 | #include "geometry_msgs/PoseStamped.h" 41 | #include "tf/LinearMath/Transform.h" 42 | #include "ros/time.h" 43 | 44 | #include "ros/console.h" 45 | 46 | namespace tf 47 | { 48 | 49 | typedef tf::Vector3 Point; 50 | typedef tf::Transform Pose; 51 | 52 | static const double QUATERNION_TOLERANCE = 0.1f; 53 | 54 | /** \brief The data type which will be cross compatable with geometry_msgs 55 | * This is the tf datatype equivilant of a MessageStamped */ 56 | template 57 | class Stamped : public T{ 58 | public: 59 | ros::Time stamp_; ///< The timestamp associated with this data 60 | std::string frame_id_; ///< The frame_id associated this data 61 | 62 | /** Default constructor */ 63 | Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation 64 | 65 | /** Full constructor */ 66 | Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id) : 67 | T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ; 68 | 69 | /** Set the data element */ 70 | void setData(const T& input){*static_cast(this) = input;}; 71 | }; 72 | 73 | /** \brief Comparison Operator for Stamped datatypes */ 74 | template 75 | bool operator==(const Stamped &a, const Stamped &b) { 76 | return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast(a) == static_cast(b); 77 | }; 78 | 79 | 80 | /** \brief The Stamped Transform datatype used by tf */ 81 | class StampedTransform : public tf::Transform 82 | { 83 | public: 84 | ros::Time stamp_; ///< The timestamp associated with this transform 85 | std::string frame_id_; ///< The frame_id of the coordinate frame in which this transform is defined 86 | std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines 87 | StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id): 88 | tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ }; 89 | 90 | /** \brief Default constructor only to be used for preallocation */ 91 | StampedTransform() { }; 92 | 93 | /** \brief Set the inherited Traonsform data */ 94 | void setData(const tf::Transform& input){*static_cast(this) = input;}; 95 | 96 | }; 97 | 98 | /** \brief Comparison operator for StampedTransform */ 99 | static inline bool operator==(const StampedTransform &a, const StampedTransform &b) { 100 | 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); 101 | }; 102 | 103 | 104 | /** \brief convert Quaternion msg to Quaternion */ 105 | static inline void quaternionMsgToTF(const geometry_msgs::Quaternion& msg, Quaternion& bt) 106 | { 107 | bt = Quaternion(msg.x, msg.y, msg.z, msg.w); 108 | if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE) 109 | { 110 | ROS_WARN("MSG to TF: Quaternion Not Properly Normalized"); 111 | bt.normalize(); 112 | } 113 | }; 114 | /** \brief convert Quaternion to Quaternion msg*/ 115 | static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quaternion& msg) 116 | { 117 | if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE) 118 | { 119 | ROS_WARN("TF to MSG: Quaternion Not Properly Normalized"); 120 | Quaternion bt_temp = bt; 121 | bt_temp.normalize(); 122 | msg.x = bt_temp.x(); msg.y = bt_temp.y(); msg.z = bt_temp.z(); msg.w = bt_temp.w(); 123 | } 124 | else 125 | { 126 | msg.x = bt.x(); msg.y = bt.y(); msg.z = bt.z(); msg.w = bt.w(); 127 | } 128 | }; 129 | 130 | /** \brief Helper function for getting yaw from a Quaternion */ 131 | static inline double getYaw(const Quaternion& bt_q){ 132 | tfScalar useless_pitch, useless_roll, yaw; 133 | tf::Matrix3x3(bt_q).getRPY( useless_roll, useless_pitch,yaw); 134 | return yaw; 135 | } 136 | 137 | /** \brief Helper function for getting yaw from a Quaternion message*/ 138 | static inline double getYaw(const geometry_msgs::Quaternion& msg_q){ 139 | Quaternion bt_q; 140 | quaternionMsgToTF(msg_q, bt_q); 141 | return getYaw(bt_q); 142 | } 143 | 144 | /** \brief construct a Quaternion from Fixed angles 145 | * \param roll The roll about the X axis 146 | * \param pitch The pitch about the Y axis 147 | * \param yaw The yaw about the Z axis 148 | * \return The quaternion constructed 149 | */ 150 | static inline tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw){ 151 | Quaternion q; 152 | q.setRPY(roll, pitch, yaw); 153 | return q; 154 | } 155 | 156 | /** \brief construct a Quaternion from yaw only 157 | * \param yaw The yaw about the Z axis 158 | * \return The quaternion constructed 159 | */ 160 | static inline Quaternion createQuaternionFromYaw(double yaw){ 161 | Quaternion q; 162 | q.setRPY(0.0, 0.0, yaw); 163 | return q; 164 | } 165 | 166 | /** \brief construct a Quaternion Message from yaw only 167 | * \param yaw The yaw about the Z axis 168 | * \return The quaternion constructed 169 | */ 170 | static inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw){ 171 | Quaternion q; 172 | q.setRPY(0.0, 0.0, yaw); 173 | geometry_msgs::Quaternion q_msg; 174 | quaternionTFToMsg(q, q_msg); 175 | return q_msg; 176 | } 177 | 178 | /** \brief construct a Quaternion Message from Fixed angles 179 | * \param roll The roll about the X axis 180 | * \param pitch The pitch about the Y axis 181 | * \param yaw The yaw about the Z axis 182 | * \return The quaternion constructed 183 | */ 184 | static inline geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw){ 185 | geometry_msgs::Quaternion q_msg; 186 | quaternionTFToMsg(createQuaternionFromRPY(roll, pitch, yaw), q_msg); 187 | return q_msg; 188 | } 189 | 190 | /** \brief construct an Identity Quaternion 191 | * \return The quaternion constructed 192 | */ 193 | static inline tf::Quaternion createIdentityQuaternion() 194 | { 195 | Quaternion q; 196 | q.setRPY(0,0,0); 197 | return q; 198 | }; 199 | 200 | /** \brief convert QuaternionStamped msg to Stamped */ 201 | static inline void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped & msg, Stamped& bt) 202 | {quaternionMsgToTF(msg.quaternion, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; 203 | /** \brief convert Stamped to QuaternionStamped msg*/ 204 | static inline void quaternionStampedTFToMsg(const Stamped& bt, geometry_msgs::QuaternionStamped & msg) 205 | {quaternionTFToMsg(bt, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; 206 | 207 | /** \brief convert Vector3 msg to Vector3 */ 208 | 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);}; 209 | /** \brief convert Vector3 to Vector3 msg*/ 210 | 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();}; 211 | 212 | /** \brief convert Vector3Stamped msg to Stamped */ 213 | static inline void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped & msg, Stamped& bt) 214 | {vector3MsgToTF(msg.vector, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; 215 | /** \brief convert Stamped to Vector3Stamped msg*/ 216 | static inline void vector3StampedTFToMsg(const Stamped& bt, geometry_msgs::Vector3Stamped & msg) 217 | {vector3TFToMsg(bt, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; 218 | 219 | 220 | /** \brief convert Point msg to Point */ 221 | 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);}; 222 | /** \brief convert Point to Point msg*/ 223 | 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();}; 224 | 225 | /** \brief convert PointStamped msg to Stamped */ 226 | static inline void pointStampedMsgToTF(const geometry_msgs::PointStamped & msg, Stamped& bt) 227 | {pointMsgToTF(msg.point, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; 228 | /** \brief convert Stamped to PointStamped msg*/ 229 | static inline void pointStampedTFToMsg(const Stamped& bt, geometry_msgs::PointStamped & msg) 230 | {pointTFToMsg(bt, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; 231 | 232 | 233 | /** \brief convert Transform msg to Transform */ 234 | static inline void transformMsgToTF(const geometry_msgs::Transform& msg, Transform& bt) 235 | {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));}; 236 | /** \brief convert Transform to Transform msg*/ 237 | static inline void transformTFToMsg(const Transform& bt, geometry_msgs::Transform& msg) 238 | {vector3TFToMsg(bt.getOrigin(), msg.translation); quaternionTFToMsg(bt.getRotation(), msg.rotation);}; 239 | 240 | /** \brief convert TransformStamped msg to tf::StampedTransform */ 241 | static inline void transformStampedMsgToTF(const geometry_msgs::TransformStamped & msg, StampedTransform& bt) 242 | {transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id; bt.child_frame_id_ = msg.child_frame_id;}; 243 | /** \brief convert tf::StampedTransform to TransformStamped msg*/ 244 | static inline void transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg) 245 | {transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.child_frame_id = bt.child_frame_id_;}; 246 | 247 | /** \brief convert Pose msg to Pose */ 248 | static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt) 249 | {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));}; 250 | /** \brief convert Pose to Pose msg*/ 251 | static inline void poseTFToMsg(const Pose& bt, geometry_msgs::Pose& msg) 252 | {pointTFToMsg(bt.getOrigin(), msg.position); quaternionTFToMsg(bt.getRotation(), msg.orientation);}; 253 | 254 | /** \brief convert PoseStamped msg to Stamped */ 255 | static inline void poseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, Stamped& bt) 256 | {poseMsgToTF(msg.pose, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; 257 | /** \brief convert Stamped to PoseStamped msg*/ 258 | static inline void poseStampedTFToMsg(const Stamped& bt, geometry_msgs::PoseStamped & msg) 259 | {poseTFToMsg(bt, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; 260 | 261 | 262 | 263 | 264 | 265 | 266 | } 267 | #endif //TF_TRANSFORM_DATATYPES_H 268 | --------------------------------------------------------------------------------