pax_global_header00006660000000000000000000000064137235237430014523gustar00rootroot0000000000000052 comment=dae26a225e6c14a5aabd25a8b08c3031e6563535 geometry2-0.7.5/000077500000000000000000000000001372352374300134515ustar00rootroot00000000000000geometry2-0.7.5/.github/000077500000000000000000000000001372352374300150115ustar00rootroot00000000000000geometry2-0.7.5/.github/CODEOWNERS000066400000000000000000000000131372352374300163760ustar00rootroot00000000000000* @tfootegeometry2-0.7.5/.gitignore000066400000000000000000000000141372352374300154340ustar00rootroot00000000000000*~ *.pyc \#*geometry2-0.7.5/geometry2/000077500000000000000000000000001372352374300153665ustar00rootroot00000000000000geometry2-0.7.5/geometry2/CHANGELOG.rst000066400000000000000000000017551372352374300174170ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package geometry2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Contributors: Shane Loretz 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- 0.5.16 (2017-07-14) ------------------- 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- * create geometry2 metapackage and make geometry_experimental depend on it for clarity of reverse dependency walking. * Contributors: Tully Foote geometry2-0.7.5/geometry2/CMakeLists.txt000066400000000000000000000001541372352374300201260ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(geometry2) find_package(catkin REQUIRED) catkin_metapackage() geometry2-0.7.5/geometry2/package.xml000066400000000000000000000015371372352374300175110ustar00rootroot00000000000000 geometry2 0.7.5 A metapackage to bring in the default packages second generation Transform Library in ros, tf2. Tully Foote Tully Foote BSD http://www.ros.org/wiki/geometry2 catkin tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools geometry2-0.7.5/test_tf2/000077500000000000000000000000001372352374300152035ustar00rootroot00000000000000geometry2-0.7.5/test_tf2/CHANGELOG.rst000066400000000000000000000250531372352374300172310ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package test_tf2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ * test_tf2: remove unused tf dependency (`#464 `_) * Cherry-picking various commits from Melodic (`#471 `_) * Revert "rework Eigen functions namespace hack" (`#436 `_) * Fixed warnings in message_filter.h (`#434 `_) the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall * Fix ambiguous call for tf2::convert on MSVC (`#444 `_) * rework ambiguous call on MSVC. * Contributors: Fabian Freihube, Robert Haschke 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ * Fix to improper ring_45 test, where 'anchor' frame for both inverse and normal transform was frame 'b' instead of frame 'a', thus creating a problem * [noetic] cherry-pick Windows fixes from melodic-devel (`#450 `_) * [Windows][melodic-devel] Fix install locations (`#442 `_) * fixed install locations of tf2 * [windows][melodic] more portable fixes. (`#443 `_) * more portable fixes. * Contributors: Patrick Beeson, Sean Yen 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Use text=True to fix Python3 incompatibility (`#446 `_) * Update shebang and add launch prefixes for python3 support (`#421 `_) * Always call catkin_package() (`#418 `_) * Merge pull request `#404 `_ from otamachan/remove-load-manifest * Remove roslib.load_manifest * Contributors: Shane Loretz, Tamaki Nishino, Tully Foote 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ 0.6.3 (2018-07-09) ------------------ * use correct unit test for test_tf2_bullet (`#301 `_) * update cmake order (`#298 `_) * Contributors: Tully Foote 0.6.2 (2018-05-02) ------------------ 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- * Merge pull request `#257 `_ from delftrobotics-forks/python3 Make tf2_py python3 compatible again * Use python3 print function. * Contributors: Maarten de Vries, Tully Foote 0.5.16 (2017-07-14) ------------------- * Remove generate_rand_vectors() from a number of tests. (`#227 `_) * Remove a slew of trailing whitespace. Signed-off-by: Chris Lalancette * Remove generate_rand_vectors() from a number of tests. It was never used, so there is no reason to carry it around. Signed-off-by: Chris Lalancette * store gtest return value as int (`#229 `_) * Contributors: Chris Lalancette, dhood 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- * Typos. * Adds unit tests for TF loaded from parameter server. This tests both success (loading a valid TF into the param server) and failures (parameter does not exist, parameter contents are invalid). * Code linting & reorganization - whitespace - indentation - re-organized code to remove duplications. whitespace & indentation changes only. simplified (de-duplicated) duplicate code. missing a duplicate variable. whitespace changes only. * Contributors: Felix Duvallet 0.5.13 (2016-03-04) ------------------- * Remove LGPL from license tags LGPL was erroneously included in 2a38724. As there are no files with it in the package. * Contributors: Jochen Sprickerhof 0.5.12 (2015-08-05) ------------------- * add utilities to get yaw, pitch, roll and identity transform * provide more conversions between types The previous conversion always assumed that it was converting a non-message type to a non-message type. Now, one, both or none can be a message or a non-message. * Contributors: Vincent Rabaud 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- 0.5.9 (2015-03-25) ------------------ 0.5.8 (2015-03-17) ------------------ * remove useless Makefile files * Contributors: Vincent Rabaud 0.5.7 (2014-12-23) ------------------ 0.5.6 (2014-09-18) ------------------ 0.5.5 (2014-06-23) ------------------ * Removed AsyncSpinner workaround * Contributors: Esteve Fernandez 0.5.4 (2014-05-07) ------------------ * Clean up warnings about autostart and add some assertions for coverage * Contributors: Tully Foote 0.5.3 (2014-02-21) ------------------ 0.5.2 (2014-02-20) ------------------ 0.5.1 (2014-02-14) ------------------ 0.5.0 (2014-02-14) ------------------ 0.4.10 (2013-12-26) ------------------- * fixing kdl linking for tests * Contributors: Tully Foote 0.4.9 (2013-11-06) ------------------ 0.4.8 (2013-11-06) ------------------ * Fixed static_transform_publisher duplicate check, added rostest. 0.4.7 (2013-08-28) ------------------ 0.4.6 (2013-08-28) ------------------ 0.4.5 (2013-07-11) ------------------ * fixing quaternion in unit test and adding a timeout on the waitForServer * fixing usage string to show quaternions and using quaternions in the test app * removing redundant declaration * disabling whole cmake invocation in test_tf2 when not CATKIN_ENABLE_TESTING 0.4.4 (2013-07-09) ------------------ 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ 0.4.1 (2013-07-05) ------------------ * fixing test target dependencies * fixing colliding target names between geometry and geometry_experimental * stripping tf2_ros dependency from tf2_bullet. Test was moved to test_tf2 0.4.0 (2013-06-27) ------------------ * splitting rospy dependency into tf2_py so tf2 is pure c++ library. * switching to console_bridge from rosconsole * moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 * Cleaning up unnecessary dependency on roscpp * converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace * Cleaning up packaging of tf2 including: removing unused nodehandle fixing overmatch on search and replace cleaning up a few dependencies and linking removing old backup of package.xml making diff minimally different from tf version of library * Restoring test packages and bullet packages. reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ 0.3.6 (2013-03-03) ------------------ 0.3.5 (2013-02-15 14:46) ------------------------ 0.3.4 (2013-02-15 13:14) ------------------------ 0.3.3 (2013-02-15 11:30) ------------------------ 0.3.2 (2013-02-15 00:42) ------------------------ 0.3.1 (2013-02-14) ------------------ 0.3.0 (2013-02-13) ------------------ * removing packages with missing deps * catkinizing geometry-experimental * add boost linkage * fixing test for header cleanup * fixing usage of bullet for migration to native bullet * Cleanup on test code, all tests pass * cleanup on optimized tests, still failing * Cleanup in compound transform test * Adding more frames to compound transform case * Compound transform test fails on optimized case after more frames added * Compound transform test has more frames in it * Cleanup of compount transform test * Compound transform at root node test fails for optimized branch * compount transform test, non-optimized * time-varying tests with different time-steps for optimized case * Time-varying test inserts data at different time-steps for non-optimized case * Helix (time-varying) test works on optimized branch * Adding more complicated case to helix test * Adding helix test for time-varying transforms in non-optimized case * Corrected ring45 values in buffer core test * Corrected values of ring45 test for non-optimized case * Ring 45 test running on non-optimized tf2 branch, from Tully's commit r880 * filling out ring test case which finds errors in the optimization * Add option to use a callback queue in the message filter * another out-the-back test * move the message filter to tf2_ros * fix warnings * merge from tf_rework * tf2::MessageFilter + tests. Still need to change it around to pass in a callback queue, since we're being triggered directly from the tf2 buffer * adding in y configuration test * a little more realistic * Don't add the request if the transform is already available. Add some new tests * working transformable callbacks with a simple (incomplete) test case * cleaning up test setup * check_v implemented and passing v test and multi tree test * working toward multi configuration tests * removing restructuring for it won't nest like I thought * continuing restructuring and filling in test case setup * restructuring before scaling * Completely remove lookupLists(). canTransform() now uses the same walking code as lookupTransform(). Also fixed a bug in the static transform publisher test * testing chaining in a ring * test dataset generator * more complicated test with interleaving static and dynamic frames passing * static transform tested and working * test in progress, need to unshelve changes. * tests passing and all throw catches removed too\! * move to tf2_ros completed. tests pass again * merge tf2_cpp and tf2_py into tf2_ros * merging and fixing broken unittest * Got transform with types working in python * A working first version of transforming and converting between different types * removing unused datatypes * removing include of old tf from tf2 * testing new argument validation and catching bug * unit test of single link one to try to debug eitan's client bug * working towards interpolation too * A working version of a test case for the python buffer client * merging * adding else to catch uncovered cases, and changing time for easier use * Adding a test for the python buffer client * using permuter now and doing a,b,c to a,b,c, at three different times including 0 * Moving tf2_tests to test_tf2 * moving test to new package * initial package created for testing tf2 geometry2-0.7.5/test_tf2/CMakeLists.txt000066400000000000000000000043611372352374300177470ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(test_tf2) find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_ros) find_package(Boost REQUIRED COMPONENTS thread) find_package(orocos_kdl REQUIRED) catkin_package() if(NOT CATKIN_ENABLE_TESTING) return() endif() include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) catkin_add_gtest(buffer_core_test test/buffer_core_test.cpp) target_link_libraries(buffer_core_test ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) catkin_add_gtest(test_tf2_message_filter test/test_message_filter.cpp) target_link_libraries(test_tf2_message_filter ${Boost_LIBRARIES} ${catkin_LIBRARIES}) catkin_add_gtest(test_convert test/test_convert.cpp) target_link_libraries(test_convert ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) catkin_add_gtest(test_utils test/test_utils.cpp) target_link_libraries(test_utils ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) add_executable(test_buffer_server EXCLUDE_FROM_ALL test/test_buffer_server.cpp) target_link_libraries(test_buffer_server ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) add_executable(test_buffer_client EXCLUDE_FROM_ALL test/test_buffer_client.cpp) target_link_libraries(test_buffer_client ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) add_rostest(test/buffer_client_tester.launch) add_executable(test_static_publisher EXCLUDE_FROM_ALL test/test_static_publisher.cpp) target_link_libraries(test_static_publisher ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) add_rostest(test/static_publisher.launch) add_executable(test_tf2_bullet EXCLUDE_FROM_ALL test/test_tf2_bullet.cpp) target_link_libraries(test_tf2_bullet ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_bullet.launch) if(TARGET tests) add_dependencies(tests test_buffer_server test_buffer_client test_static_publisher test_tf2_bullet) endif() # used as a test fixture if(TARGET tf2_ros_static_transform_publisher) add_dependencies(tests tf2_ros_static_transform_publisher test_static_publisher) endif() geometry2-0.7.5/test_tf2/mainpage.dox000066400000000000000000000011121372352374300174730ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b test_tf2 is ... \section codeapi Code API */ geometry2-0.7.5/test_tf2/package.xml000066400000000000000000000024101372352374300173150ustar00rootroot00000000000000 test_tf2 0.7.5 tf2 unit tests Tully Foote Eitan Marder-Eppstein Tully Foote BSD http://www.ros.org/wiki/geometry_experimental catkin rosconsole roscpp rostest tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs tf2_eigen rosconsole roscpp rostest tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs tf2_eigen rosunit rosbash geometry2-0.7.5/test_tf2/test/000077500000000000000000000000001372352374300161625ustar00rootroot00000000000000geometry2-0.7.5/test_tf2/test/buffer_client_tester.launch000066400000000000000000000005011372352374300235470ustar00rootroot00000000000000 geometry2-0.7.5/test_tf2/test/buffer_core_test.cpp000066400000000000000000003026041372352374300222130ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include "tf2/exceptions.h" #include #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "rostest/permuter.h" void setIdentity(geometry_msgs::Transform& trans) { trans.translation.x = 0; trans.translation.y = 0; trans.translation.z = 0; trans.rotation.x = 0; trans.rotation.y = 0; trans.rotation.z = 0; trans.rotation.w = 1; } void push_back_i(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { /* "a" v (1,0) "b" v (1,0) "c" */ children.push_back("b"); parents.push_back("a"); dx.push_back(1.0); dy.push_back(0.0); children.push_back("c"); parents.push_back("b"); dx.push_back(1.0); dy.push_back(0.0); } void push_back_y(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { /* "a" v (1,0) "b" ------(0,1)-----> "d" v (1,0) v (0,1) "c" "e" */ // a>b children.push_back("b"); parents.push_back("a"); dx.push_back(1.0); dy.push_back(0.0); // b>c children.push_back("c"); parents.push_back("b"); dx.push_back(1.0); dy.push_back(0.0); // b>d children.push_back("d"); parents.push_back("b"); dx.push_back(0.0); dy.push_back(1.0); // d>e children.push_back("e"); parents.push_back("d"); dx.push_back(0.0); dy.push_back(1.0); } void push_back_v(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { /* "a" ------(0,1)-----> "f" v (1,0) v (0,1) "b" "g" v (1,0) "c" */ // a>b children.push_back("b"); parents.push_back("a"); dx.push_back(1.0); dy.push_back(0.0); // b>c children.push_back("c"); parents.push_back("b"); dx.push_back(1.0); dy.push_back(0.0); // a>f children.push_back("f"); parents.push_back("a"); dx.push_back(0.0); dy.push_back(1.0); // f>g children.push_back("g"); parents.push_back("f"); dx.push_back(0.0); dy.push_back(1.0); } void push_back_1(std::vector& children, std::vector& parents, std::vector& dx, std::vector& dy) { children.push_back("2"); parents.push_back("1"); dx.push_back(1.0); dy.push_back(0.0); } void setupTree(tf2::BufferCore& mBC, const std::string& mode, const ros::Time & time, const ros::Duration& interpolation_space = ros::Duration()) { ROS_DEBUG("Clearing Buffer Core for new test setup"); mBC.clear(); ROS_DEBUG("Setting up test tree for formation %s", mode.c_str()); std::vector children; std::vector parents; std::vector dx, dy; if (mode == "i") { push_back_i(children, parents, dx, dy); } else if (mode == "y") { push_back_y(children, parents, dx, dy); } else if (mode == "v") { push_back_v(children, parents, dx, dy); } else if (mode == "ring_45") { /* Form a ring of transforms at every 45 degrees on the unit circle. */ std::vector frames; frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); frames.push_back("d"); frames.push_back("e"); frames.push_back("f"); frames.push_back("g"); frames.push_back("h"); frames.push_back("i"); for (uint8_t iteration = 0; iteration < 2; ++iteration) { double direction = 1; std::string frame_prefix; if (iteration == 0) { frame_prefix = "inverse_"; direction = -1; } else frame_prefix =""; for (uint64_t i = 1; i < frames.size(); i++) { geometry_msgs::TransformStamped ts; setIdentity(ts.transform); ts.transform.translation.x = direction * ( sqrt(2)/2 - 1); ts.transform.translation.y = direction * sqrt(2)/2; ts.transform.rotation.x = 0; ts.transform.rotation.y = 0; ts.transform.rotation.z = sin(direction * M_PI/8); ts.transform.rotation.w = cos(direction * M_PI/8); if (time > ros::Time() + (interpolation_space * .5)) ts.header.stamp = time - (interpolation_space * .5); else ts.header.stamp = ros::Time(); ts.child_frame_id = frame_prefix + frames[i]; if (i > 1) ts.header.frame_id = frame_prefix + frames[i-1]; else ts.header.frame_id = frames[i-1]; EXPECT_TRUE(mBC.setTransform(ts, "authority")); if (interpolation_space > ros::Duration()) { ts.header.stamp = time + interpolation_space * .5; EXPECT_TRUE(mBC.setTransform(ts, "authority")); } } } return; // nonstandard setup return before standard executinog } else if (mode == "1") { push_back_1(children, parents, dx, dy); } else if (mode =="1_v") { push_back_1(children, parents, dx, dy); push_back_v(children, parents, dx, dy); } else EXPECT_FALSE("Undefined mode for tree setup. Test harness improperly setup."); /// Standard for (uint64_t i = 0; i < children.size(); i++) { geometry_msgs::TransformStamped ts; setIdentity(ts.transform); ts.transform.translation.x = dx[i]; ts.transform.translation.y = dy[i]; if (time > ros::Time() + (interpolation_space * .5)) ts.header.stamp = time - (interpolation_space * .5); else ts.header.stamp = ros::Time(); ts.header.frame_id = parents[i]; ts.child_frame_id = children[i]; EXPECT_TRUE(mBC.setTransform(ts, "authority")); if (interpolation_space > ros::Duration()) { ts.header.stamp = time + interpolation_space * .5; EXPECT_TRUE(mBC.setTransform(ts, "authority")); } } } TEST(BufferCore_setTransform, NoInsertOnSelfTransform) { tf2::BufferCore mBC; geometry_msgs::TransformStamped tranStamped; setIdentity(tranStamped.transform); tranStamped.header.stamp = ros::Time().fromNSec(10.0); tranStamped.header.frame_id = "same_frame"; tranStamped.child_frame_id = "same_frame"; EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); } TEST(BufferCore_setTransform, NoInsertWithNan) { tf2::BufferCore mBC; geometry_msgs::TransformStamped tranStamped; setIdentity(tranStamped.transform); tranStamped.header.stamp = ros::Time().fromNSec(10.0); tranStamped.header.frame_id = "same_frame"; tranStamped.child_frame_id = "other_frame"; EXPECT_TRUE(mBC.setTransform(tranStamped, "authority")); tranStamped.transform.translation.x = std::nan(""); EXPECT_TRUE(std::isnan(tranStamped.transform.translation.x)); EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); } TEST(BufferCore_setTransform, NoInsertWithNoFrameID) { tf2::BufferCore mBC; geometry_msgs::TransformStamped tranStamped; setIdentity(tranStamped.transform); tranStamped.header.stamp = ros::Time().fromNSec(10.0); tranStamped.header.frame_id = "same_frame"; tranStamped.child_frame_id = ""; EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); tranStamped.child_frame_id = "/"; EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); } TEST(BufferCore_setTransform, NoInsertWithNoParentID) { tf2::BufferCore mBC; geometry_msgs::TransformStamped tranStamped; setIdentity(tranStamped.transform); tranStamped.header.stamp = ros::Time().fromNSec(10.0); tranStamped.header.frame_id = ""; tranStamped.child_frame_id = "some_frame"; EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); tranStamped.header.frame_id = "/"; EXPECT_FALSE(mBC.setTransform(tranStamped, "authority")); } /* TEST(tf, ListOneInverse) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped (btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( uint64_t i = 0; i < runs ; i++ ) { Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "child"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("my_parent",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, ListTwoInverse) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "child", "grandchild"); mTR.setTransform(tranStamped2); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( unsigned int i = 0; i < runs ; i++ ) { Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "grandchild"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("my_parent",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 2*xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 2*yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 2*zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, ListOneForward) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( uint64_t i = 0; i < runs ; i++ ) { Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("child",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), -xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), -yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), -zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, ListTwoForward) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parent", "child"); mTR.setTransform(tranStamped); StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "child", "grandchild"); mTR.setTransform(tranStamped2); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( unsigned int i = 0; i < runs ; i++ ) { Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "my_parent"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("grandchild",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), -2*xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), -2*yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), -2*zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, TransformThrougRoot) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100), "my_parent", "childA"); mTR.setTransform(tranStamped); StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(1000 + i*100), "my_parent", "childB"); mTR.setTransform(tranStamped2); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( unsigned int i = 0; i < runs ; i++ ) { Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000 + i*100), "childA"); try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("childB",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon); } catch (tf::TransformException & ex) { std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl; bool exception_improperly_thrown = true; EXPECT_FALSE(exception_improperly_thrown); } } } TEST(tf, TransformThroughNO_PARENT) { unsigned int runs = 4; double epsilon = 1e-6; seed_rand(); tf::Transformer mTR(true); std::vector xvalues(runs), yvalues(runs), zvalues(runs); for ( unsigned int i = 0; i < runs ; i++ ) { xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; StampedTransform tranStamped(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parentA", "childA"); mTR.setTransform(tranStamped); StampedTransform tranStamped2(btTransform(btQuaternion(0,0,0,1), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time().fromNSec(10 + i), "my_parentB", "childB"); mTR.setTransform(tranStamped2); } // std::cout << mTR.allFramesAsString() << std::endl; // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl; for ( unsigned int i = 0; i < runs ; i++ ) { Stamped inpose (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10 + i), "childA"); bool exception_thrown = false; try{ Stamped outpose; outpose.setIdentity(); //to make sure things are getting mutated mTR.transformPose("childB",inpose, outpose); EXPECT_NEAR(outpose.getOrigin().x(), 0*xvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().y(), 0*yvalues[i], epsilon); EXPECT_NEAR(outpose.getOrigin().z(), 0*zvalues[i], epsilon); } catch (tf::TransformException & ex) { exception_thrown = true; } EXPECT_TRUE(exception_thrown); } } */ TEST(BufferCore_lookupTransform, i_configuration) { double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; times.push_back(ros::Time(1.0)); times.push_back(ros::Time(10.0)); times.push_back(ros::Time(0.0)); ros::Time eval_time; permuter.addOptionSet(times, &eval_time); std::vector durations; durations.push_back(ros::Duration(1.0)); durations.push_back(ros::Duration(0.001)); durations.push_back(ros::Duration(0.1)); ros::Duration interpolation_space; // permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); std::string target_frame; permuter.addOptionSet(frames, &target_frame); while (permuter.step()) { tf2::BufferCore mBC; setupTree(mBC, "i", eval_time, interpolation_space); geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon); //Zero distance if (source_frame == target_frame) { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); } else if ((source_frame == "a" && target_frame =="b") || (source_frame == "b" && target_frame =="c")) { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); } else if ((source_frame == "b" && target_frame =="a") || (source_frame == "c" && target_frame =="b")) { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); } else if (source_frame == "a" && target_frame =="c") { EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon); } else if (source_frame == "c" && target_frame =="a") { EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); } else { EXPECT_FALSE("i configuration: Shouldn't get here"); printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); } } } /* Check 1 result return false if test parameters unmet */ bool check_1_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon) { //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon); //Zero distance if (source_frame == target_frame) { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); } else if (source_frame == "1" && target_frame =="2") { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); } else if (source_frame == "2" && target_frame =="1") { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); } else { //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); return false; } return true; } /* Check v result return false if test parameters unmet */ bool check_v_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon) { //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon); //Zero distance if (source_frame == target_frame) { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); } else if ((source_frame == "a" && target_frame =="b") || (source_frame == "b" && target_frame =="c")) { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); } else if ((source_frame == "b" && target_frame =="a") || (source_frame == "c" && target_frame =="b")) { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); } else if ((source_frame == "a" && target_frame =="f") || (source_frame == "f" && target_frame =="g")) { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon); } else if ((source_frame == "f" && target_frame =="a") || (source_frame == "g" && target_frame =="f")) { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon); } else if (source_frame == "a" && target_frame =="g") { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon); } else if (source_frame == "g" && target_frame =="a") { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon); } else if (source_frame == "a" && target_frame =="c") { EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); } else if (source_frame == "c" && target_frame =="a") { EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); } else if (source_frame == "b" && target_frame =="f") { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon); } else if (source_frame == "f" && target_frame =="b") { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon); } else if (source_frame == "c" && target_frame =="f") { EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon); } else if (source_frame == "f" && target_frame =="c") { EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon); } else if (source_frame == "b" && target_frame =="g") { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon); } else if (source_frame == "g" && target_frame =="b") { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon); } else if (source_frame == "c" && target_frame =="g") { EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon); } else if (source_frame == "g" && target_frame =="c") { EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon); } else { //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); return false; } return true; } /* Check v result return false if test parameters unmet */ bool check_y_result(const geometry_msgs::TransformStamped& outpose, const std::string& source_frame, const std::string& target_frame, const ros::Time& eval_time, double epsilon) { //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.w, 1, epsilon); //Zero distance if (source_frame == target_frame) { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); } else if ((source_frame == "a" && target_frame =="b") || (source_frame == "b" && target_frame =="c")) { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); } else if ((source_frame == "b" && target_frame =="a") || (source_frame == "c" && target_frame =="b")) { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); } else if ((source_frame == "b" && target_frame =="d") || (source_frame == "d" && target_frame =="e")) { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon); } else if ((source_frame == "d" && target_frame =="b") || (source_frame == "e" && target_frame =="d")) { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon); } else if (source_frame == "b" && target_frame =="e") { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon); } else if (source_frame == "e" && target_frame =="b") { EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon); } else if (source_frame == "a" && target_frame =="c") { EXPECT_NEAR(outpose.transform.translation.x, 2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); } else if (source_frame == "c" && target_frame =="a") { EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); } else if (source_frame == "a" && target_frame =="d") { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon); } else if (source_frame == "d" && target_frame =="a") { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon); } else if (source_frame == "c" && target_frame =="d") { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon); } else if (source_frame == "d" && target_frame =="c") { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon); } else if (source_frame == "a" && target_frame =="e") { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon); } else if (source_frame == "e" && target_frame =="a") { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon); } else if (source_frame == "c" && target_frame =="e") { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 2, epsilon); } else if (source_frame == "e" && target_frame =="c") { EXPECT_NEAR(outpose.transform.translation.x, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -2, epsilon); } else { //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); return false; } return true; } TEST(BufferCore_lookupTransform, one_link_configuration) { double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; times.push_back(ros::Time(1.0)); times.push_back(ros::Time(10.0)); times.push_back(ros::Time(0.0)); ros::Time eval_time; permuter.addOptionSet(times, &eval_time); std::vector durations; durations.push_back(ros::Duration(1.0)); durations.push_back(ros::Duration(0.001)); durations.push_back(ros::Duration(0.1)); ros::Duration interpolation_space; // permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("1"); frames.push_back("2"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); std::string target_frame; permuter.addOptionSet(frames, &target_frame); while (permuter.step()) { tf2::BufferCore mBC; setupTree(mBC, "1", eval_time, interpolation_space); geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon)); } } TEST(BufferCore_lookupTransform, v_configuration) { double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; times.push_back(ros::Time(1.0)); times.push_back(ros::Time(10.0)); times.push_back(ros::Time(0.0)); ros::Time eval_time; permuter.addOptionSet(times, &eval_time); std::vector durations; durations.push_back(ros::Duration(1.0)); durations.push_back(ros::Duration(0.001)); durations.push_back(ros::Duration(0.1)); ros::Duration interpolation_space; // permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); frames.push_back("f"); frames.push_back("g"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); std::string target_frame; permuter.addOptionSet(frames, &target_frame); while (permuter.step()) { tf2::BufferCore mBC; setupTree(mBC, "v", eval_time, interpolation_space); geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon)); } } TEST(BufferCore_lookupTransform, y_configuration) { double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; times.push_back(ros::Time(1.0)); times.push_back(ros::Time(10.0)); times.push_back(ros::Time(0.0)); ros::Time eval_time; permuter.addOptionSet(times, &eval_time); std::vector durations; durations.push_back(ros::Duration(1.0)); durations.push_back(ros::Duration(0.001)); durations.push_back(ros::Duration(0.1)); ros::Duration interpolation_space; // permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); frames.push_back("d"); frames.push_back("e"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); std::string target_frame; permuter.addOptionSet(frames, &target_frame); while (permuter.step()) { tf2::BufferCore mBC; setupTree(mBC, "y", eval_time, interpolation_space); geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); EXPECT_TRUE(check_y_result(outpose, source_frame, target_frame, eval_time, epsilon)); } } TEST(BufferCore_lookupTransform, multi_configuration) { double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; times.push_back(ros::Time(1.0)); times.push_back(ros::Time(10.0)); times.push_back(ros::Time(0.0)); ros::Time eval_time; permuter.addOptionSet(times, &eval_time); std::vector durations; durations.push_back(ros::Duration(1.0)); durations.push_back(ros::Duration(0.001)); durations.push_back(ros::Duration(0.1)); ros::Duration interpolation_space; // permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("1"); frames.push_back("2"); frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); frames.push_back("f"); frames.push_back("g"); std::string source_frame; permuter.addOptionSet(frames, &source_frame); std::string target_frame; permuter.addOptionSet(frames, &target_frame); while (permuter.step()) { tf2::BufferCore mBC; setupTree(mBC, "1_v", eval_time, interpolation_space); if (mBC.canTransform(source_frame, target_frame, eval_time)) { geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); if ((source_frame == "1" || source_frame =="2") && (target_frame =="1" || target_frame == "2")) EXPECT_TRUE(check_1_result(outpose, source_frame, target_frame, eval_time, epsilon)); else if ((source_frame == "a" || source_frame == "b" || source_frame == "c" || source_frame == "f" || source_frame == "g") && (target_frame == "a" || target_frame == "b" || target_frame == "c" || target_frame == "f" || target_frame == "g")) EXPECT_TRUE(check_v_result(outpose, source_frame, target_frame, eval_time, epsilon)); else EXPECT_FALSE("Frames unhandled"); } else EXPECT_TRUE(((source_frame == "a" || source_frame =="b" || source_frame == "c" || source_frame == "f" || source_frame == "g") && (target_frame == "1" || target_frame == "2") ) || ((target_frame == "a" || target_frame =="b" || target_frame == "c" || target_frame == "f" || target_frame == "g") && (source_frame == "1" || source_frame == "2")) ); } } #define CHECK_QUATERNION_NEAR(_q1, _x, _y, _z, _w, _epsilon) \ { \ btQuaternion q1(_q1.x, _q1.y, _q1.z, _q1.w); \ btQuaternion q2(_x, _y, _z, _w); \ double angle = q1.angle(q2); \ EXPECT_TRUE(fabs(angle) < _epsilon || fabs(angle - M_PI) < _epsilon); \ } #define CHECK_TRANSFORMS_NEAR(_out, _expected, _eps) \ EXPECT_NEAR(_out.transform.translation.x, _expected.getOrigin().x(), epsilon); \ EXPECT_NEAR(_out.transform.translation.y, _expected.getOrigin().y(), epsilon); \ EXPECT_NEAR(_out.transform.translation.z, _expected.getOrigin().z(), epsilon); \ CHECK_QUATERNION_NEAR(_out.transform.rotation, _expected.getRotation().x(), _expected.getRotation().y(), _expected.getRotation().z(), _expected.getRotation().w(), _eps); // Simple test with compound transform TEST(BufferCore_lookupTransform, compound_xfm_configuration) { /* * Frames * * root->a * * root->b->c->d * */ double epsilon = 2e-5; // Larger epsilon for interpolation values tf2::BufferCore mBC; geometry_msgs::TransformStamped tsa; tsa.header.frame_id = "root"; tsa.child_frame_id = "a"; tsa.transform.translation.x = 1.0; tsa.transform.translation.y = 1.0; tsa.transform.translation.z = 1.0; btQuaternion q1; q1.setEuler(0.25, .5, .75); tsa.transform.rotation.x = q1.x(); tsa.transform.rotation.y = q1.y(); tsa.transform.rotation.z = q1.z(); tsa.transform.rotation.w = q1.w(); EXPECT_TRUE(mBC.setTransform(tsa, "authority")); geometry_msgs::TransformStamped tsb; tsb.header.frame_id = "root"; tsb.child_frame_id = "b"; tsb.transform.translation.x = -1.0; tsb.transform.translation.y = 0.0; tsb.transform.translation.z = -1.0; btQuaternion q2; q2.setEuler(1.0, 0.25, 0.5); tsb.transform.rotation.x = q2.x(); tsb.transform.rotation.y = q2.y(); tsb.transform.rotation.z = q2.z(); tsb.transform.rotation.w = q2.w(); EXPECT_TRUE(mBC.setTransform(tsb, "authority")); geometry_msgs::TransformStamped tsc; tsc.header.frame_id = "b"; tsc.child_frame_id = "c"; tsc.transform.translation.x = 0.0; tsc.transform.translation.y = 2.0; tsc.transform.translation.z = 0.5; btQuaternion q3; q3.setEuler(0.25, .75, 1.25); tsc.transform.rotation.x = q3.x(); tsc.transform.rotation.y = q3.y(); tsc.transform.rotation.z = q3.z(); tsc.transform.rotation.w = q3.w(); EXPECT_TRUE(mBC.setTransform(tsc, "authority")); geometry_msgs::TransformStamped tsd; tsd.header.frame_id = "c"; tsd.child_frame_id = "d"; tsd.transform.translation.x = 0.5; tsd.transform.translation.y = -1; tsd.transform.translation.z = 1.5; btQuaternion q4; q4.setEuler(-0.5, 1.0, -.75); tsd.transform.rotation.x = q4.x(); tsd.transform.rotation.y = q4.y(); tsd.transform.rotation.z = q4.z(); tsd.transform.rotation.w = q4.w(); EXPECT_TRUE(mBC.setTransform(tsd, "authority")); btTransform ta, tb, tc, td, expected_ab, expected_bc, expected_cb, expected_ac, expected_ba, expected_ca, expected_ad, expected_da, expected_bd, expected_db, expected_rootd, expected_rootc; ta.setOrigin(btVector3(1.0, 1.0, 1.0)); ta.setRotation(q1); tb.setOrigin(btVector3(-1.0, 0.0, -1.0)); tb.setRotation(q2); tc.setOrigin(btVector3(0.0, 2.0, 0.5)); tc.setRotation(q3); td.setOrigin(btVector3(0.5, -1, 1.5)); td.setRotation(q4); expected_ab = ta.inverse() * tb; expected_ac = ta.inverse() * tb * tc; expected_ad = ta.inverse() * tb * tc * td; expected_cb = tc.inverse(); expected_bc = tc; expected_bd = tc * td; expected_db = expected_bd.inverse(); expected_ba = tb.inverse() * ta; expected_ca = tc.inverse() * tb.inverse() * ta; expected_da = td.inverse() * tc.inverse() * tb.inverse() * ta; expected_rootd = tb * tc * td; expected_rootc = tb * tc; // root -> b -> c geometry_msgs::TransformStamped out_rootc = mBC.lookupTransform("root", "c", ros::Time()); CHECK_TRANSFORMS_NEAR(out_rootc, expected_rootc, epsilon); // root -> b -> c -> d geometry_msgs::TransformStamped out_rootd = mBC.lookupTransform("root", "d", ros::Time()); CHECK_TRANSFORMS_NEAR(out_rootd, expected_rootd, epsilon); // a <- root -> b geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", ros::Time()); CHECK_TRANSFORMS_NEAR(out_ab, expected_ab, epsilon); geometry_msgs::TransformStamped out_ba = mBC.lookupTransform("b", "a", ros::Time()); CHECK_TRANSFORMS_NEAR(out_ba, expected_ba, epsilon); // a <- root -> b -> c geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", ros::Time()); CHECK_TRANSFORMS_NEAR(out_ac, expected_ac, epsilon); geometry_msgs::TransformStamped out_ca = mBC.lookupTransform("c", "a", ros::Time()); CHECK_TRANSFORMS_NEAR(out_ca, expected_ca, epsilon); // a <- root -> b -> c -> d geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", ros::Time()); CHECK_TRANSFORMS_NEAR(out_ad, expected_ad, epsilon); geometry_msgs::TransformStamped out_da = mBC.lookupTransform("d", "a", ros::Time()); CHECK_TRANSFORMS_NEAR(out_da, expected_da, epsilon); // b -> c geometry_msgs::TransformStamped out_cb = mBC.lookupTransform("c", "b", ros::Time()); CHECK_TRANSFORMS_NEAR(out_cb, expected_cb, epsilon); geometry_msgs::TransformStamped out_bc = mBC.lookupTransform("b", "c", ros::Time()); CHECK_TRANSFORMS_NEAR(out_bc, expected_bc, epsilon); // b -> c -> d geometry_msgs::TransformStamped out_bd = mBC.lookupTransform("b", "d", ros::Time()); CHECK_TRANSFORMS_NEAR(out_bd, expected_bd, epsilon); geometry_msgs::TransformStamped out_db = mBC.lookupTransform("d", "b", ros::Time()); CHECK_TRANSFORMS_NEAR(out_db, expected_db, epsilon); } // Time varying transforms, testing interpolation TEST(BufferCore_lookupTransform, helix_configuration) { double epsilon = 2e-5; // Larger epsilon for interpolation values tf2::BufferCore mBC; ros::Time t0 = ros::Time() + ros::Duration(10); ros::Duration step = ros::Duration(0.05); ros::Duration half_step = ros::Duration(0.025); ros::Time t1 = t0 + ros::Duration(5.0); /* * a->b->c * * b.z = vel * (t - t0) * c.x = cos(theta * (t - t0)) * c.y = sin(theta * (t - t0)) * * a->d * * d.z = 2 * cos(theta * (t - t0)) * a->d transforms are at half-step between a->b->c transforms */ double theta = 0.25; double vel = 1.0; for (ros::Time t = t0; t <= t1; t += step) { ros::Time t2 = t + half_step; double dt = (t - t0).toSec(); double dt2 = (t2 - t0).toSec(); geometry_msgs::TransformStamped ts; ts.header.frame_id = "a"; ts.header.stamp = t; ts.child_frame_id = "b"; ts.transform.translation.z = vel * dt; ts.transform.rotation.w = 1.0; EXPECT_TRUE(mBC.setTransform(ts, "authority")); geometry_msgs::TransformStamped ts2; ts2.header.frame_id = "b"; ts2.header.stamp = t; ts2.child_frame_id = "c"; ts2.transform.translation.x = cos(theta * dt); ts2.transform.translation.y = sin(theta * dt); btQuaternion q; q.setEuler(0,0,theta*dt); ts2.transform.rotation.z = q.z(); ts2.transform.rotation.w = q.w(); EXPECT_TRUE(mBC.setTransform(ts2, "authority")); geometry_msgs::TransformStamped ts3; ts3.header.frame_id = "a"; ts3.header.stamp = t2; ts3.child_frame_id = "d"; ts3.transform.translation.z = cos(theta * dt2); ts3.transform.rotation.w = 1.0; EXPECT_TRUE(mBC.setTransform(ts3, "authority")); } for (ros::Time t = t0 + half_step; t < t1; t += step) { ros::Time t2 = t + half_step; double dt = (t - t0).toSec(); double dt2 = (t2 - t0).toSec(); geometry_msgs::TransformStamped out_ab = mBC.lookupTransform("a", "b", t); EXPECT_NEAR(out_ab.transform.translation.z, vel * dt, epsilon); geometry_msgs::TransformStamped out_ac = mBC.lookupTransform("a", "c", t); EXPECT_NEAR(out_ac.transform.translation.x, cos(theta * dt), epsilon); EXPECT_NEAR(out_ac.transform.translation.y, sin(theta * dt), epsilon); EXPECT_NEAR(out_ac.transform.translation.z, vel * dt, epsilon); btQuaternion q; q.setEuler(0,0,theta*dt); CHECK_QUATERNION_NEAR(out_ac.transform.rotation, 0, 0, q.z(), q.w(), epsilon); geometry_msgs::TransformStamped out_ad = mBC.lookupTransform("a", "d", t); EXPECT_NEAR(out_ad.transform.translation.z, cos(theta * dt), epsilon); geometry_msgs::TransformStamped out_cd = mBC.lookupTransform("c", "d", t2); EXPECT_NEAR(out_cd.transform.translation.x, -1, epsilon); EXPECT_NEAR(out_cd.transform.translation.y, 0, epsilon); EXPECT_NEAR(out_cd.transform.translation.z, cos(theta * dt2) - vel * dt2, epsilon); btQuaternion mq; mq.setEuler(0,0,-theta*dt2); CHECK_QUATERNION_NEAR(out_cd.transform.rotation, 0, 0, mq.z(), mq.w(), epsilon); } // Advanced API for (ros::Time t = t0 + half_step; t < t1; t += (step + step)) { ros::Time t2 = t + step; double dt = (t - t0).toSec(); double dt2 = (t2 - t0).toSec(); geometry_msgs::TransformStamped out_cd2 = mBC.lookupTransform("c", t, "d", t2, "a"); EXPECT_NEAR(out_cd2.transform.translation.x, -1, epsilon); EXPECT_NEAR(out_cd2.transform.translation.y, 0, epsilon); EXPECT_NEAR(out_cd2.transform.translation.z, cos(theta * dt2) - vel * dt, epsilon); btQuaternion mq2; mq2.setEuler(0,0,-theta*dt); CHECK_QUATERNION_NEAR(out_cd2.transform.rotation, 0, 0, mq2.z(), mq2.w(), epsilon); } } TEST(BufferCore_lookupTransform, ring_45_configuration) { double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; times.push_back(ros::Time(1.0)); times.push_back(ros::Time(10.0)); times.push_back(ros::Time(0.0)); ros::Time eval_time; permuter.addOptionSet(times, &eval_time); std::vector durations; durations.push_back(ros::Duration(1.0)); durations.push_back(ros::Duration(0.001)); durations.push_back(ros::Duration(0.1)); ros::Duration interpolation_space; // permuter.addOptionSet(durations, &interpolation_space); std::vector frames; frames.push_back("a"); frames.push_back("b"); frames.push_back("c"); frames.push_back("d"); frames.push_back("e"); frames.push_back("f"); frames.push_back("g"); frames.push_back("h"); frames.push_back("i"); /* frames.push_back("inverse_b"); frames.push_back("inverse_c"); frames.push_back("inverse_d"); frames.push_back("inverse_e"); frames.push_back("inverse_f"); frames.push_back("inverse_g"); frames.push_back("inverse_h"); frames.push_back("inverse_i");*/ std::string source_frame; permuter.addOptionSet(frames, &source_frame); std::string target_frame; permuter.addOptionSet(frames, &target_frame); while (permuter.step()) { tf2::BufferCore mBC; setupTree(mBC, "ring_45", eval_time, interpolation_space); geometry_msgs::TransformStamped outpose = mBC.lookupTransform(source_frame, target_frame, eval_time); //printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); EXPECT_EQ(outpose.header.stamp, eval_time); EXPECT_EQ(outpose.header.frame_id, source_frame); EXPECT_EQ(outpose.child_frame_id, target_frame); //Zero distance or all the way if (source_frame == target_frame || (source_frame == "a" && target_frame == "i") || (source_frame == "i" && target_frame == "a") || (source_frame == "a" && target_frame == "inverse_i") || (source_frame == "inverse_i" && target_frame == "a") ) { //printf ("here %s %s\n", source_frame.c_str(), target_frame.c_str()); EXPECT_NEAR(outpose.transform.translation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, 0, epsilon); EXPECT_NEAR(fabs(outpose.transform.rotation.w), 1, epsilon); } // Chaining 1 else if ((source_frame == "a" && target_frame =="b") || (source_frame == "b" && target_frame =="c") || (source_frame == "c" && target_frame =="d") || (source_frame == "d" && target_frame =="e") || (source_frame == "e" && target_frame =="f") || (source_frame == "f" && target_frame =="g") || (source_frame == "g" && target_frame =="h") || (source_frame == "h" && target_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/8), epsilon); } // Inverse Chaining 1 else if ((source_frame == "b" && target_frame =="a") || (source_frame == "c" && target_frame =="b") || (source_frame == "d" && target_frame =="c") || (source_frame == "e" && target_frame =="d") || (source_frame == "f" && target_frame =="e") || (source_frame == "g" && target_frame =="f") || (source_frame == "h" && target_frame =="g") || (source_frame == "i" && target_frame =="h") ) { EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2)/2 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/8), epsilon); } // Chaining 2 else if ((source_frame == "a" && target_frame =="c") || (source_frame == "b" && target_frame =="d") || (source_frame == "c" && target_frame =="e") || (source_frame == "d" && target_frame =="f") || (source_frame == "e" && target_frame =="g") || (source_frame == "f" && target_frame =="h") || (source_frame == "g" && target_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/4), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/4), epsilon); } // Inverse Chaining 2 else if ((source_frame == "c" && target_frame =="a") || (source_frame == "d" && target_frame =="b") || (source_frame == "e" && target_frame =="c") || (source_frame == "f" && target_frame =="d") || (source_frame == "g" && target_frame =="e") || (source_frame == "h" && target_frame =="f") || (source_frame == "i" && target_frame =="g") ) { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/4), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/4), epsilon); } // Chaining 3 else if ((source_frame == "a" && target_frame =="d") || (source_frame == "b" && target_frame =="e") || (source_frame == "c" && target_frame =="f") || (source_frame == "d" && target_frame =="g") || (source_frame == "e" && target_frame =="h") || (source_frame == "f" && target_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*3/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*3/8), epsilon); } // Inverse Chaining 3 else if ((target_frame == "a" && source_frame =="d") || (target_frame == "b" && source_frame =="e") || (target_frame == "c" && source_frame =="f") || (target_frame == "d" && source_frame =="g") || (target_frame == "e" && source_frame =="h") || (target_frame == "f" && source_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, - sqrt(2)/2 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*3/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*3/8), epsilon); } // Chaining 4 else if ((source_frame == "a" && target_frame =="e") || (source_frame == "b" && target_frame =="f") || (source_frame == "c" && target_frame =="g") || (source_frame == "d" && target_frame =="h") || (source_frame == "e" && target_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI/2), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI/2), epsilon); } // Inverse Chaining 4 else if ((target_frame == "a" && source_frame =="e") || (target_frame == "b" && source_frame =="f") || (target_frame == "c" && source_frame =="g") || (target_frame == "d" && source_frame =="h") || (target_frame == "e" && source_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI/2), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI/2), epsilon); } // Chaining 5 else if ((source_frame == "a" && target_frame =="f") || (source_frame == "b" && target_frame =="g") || (source_frame == "c" && target_frame =="h") || (source_frame == "d" && target_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) /2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, - sqrt(2) /2 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*5/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*5/8), epsilon); } // Inverse Chaining 5 else if ((target_frame == "a" && source_frame =="f") || (target_frame == "b" && source_frame =="g") || (target_frame == "c" && source_frame =="h") || (target_frame == "d" && source_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2)/2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*5/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*5/8), epsilon); } // Chaining 6 else if ((source_frame == "a" && target_frame =="g") || (source_frame == "b" && target_frame =="h") || (source_frame == "c" && target_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*6/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*6/8), epsilon); } // Inverse Chaining 6 else if ((target_frame == "a" && source_frame =="g") || (target_frame == "b" && source_frame =="h") || (target_frame == "c" && source_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*6/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*6/8), epsilon); } // Chaining 7 else if ((source_frame == "a" && target_frame =="h") || (source_frame == "b" && target_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2)/2 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI*7/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI*7/8), epsilon); } // Inverse Chaining 7 else if ((target_frame == "a" && source_frame =="h") || (target_frame == "b" && source_frame =="i") ) { EXPECT_NEAR(outpose.transform.translation.x, sqrt(2)/2 - 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, sqrt(2)/2 , epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI*7/8), epsilon); EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI*7/8), epsilon); } else { EXPECT_FALSE("Ring_45 testing Shouldn't get here"); printf("source_frame %s target_frame %s time %f\n", source_frame.c_str(), target_frame.c_str(), eval_time.toSec()); } } } TEST(BufferCore_lookupTransform, invalid_arguments) { tf2::BufferCore mBC; setupTree(mBC, "i", ros::Time(1.0)); EXPECT_NO_THROW(mBC.lookupTransform("b", "a", ros::Time())); //Empty frame_id EXPECT_THROW(mBC.lookupTransform("", "a", ros::Time()), tf2::InvalidArgumentException); EXPECT_THROW(mBC.lookupTransform("b", "", ros::Time()), tf2::InvalidArgumentException); //frame_id with / EXPECT_THROW(mBC.lookupTransform("/b", "a", ros::Time()), tf2::InvalidArgumentException); EXPECT_THROW(mBC.lookupTransform("b", "/a", ros::Time()), tf2::InvalidArgumentException); }; TEST(BufferCore_canTransform, invalid_arguments) { tf2::BufferCore mBC; setupTree(mBC, "i", ros::Time(1.0)); EXPECT_TRUE(mBC.canTransform("b", "a", ros::Time())); //Empty frame_id EXPECT_FALSE(mBC.canTransform("", "a", ros::Time())); EXPECT_FALSE(mBC.canTransform("b", "", ros::Time())); //frame_id with / EXPECT_FALSE(mBC.canTransform("/b", "a", ros::Time())); EXPECT_FALSE(mBC.canTransform("b", "/a", ros::Time())); }; struct TransformableHelper { TransformableHelper() : called(false) {} void callback(tf2::TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame, ros::Time time, tf2::TransformableResult result) { called = true; } bool called; }; TEST(BufferCore_transformableCallbacks, alreadyTransformable) { tf2::BufferCore b; TransformableHelper h; geometry_msgs::TransformStamped t; t.header.stamp = ros::Time(1); t.header.frame_id = "a"; t.child_frame_id = "b"; t.transform.rotation.w = 1.0; b.setTransform(t, "me"); tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5)); EXPECT_EQ(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(1)), 0U); } TEST(BufferCore_transformableCallbacks, waitForNewTransform) { tf2::BufferCore b; TransformableHelper h; tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5)); EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(10)), 0U); geometry_msgs::TransformStamped t; for (uint32_t i = 1; i <= 10; ++i) { t.header.stamp = ros::Time(i); t.header.frame_id = "a"; t.child_frame_id = "b"; t.transform.rotation.w = 1.0; b.setTransform(t, "me"); if (i < 10) { ASSERT_FALSE(h.called); } else { ASSERT_TRUE(h.called); } } } TEST(BufferCore_transformableCallbacks, waitForOldTransform) { tf2::BufferCore b; TransformableHelper h; tf2::TransformableCallbackHandle cb_handle = b.addTransformableCallback(boost::bind(&TransformableHelper::callback, &h, _1, _2, _3, _4, _5)); EXPECT_GT(b.addTransformableRequest(cb_handle, "a", "b", ros::Time(1)), 0U); geometry_msgs::TransformStamped t; for (uint32_t i = 10; i > 0; --i) { t.header.stamp = ros::Time(i); t.header.frame_id = "a"; t.child_frame_id = "b"; t.transform.rotation.w = 1.0; b.setTransform(t, "me"); if (i > 1) { ASSERT_FALSE(h.called); } else { ASSERT_TRUE(h.called); } } } /* TEST(tf, Exceptions) { tf::Transformer mTR(true); Stamped outpose; //connectivity when no data EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(10000000))); try { mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000000) , "me"), outpose); EXPECT_FALSE("ConnectivityException Not Thrown"); } catch ( tf::LookupException &ex) { EXPECT_TRUE("Lookupgh Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } mTR.setTransform( StampedTransform(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(100000), "parent", "me")); //Extrapolation not valid with one value EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000))); try { mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(300000), "parent", "me")); //NO Extration when Interpolating //inverse list EXPECT_TRUE(mTR.canTransform("parent", "me", ros::Time().fromNSec(200000))); try { mTR.transformPose("parent",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "me"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_FALSE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } //forward list EXPECT_TRUE(mTR.canTransform("me", "parent", ros::Time().fromNSec(200000))); try { mTR.transformPose("me",Stamped(btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(200000) , "parent"), outpose); EXPECT_TRUE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_FALSE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } //Extrapolating backwards //inverse list EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(1000))); try { mTR.transformPose("parent",Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "me"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } //forwards list EXPECT_FALSE(mTR.canTransform("me", "parent", ros::Time().fromNSec(1000))); try { mTR.transformPose("me",Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000) , "parent"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } // Test extrapolation inverse and forward linkages FORWARD //inverse list EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000))); try { mTR.transformPose("parent", Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "me"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } //forward list EXPECT_FALSE(mTR.canTransform("parent", "me", ros::Time().fromNSec(350000))); try { mTR.transformPose("me", Stamped (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(350000) , "parent"), outpose); EXPECT_FALSE("ExtrapolationException Not Thrown"); } catch ( tf::ExtrapolationException &ex) { EXPECT_TRUE("Extrapolation Exception Caught"); } catch (tf::TransformException& ex) { printf("%s\n",ex.what()); EXPECT_FALSE("Other Exception Caught"); } } TEST(tf, NoExtrapolationExceptionFromParent) { tf::Transformer mTR(true, ros::Duration().fromNSec(1000000)); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "a")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "b")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent", "b")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent's parent", "parent's parent")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent", "parent")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(10000), "parent's parent's parent", "parent's parent")); Stamped output; try { mTR.transformPoint( "b", Stamped(Point(1,1,1), ros::Time().fromNSec(2000), "a"), output); } catch (ExtrapolationException &ex) { EXPECT_FALSE("Shouldn't have gotten this exception"); } }; TEST(tf, ExtrapolationFromOneValue) { tf::Transformer mTR(true, ros::Duration().fromNSec(1000000)); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent's parent", "parent")); Stamped output; bool excepted = false; //Past time try { mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_TRUE(excepted); excepted = false; //Future one element try { mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(100000), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_TRUE(excepted); //Past multi link excepted = false; try { mTR.transformPoint( "parent's parent", Stamped(Point(1,1,1), ros::Time().fromNSec(1), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_TRUE(excepted); //Future case multi link excepted = false; try { mTR.transformPoint( "parent's parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_TRUE(excepted); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(20000), "parent", "a")); excepted = false; try { mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time().fromNSec(10000), "a"), output); } catch (ExtrapolationException &ex) { excepted = true; } EXPECT_FALSE(excepted); }; TEST(tf, getLatestCommonTime) { tf::Transformer mTR(true); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(1000), "parent", "a")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(2000), "parent's parent", "parent")); //simple case ros::Time t; mTR.getLatestCommonTime("a", "parent's parent", t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(1000)); //no connection EXPECT_EQ(tf::LOOKUP_ERROR, mTR.getLatestCommonTime("a", "not valid", t, NULL)); EXPECT_EQ(t, ros::Time()); //testing with update mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(3000), "parent", "a")); mTR.getLatestCommonTime("a", "parent's parent",t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(2000)); //longer chain mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(3000), "b", "c")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(9000), "c", "d")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(5000), "f", "e")); //shared parent mTR.getLatestCommonTime("a", "b",t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(3000)); //two degrees mTR.getLatestCommonTime("a", "c", t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(3000)); //reversed mTR.getLatestCommonTime("c", "a", t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(3000)); //three degrees mTR.getLatestCommonTime("a", "d", t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(3000)); //reversed mTR.getLatestCommonTime("d", "a", t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(3000)); //disconnected tree mTR.getLatestCommonTime("e", "f", t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(5000)); //reversed order mTR.getLatestCommonTime("f", "e", t, NULL); EXPECT_EQ(t, ros::Time().fromNSec(5000)); mTR.setExtrapolationLimit(ros::Duration().fromNSec(20000)); //check timestamps resulting tf::Stamped output, output2; try { mTR.transformPoint( "parent", Stamped(Point(1,1,1), ros::Time(), "b"), output); mTR.transformPoint( "a", ros::Time(),Stamped(Point(1,1,1), ros::Time(), "b"), "c", output2); } catch (tf::TransformException &ex) { printf("%s\n", ex.what()); EXPECT_FALSE("Shouldn't get this Exception"); } EXPECT_EQ(output.stamp_, ros::Time().fromNSec(4000)); EXPECT_EQ(output2.stamp_, ros::Time().fromNSec(3000)); //zero length lookup zero time ros::Time now1 = ros::Time::now(); ros::Time time_output; mTR.getLatestCommonTime("a", "a", time_output, NULL); EXPECT_LE(now1.toSec(), time_output.toSec()); EXPECT_LE(time_output.toSec(), ros::Time::now().toSec()); } TEST(tf, RepeatedTimes) { Transformer mTR; mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "parent", "b")); tf::StampedTransform output; try{ mTR.lookupTransform("parent", "b" , ros::Time().fromNSec(4000), output); EXPECT_TRUE(!std::isnan(output.getOrigin().x())); EXPECT_TRUE(!std::isnan(output.getOrigin().y())); EXPECT_TRUE(!std::isnan(output.getOrigin().z())); EXPECT_TRUE(!std::isnan(output.getRotation().x())); EXPECT_TRUE(!std::isnan(output.getRotation().y())); EXPECT_TRUE(!std::isnan(output.getRotation().z())); EXPECT_TRUE(!std::isnan(output.getRotation().w())); } catch (...) { EXPECT_FALSE("Excetion improperly thrown"); } } TEST(tf, frameExists) { Transformer mTR; // test with fully qualified name EXPECT_FALSE(mTR.frameExists("/b"));; EXPECT_FALSE(mTR.frameExists("/parent")); EXPECT_FALSE(mTR.frameExists("/other")); EXPECT_FALSE(mTR.frameExists("/frame")); //test with resolveping EXPECT_FALSE(mTR.frameExists("b"));; EXPECT_FALSE(mTR.frameExists("parent")); EXPECT_FALSE(mTR.frameExists("other")); EXPECT_FALSE(mTR.frameExists("frame")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b")); // test with fully qualified name EXPECT_TRUE(mTR.frameExists("/b")); EXPECT_TRUE(mTR.frameExists("/parent")); EXPECT_FALSE(mTR.frameExists("/other")); EXPECT_FALSE(mTR.frameExists("/frame")); //Test with resolveping EXPECT_TRUE(mTR.frameExists("b")); EXPECT_TRUE(mTR.frameExists("parent")); EXPECT_FALSE(mTR.frameExists("other")); EXPECT_FALSE(mTR.frameExists("frame")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/frame", "/other")); // test with fully qualified name EXPECT_TRUE(mTR.frameExists("/b")); EXPECT_TRUE(mTR.frameExists("/parent")); EXPECT_TRUE(mTR.frameExists("/other")); EXPECT_TRUE(mTR.frameExists("/frame")); //Test with resolveping EXPECT_TRUE(mTR.frameExists("b")); EXPECT_TRUE(mTR.frameExists("parent")); EXPECT_TRUE(mTR.frameExists("other")); EXPECT_TRUE(mTR.frameExists("frame")); } TEST(tf, resolve) { //no prefix EXPECT_STREQ("/id", tf::resolve("","id").c_str()); //prefix w/o / EXPECT_STREQ("/asdf/id", tf::resolve("asdf","id").c_str()); //prefix w / EXPECT_STREQ("/asdf/id", tf::resolve("/asdf","id").c_str()); // frame_id w / -> no prefix EXPECT_STREQ("/id", tf::resolve("asdf","/id").c_str()); // frame_id w / -> no prefix EXPECT_STREQ("/id", tf::resolve("/asdf","/id").c_str()); } TEST(tf, canTransform) { Transformer mTR; //confirm zero length list disconnected will return true EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time())); EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", ros::Time::now())); //Create a two link tree between times 10 and 20 for (int i = 10; i < 20; i++) { mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "child")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child")); } // four different timestamps related to tf state ros::Time zero_time = ros::Time().fromSec(0); ros::Time old_time = ros::Time().fromSec(5); ros::Time valid_time = ros::Time().fromSec(15); ros::Time future_time = ros::Time().fromSec(25); //confirm zero length list disconnected will return true EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", zero_time)); EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", old_time)); EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", valid_time)); EXPECT_TRUE(mTR.canTransform("some_frame","some_frame", future_time)); // Basic API Tests //Valid data should pass EXPECT_TRUE(mTR.canTransform("child", "parent", valid_time)); EXPECT_TRUE(mTR.canTransform("child", "other_child", valid_time)); //zero data should pass EXPECT_TRUE(mTR.canTransform("child", "parent", zero_time)); EXPECT_TRUE(mTR.canTransform("child", "other_child", zero_time)); //Old data should fail EXPECT_FALSE(mTR.canTransform("child", "parent", old_time)); EXPECT_FALSE(mTR.canTransform("child", "other_child", old_time)); //Future data should fail EXPECT_FALSE(mTR.canTransform("child", "parent", future_time)); EXPECT_FALSE(mTR.canTransform("child", "other_child", future_time)); //Same Frame should pass for all times EXPECT_TRUE(mTR.canTransform("child", "child", zero_time)); EXPECT_TRUE(mTR.canTransform("child", "child", old_time)); EXPECT_TRUE(mTR.canTransform("child", "child", valid_time)); EXPECT_TRUE(mTR.canTransform("child", "child", future_time)); // Advanced API Tests // Source = Fixed //zero data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "child")); EXPECT_TRUE(mTR.canTransform("child", zero_time, "other_child", valid_time, "child")); //Old data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", old_time, "parent", valid_time, "child")); EXPECT_TRUE(mTR.canTransform("child", old_time, "other_child", valid_time, "child")); //valid data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "child")); EXPECT_TRUE(mTR.canTransform("child", valid_time, "other_child", valid_time, "child")); //future data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", future_time, "parent", valid_time, "child")); EXPECT_TRUE(mTR.canTransform("child", future_time, "other_child", valid_time, "child")); //transforming through fixed into the past EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", old_time, "child")); EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", old_time, "child")); //transforming through fixed into the future EXPECT_FALSE(mTR.canTransform("child", valid_time, "parent", future_time, "child")); EXPECT_FALSE(mTR.canTransform("child", valid_time, "other_child", future_time, "child")); // Target = Fixed //zero data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", zero_time, "parent", valid_time, "parent")); //Old data in fixed frame should pass EXPECT_FALSE(mTR.canTransform("child", old_time, "parent", valid_time, "parent")); //valid data in fixed frame should pass EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent")); //future data in fixed frame should pass EXPECT_FALSE(mTR.canTransform("child", future_time, "parent", valid_time, "parent")); //transforming through fixed into the zero EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", zero_time, "parent")); //transforming through fixed into the past EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", old_time, "parent")); //transforming through fixed into the valid EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", valid_time, "parent")); //transforming through fixed into the future EXPECT_TRUE(mTR.canTransform("child", valid_time, "parent", future_time, "parent")); } TEST(tf, lookupTransform) { Transformer mTR; //Create a two link tree between times 10 and 20 for (int i = 10; i < 20; i++) { mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "child")); mTR.setTransform( StampedTransform (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromSec(i), "parent", "other_child")); } // four different timestamps related to tf state ros::Time zero_time = ros::Time().fromSec(0); ros::Time old_time = ros::Time().fromSec(5); ros::Time valid_time = ros::Time().fromSec(15); ros::Time future_time = ros::Time().fromSec(25); //output tf::StampedTransform output; // Basic API Tests try { //confirm zero length list disconnected will return true mTR.lookupTransform("some_frame","some_frame", zero_time, output); mTR.lookupTransform("some_frame","some_frame", old_time, output); mTR.lookupTransform("some_frame","some_frame", valid_time, output); mTR.lookupTransform("some_frame","some_frame", future_time, output); mTR.lookupTransform("child","child", future_time, output); mTR.lookupTransform("other_child","other_child", future_time, output); //Valid data should pass mTR.lookupTransform("child", "parent", valid_time, output); mTR.lookupTransform("child", "other_child", valid_time, output); //zero data should pass mTR.lookupTransform("child", "parent", zero_time, output); mTR.lookupTransform("child", "other_child", zero_time, output); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception thrown"); } try { //Old data should fail mTR.lookupTransform("child", "parent", old_time, output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //Future data should fail mTR.lookupTransform("child", "parent", future_time, output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //Same Frame should pass for all times mTR.lookupTransform("child", "child", zero_time, output); mTR.lookupTransform("child", "child", old_time, output); mTR.lookupTransform("child", "child", valid_time, output); mTR.lookupTransform("child", "child", future_time, output); // Advanced API Tests // Source = Fixed //zero data in fixed frame should pass mTR.lookupTransform("child", zero_time, "parent", valid_time, "child", output); mTR.lookupTransform("child", zero_time, "other_child", valid_time, "child", output); //Old data in fixed frame should pass mTR.lookupTransform("child", old_time, "parent", valid_time, "child", output); mTR.lookupTransform("child", old_time, "other_child", valid_time, "child", output); //valid data in fixed frame should pass mTR.lookupTransform("child", valid_time, "parent", valid_time, "child", output); mTR.lookupTransform("child", valid_time, "other_child", valid_time, "child", output); //future data in fixed frame should pass mTR.lookupTransform("child", future_time, "parent", valid_time, "child", output); mTR.lookupTransform("child", future_time, "other_child", valid_time, "child", output); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception incorrectly thrown"); } try { //transforming through fixed into the past mTR.lookupTransform("child", valid_time, "parent", old_time, "child", output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //transforming through fixed into the future mTR.lookupTransform("child", valid_time, "parent", future_time, "child", output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { // Target = Fixed //zero data in fixed frame should pass mTR.lookupTransform("child", zero_time, "parent", valid_time, "parent", output); //valid data in fixed frame should pass mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception incorrectly thrown"); } try { //Old data in fixed frame should pass mTR.lookupTransform("child", old_time, "parent", valid_time, "parent", output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //future data in fixed frame should pass mTR.lookupTransform("child", future_time, "parent", valid_time, "parent", output); EXPECT_FALSE("Exception should have been thrown"); } catch (tf::TransformException) { EXPECT_TRUE("Exception Thrown Correctly"); } try { //transforming through fixed into the zero mTR.lookupTransform("child", valid_time, "parent", zero_time, "parent", output); //transforming through fixed into the past mTR.lookupTransform("child", valid_time, "parent", old_time, "parent", output); //transforming through fixed into the valid mTR.lookupTransform("child", valid_time, "parent", valid_time, "parent", output); //transforming through fixed into the future mTR.lookupTransform("child", valid_time, "parent", future_time, "parent", output); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception improperly thrown"); } //make sure zero goes to now for zero length try { ros::Time now1 = ros::Time::now(); mTR.lookupTransform("a", "a", ros::Time(),output); EXPECT_LE(now1.toSec(), output.stamp_.toSec()); EXPECT_LE(output.stamp_.toSec(), ros::Time::now().toSec()); } catch (tf::TransformException &ex) { printf("Exception improperly thrown: %s", ex.what()); EXPECT_FALSE("Exception improperly thrown"); } } TEST(tf, getFrameStrings) { Transformer mTR; mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/parent", "/b")); std::vector frames_string; mTR.getFrameStrings(frames_string); ASSERT_EQ(frames_string.size(), (unsigned)2); EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str()); EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str()); mTR.setTransform( StampedTransform (btTransform(btQuaternion(0,0,0,1), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/frame", "/other")); mTR.getFrameStrings(frames_string); ASSERT_EQ(frames_string.size(), (unsigned)4); EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str()); EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str()); EXPECT_STREQ(frames_string[2].c_str(), std::string("/other").c_str()); EXPECT_STREQ(frames_string[3].c_str(), std::string("/frame").c_str()); } bool expectInvalidQuaternion(tf::Quaternion q) { try { tf::assertQuaternionValid(q); printf("this should have thrown\n"); return false; } catch (tf::InvalidArgument &ex) { return true; } catch (...) { printf("A different type of exception was expected\n"); return false; } return false; } bool expectValidQuaternion(tf::Quaternion q) { try { tf::assertQuaternionValid(q); } catch (tf::TransformException &ex) { return false; } return true; } bool expectInvalidQuaternion(geometry_msgs::Quaternion q) { try { tf::assertQuaternionValid(q); printf("this should have thrown\n"); return false; } catch (tf::InvalidArgument &ex) { return true; } catch (...) { printf("A different type of exception was expected\n"); return false; } return false; } bool expectValidQuaternion(geometry_msgs::Quaternion q) { try { tf::assertQuaternionValid(q); } catch (tf::TransformException &ex) { return false; } return true; } TEST(tf, assertQuaternionValid) { tf::Quaternion q(1,0,0,0); EXPECT_TRUE(expectValidQuaternion(q)); q.setX(0); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setY(1); EXPECT_TRUE(expectValidQuaternion(q)); q.setZ(1); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setY(0); EXPECT_TRUE(expectValidQuaternion(q)); q.setW(1); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setZ(0); EXPECT_TRUE(expectValidQuaternion(q)); q.setZ(sqrt(2.0)/2.0); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setW(sqrt(2.0)/2.0); EXPECT_TRUE(expectValidQuaternion(q)); q.setZ(sqrt(2.0)/2.0 + 0.01); EXPECT_TRUE(expectInvalidQuaternion(q)); q.setZ(sqrt(2.0)/2.0 - 0.01); EXPECT_TRUE(expectInvalidQuaternion(q)); EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument); // Waiting for gtest 1.1 or later // EXPECT_NO_THROW(tf::assertQuaternionValid(q)); //q.setX(0); //EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument); //q.setY(1); //EXPECT_NO_THROW(tf::assertQuaternionValid(q)); } TEST(tf, assertQuaternionMsgValid) { geometry_msgs::Quaternion q; q.x = 1;//others zeroed to start EXPECT_TRUE(expectValidQuaternion(q)); q.x = 0; EXPECT_TRUE(expectInvalidQuaternion(q)); q.y = 1; EXPECT_TRUE(expectValidQuaternion(q)); q.z = 1; EXPECT_TRUE(expectInvalidQuaternion(q)); q.y = 0; EXPECT_TRUE(expectValidQuaternion(q)); q.w = 1; EXPECT_TRUE(expectInvalidQuaternion(q)); q.z = 0; EXPECT_TRUE(expectValidQuaternion(q)); q.z = sqrt(2.0)/2.0; EXPECT_TRUE(expectInvalidQuaternion(q)); q.w = sqrt(2.0)/2.0; EXPECT_TRUE(expectValidQuaternion(q)); q.z = sqrt(2.0)/2.0 + 0.01; EXPECT_TRUE(expectInvalidQuaternion(q)); q.z = sqrt(2.0)/2.0 - 0.01; EXPECT_TRUE(expectInvalidQuaternion(q)); // Waiting for gtest 1.1 or later // EXPECT_NO_THROW(tf::assertQuaternionValid(q)); //q.x = 0); //EXPECT_THROW(tf::assertQuaternionValid(q), tf::InvalidArgument); //q.y = 1); //EXPECT_NO_THROW(tf::assertQuaternionValid(q)); } TEST(tf2_stamped, OperatorEqualEqual) { btTransform transform0, transform1, transform0a; transform0.setIdentity(); transform0a.setIdentity(); transform1.setIdentity(); transform1.setOrigin(btVector3(1, 0, 0)); tf2::StampedTransform stamped_transform_reference(transform0a, ros::Time(), "frame_id", "child_frame_id"); tf2::StampedTransform stamped_transform0A(transform0, ros::Time(), "frame_id", "child_frame_id"); EXPECT_TRUE(stamped_transform0A == stamped_transform_reference); // Equal tf2::StampedTransform stamped_transform0B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id"); EXPECT_FALSE(stamped_transform0B == stamped_transform_reference); // Different Frame id tf2::StampedTransform stamped_transform0C(transform0, ros::Time(1.0), "frame_id", "child_frame_id"); EXPECT_FALSE(stamped_transform0C == stamped_transform_reference); // Different Time tf2::StampedTransform stamped_transform0D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id"); EXPECT_FALSE(stamped_transform0D == stamped_transform_reference); // Different frame id and time tf2::StampedTransform stamped_transform0E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id"); EXPECT_FALSE(stamped_transform0E == stamped_transform_reference); // Different transform, frame id tf2::StampedTransform stamped_transform0F(transform1, ros::Time(1.0), "frame_id", "child_frame_id"); EXPECT_FALSE(stamped_transform0F == stamped_transform_reference); // Different transform, time tf2::StampedTransform stamped_transform0G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id"); EXPECT_FALSE(stamped_transform0G == stamped_transform_reference); // Different transform, frame id and time tf2::StampedTransform stamped_transform0H(transform1, ros::Time(), "frame_id", "child_frame_id"); EXPECT_FALSE(stamped_transform0H == stamped_transform_reference); // Different transform //Different child_frame_id tf2::StampedTransform stamped_transform1A(transform0, ros::Time(), "frame_id", "child_frame_id2"); EXPECT_FALSE(stamped_transform1A == stamped_transform_reference); // Equal tf2::StampedTransform stamped_transform1B(transform0, ros::Time(), "frame_id_not_equal", "child_frame_id2"); EXPECT_FALSE(stamped_transform1B == stamped_transform_reference); // Different Frame id tf2::StampedTransform stamped_transform1C(transform0, ros::Time(1.0), "frame_id", "child_frame_id2"); EXPECT_FALSE(stamped_transform1C == stamped_transform_reference); // Different Time tf2::StampedTransform stamped_transform1D(transform0, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2"); EXPECT_FALSE(stamped_transform1D == stamped_transform_reference); // Different frame id and time tf2::StampedTransform stamped_transform1E(transform1, ros::Time(), "frame_id_not_equal", "child_frame_id2"); EXPECT_FALSE(stamped_transform1E == stamped_transform_reference); // Different transform, frame id tf2::StampedTransform stamped_transform1F(transform1, ros::Time(1.0), "frame_id", "child_frame_id2"); EXPECT_FALSE(stamped_transform1F == stamped_transform_reference); // Different transform, time tf2::StampedTransform stamped_transform1G(transform1, ros::Time(1.0), "frame_id_not_equal", "child_frame_id2"); EXPECT_FALSE(stamped_transform1G == stamped_transform_reference); // Different transform, frame id and time tf2::StampedTransform stamped_transform1H(transform1, ros::Time(), "frame_id", "child_frame_id2"); EXPECT_FALSE(stamped_transform1H == stamped_transform_reference); // Different transform } TEST(tf2_stamped, OperatorEqual) { btTransform pose0, pose1, pose0a; pose0.setIdentity(); pose1.setIdentity(); pose1.setOrigin(btVector3(1, 0, 0)); tf2::Stamped stamped_pose0(pose0, ros::Time(), "frame_id"); tf2::Stamped stamped_pose1(pose1, ros::Time(1.0), "frame_id_not_equal"); EXPECT_FALSE(stamped_pose1 == stamped_pose0); stamped_pose1 = stamped_pose0; EXPECT_TRUE(stamped_pose1 == stamped_pose0); } */ int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::Time::init(); //needed for ros::TIme::now() ros::init(argc, argv, "tf_unittest"); return RUN_ALL_TESTS(); } geometry2-0.7.5/test_tf2/test/static_publisher.launch000066400000000000000000000030361372352374300227240ustar00rootroot00000000000000 geometry2-0.7.5/test_tf2/test/test_buffer_client.cpp000066400000000000000000000073571372352374300225500ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #include #include #include #include #include #include static const double EPS = 1e-3; TEST(tf2_ros, buffer_client) { tf2_ros::BufferClient client("tf_action"); //make sure that things are set up ASSERT_TRUE(client.waitForServer(ros::Duration(4.0))); geometry_msgs::PointStamped p1; p1.header.frame_id = "a"; p1.header.stamp = ros::Time(); p1.point.x = 0.0; p1.point.y = 0.0; p1.point.z = 0.0; try { geometry_msgs::PointStamped p2 = client.transform(p1, "b"); ROS_INFO("p1: (%.2f, %.2f, %.2f), p2: (%.2f, %.2f, %.2f)", p1.point.x, p1.point.y, p1.point.z, p2.point.x, p2.point.y, p2.point.z); EXPECT_NEAR(p2.point.x, -5.0, EPS); EXPECT_NEAR(p2.point.y, -6.0, EPS); EXPECT_NEAR(p2.point.z, -7.0, EPS); } catch(tf2::TransformException& ex) { ROS_ERROR("Failed to transform: %s", ex.what()); ASSERT_FALSE("Should not get here"); } } TEST(tf2_ros, buffer_client_different_types) { tf2_ros::BufferClient client("tf_action"); //make sure that things are set up ASSERT_TRUE(client.waitForServer(ros::Duration(4.0))); tf2::Stamped k1(KDL::Vector(0, 0, 0), ros::Time(), "a"); try { tf2::Stamped b1; client.transform(k1, b1, "b"); ROS_INFO_STREAM("Bullet: (" << b1[0] << ", " << b1[1] << ", " << b1[2] << ")"); ROS_INFO_STREAM("KDL: (" << k1[0] << ", " << k1[1] << ", " << k1[2] << ")"); EXPECT_NEAR(b1[0], -5.0, EPS); EXPECT_NEAR(b1[1], -6.0, EPS); EXPECT_NEAR(b1[2], -7.0, EPS); EXPECT_EQ(b1.frame_id_, "b"); EXPECT_EQ(k1.frame_id_, "a"); } catch(tf2::TransformException& ex) { ROS_ERROR("Failed to transform: %s", ex.what()); ASSERT_FALSE("Should not get here"); } } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "buffer_client_test"); return RUN_ALL_TESTS(); } geometry2-0.7.5/test_tf2/test/test_buffer_client.py000077500000000000000000000062541372352374300224140ustar00rootroot00000000000000#! /usr/bin/env python #*********************************************************** #* Software License Agreement (BSD License) #* #* Copyright (c) 2009, Willow Garage, Inc. #* All rights reserved. #* #* Redistribution and use in source and binary forms, with or without #* modification, are permitted provided that the following conditions #* are met: #* #* * Redistributions of source code must retain the above copyright #* notice, this list of conditions and the following disclaimer. #* * Redistributions in binary form must reproduce the above #* copyright notice, this list of conditions and the following #* disclaimer in the documentation and/or other materials provided #* with the distribution. #* * Neither the name of Willow Garage, Inc. nor the names of its #* contributors may be used to endorse or promote products derived #* from this software without specific prior written permission. #* #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #* #* Author: Eitan Marder-Eppstein #*********************************************************** PKG = 'test_tf2' import sys import unittest import tf2_py as tf2 import tf2_ros import tf2_kdl import tf2_geometry_msgs from geometry_msgs.msg import PointStamped import rospy import PyKDL class TestBufferClient(unittest.TestCase): def test_buffer_client(self): client = tf2_ros.BufferClient("tf_action") client.wait_for_server() p1 = PointStamped() p1.header.frame_id = "a" p1.header.stamp = rospy.Time(0.0) p1.point.x = 0.0 p1.point.y = 0.0 p1.point.z = 0.0 try: p2 = client.transform(p1, "b") rospy.loginfo("p1: %s, p2: %s" % (p1, p2)) except tf2.TransformException as e: rospy.logerr("%s" % e) def test_transform_type(self): client = tf2_ros.BufferClient("tf_action") client.wait_for_server() p1 = PointStamped() p1.header.frame_id = "a" p1.header.stamp = rospy.Time(0.0) p1.point.x = 0.0 p1.point.y = 0.0 p1.point.z = 0.0 try: p2 = client.transform(p1, "b", new_type = PyKDL.Vector) rospy.loginfo("p1: %s, p2: %s" % (str(p1), str(p2))) except tf2.TransformException as e: rospy.logerr("%s" % e) if __name__ == '__main__': rospy.init_node("test_buffer_client") import rostest rostest.rosrun(PKG, 'test_buffer_client', TestBufferClient) geometry2-0.7.5/test_tf2/test/test_buffer_server.cpp000066400000000000000000000042031372352374300225630ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #include #include #include #include int main(int argc, char** argv) { ros::init(argc, argv, "buffer_server_test"); tf2_ros::Buffer buffer; tf2_ros::TransformListener tfl(buffer); tf2_ros::BufferServer server(buffer, "tf_action", false); server.start(); ros::spin(); } geometry2-0.7.5/test_tf2/test/test_convert.cpp000066400000000000000000000075671372352374300214240ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #include #include #include #include #include #include #include TEST(tf2Convert, kdlToBullet) { double epsilon = 1e-9; tf2::Stamped b(btVector3(1,2,3), ros::Time(), "my_frame"); tf2::Stamped b1 = b; tf2::Stamped k1; tf2::convert(b1, k1); tf2::Stamped b2; tf2::convert(k1, b2); EXPECT_EQ(b.frame_id_, b2.frame_id_); EXPECT_NEAR(b.stamp_.toSec(), b2.stamp_.toSec(), epsilon); EXPECT_NEAR(b.x(), b2.x(), epsilon); EXPECT_NEAR(b.y(), b2.y(), epsilon); EXPECT_NEAR(b.z(), b2.z(), epsilon); EXPECT_EQ(b1.frame_id_, b2.frame_id_); EXPECT_NEAR(b1.stamp_.toSec(), b2.stamp_.toSec(), epsilon); EXPECT_NEAR(b1.x(), b2.x(), epsilon); EXPECT_NEAR(b1.y(), b2.y(), epsilon); EXPECT_NEAR(b1.z(), b2.z(), epsilon); } TEST(tf2Convert, kdlBulletROSConversions) { double epsilon = 1e-9; tf2::Stamped b1(btVector3(1,2,3), ros::Time(), "my_frame"), b2, b3, b4; geometry_msgs::PointStamped r1, r2, r3; tf2::Stamped k1, k2, k3; // Do bullet -> self -> bullet -> KDL -> self -> KDL -> ROS -> self -> ROS -> KDL -> bullet -> ROS -> bullet tf2::convert(b1, b1); tf2::convert(b1, b2); tf2::convert(b2, k1); tf2::convert(k1, k1); tf2::convert(k1, k2); tf2::convert(k2, r1); tf2::convert(r1, r1); tf2::convert(r1, r2); tf2::convert(r2, k3); tf2::convert(k3, b3); tf2::convert(b3, r3); tf2::convert(r3, b4); EXPECT_EQ(b1.frame_id_, b4.frame_id_); EXPECT_NEAR(b1.stamp_.toSec(), b4.stamp_.toSec(), epsilon); EXPECT_NEAR(b1.x(), b4.x(), epsilon); EXPECT_NEAR(b1.y(), b4.y(), epsilon); EXPECT_NEAR(b1.z(), b4.z(), epsilon); } TEST(tf2Convert, ConvertTf2Quaternion) { tf2::Quaternion tq(1,2,3,4); Eigen::Quaterniond eq; tf2::convert(tq, eq); EXPECT_EQ(tq.w(), eq.w()); EXPECT_EQ(tq.x(), eq.x()); EXPECT_EQ(tq.y(), eq.y()); EXPECT_EQ(tq.z(), eq.z()); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry2-0.7.5/test_tf2/test/test_convert.py000077500000000000000000000047731372352374300212710ustar00rootroot00000000000000#! /usr/bin/env python #*********************************************************** #* Software License Agreement (BSD License) #* #* Copyright (c) 2009, Willow Garage, Inc. #* All rights reserved. #* #* Redistribution and use in source and binary forms, with or without #* modification, are permitted provided that the following conditions #* are met: #* #* * Redistributions of source code must retain the above copyright #* notice, this list of conditions and the following disclaimer. #* * Redistributions in binary form must reproduce the above #* copyright notice, this list of conditions and the following #* disclaimer in the documentation and/or other materials provided #* with the distribution. #* * Neither the name of Willow Garage, Inc. nor the names of its #* contributors may be used to endorse or promote products derived #* from this software without specific prior written permission. #* #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #* #* Author: Eitan Marder-Eppstein #*********************************************************** from __future__ import print_function PKG = 'test_tf2' import sys import unittest import tf2_py as tf2 import tf2_ros import tf2_geometry_msgs from geometry_msgs.msg import PointStamped import rospy import tf2_kdl import PyKDL class TestConvert(unittest.TestCase): def test_convert(self): p = tf2_ros.Stamped(PyKDL.Vector(1, 2, 3), rospy.Time(), 'my_frame') print(p) msg = tf2_ros.convert(p, PointStamped) print(msg) p2 = tf2_ros.convert(msg, PyKDL.Vector) print(p2) p2[0] = 100 print(p) print(p2) print(p2.header) if __name__ == '__main__': import rostest rostest.unitrun(PKG, 'test_buffer_client', TestConvert) geometry2-0.7.5/test_tf2/test/test_message_filter.cpp000066400000000000000000000242451372352374300227250ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Josh Faust */ #include #include #include #include #include #include "ros/ros.h" #include "ros/callback_queue.h" #include using namespace tf2; using namespace tf2_ros; class Notification { public: Notification(int expected_count) : count_(0), expected_count_(expected_count), failure_count_(0) { } void notify(const geometry_msgs::PointStamped::ConstPtr& message) { ++count_; } void failure(const geometry_msgs::PointStamped::ConstPtr& message, FilterFailureReason reason) { ++failure_count_; } int count_; int expected_count_; int failure_count_; }; TEST(MessageFilter, noTransforms) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = ros::Time(1); msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); } TEST(MessageFilter, noTransformsSameFrame) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = ros::Time(1); msg->header.frame_id = "frame1"; filter.add(msg); EXPECT_EQ(1, n.count_); } geometry_msgs::TransformStamped createTransform(Quaternion q, Vector3 v, ros::Time stamp, const std::string& frame1, const std::string& frame2) { geometry_msgs::TransformStamped t; t.header.frame_id = frame1; t.child_frame_id = frame2; t.header.stamp = stamp; t.transform.translation.x = v.x(); t.transform.translation.y = v.y(); t.transform.translation.z = v.z(); t.transform.rotation.x = q.x(); t.transform.rotation.y = q.y(); t.transform.rotation.z = q.z(); t.transform.rotation.w = q.w(); return t; } TEST(MessageFilter, preexistingTransforms) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); ros::Time stamp(1); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, postTransforms) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); ros::Time stamp(1); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, queueSize) { BufferCore bc; Notification n(10); MessageFilter filter(bc, "frame1", 10, 0); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp(1); for (int i = 0; i < 20; ++i) { geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); } EXPECT_EQ(0, n.count_); EXPECT_EQ(10, n.failure_count_); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(10, n.count_); } TEST(MessageFilter, setTargetFrame) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.setTargetFrame("frame1000"); ros::Time stamp(1); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1000", "frame2"), "me"); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.count_); } TEST(MessageFilter, multipleTargetFrames) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "", 1, 0); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); std::vector target_frames; target_frames.push_back("frame1"); target_frames.push_back("frame2"); filter.setTargetFrames(target_frames); ros::Time stamp(1); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame3"), "me"); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame3"; filter.add(msg); EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet) //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0)); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); // frame2->frame3 now exists } TEST(MessageFilter, tolerance) { ros::Duration offset(0.2); BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); filter.setTolerance(offset); ros::Time stamp(1); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); //No return due to lack of space for offset bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + (offset * 1.1), "frame1", "frame2"), "me"); EXPECT_EQ(1, n.count_); // Now have data for the message published earlier msg->header.stamp = stamp + offset; filter.add(msg); EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset } TEST(MessageFilter, outTheBackFailure) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp(1); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me"); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + ros::Duration(10000), "frame1", "frame2"), "me"); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(1, n.failure_count_); } TEST(MessageFilter, outTheBackFailure2) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); ros::Time stamp(1); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = stamp; msg->header.frame_id = "frame2"; filter.add(msg); EXPECT_EQ(0, n.count_); EXPECT_EQ(0, n.failure_count_); bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + ros::Duration(10000), "frame1", "frame2"), "me"); EXPECT_EQ(1, n.failure_count_); } TEST(MessageFilter, emptyFrameIDFailure) { BufferCore bc; Notification n(1); MessageFilter filter(bc, "frame1", 1, 0); filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2)); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.frame_id = ""; filter.add(msg); EXPECT_EQ(1, n.failure_count_); } TEST(MessageFilter, callbackQueue) { BufferCore bc; Notification n(1); ros::CallbackQueue queue; MessageFilter filter(bc, "frame1", 1, &queue); filter.registerCallback(boost::bind(&Notification::notify, &n, _1)); geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped); msg->header.stamp = ros::Time(1); msg->header.frame_id = "frame1"; filter.add(msg); EXPECT_EQ(0, n.count_); queue.callAvailable(); EXPECT_EQ(1, n.count_); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); int ret = RUN_ALL_TESTS(); return ret; } geometry2-0.7.5/test_tf2/test/test_static_publisher.cpp000066400000000000000000000124501372352374300232730ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "tf2/exceptions.h" #include #include #include "rostest/permuter.h" #include "tf2_ros/transform_listener.h" TEST(StaticTransformPublisher, a_b_different_times) { tf2_ros::Buffer mB; tf2_ros::TransformListener tfl(mB); EXPECT_TRUE(mB.canTransform("a", "b", ros::Time(), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("a", "b", ros::Time(100), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("a", "b", ros::Time(1000), ros::Duration(1.0))); }; TEST(StaticTransformPublisher, a_c_different_times) { tf2_ros::Buffer mB; tf2_ros::TransformListener tfl(mB); EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(100), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(1000), ros::Duration(1.0))); }; TEST(StaticTransformPublisher, a_d_different_times) { tf2_ros::Buffer mB; tf2_ros::TransformListener tfl(mB); geometry_msgs::TransformStamped ts; ts.transform.rotation.w = 1; ts.header.frame_id = "c"; ts.header.stamp = ros::Time(10.0); ts.child_frame_id = "d"; // make sure listener has populated EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(100), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("a", "c", ros::Time(1000), ros::Duration(1.0))); mB.setTransform(ts, "authority"); //printf("%s\n", mB.allFramesAsString().c_str()); EXPECT_TRUE(mB.canTransform("c", "d", ros::Time(10), ros::Duration(0))); EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(), ros::Duration(0))); EXPECT_FALSE(mB.canTransform("a", "d", ros::Time(1), ros::Duration(0))); EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(10), ros::Duration(0))); EXPECT_FALSE(mB.canTransform("a", "d", ros::Time(100), ros::Duration(0))); }; TEST(StaticTransformPublisher, multiple_parent_test) { tf2_ros::Buffer mB; tf2_ros::TransformListener tfl(mB); tf2_ros::StaticTransformBroadcaster stb; geometry_msgs::TransformStamped ts; ts.transform.rotation.w = 1; ts.header.frame_id = "c"; ts.header.stamp = ros::Time(10.0); ts.child_frame_id = "d"; stb.sendTransform(ts); // make sure listener has populated EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(100), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("a", "d", ros::Time(1000), ros::Duration(1.0))); // Publish new transform with child 'd', should replace old one in static tf ts.header.frame_id = "new_parent"; stb.sendTransform(ts); ts.child_frame_id = "other_child"; stb.sendTransform(ts); ts.child_frame_id = "other_child2"; stb.sendTransform(ts); EXPECT_TRUE(mB.canTransform("new_parent", "d", ros::Time(), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("new_parent", "other_child", ros::Time(), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("new_parent", "other_child2", ros::Time(), ros::Duration(1.0))); EXPECT_FALSE(mB.canTransform("a", "d", ros::Time(), ros::Duration(1.0))); }; TEST(StaticTransformPublisher, tf_from_param_server_valid) { // This TF is loaded from the parameter server; ensure it is valid. tf2_ros::Buffer mB; tf2_ros::TransformListener tfl(mB); EXPECT_TRUE(mB.canTransform("robot_calibration", "world", ros::Time(), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("robot_calibration", "world", ros::Time(100), ros::Duration(1.0))); EXPECT_TRUE(mB.canTransform("robot_calibration", "world", ros::Time(1000), ros::Duration(1.0))); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "tf_unittest"); return RUN_ALL_TESTS(); } geometry2-0.7.5/test_tf2/test/test_static_publisher.py000077500000000000000000000101171372352374300231420ustar00rootroot00000000000000#! /usr/bin/env python #*********************************************************** #* Software License Agreement (BSD License) #* #* Copyright (c) 2016, Felix Duvallet #* All rights reserved. #* #* Redistribution and use in source and binary forms, with or without #* modification, are permitted provided that the following conditions #* are met: #* #* * Redistributions of source code must retain the above copyright #* notice, this list of conditions and the following disclaimer. #* * Redistributions in binary form must reproduce the above #* copyright notice, this list of conditions and the following #* disclaimer in the documentation and/or other materials provided #* with the distribution. #* * Neither the name of Willow Garage, Inc. nor the names of its #* contributors may be used to endorse or promote products derived #* from this software without specific prior written permission. #* #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #* #* Author: Felix Duvallet #*********************************************************** import subprocess import unittest import rospy PKG = 'test_tf2' class TestStaticPublisher(unittest.TestCase): """ These tests ensure the static transform publisher dies gracefully when provided with an invalid (or non-existent) transform parameter. These tests are started by the static_publisher.launch, which loads parameters into the param server. We check the output to make sure the correct error is occurring, since the return code is always -1 (255). Note that this *could* cause a problem if a valid TF is stored in the param server for one of the names; in this case the subprocess would never return and the test would run forever. """ def test_publisher_no_args(self): # Start the publisher with no argument. cmd = 'rosrun tf2_ros static_transform_publisher' with self.assertRaises(subprocess.CalledProcessError) as cm: ret = subprocess.check_output( cmd.split(' '), stderr=subprocess.STDOUT, text=True) self.assertEqual(255, cm.exception.returncode) self.assertIn('not having the right number of arguments', cm.exception.output) def test_publisher_nonexistent_param(self): # Here there is no paramater by that name. cmd = 'rosrun tf2_ros static_transform_publisher /test_tf2/tf_null' with self.assertRaises(subprocess.CalledProcessError) as cm: ret = subprocess.check_output( cmd.split(' '), stderr=subprocess.STDOUT, text=True) self.assertEqual(255, cm.exception.returncode) self.assertIn('Could not read TF', cm.exception.output) def test_publisher_invalid_param(self): # Here there is an invalid parameter stored in the parameter server. cmd = 'rosrun tf2_ros static_transform_publisher /test_tf2/tf_invalid' with self.assertRaises(subprocess.CalledProcessError) as cm: ret = subprocess.check_output( cmd.split(' '), stderr=subprocess.STDOUT, text=True) self.assertEqual(255, cm.exception.returncode) self.assertIn('Could not validate XmlRpcC', cm.exception.output) if __name__ == '__main__': rospy.init_node("test_static_publisher_py") import rostest rostest.rosrun(PKG, 'test_static_publisher_py', TestStaticPublisher) geometry2-0.7.5/test_tf2/test/test_tf2_bullet.cpp000066400000000000000000000070751372352374300220000ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #include #include #include #include #include tf2_ros::Buffer* tf_buffer; static const double EPS = 1e-3; TEST(TfBullet, Transform) { tf2::Stamped v1(btTransform(btQuaternion(1,0,0,0), btVector3(1,2,3)), ros::Time(2.0), "A"); // simple api btTransform v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.getOrigin().getX(), -9, EPS); EXPECT_NEAR(v_simple.getOrigin().getY(), 18, EPS); EXPECT_NEAR(v_simple.getOrigin().getZ(), 27, EPS); // advanced api btTransform v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "B", ros::Duration(3.0)); EXPECT_NEAR(v_advanced.getOrigin().getX(), -9, EPS); EXPECT_NEAR(v_advanced.getOrigin().getY(), 18, EPS); EXPECT_NEAR(v_advanced.getOrigin().getZ(), 27, EPS); } TEST(TfBullet, Vector) { tf2::Stamped v1(btVector3(1,2,3), ros::Time(2.0), "A"); // simple api btVector3 v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.getX(), -9, EPS); EXPECT_NEAR(v_simple.getY(), 18, EPS); EXPECT_NEAR(v_simple.getZ(), 27, EPS); // advanced api btVector3 v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "B", ros::Duration(3.0)); EXPECT_NEAR(v_advanced.getX(), -9, EPS); EXPECT_NEAR(v_advanced.getY(), 18, EPS); EXPECT_NEAR(v_advanced.getZ(), 27, EPS); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test"); ros::NodeHandle n; tf_buffer = new tf2_ros::Buffer(); // populate buffer geometry_msgs::TransformStamped t; t.transform.translation.x = 10; t.transform.translation.y = 20; t.transform.translation.z = 30; t.transform.rotation.x = 1; t.header.stamp = ros::Time(2.0); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); int ret = RUN_ALL_TESTS(); delete tf_buffer; return ret; } geometry2-0.7.5/test_tf2/test/test_tf2_bullet.launch000066400000000000000000000001611372352374300224550ustar00rootroot00000000000000 geometry2-0.7.5/test_tf2/test/test_tf_invalid.yaml000066400000000000000000000001321372352374300222200ustar00rootroot00000000000000# This is not a valid TF. child_frame_id: calibration some_data: - 1 - 2 - 3 geometry2-0.7.5/test_tf2/test/test_tf_valid.yaml000066400000000000000000000004211372352374300216720ustar00rootroot00000000000000header: seq: 0 stamp: secs: 1619 nsecs: 601000000 frame_id: world child_frame_id: robot_calibration transform: translation: x: 0.75 y: 0.5 z: 1.0 rotation: x: -0.62908825919 y: 0.210952809338 z: 0.640171445021 w: 0.38720459109 geometry2-0.7.5/test_tf2/test/test_utils.cpp000066400000000000000000000047161372352374300210750ustar00rootroot00000000000000// Copyright 2014 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include #include #include #include #include double epsilon = 1e-9; template void yprTest(const T& t, double yaw1, double pitch1, double roll1) { double yaw2, pitch2, roll2; tf2::getEulerYPR(t, yaw2, pitch2, roll2); EXPECT_NEAR(yaw1, yaw2, epsilon); EXPECT_NEAR(pitch1, pitch2, epsilon); EXPECT_NEAR(roll1, roll2, epsilon); EXPECT_NEAR(tf2::getYaw(t), yaw1, epsilon); } TEST(tf2Utils, yaw) { double x, y, z, w; x = 0.4; y = 0.5; z = 0.6; w = 0.7; double yaw1, pitch1, roll1; // Compute results one way with KDL KDL::Rotation::Quaternion(x, y, z, w).GetRPY(roll1, pitch1, yaw1); { // geometry_msgs::Quaternion geometry_msgs::Quaternion q; q.x = x; q.y =y; q.z = z; q.w = w; yprTest(q, yaw1, pitch1, roll1); // geometry_msgs::QuaternionStamped geometry_msgs::QuaternionStamped qst; qst.quaternion = q; yprTest(qst, yaw1, pitch1, roll1); } { // tf2::Quaternion tf2::Quaternion q(x, y, z, w); yprTest(q, yaw1, pitch1, roll1); // tf2::Stamped tf2::Stamped sq; sq.setData(q); yprTest(sq, yaw1, pitch1, roll1); } } TEST(tf2Utils, identity) { geometry_msgs::Transform t; t.translation.x = 0.1; t.translation.y = 0.2; t.translation.z = 0.3; t.rotation.x = 0.4; t.rotation.y = 0.5; t.rotation.z = 0.6; t.rotation.w = 0.7; // Test identity t = tf2::getTransformIdentity(); EXPECT_EQ(t.translation.x, 0); EXPECT_EQ(t.translation.y, 0); EXPECT_EQ(t.translation.z, 0); EXPECT_EQ(t.rotation.x, 0); EXPECT_EQ(t.rotation.y, 0); EXPECT_EQ(t.rotation.z, 0); EXPECT_EQ(t.rotation.w, 1); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2/000077500000000000000000000000001372352374300141445ustar00rootroot00000000000000geometry2-0.7.5/tf2/CHANGELOG.rst000066400000000000000000000552201372352374300161710ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2 ^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ * restore buffer sizes * Contributors: Tully Foote 0.7.4 (2020-09-01) ------------------ * Fix potential buffer overrun of snprintf (`#479 `_) * Contributors: Atsushi Watanabe 0.7.3 (2020-08-25) ------------------ * Use snprintf instead of stringstream to increase performance of lookupTransform() in error cases. * Do not waste time constructing error string if nobody is interested in it in canTransform(). (`#469 `_) * Output time difference of extrapolation exceptions (`#477 `_) * Cherry-picking various commits from Melodic (`#471 `_) * Revert "rework Eigen functions namespace hack" (`#436 `_) * Fixed warnings in message_filter.h (`#434 `_) the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall * Fix ambiguous call for tf2::convert on MSVC (`#444 `_) * rework ambiguous call on MSVC. * Contributors: Lucas Walter, Martin Pecka, Robert Haschke 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ * Fix to improper ring_45 test, where 'anchor' frame for both inverse and normal transform was frame 'b' instead of frame 'a', thus creating a problem * Don't insert a TF frame is one of the same timestamp already exists, instead just overwrite it. * [Noetic] Add tf2::Stamped::operator=() to fix warnings downstream (`#453 `_) * Add tf2::Stamped::operator=() * [noetic] cherry-pick Windows fixes from melodic-devel (`#450 `_) * [Windows][melodic-devel] Fix install locations (`#442 `_) * fixed install locations of tf2 * [windows][melodic] more portable fixes. (`#443 `_) * more portable fixes. * Contributors: Patrick Beeson, Robert Haschke, Sean Yen, Shane Loretz 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) Signed-off-by: Shane Loretz * Fix compile error missing ros/ros.h (`#400 `_) * ros/ros.h -> ros/time.h tf2 package depends on rostime * tf2_bullet doesn't need ros.h Signed-off-by: Shane Loretz * tf2_eigen doesn't need ros/ros.h Signed-off-by: Shane Loretz * Merge pull request `#367 `_ from kejxu/add_tf2_namespace_to_avoid_name_collision rework Eigen functions namespace hack * separate transform function declarations into transform_functions.h * use ROS_DEPRECATED macro for portability (`#362 `_) * use ROS_DEPRECATED for better portability * change ROS_DEPRECATED position (`#5 `_) * Remove `signals` from find_package(Boost COMPONENTS ...). tf2 is using signals2, which is not the same library. Additionally, signals2 has always been header only, and header only libraries must not be listed in find_package. Boost 1.69 removed the old signals library entirely, so the otherwise useless `COMPONENTS signals` actually breaks the build. * Remove legacy inclusion in CMakeLists of tf2. * Contributors: James Xu, Maarten de Vries, Marco Tranzatto, Shane Loretz, Tully Foote 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ * Resolved pedantic warnings * fix issue `#315 `_ * fixed nan interpoaltion issue * Contributors: Keller Fabian Rudolf (CC-AD/EYC3), Kuang Fangjun, Martin Ganeff 0.6.3 (2018-07-09) ------------------ * preserve constness of const argument to avoid warnings (`#307 `_) * Change comment style for unused doxygen (`#297 `_) * Contributors: Jacob Perron, Tully Foote 0.6.2 (2018-05-02) ------------------ 0.6.1 (2018-03-21) ------------------ * Replaced deprecated console_bridge macro calls (tests) * Contributors: Johannes Meyer, Tully Foote 0.6.0 (2018-03-21) ------------------ * Replaced deprecated log macro calls * Contributors: Tim Rakowski, Tully Foote 0.5.17 (2018-01-01) ------------------- * Merge pull request `#278 `_ from ros/chain_as_vec_test2 Clean up results of _chainAsVector * Simple test to check BufferCore::_chainAsVector. Unit tests for walk and chain passing now. * Merge pull request `#267 `_ from at-wat/speedup-timecache-for-large-buffer Speed-up TimeCache search for large cache time. * Merge pull request `#265 `_ from vsherrod/interpolation_fix Corrected time output on interpolation function. * Add time_interval option to tf2 speed-test. * Merge pull request `#269 `_ from ros/frames_as_yaml allFrameAsYaml consistently outputting a dict * resolve https://github.com/ros/geometry/pull/153 at the source instead of needing the workaround. * Speed-up TimeCache search for large cache time. * Modified tests for correct time in interpolation to existing tests. * Corrected time output on interpolation function. Added unit test to check for this. * Contributors: Atsushi Watanabe, Miguel Prada, Tully Foote, Vallan Sherrod 0.5.16 (2017-07-14) ------------------- * remove explicit templating to standardize on overloading. But provide backwards compatibility with deprecation. * Merge pull request `#144 `_ from clearpathrobotics/dead_lock_fix Solve a bug that causes a deadlock in MessageFilter * Resolve 2 places where the error_msg would not be propogated. Fixes `#198 `_ * Remove generate_rand_vectors() from a number of tests. (`#227 `_) * fixing include directory order to support overlays (`#231 `_) * replaced dependencies on tf2_msgs_gencpp by exported dependencies * Document the lifetime of the returned reference for getFrameId getTimestamp * relax normalization tolerance. `#196 `_ was too strict for some use cases. (`#220 `_) * Solve a bug that causes a deadlock in MessageFilter * Contributors: Adel Fakih, Chris Lalancette, Christopher Wecht, Tully Foote, dhood 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- * fixes `#194 `_ check for quaternion normalization before inserting into storage (`#196 `_) * check for quaternion normalization before inserting into storage * Add test to check for transform failure on invalid quaternion input * updating getAngleShortestPath() (`#187 `_) * Move internal cache functions into a namespace Fixes https://github.com/ros/geometry2/issues/175 * Link properly to convert.h * Landing page for tf2 describing the conversion interface * Fix comment on BufferCore::MAX_GRAPH_DEPTH. * Contributors: Jackie Kay, Phil Osteen, Tully Foote, alex, gavanderhoorn 0.5.13 (2016-03-04) ------------------- 0.5.12 (2015-08-05) ------------------- * add utilities to get yaw, pitch, roll and identity transform * provide more conversions between types The previous conversion always assumed that it was converting a non-message type to a non-message type. Now, one, both or none can be a message or a non-message. * Contributors: Vincent Rabaud 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- * move lct_cache into function local memoryfor `#92 `_ * Clean up range checking. Re: `#92 `_ * Fixed chainToVector * release lock before possibly invoking user callbacks. Fixes `#91 `_ * Contributors: Jackie Kay, Tully Foote 0.5.9 (2015-03-25) ------------------ * fixing edge case where two no frame id lookups matched in getLatestCommonTime * Contributors: Tully Foote 0.5.8 (2015-03-17) ------------------ * change from default argument to overload to avoid linking issue `#84 `_ * remove useless Makefile files * Remove unused assignments in max/min functions * change _allFramesAsDot() -> _allFramesAsDot(double current_time) * Contributors: Jon Binney, Kei Okada, Tully Foote, Vincent Rabaud 0.5.7 (2014-12-23) ------------------ 0.5.6 (2014-09-18) ------------------ 0.5.5 (2014-06-23) ------------------ * convert to use console bridge from upstream debian package https://github.com/ros/rosdistro/issues/4633 * Fix format string * Contributors: Austin, Tully Foote 0.5.4 (2014-05-07) ------------------ * switch to boost signals2 following `ros/ros_comm#267 `_, blocking `ros/geometry#23 `_ * Contributors: Tully Foote 0.5.3 (2014-02-21) ------------------ 0.5.2 (2014-02-20) ------------------ 0.5.1 (2014-02-14) ------------------ 0.5.0 (2014-02-14) ------------------ 0.4.10 (2013-12-26) ------------------- * updated error message. fixes `#38 `_ * tf2: add missing console bridge include directories (fix `#48 `_) * Fix const correctness of tf2::Vector3 rotate() method The method does not modify the class thus should be const. This has already been fixed in Bullet itself. * Contributors: Dirk Thomas, Timo Rohling, Tully Foote 0.4.9 (2013-11-06) ------------------ 0.4.8 (2013-11-06) ------------------ * moving python documentation to tf2_ros from tf2 to follow the code * removing legacy rospy dependency. implementation removed in 0.4.0 fixes `#27 `_ 0.4.7 (2013-08-28) ------------------ * switching to use allFramesAsStringNoLock inside of getLatestCommonTime and walkToParent and locking in public API _getLatestCommonTime instead re `#23 `_ * Fixes a crash in tf's view_frames related to dot code generation in allFramesAsDot 0.4.6 (2013-08-28) ------------------ * cleaner fix for `#19 `_ * fix pointer initialization. Fixes `#19 `_ * fixes `#18 `_ for hydro * package.xml: corrected typo in description 0.4.5 (2013-07-11) ------------------ * adding _chainAsVector method for https://github.com/ros/geometry/issues/18 * adding _allFramesAsDot for backwards compatability https://github.com/ros/geometry/issues/18 0.4.4 (2013-07-09) ------------------ * making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. * tf2: Fixes a warning on OS X, but generally safer Replaces the use of pointers with shared_ptrs, this allows the polymorphism and makes it so that the compiler doesn't yell at us about calling delete on a class with a public non-virtual destructor. * tf2: Fixes compiler warnings on OS X This exploited a gcc specific extension and is not C++ standard compliant. There used to be a "fix" for OS X which no longer applies. I think it is ok to use this as an int instead of a double, but another way to fix it would be to use a define. * tf2: Fixes linkedit errors on OS X 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ * adding getCacheLength() to parallel old tf API * removing legacy static const variable MAX_EXTRAPOLATION_DISTANCE copied from tf unnecessesarily 0.4.1 (2013-07-05) ------------------ * adding old style callback notifications to BufferCore to enable backwards compatability of message filters * exposing dedicated thread logic in BufferCore and checking in Buffer * more methods to expose, and check for empty cache before getting latest timestamp * adding methods to enable backwards compatability for passing through to tf::Transformer 0.4.0 (2013-06-27) ------------------ * splitting rospy dependency into tf2_py so tf2 is pure c++ library. * switching to console_bridge from rosconsole * moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 * Cleaning up unnecessary dependency on roscpp * Cleaning up packaging of tf2 including: removing unused nodehandle fixing overmatch on search and replace cleaning up a few dependencies and linking removing old backup of package.xml making diff minimally different from tf version of library * suppressing bullet LinearMath copy inside of tf2, so it will not collide, and should not be used externally. * Restoring test packages and bullet packages. reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ * fixing includes in unit tests * Make PythonLibs find_package python2 specific On systems with python 3 installed and default, find_package(PythonLibs) will find the python 3 paths and libraries. However, the c++ include structure seems to be different in python 3 and tf2 uses includes that are no longer present or deprecated. Until the includes are made to be python 3 compliant, we should specify that the version of python found must be python 2. 0.3.6 (2013-03-03) ------------------ 0.3.5 (2013-02-15 14:46) ------------------------ * 0.3.4 -> 0.3.5 0.3.4 (2013-02-15 13:14) ------------------------ * 0.3.3 -> 0.3.4 * moving LinearMath includes to include/tf2 0.3.3 (2013-02-15 11:30) ------------------------ * 0.3.2 -> 0.3.3 * fixing include installation of tf2 0.3.2 (2013-02-15 00:42) ------------------------ * 0.3.1 -> 0.3.2 * fixed missing include export & tf2_ros dependecy 0.3.1 (2013-02-14) ------------------ * 0.3.0 -> 0.3.1 * fixing PYTHON installation directory 0.3.0 (2013-02-13) ------------------ * switching to version 0.3.0 * adding setup.py to tf2 package * fixed tf2 exposing python functionality * removed line that was killing tf2_ros.so * fixing catkin message dependencies * removing packages with missing deps * adding missing package.xml * adding missing package.xml * adding missing package.xml * catkinizing geometry-experimental * removing bullet headers from use in header files * removing bullet headers from use in header files * merging my recent changes * setting child_frame_id overlooked in revision 6a0eec022be0 which fixed failing tests * allFramesAsString public and internal methods seperated. Public method is locked, private method is not * fixing another scoped lock * fixing one scoped lock * fixing test compilation * merge * Error message fix, ros-pkg5085 * Check if target equals to source before validation * When target_frame == source_frame, just returns an identity transform. * adding addition ros header includes for strictness * Fixed optimized lookups with compound transforms * Fixed problem in tf2 optimized branch. Quaternion multiplication order was incorrect * fix compilation on 32-bit * Josh fix: Final inverse transform composition (missed multiplying the sourcd->top vector by the target->top inverse orientation). b44877d2b054 * Josh change: fix first/last time case. 46bf33868e0d * fix transform accumulation to parent * fix parent lookup, now works on the real pr2's tree * move the message filter to tf2_ros * tf2::MessageFilter + tests. Still need to change it around to pass in a callback queue, since we're being triggered directly from the tf2 buffer * Don't add the request if the transform is already available. Add some new tests * working transformable callbacks with a simple (incomplete) test case * first pass at a transformable callback api, not tested yet * add interpolation cases * fix getLatestCommonTime -- no longer returns the latest of any of the times * Some more optimization -- allow findClosest to inline * another minor speedup * Minorly speed up canTransform by not requiring the full data lookup, and only looking up the parent * Add explicit operator= so that we can see the time in it on a profile graph. Also some minor cleanup * minor cleanup * add 3 more cases to the speed test * Remove use of btTransform at all from transform accumulation, since the conversion to/from is unnecessary, expensive, and can introduce floating point error * Don't use btTransform as an intermediate when accumulating transforms, as constructing them takes quite a bit of time * Completely remove lookupLists(). canTransform() now uses the same walking code as lookupTransform(). Also fixed a bug in the static transform publisher test * Genericise the walk-to-top-parent code in lookupTransform so that it will be able to be used by canTransform as well (minus the cost of actually computing the transform) * remove id lookup that wasn't doing anything * Some more optimization: * Reduce # of TransformStorage copies made in TimeCache::getData() * Remove use of lookupLists from getLatestCommonTime * lookupTransform() no longer uses lookupLists unless it's called with Time(0). Removes lots of object construction/destruction due to removal of pushing back on the lists * Remove CompactFrameID in favor of a typedef * these mode checks are no longer necessary * Fix crash when testing extrapolation on the forward transforms * Update cache unit tests to work with the changes TransformStorage. Also make sure that BT_USE_DOUBLE_PRECISION is set for tf2. * remove exposure of time_cache.h from buffer_core.h * Removed the mutex from TimeCache, as it's unnecessary (BufferCore needs to have its own mutex locked anyway), and this speeds things up by about 20% Also fixed a number of thread-safety problems * Optimize test_extrapolation a bit, 25% speedup of lookupTransform * use a hash map for looking up frame numbers, speeds up lookupTransform by ~8% * Cache vectors used for looking up transforms. Speeds up lookupTransform by another 10% * speed up lookupTransform by another 25% * speed up lookupTransform by another 2x. also reduces the memory footprint of the cache significantly * sped up lookupTransform by another 2x * First add of a simple speed test Sped up lookupTransform 2x * roscpp dependency explicit, instead of relying on implicit * static transform tested and working * tests passing and all throw catches removed too\! * validating frame_ids up front for lookup exceptions * working with single base class vector * tests passing for static storage * making method private for clarity * static cache implementation and test * cleaning up API doc typos * sphinx docs for Buffer * new dox mainpage * update tf2 manifest * commenting out twist * Changed cache_time to cache_time_ to follow C++ style guide, also initialized it to actually get things to work * no more rand in cache tests * Changing tf2_py.cpp to use underscores instead of camelCase * removing all old converter functions from transform_datatypes.h * removing last references to transform_datatypes.h in tf2 * transform conversions internalized * removing unused datatypes * copying bullet transform headers into tf2 and breaking bullet dependency * merge * removing dependency on tf * removing include of old tf from tf2 * update doc * merge * kdl unittest passing * Spaces instead of tabs in YAML grrrr * Adding quotes for parent * canTransform advanced ported * Hopefully fixing YAML syntax * new version of view_frames in new tf2_tools package * testing new argument validation and catching bug * Python support for debugging * merge * adding validation of frame_ids in queries with warnings and exceptions where appropriate * Exposing ability to get frames as a string * A compiling version of YAML debugging interface for BufferCore * placeholder for tf debug * fixing tf:: to tf2:: ns issues and stripping slashes on set in tf2 for backwards compatiabily * Adding a python version of the BufferClient * moving test to new package * merging * working unit test for BufferCore::lookupTransform * removing unused method test and converting NO_PARENT test to new API * Adding some comments * Moving the python bindings for tf2 to the tf2 package from the tf2_py package * buffercore tests upgraded * porting tf_unittest while running incrmentally instead of block copy * BufferCore::clear ported forward * successfully changed lookupTransform advanced to new version * switching to new implementation of lookupTransform tests still passing * compiling lookupTransform new version * removing tf_prefix from BufferCore. BuferCore is independent of any frame_ids. tf_prefix should be implemented at the ROS API level. * initializing tf_prefix * adding missing initialization * suppressing warnings * more tests ported * removing tests for apis not ported forward * setTransform tests ported * old tests in new package passing due to backwards dependency. now for the fun, port all 1500 lines :-) * setTransform working in new framework as well as old * porting more methods * more compatability * bringing in helper functions for buffer_core from tf.h/cpp * rethrowing to new exceptions * converting Storage to geometry_msgs::TransformStamped * removing deprecated useage * cleaning up includes * moving all implementations into cpp file * switching test to new class from old one * Compiling version of the buffer client * moving listener to tf_cpp * removing listener, it should be in another package * most of listener * add cantransform implementation * removing deprecated API usage * initial import of listener header * move implementation into library * 2 tests of buffer * moving executables back into bin * compiling again with new design * rename tfcore to buffercore * almost compiling version of template code * compiling tf2_core simple test * add test to start compiling * copying in tf_unittest for tf_core testing template * prototype of tf2_core implemented using old tf. * first version of template functions * remove timeouts * properly naming tf2_core.h from tf_core.h * working cache test with tf2 lib * first unit test passing, not yet ported * tf_core api * tf2 v2 * aborting port * moving across time cache tf and datatypes headers * copying exceptions from tf * switching to tf2 from tf_core geometry2-0.7.5/tf2/CMakeLists.txt000066400000000000000000000040441372352374300167060ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2) find_package(console_bridge REQUIRED) find_package(catkin REQUIRED COMPONENTS geometry_msgs rostime tf2_msgs) find_package(Boost REQUIRED COMPONENTS system thread) catkin_package( INCLUDE_DIRS include LIBRARIES tf2 DEPENDS console_bridge CATKIN_DEPENDS geometry_msgs tf2_msgs rostime) include_directories(include ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS}) # export user definitions #CPP Libraries add_library(tf2 src/cache.cpp src/buffer_core.cpp src/static_cache.cpp) target_link_libraries(tf2 ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES}) add_dependencies(tf2 ${catkin_EXPORTED_TARGETS}) install(TARGETS tf2 ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) # Tests if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_cache_unittest test/cache_unittest.cpp) target_link_libraries(test_cache_unittest tf2 ${console_bridge_LIBRARIES}) add_dependencies(test_cache_unittest ${catkin_EXPORTED_TARGETS}) catkin_add_gtest(test_static_cache_unittest test/static_cache_test.cpp) target_link_libraries(test_static_cache_unittest tf2 ${console_bridge_LIBRARIES}) add_dependencies(test_static_cache_unittest ${catkin_EXPORTED_TARGETS}) catkin_add_gtest(test_simple test/simple_tf2_core.cpp) target_link_libraries(test_simple tf2 ${console_bridge_LIBRARIES}) add_dependencies(test_simple ${catkin_EXPORTED_TARGETS}) add_executable(speed_test EXCLUDE_FROM_ALL test/speed_test.cpp) target_link_libraries(speed_test tf2 ${console_bridge_LIBRARIES}) add_dependencies(tests speed_test) add_dependencies(tests ${catkin_EXPORTED_TARGETS}) catkin_add_gtest(test_transform_datatypes test/test_transform_datatypes.cpp) target_link_libraries(test_transform_datatypes tf2 ${console_bridge_LIBRARIES}) add_dependencies(test_transform_datatypes ${catkin_EXPORTED_TARGETS}) endif() geometry2-0.7.5/tf2/include/000077500000000000000000000000001372352374300155675ustar00rootroot00000000000000geometry2-0.7.5/tf2/include/tf2/000077500000000000000000000000001372352374300162625ustar00rootroot00000000000000geometry2-0.7.5/tf2/include/tf2/LinearMath/000077500000000000000000000000001372352374300203065ustar00rootroot00000000000000geometry2-0.7.5/tf2/include/tf2/LinearMath/Matrix3x3.h000066400000000000000000000517211372352374300222670ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF2_MATRIX3x3_H #define TF2_MATRIX3x3_H #include "Vector3.h" #include "Quaternion.h" #include namespace tf2 { #define Matrix3x3Data Matrix3x3DoubleData /**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. * Make sure to only include a pure orthogonal matrix without scaling. */ class Matrix3x3 { ///Data storage for the matrix, each vector is a row of the matrix Vector3 m_el[3]; public: /** @brief No initializaion constructor */ Matrix3x3 () {} // explicit Matrix3x3(const tf2Scalar *m) { setFromOpenGLSubMatrix(m); } /**@brief Constructor from Quaternion */ explicit Matrix3x3(const Quaternion& q) { setRotation(q); } /* template Matrix3x3(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) { setEulerYPR(yaw, pitch, roll); } */ /** @brief Constructor with row major formatting */ Matrix3x3(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz, const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz, const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz) { setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz); } /** @brief Copy constructor */ TF2SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other) { m_el[0] = other.m_el[0]; m_el[1] = other.m_el[1]; m_el[2] = other.m_el[2]; } /** @brief Assignment Operator */ TF2SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other) { m_el[0] = other.m_el[0]; m_el[1] = other.m_el[1]; m_el[2] = other.m_el[2]; return *this; } /** @brief Get a column of the matrix as a vector * @param i Column number 0 indexed */ TF2SIMD_FORCE_INLINE Vector3 getColumn(int i) const { return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]); } /** @brief Get a row of the matrix as a vector * @param i Row number 0 indexed */ TF2SIMD_FORCE_INLINE const Vector3& getRow(int i) const { tf2FullAssert(0 <= i && i < 3); return m_el[i]; } /** @brief Get a mutable reference to a row of the matrix as a vector * @param i Row number 0 indexed */ TF2SIMD_FORCE_INLINE Vector3& operator[](int i) { tf2FullAssert(0 <= i && i < 3); return m_el[i]; } /** @brief Get a const reference to a row of the matrix as a vector * @param i Row number 0 indexed */ TF2SIMD_FORCE_INLINE const Vector3& operator[](int i) const { tf2FullAssert(0 <= i && i < 3); return m_el[i]; } /** @brief Multiply by the target matrix on the right * @param m Rotation matrix to be applied * Equivilant to this = this * m */ Matrix3x3& operator*=(const Matrix3x3& m); /** @brief Set from a carray of tf2Scalars * @param m A pointer to the beginning of an array of 9 tf2Scalars */ void setFromOpenGLSubMatrix(const tf2Scalar *m) { m_el[0].setValue(m[0],m[4],m[8]); m_el[1].setValue(m[1],m[5],m[9]); m_el[2].setValue(m[2],m[6],m[10]); } /** @brief Set the values of the matrix explicitly (row major) * @param xx Top left * @param xy Top Middle * @param xz Top Right * @param yx Middle Left * @param yy Middle Middle * @param yz Middle Right * @param zx Bottom Left * @param zy Bottom Middle * @param zz Bottom Right*/ void setValue(const tf2Scalar& xx, const tf2Scalar& xy, const tf2Scalar& xz, const tf2Scalar& yx, const tf2Scalar& yy, const tf2Scalar& yz, const tf2Scalar& zx, const tf2Scalar& zy, const tf2Scalar& zz) { m_el[0].setValue(xx,xy,xz); m_el[1].setValue(yx,yy,yz); m_el[2].setValue(zx,zy,zz); } /** @brief Set the matrix from a quaternion * @param q The Quaternion to match */ void setRotation(const Quaternion& q) { tf2Scalar d = q.length2(); tf2FullAssert(d != tf2Scalar(0.0)); tf2Scalar s = tf2Scalar(2.0) / d; tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy, xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx, xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy)); } /** @brief Set the matrix from euler angles using YPR around ZYX respectively * @param yaw Yaw about Z axis * @param pitch Pitch about Y axis * @param roll Roll about X axis */ ROS_DEPRECATED void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) { setEulerYPR(yaw, pitch, roll); } /** @brief Set the matrix from euler angles YPR around ZYX axes * @param eulerZ Yaw aboud Z axis * @param eulerY Pitch around Y axis * @param eulerX Roll about X axis * * These angles are used to produce a rotation matrix. The euler * angles are applied in ZYX order. I.e a vector is first rotated * about X then Y and then Z **/ void setEulerYPR(tf2Scalar eulerZ, tf2Scalar eulerY,tf2Scalar eulerX) { tf2Scalar ci ( tf2Cos(eulerX)); tf2Scalar cj ( tf2Cos(eulerY)); tf2Scalar ch ( tf2Cos(eulerZ)); tf2Scalar si ( tf2Sin(eulerX)); tf2Scalar sj ( tf2Sin(eulerY)); tf2Scalar sh ( tf2Sin(eulerZ)); tf2Scalar cc = ci * ch; tf2Scalar cs = ci * sh; tf2Scalar sc = si * ch; tf2Scalar ss = si * sh; setValue(cj * ch, sj * sc - cs, sj * cc + ss, cj * sh, sj * ss + cc, sj * cs - sc, -sj, cj * si, cj * ci); } /** @brief Set the matrix using RPY about XYZ fixed axes * @param roll Roll about X axis * @param pitch Pitch around Y axis * @param yaw Yaw aboud Z axis * **/ void setRPY(tf2Scalar roll, tf2Scalar pitch,tf2Scalar yaw) { setEulerYPR(yaw, pitch, roll); } /**@brief Set the matrix to the identity */ void setIdentity() { setValue(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0)); } static const Matrix3x3& getIdentity() { static const Matrix3x3 identityMatrix(tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0), tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(1.0)); return identityMatrix; } /**@brief Fill the values of the matrix into a 9 element array * @param m The array to be filled */ void getOpenGLSubMatrix(tf2Scalar *m) const { m[0] = tf2Scalar(m_el[0].x()); m[1] = tf2Scalar(m_el[1].x()); m[2] = tf2Scalar(m_el[2].x()); m[3] = tf2Scalar(0.0); m[4] = tf2Scalar(m_el[0].y()); m[5] = tf2Scalar(m_el[1].y()); m[6] = tf2Scalar(m_el[2].y()); m[7] = tf2Scalar(0.0); m[8] = tf2Scalar(m_el[0].z()); m[9] = tf2Scalar(m_el[1].z()); m[10] = tf2Scalar(m_el[2].z()); m[11] = tf2Scalar(0.0); } /**@brief Get the matrix represented as a quaternion * @param q The quaternion which will be set */ void getRotation(Quaternion& q) const { tf2Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z(); tf2Scalar temp[4]; if (trace > tf2Scalar(0.0)) { tf2Scalar s = tf2Sqrt(trace + tf2Scalar(1.0)); temp[3]=(s * tf2Scalar(0.5)); s = tf2Scalar(0.5) / s; temp[0]=((m_el[2].y() - m_el[1].z()) * s); temp[1]=((m_el[0].z() - m_el[2].x()) * s); temp[2]=((m_el[1].x() - m_el[0].y()) * s); } else { int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; tf2Scalar s = tf2Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf2Scalar(1.0)); temp[i] = s * tf2Scalar(0.5); s = tf2Scalar(0.5) / s; temp[3] = (m_el[k][j] - m_el[j][k]) * s; temp[j] = (m_el[j][i] + m_el[i][j]) * s; temp[k] = (m_el[k][i] + m_el[i][k]) * s; } q.setValue(temp[0],temp[1],temp[2],temp[3]); } /**@brief Get the matrix represented as euler angles around ZYX * @param yaw Yaw around Z axis * @param pitch Pitch around Y axis * @param roll around X axis * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ ROS_DEPRECATED void getEulerZYX(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const { getEulerYPR(yaw, pitch, roll, solution_number); }; /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR * @param yaw Yaw around Z axis * @param pitch Pitch around Y axis * @param roll around X axis */ void getEulerYPR(tf2Scalar& yaw, tf2Scalar& pitch, tf2Scalar& roll, unsigned int solution_number = 1) const { struct Euler { tf2Scalar yaw; tf2Scalar pitch; tf2Scalar roll; }; Euler euler_out; Euler euler_out2; //second solution //get the pointer to the raw data // Check that pitch is not at a singularity // Check that pitch is not at a singularity if (tf2Fabs(m_el[2].x()) >= 1) { euler_out.yaw = 0; euler_out2.yaw = 0; // From difference of angles formula tf2Scalar delta = tf2Atan2(m_el[2].y(),m_el[2].z()); if (m_el[2].x() < 0) //gimbal locked down { euler_out.pitch = TF2SIMD_PI / tf2Scalar(2.0); euler_out2.pitch = TF2SIMD_PI / tf2Scalar(2.0); euler_out.roll = delta; euler_out2.roll = delta; } else // gimbal locked up { euler_out.pitch = -TF2SIMD_PI / tf2Scalar(2.0); euler_out2.pitch = -TF2SIMD_PI / tf2Scalar(2.0); euler_out.roll = delta; euler_out2.roll = delta; } } else { euler_out.pitch = - tf2Asin(m_el[2].x()); euler_out2.pitch = TF2SIMD_PI - euler_out.pitch; euler_out.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out.pitch), m_el[2].z()/tf2Cos(euler_out.pitch)); euler_out2.roll = tf2Atan2(m_el[2].y()/tf2Cos(euler_out2.pitch), m_el[2].z()/tf2Cos(euler_out2.pitch)); euler_out.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out.pitch), m_el[0].x()/tf2Cos(euler_out.pitch)); euler_out2.yaw = tf2Atan2(m_el[1].x()/tf2Cos(euler_out2.pitch), m_el[0].x()/tf2Cos(euler_out2.pitch)); } if (solution_number == 1) { yaw = euler_out.yaw; pitch = euler_out.pitch; roll = euler_out.roll; } else { yaw = euler_out2.yaw; pitch = euler_out2.pitch; roll = euler_out2.roll; } } /**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ * @param roll around X axis * @param pitch Pitch around Y axis * @param yaw Yaw around Z axis * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ void getRPY(tf2Scalar& roll, tf2Scalar& pitch, tf2Scalar& yaw, unsigned int solution_number = 1) const { getEulerYPR(yaw, pitch, roll, solution_number); } /**@brief Create a scaled copy of the matrix * @param s Scaling vector The elements of the vector will scale each column */ Matrix3x3 scaled(const Vector3& s) const { return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(), m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(), m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z()); } /**@brief Return the determinant of the matrix */ tf2Scalar determinant() const; /**@brief Return the adjoint of the matrix */ Matrix3x3 adjoint() const; /**@brief Return the matrix with all values non negative */ Matrix3x3 absolute() const; /**@brief Return the transpose of the matrix */ Matrix3x3 transpose() const; /**@brief Return the inverse of the matrix */ Matrix3x3 inverse() const; Matrix3x3 transposeTimes(const Matrix3x3& m) const; Matrix3x3 timesTranspose(const Matrix3x3& m) const; TF2SIMD_FORCE_INLINE tf2Scalar tdotx(const Vector3& v) const { return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z(); } TF2SIMD_FORCE_INLINE tf2Scalar tdoty(const Vector3& v) const { return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z(); } TF2SIMD_FORCE_INLINE tf2Scalar tdotz(const Vector3& v) const { return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); } /**@brief diagonalizes this matrix by the Jacobi method. * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original * coordinate system, i.e., old_this = rot * new_this * rot^T. * @param threshold See iteration * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. * * Note that this matrix is assumed to be symmetric. */ void diagonalize(Matrix3x3& rot, tf2Scalar threshold, int maxSteps) { rot.setIdentity(); for (int step = maxSteps; step > 0; step--) { // find off-diagonal element [p][q] with largest magnitude int p = 0; int q = 1; int r = 2; tf2Scalar max = tf2Fabs(m_el[0][1]); tf2Scalar v = tf2Fabs(m_el[0][2]); if (v > max) { q = 2; r = 1; max = v; } v = tf2Fabs(m_el[1][2]); if (v > max) { p = 1; q = 2; r = 0; max = v; } tf2Scalar t = threshold * (tf2Fabs(m_el[0][0]) + tf2Fabs(m_el[1][1]) + tf2Fabs(m_el[2][2])); if (max <= t) { if (max <= TF2SIMD_EPSILON * t) { return; } step = 1; } // compute Jacobi rotation J which leads to a zero for element [p][q] tf2Scalar mpq = m_el[p][q]; tf2Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); tf2Scalar theta2 = theta * theta; tf2Scalar cos; tf2Scalar sin; if (theta2 * theta2 < tf2Scalar(10 / TF2SIMD_EPSILON)) { t = (theta >= 0) ? 1 / (theta + tf2Sqrt(1 + theta2)) : 1 / (theta - tf2Sqrt(1 + theta2)); cos = 1 / tf2Sqrt(1 + t * t); sin = cos * t; } else { // approximation for large theta-value, i.e., a nearly diagonal matrix t = 1 / (theta * (2 + tf2Scalar(0.5) / theta2)); cos = 1 - tf2Scalar(0.5) * t * t; sin = cos * t; } // apply rotation to matrix (this = J^T * this * J) m_el[p][q] = m_el[q][p] = 0; m_el[p][p] -= t * mpq; m_el[q][q] += t * mpq; tf2Scalar mrp = m_el[r][p]; tf2Scalar mrq = m_el[r][q]; m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; // apply rotation to rot (rot = rot * J) for (int i = 0; i < 3; i++) { Vector3& row = rot[i]; mrp = row[p]; mrq = row[q]; row[p] = cos * mrp - sin * mrq; row[q] = cos * mrq + sin * mrp; } } } /**@brief Calculate the matrix cofactor * @param r1 The first row to use for calculating the cofactor * @param c1 The first column to use for calculating the cofactor * @param r1 The second row to use for calculating the cofactor * @param c1 The second column to use for calculating the cofactor * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details */ tf2Scalar cofac(int r1, int c1, int r2, int c2) const { return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; } void serialize(struct Matrix3x3Data& dataOut) const; void serializeFloat(struct Matrix3x3FloatData& dataOut) const; void deSerialize(const struct Matrix3x3Data& dataIn); void deSerializeFloat(const struct Matrix3x3FloatData& dataIn); void deSerializeDouble(const struct Matrix3x3DoubleData& dataIn); }; TF2SIMD_FORCE_INLINE Matrix3x3& Matrix3x3::operator*=(const Matrix3x3& m) { setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); return *this; } TF2SIMD_FORCE_INLINE tf2Scalar Matrix3x3::determinant() const { return tf2Triple((*this)[0], (*this)[1], (*this)[2]); } TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::absolute() const { return Matrix3x3( tf2Fabs(m_el[0].x()), tf2Fabs(m_el[0].y()), tf2Fabs(m_el[0].z()), tf2Fabs(m_el[1].x()), tf2Fabs(m_el[1].y()), tf2Fabs(m_el[1].z()), tf2Fabs(m_el[2].x()), tf2Fabs(m_el[2].y()), tf2Fabs(m_el[2].z())); } TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::transpose() const { return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(), m_el[0].y(), m_el[1].y(), m_el[2].y(), m_el[0].z(), m_el[1].z(), m_el[2].z()); } TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::adjoint() const { return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); } TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::inverse() const { Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); tf2Scalar det = (*this)[0].dot(co); tf2FullAssert(det != tf2Scalar(0.0)); tf2Scalar s = tf2Scalar(1.0) / det; return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); } TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::transposeTimes(const Matrix3x3& m) const { return Matrix3x3( m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(), m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(), m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(), m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); } TF2SIMD_FORCE_INLINE Matrix3x3 Matrix3x3::timesTranspose(const Matrix3x3& m) const { return Matrix3x3( m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); } TF2SIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3& m, const Vector3& v) { return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); } TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3& v, const Matrix3x3& m) { return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); } TF2SIMD_FORCE_INLINE Matrix3x3 operator*(const Matrix3x3& m1, const Matrix3x3& m2) { return Matrix3x3( m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); } /* TF2SIMD_FORCE_INLINE Matrix3x3 tf2MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) { return Matrix3x3( m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); } */ /**@brief Equality operator between two matrices * It will test all elements are equal. */ TF2SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2) { return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); } ///for serialization struct Matrix3x3FloatData { Vector3FloatData m_el[3]; }; ///for serialization struct Matrix3x3DoubleData { Vector3DoubleData m_el[3]; }; TF2SIMD_FORCE_INLINE void Matrix3x3::serialize(struct Matrix3x3Data& dataOut) const { for (int i=0;i<3;i++) m_el[i].serialize(dataOut.m_el[i]); } TF2SIMD_FORCE_INLINE void Matrix3x3::serializeFloat(struct Matrix3x3FloatData& dataOut) const { for (int i=0;i<3;i++) m_el[i].serializeFloat(dataOut.m_el[i]); } TF2SIMD_FORCE_INLINE void Matrix3x3::deSerialize(const struct Matrix3x3Data& dataIn) { for (int i=0;i<3;i++) m_el[i].deSerialize(dataIn.m_el[i]); } TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeFloat(const struct Matrix3x3FloatData& dataIn) { for (int i=0;i<3;i++) m_el[i].deSerializeFloat(dataIn.m_el[i]); } TF2SIMD_FORCE_INLINE void Matrix3x3::deSerializeDouble(const struct Matrix3x3DoubleData& dataIn) { for (int i=0;i<3;i++) m_el[i].deSerializeDouble(dataIn.m_el[i]); } } #endif //TF2_MATRIX3x3_H geometry2-0.7.5/tf2/include/tf2/LinearMath/MinMax.h000066400000000000000000000032611372352374300216520ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef GEN_MINMAX_H #define GEN_MINMAX_H template TF2SIMD_FORCE_INLINE const T& tf2Min(const T& a, const T& b) { return a < b ? a : b ; } template TF2SIMD_FORCE_INLINE const T& tf2Max(const T& a, const T& b) { return a > b ? a : b; } template TF2SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) { return a < lb ? lb : (ub < a ? ub : a); } template TF2SIMD_FORCE_INLINE void tf2SetMin(T& a, const T& b) { if (b < a) { a = b; } } template TF2SIMD_FORCE_INLINE void tf2SetMax(T& a, const T& b) { if (a < b) { a = b; } } template TF2SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) { if (a < lb) { a = lb; } else if (ub < a) { a = ub; } } #endif geometry2-0.7.5/tf2/include/tf2/LinearMath/QuadWord.h000066400000000000000000000137061372352374300222140ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF2SIMD_QUADWORD_H #define TF2SIMD_QUADWORD_H #include "Scalar.h" #include "MinMax.h" #if defined (__CELLOS_LV2) && defined (__SPU__) #include #endif namespace tf2 { /**@brief The QuadWord class is base class for Vector3 and Quaternion. * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. */ #ifndef USE_LIBSPE2 ATTRIBUTE_ALIGNED16(class) QuadWord #else class QuadWord #endif { protected: #if defined (__SPU__) && defined (__CELLOS_LV2__) union { vec_float4 mVec128; tf2Scalar m_floats[4]; }; public: vec_float4 get128() const { return mVec128; } protected: #else //__CELLOS_LV2__ __SPU__ tf2Scalar m_floats[4]; #endif //__CELLOS_LV2__ __SPU__ public: /**@brief Return the x value */ TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; } /**@brief Return the y value */ TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; } /**@brief Return the z value */ TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; } /**@brief Set the x value */ TF2SIMD_FORCE_INLINE void setX(tf2Scalar x) { m_floats[0] = x;}; /**@brief Set the y value */ TF2SIMD_FORCE_INLINE void setY(tf2Scalar y) { m_floats[1] = y;}; /**@brief Set the z value */ TF2SIMD_FORCE_INLINE void setZ(tf2Scalar z) { m_floats[2] = z;}; /**@brief Set the w value */ TF2SIMD_FORCE_INLINE void setW(tf2Scalar w) { m_floats[3] = w;}; /**@brief Return the x value */ TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; } /**@brief Return the y value */ TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; } /**@brief Return the z value */ TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; } /**@brief Return the w value */ TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; } //TF2SIMD_FORCE_INLINE tf2Scalar& operator[](int i) { return (&m_floats[0])[i]; } //TF2SIMD_FORCE_INLINE const tf2Scalar& operator[](int i) const { return (&m_floats[0])[i]; } ///operator tf2Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. TF2SIMD_FORCE_INLINE operator tf2Scalar *() { return &m_floats[0]; } TF2SIMD_FORCE_INLINE operator const tf2Scalar *() const { return &m_floats[0]; } TF2SIMD_FORCE_INLINE bool operator==(const QuadWord& other) const { return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); } TF2SIMD_FORCE_INLINE bool operator!=(const QuadWord& other) const { return !(*this == other); } /**@brief Set x,y,z and zero w * @param x Value of x * @param y Value of y * @param z Value of z */ TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) { m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; m_floats[3] = 0.f; } /* void getValue(tf2Scalar *m) const { m[0] = m_floats[0]; m[1] = m_floats[1]; m[2] = m_floats[2]; } */ /**@brief Set the values * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) { m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; m_floats[3]=w; } /**@brief No initialization constructor */ TF2SIMD_FORCE_INLINE QuadWord() // :m_floats[0](tf2Scalar(0.)),m_floats[1](tf2Scalar(0.)),m_floats[2](tf2Scalar(0.)),m_floats[3](tf2Scalar(0.)) { } /**@brief Three argument constructor (zeros w) * @param x Value of x * @param y Value of y * @param z Value of z */ TF2SIMD_FORCE_INLINE QuadWord(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) { m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f; } /**@brief Initializing constructor * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ TF2SIMD_FORCE_INLINE QuadWord(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) { m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w; } /**@brief Set each element to the max of the current values and the values of another QuadWord * @param other The other QuadWord to compare with */ TF2SIMD_FORCE_INLINE void setMax(const QuadWord& other) { tf2SetMax(m_floats[0], other.m_floats[0]); tf2SetMax(m_floats[1], other.m_floats[1]); tf2SetMax(m_floats[2], other.m_floats[2]); tf2SetMax(m_floats[3], other.m_floats[3]); } /**@brief Set each element to the min of the current values and the values of another QuadWord * @param other The other QuadWord to compare with */ TF2SIMD_FORCE_INLINE void setMin(const QuadWord& other) { tf2SetMin(m_floats[0], other.m_floats[0]); tf2SetMin(m_floats[1], other.m_floats[1]); tf2SetMin(m_floats[2], other.m_floats[2]); tf2SetMin(m_floats[3], other.m_floats[3]); } }; } #endif //TF2SIMD_QUADWORD_H geometry2-0.7.5/tf2/include/tf2/LinearMath/Quaternion.h000066400000000000000000000374161372352374300226170ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF2_QUATERNION_H_ #define TF2_QUATERNION_H_ #include "Vector3.h" #include "QuadWord.h" #include namespace tf2 { /**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */ class Quaternion : public QuadWord { public: /**@brief No initialization constructor */ Quaternion() {} // template // explicit Quaternion(const tf2Scalar *v) : Tuple4(v) {} /**@brief Constructor from scalars */ Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w) : QuadWord(x, y, z, w) {} /**@brief Axis angle Constructor * @param axis The axis which the rotation is around * @param angle The magnitude of the rotation around the angle (Radians) */ Quaternion(const Vector3& axis, const tf2Scalar& angle) { setRotation(axis, angle); } /**@brief Constructor from Euler angles * @param yaw Angle around Y unless TF2_EULER_DEFAULT_ZYX defined then Z * @param pitch Angle around X unless TF2_EULER_DEFAULT_ZYX defined then Y * @param roll Angle around Z unless TF2_EULER_DEFAULT_ZYX defined then X */ ROS_DEPRECATED Quaternion(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) { #ifndef TF2_EULER_DEFAULT_ZYX setEuler(yaw, pitch, roll); #else setRPY(roll, pitch, yaw); #endif } /**@brief Set the rotation using axis angle notation * @param axis The axis around which to rotate * @param angle The magnitude of the rotation in Radians */ void setRotation(const Vector3& axis, const tf2Scalar& angle) { tf2Scalar d = axis.length(); tf2Assert(d != tf2Scalar(0.0)); tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d; setValue(axis.x() * s, axis.y() * s, axis.z() * s, tf2Cos(angle * tf2Scalar(0.5))); } /**@brief Set the quaternion using Euler angles * @param yaw Angle around Y * @param pitch Angle around X * @param roll Angle around Z */ void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) { tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5); tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5); tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5); tf2Scalar cosYaw = tf2Cos(halfYaw); tf2Scalar sinYaw = tf2Sin(halfYaw); tf2Scalar cosPitch = tf2Cos(halfPitch); tf2Scalar sinPitch = tf2Sin(halfPitch); tf2Scalar cosRoll = tf2Cos(halfRoll); tf2Scalar sinRoll = tf2Sin(halfRoll); setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); } /**@brief Set the quaternion using fixed axis RPY * @param roll Angle around X * @param pitch Angle around Y * @param yaw Angle around Z*/ void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw) { tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5); tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5); tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5); tf2Scalar cosYaw = tf2Cos(halfYaw); tf2Scalar sinYaw = tf2Sin(halfYaw); tf2Scalar cosPitch = tf2Cos(halfPitch); tf2Scalar sinPitch = tf2Sin(halfPitch); tf2Scalar cosRoll = tf2Cos(halfRoll); tf2Scalar sinRoll = tf2Sin(halfRoll); setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx } /**@brief Set the quaternion using euler angles * @param yaw Angle around Z * @param pitch Angle around Y * @param roll Angle around X */ ROS_DEPRECATED void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) { setRPY(roll, pitch, yaw); } /**@brief Add two quaternions * @param q The quaternion to add to this one */ TF2SIMD_FORCE_INLINE Quaternion& operator+=(const Quaternion& q) { m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; return *this; } /**@brief Sutf2ract out a quaternion * @param q The quaternion to sutf2ract from this one */ Quaternion& operator-=(const Quaternion& q) { m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; return *this; } /**@brief Scale this quaternion * @param s The scalar to scale by */ Quaternion& operator*=(const tf2Scalar& s) { m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; return *this; } /**@brief Multiply this quaternion by q on the right * @param q The other quaternion * Equivilant to this = this * q */ Quaternion& operator*=(const Quaternion& q) { setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); return *this; } /**@brief Return the dot product between this quaternion and another * @param q The other quaternion */ tf2Scalar dot(const Quaternion& q) const { return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; } /**@brief Return the length squared of the quaternion */ tf2Scalar length2() const { return dot(*this); } /**@brief Return the length of the quaternion */ tf2Scalar length() const { return tf2Sqrt(length2()); } /**@brief Normalize the quaternion * Such that x^2 + y^2 + z^2 +w^2 = 1 */ Quaternion& normalize() { return *this /= length(); } /**@brief Return a scaled version of this quaternion * @param s The scale factor */ TF2SIMD_FORCE_INLINE Quaternion operator*(const tf2Scalar& s) const { return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s); } /**@brief Return an inversely scaled versionof this quaternion * @param s The inverse scale factor */ Quaternion operator/(const tf2Scalar& s) const { tf2Assert(s != tf2Scalar(0.0)); return *this * (tf2Scalar(1.0) / s); } /**@brief Inversely scale this quaternion * @param s The scale factor */ Quaternion& operator/=(const tf2Scalar& s) { tf2Assert(s != tf2Scalar(0.0)); return *this *= tf2Scalar(1.0) / s; } /**@brief Return a normalized version of this quaternion */ Quaternion normalized() const { return *this / length(); } /**@brief Return the ***half*** angle between this quaternion and the other * @param q The other quaternion */ tf2Scalar angle(const Quaternion& q) const { tf2Scalar s = tf2Sqrt(length2() * q.length2()); tf2Assert(s != tf2Scalar(0.0)); return tf2Acos(dot(q) / s); } /**@brief Return the angle between this quaternion and the other along the shortest path * @param q The other quaternion */ tf2Scalar angleShortestPath(const Quaternion& q) const { tf2Scalar s = tf2Sqrt(length2() * q.length2()); tf2Assert(s != tf2Scalar(0.0)); if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp return tf2Acos(dot(-q) / s) * tf2Scalar(2.0); else return tf2Acos(dot(q) / s) * tf2Scalar(2.0); } /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */ tf2Scalar getAngle() const { tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]); return s; } /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */ tf2Scalar getAngleShortestPath() const { tf2Scalar s; if (m_floats[3] >= 0) s = tf2Scalar(2.) * tf2Acos(m_floats[3]); else s = tf2Scalar(2.) * tf2Acos(-m_floats[3]); return s; } /**@brief Return the axis of the rotation represented by this quaternion */ Vector3 getAxis() const { tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.)); if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero return Vector3(1.0, 0.0, 0.0); // Arbitrary tf2Scalar s = tf2Sqrt(s_squared); return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); } /**@brief Return the inverse of this quaternion */ Quaternion inverse() const { return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); } /**@brief Return the sum of this quaternion and the other * @param q2 The other quaternion */ TF2SIMD_FORCE_INLINE Quaternion operator+(const Quaternion& q2) const { const Quaternion& q1 = *this; return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); } /**@brief Return the difference between this quaternion and the other * @param q2 The other quaternion */ TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion& q2) const { const Quaternion& q1 = *this; return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); } /**@brief Return the negative of this quaternion * This simply negates each element */ TF2SIMD_FORCE_INLINE Quaternion operator-() const { const Quaternion& q2 = *this; return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); } /**@todo document this and it's use */ TF2SIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const { Quaternion diff,sum; diff = *this - qd; sum = *this + qd; if( diff.dot(diff) > sum.dot(sum) ) return qd; return (-qd); } /**@todo document this and it's use */ TF2SIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const { Quaternion diff,sum; diff = *this - qd; sum = *this + qd; if( diff.dot(diff) < sum.dot(sum) ) return qd; return (-qd); } /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion * @param q The other quaternion to interpolate with * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. * Slerp interpolates assuming constant velocity. */ Quaternion slerp(const Quaternion& q, const tf2Scalar& t) const { tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0); if (theta != tf2Scalar(0.0)) { tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta); tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta); tf2Scalar s1 = tf2Sin(t * theta); if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d, (m_floats[1] * s0 + -q.y() * s1) * d, (m_floats[2] * s0 + -q.z() * s1) * d, (m_floats[3] * s0 + -q.m_floats[3] * s1) * d); else return Quaternion((m_floats[0] * s0 + q.x() * s1) * d, (m_floats[1] * s0 + q.y() * s1) * d, (m_floats[2] * s0 + q.z() * s1) * d, (m_floats[3] * s0 + q.m_floats[3] * s1) * d); } else { return *this; } } static const Quaternion& getIdentity() { static const Quaternion identityQuat(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.)); return identityQuat; } TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; } }; /**@brief Return the negative of a quaternion */ TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion& q) { return Quaternion(-q.x(), -q.y(), -q.z(), -q.w()); } /**@brief Return the product of two quaternions */ TF2SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q1, const Quaternion& q2) { return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(), q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(), q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(), q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); } TF2SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q, const Vector3& w) { return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(), q.w() * w.y() + q.z() * w.x() - q.x() * w.z(), q.w() * w.z() + q.x() * w.y() - q.y() * w.x(), -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); } TF2SIMD_FORCE_INLINE Quaternion operator*(const Vector3& w, const Quaternion& q) { return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(), w.y() * q.w() + w.z() * q.x() - w.x() * q.z(), w.z() * q.w() + w.x() * q.y() - w.y() * q.x(), -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); } /**@brief Calculate the dot product between two quaternions */ TF2SIMD_FORCE_INLINE tf2Scalar dot(const Quaternion& q1, const Quaternion& q2) { return q1.dot(q2); } /**@brief Return the length of a quaternion */ TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion& q) { return q.length(); } /**@brief Return the ***half*** angle between two quaternions*/ TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion& q1, const Quaternion& q2) { return q1.angle(q2); } /**@brief Return the shortest angle between two quaternions*/ TF2SIMD_FORCE_INLINE tf2Scalar angleShortestPath(const Quaternion& q1, const Quaternion& q2) { return q1.angleShortestPath(q2); } /**@brief Return the inverse of a quaternion*/ TF2SIMD_FORCE_INLINE Quaternion inverse(const Quaternion& q) { return q.inverse(); } /**@brief Return the result of spherical linear interpolation betwen two quaternions * @param q1 The first quaternion * @param q2 The second quaternion * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 * Slerp assumes constant velocity between positions. */ TF2SIMD_FORCE_INLINE Quaternion slerp(const Quaternion& q1, const Quaternion& q2, const tf2Scalar& t) { return q1.slerp(q2, t); } TF2SIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion& rotation, const Vector3& v) { Quaternion q = rotation * v; q *= rotation.inverse(); return Vector3(q.getX(),q.getY(),q.getZ()); } TF2SIMD_FORCE_INLINE Quaternion shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized { Vector3 c = v0.cross(v1); tf2Scalar d = v0.dot(v1); if (d < -1.0 + TF2SIMD_EPSILON) { Vector3 n,unused; tf2PlaneSpace1(v0,n,unused); return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0 } tf2Scalar s = tf2Sqrt((1.0f + d) * 2.0f); tf2Scalar rs = 1.0f / s; return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); } TF2SIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3& v0,Vector3& v1) { v0.normalize(); v1.normalize(); return shortestArcQuat(v0,v1); } } #endif geometry2-0.7.5/tf2/include/tf2/LinearMath/Scalar.h000066400000000000000000000321751372352374300216740ustar00rootroot00000000000000/* Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF2_SCALAR_H #define TF2_SCALAR_H #ifdef TF2_MANAGED_CODE //Aligned data types not supported in managed code #pragma unmanaged #endif #include #include //size_t for MSVC 6.0 #include #include #include #if defined(DEBUG) || defined (_DEBUG) #define TF2_DEBUG #endif #ifdef _WIN32 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) #define TF2SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #else //#define TF2_HAS_ALIGNED_ALLOCATOR #pragma warning(disable : 4324) // disable padding warning // #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. // #pragma warning(disable:4996) //Turn off warnings about deprecated C routines // #pragma warning(disable:4786) // Disable the "debug name too long" warning #define TF2SIMD_FORCE_INLINE __forceinline #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a #ifdef _XBOX #define TF2_USE_VMX128 #include #define TF2_HAVE_NATIVE_FSEL #define tf2Fsel(a,b,c) __fsel((a),(b),(c)) #else #endif//_XBOX #endif //__MINGW32__ #include #ifdef TF2_DEBUG #define tf2Assert assert #else #define tf2Assert(x) #endif //tf2FullAssert is optional, slows down a lot #define tf2FullAssert(x) #define tf2Likely(_c) _c #define tf2Unlikely(_c) _c #else #if defined (__CELLOS_LV2__) #define TF2SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include #endif #ifdef TF2_DEBUG #define tf2Assert assert #else #define tf2Assert(x) #endif //tf2FullAssert is optional, slows down a lot #define tf2FullAssert(x) #define tf2Likely(_c) _c #define tf2Unlikely(_c) _c #else #ifdef USE_LIBSPE2 #define TF2SIMD_FORCE_INLINE __inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include #endif #ifdef TF2_DEBUG #define tf2Assert assert #else #define tf2Assert(x) #endif //tf2FullAssert is optional, slows down a lot #define tf2FullAssert(x) #define tf2Likely(_c) __builtin_expect((_c), 1) #define tf2Unlikely(_c) __builtin_expect((_c), 0) #else //non-windows systems #define TF2SIMD_FORCE_INLINE inline ///@todo: check out alignment methods for other platforms/compilers ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #ifndef assert #include #endif #if defined(DEBUG) || defined (_DEBUG) #define tf2Assert assert #else #define tf2Assert(x) #endif //tf2FullAssert is optional, slows down a lot #define tf2FullAssert(x) #define tf2Likely(_c) _c #define tf2Unlikely(_c) _c #endif // LIBSPE2 #endif //__CELLOS_LV2__ #endif ///The tf2Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision. typedef double tf2Scalar; //this number could be bigger in double precision #define TF2_LARGE_FLOAT 1e30 #define TF2_DECLARE_ALIGNED_ALLOCATOR() \ TF2SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \ TF2SIMD_FORCE_INLINE void operator delete(void* ptr) { tf2AlignedFree(ptr); } \ TF2SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ TF2SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ TF2SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return tf2AlignedAlloc(sizeInBytes,16); } \ TF2SIMD_FORCE_INLINE void operator delete[](void* ptr) { tf2AlignedFree(ptr); } \ TF2SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ TF2SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x) { return sqrt(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Fabs(tf2Scalar x) { return fabs(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x) { return cos(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x) { return sin(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Tan(tf2Scalar x) { return tan(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x) { if (xtf2Scalar(1)) x=tf2Scalar(1); return acos(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x) { if (xtf2Scalar(1)) x=tf2Scalar(1); return asin(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan(tf2Scalar x) { return atan(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2(tf2Scalar x, tf2Scalar y) { return atan2(x, y); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Exp(tf2Scalar x) { return exp(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Log(tf2Scalar x) { return log(x); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x,tf2Scalar y) { return pow(x,y); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Fmod(tf2Scalar x,tf2Scalar y) { return fmod(x,y); } #define TF2SIMD_2_PI tf2Scalar(6.283185307179586232) #define TF2SIMD_PI (TF2SIMD_2_PI * tf2Scalar(0.5)) #define TF2SIMD_HALF_PI (TF2SIMD_2_PI * tf2Scalar(0.25)) #define TF2SIMD_RADS_PER_DEG (TF2SIMD_2_PI / tf2Scalar(360.0)) #define TF2SIMD_DEGS_PER_RAD (tf2Scalar(360.0) / TF2SIMD_2_PI) #define TF2SIMDSQRT12 tf2Scalar(0.7071067811865475244008443621048490) #define tf2RecipSqrt(x) ((tf2Scalar)(tf2Scalar(1.0)/tf2Sqrt(tf2Scalar(x)))) /* reciprocal square root */ #define TF2SIMD_EPSILON DBL_EPSILON #define TF2SIMD_INFINITY DBL_MAX TF2SIMD_FORCE_INLINE tf2Scalar tf2Atan2Fast(tf2Scalar y, tf2Scalar x) { tf2Scalar coeff_1 = TF2SIMD_PI / 4.0f; tf2Scalar coeff_2 = 3.0f * coeff_1; tf2Scalar abs_y = tf2Fabs(y); tf2Scalar angle; if (x >= 0.0f) { tf2Scalar r = (x - abs_y) / (x + abs_y); angle = coeff_1 - coeff_1 * r; } else { tf2Scalar r = (x + abs_y) / (abs_y - x); angle = coeff_2 - coeff_1 * r; } return (y < 0.0f) ? -angle : angle; } TF2SIMD_FORCE_INLINE bool tf2FuzzyZero(tf2Scalar x) { return tf2Fabs(x) < TF2SIMD_EPSILON; } TF2SIMD_FORCE_INLINE bool tf2Equal(tf2Scalar a, tf2Scalar eps) { return (((a) <= eps) && !((a) < -eps)); } TF2SIMD_FORCE_INLINE bool tf2GreaterEqual (tf2Scalar a, tf2Scalar eps) { return (!((a) <= eps)); } TF2SIMD_FORCE_INLINE int tf2IsNegative(tf2Scalar x) { return x < tf2Scalar(0.0) ? 1 : 0; } TF2SIMD_FORCE_INLINE tf2Scalar tf2Radians(tf2Scalar x) { return x * TF2SIMD_RADS_PER_DEG; } TF2SIMD_FORCE_INLINE tf2Scalar tf2Degrees(tf2Scalar x) { return x * TF2SIMD_DEGS_PER_RAD; } #define TF2_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name #ifndef tf2Fsel TF2SIMD_FORCE_INLINE tf2Scalar tf2Fsel(tf2Scalar a, tf2Scalar b, tf2Scalar c) { return a >= 0 ? b : c; } #endif #define tf2Fsels(a,b,c) (tf2Scalar)tf2Fsel(a,b,c) TF2SIMD_FORCE_INLINE bool tf2MachineIsLittleEndian() { long int i = 1; const char *p = (const char *) &i; if (p[0] == 1) // Lowest address contains the least significant byte return true; else return false; } ///tf2Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 ///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html TF2SIMD_FORCE_INLINE unsigned tf2Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) { // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero // Rely on positive value or'ed with its negative having sign bit on // and zero value or'ed with its negative (which is still zero) having sign bit off // Use arithmetic shift right, shifting the sign bit through all 32 bits unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); unsigned testEqz = ~testNz; return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); } TF2SIMD_FORCE_INLINE int tf2Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) { unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); unsigned testEqz = ~testNz; return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); } TF2SIMD_FORCE_INLINE float tf2Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) { #ifdef TF2_HAVE_NATIVE_FSEL return (float)tf2Fsel((tf2Scalar)condition - tf2Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); #else return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; #endif } template TF2SIMD_FORCE_INLINE void tf2Swap(T& a, T& b) { T tmp = a; a = b; b = tmp; } //PCK: endian swapping functions TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(unsigned val) { return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); } TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(unsigned short val) { return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); } TF2SIMD_FORCE_INLINE unsigned tf2SwapEndian(int val) { return tf2SwapEndian((unsigned)val); } TF2SIMD_FORCE_INLINE unsigned short tf2SwapEndian(short val) { return tf2SwapEndian((unsigned short) val); } ///tf2SwapFloat uses using char pointers to swap the endianness ////tf2SwapFloat/tf2SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values ///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. ///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. ///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. ///so instead of returning a float/double, we return integer/long long integer TF2SIMD_FORCE_INLINE unsigned int tf2SwapEndianFloat(float d) { unsigned int a = 0; unsigned char *dst = (unsigned char *)&a; unsigned char *src = (unsigned char *)&d; dst[0] = src[3]; dst[1] = src[2]; dst[2] = src[1]; dst[3] = src[0]; return a; } // unswap using char pointers TF2SIMD_FORCE_INLINE float tf2UnswapEndianFloat(unsigned int a) { float d = 0.0f; unsigned char *src = (unsigned char *)&a; unsigned char *dst = (unsigned char *)&d; dst[0] = src[3]; dst[1] = src[2]; dst[2] = src[1]; dst[3] = src[0]; return d; } // swap using char pointers TF2SIMD_FORCE_INLINE void tf2SwapEndianDouble(double d, unsigned char* dst) { unsigned char *src = (unsigned char *)&d; dst[0] = src[7]; dst[1] = src[6]; dst[2] = src[5]; dst[3] = src[4]; dst[4] = src[3]; dst[5] = src[2]; dst[6] = src[1]; dst[7] = src[0]; } // unswap using char pointers TF2SIMD_FORCE_INLINE double tf2UnswapEndianDouble(const unsigned char *src) { double d = 0.0; unsigned char *dst = (unsigned char *)&d; dst[0] = src[7]; dst[1] = src[6]; dst[2] = src[5]; dst[3] = src[4]; dst[4] = src[3]; dst[5] = src[2]; dst[6] = src[1]; dst[7] = src[0]; return d; } // returns normalized value in range [-TF2SIMD_PI, TF2SIMD_PI] TF2SIMD_FORCE_INLINE tf2Scalar tf2NormalizeAngle(tf2Scalar angleInRadians) { angleInRadians = tf2Fmod(angleInRadians, TF2SIMD_2_PI); if(angleInRadians < -TF2SIMD_PI) { return angleInRadians + TF2SIMD_2_PI; } else if(angleInRadians > TF2SIMD_PI) { return angleInRadians - TF2SIMD_2_PI; } else { return angleInRadians; } } ///rudimentary class to provide type info struct tf2TypedObject { tf2TypedObject(int objectType) :m_objectType(objectType) { } int m_objectType; inline int getObjectType() const { return m_objectType; } }; #endif //TF2SIMD___SCALAR_H geometry2-0.7.5/tf2/include/tf2/LinearMath/Transform.h000066400000000000000000000206371372352374300224420ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef tf2_Transform_H #define tf2_Transform_H #include "Matrix3x3.h" namespace tf2 { #define TransformData TransformDoubleData /**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear. *It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */ class Transform { ///Storage for the rotation Matrix3x3 m_basis; ///Storage for the translation Vector3 m_origin; public: /**@brief No initialization constructor */ Transform() {} /**@brief Constructor from Quaternion (optional Vector3 ) * @param q Rotation from quaternion * @param c Translation from Vector (default 0,0,0) */ explicit TF2SIMD_FORCE_INLINE Transform(const Quaternion& q, const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0))) : m_basis(q), m_origin(c) {} /**@brief Constructor from Matrix3x3 (optional Vector3) * @param b Rotation from Matrix * @param c Translation from Vector default (0,0,0)*/ explicit TF2SIMD_FORCE_INLINE Transform(const Matrix3x3& b, const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0))) : m_basis(b), m_origin(c) {} /**@brief Copy constructor */ TF2SIMD_FORCE_INLINE Transform (const Transform& other) : m_basis(other.m_basis), m_origin(other.m_origin) { } /**@brief Assignment Operator */ TF2SIMD_FORCE_INLINE Transform& operator=(const Transform& other) { m_basis = other.m_basis; m_origin = other.m_origin; return *this; } /**@brief Set the current transform as the value of the product of two transforms * @param t1 Transform 1 * @param t2 Transform 2 * This = Transform1 * Transform2 */ TF2SIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) { m_basis = t1.m_basis * t2.m_basis; m_origin = t1(t2.m_origin); } /* void multInverseLeft(const Transform& t1, const Transform& t2) { Vector3 v = t2.m_origin - t1.m_origin; m_basis = tf2MultTransposeLeft(t1.m_basis, t2.m_basis); m_origin = v * t1.m_basis; } */ /**@brief Return the transform of the vector */ TF2SIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const { return Vector3(m_basis[0].dot(x) + m_origin.x(), m_basis[1].dot(x) + m_origin.y(), m_basis[2].dot(x) + m_origin.z()); } /**@brief Return the transform of the vector */ TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const { return (*this)(x); } /**@brief Return the transform of the Quaternion */ TF2SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const { return getRotation() * q; } /**@brief Return the basis matrix for the rotation */ TF2SIMD_FORCE_INLINE Matrix3x3& getBasis() { return m_basis; } /**@brief Return the basis matrix for the rotation */ TF2SIMD_FORCE_INLINE const Matrix3x3& getBasis() const { return m_basis; } /**@brief Return the origin vector translation */ TF2SIMD_FORCE_INLINE Vector3& getOrigin() { return m_origin; } /**@brief Return the origin vector translation */ TF2SIMD_FORCE_INLINE const Vector3& getOrigin() const { return m_origin; } /**@brief Return a quaternion representing the rotation */ Quaternion getRotation() const { Quaternion q; m_basis.getRotation(q); return q; } /**@brief Set from an array * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void setFromOpenGLMatrix(const tf2Scalar *m) { m_basis.setFromOpenGLSubMatrix(m); m_origin.setValue(m[12],m[13],m[14]); } /**@brief Fill an array representation * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ void getOpenGLMatrix(tf2Scalar *m) const { m_basis.getOpenGLSubMatrix(m); m[12] = m_origin.x(); m[13] = m_origin.y(); m[14] = m_origin.z(); m[15] = tf2Scalar(1.0); } /**@brief Set the translational element * @param origin The vector to set the translation to */ TF2SIMD_FORCE_INLINE void setOrigin(const Vector3& origin) { m_origin = origin; } TF2SIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const; /**@brief Set the rotational element by Matrix3x3 */ TF2SIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis) { m_basis = basis; } /**@brief Set the rotational element by Quaternion */ TF2SIMD_FORCE_INLINE void setRotation(const Quaternion& q) { m_basis.setRotation(q); } /**@brief Set this transformation to the identity */ void setIdentity() { m_basis.setIdentity(); m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0)); } /**@brief Multiply this Transform by another(this = this * another) * @param t The other transform */ Transform& operator*=(const Transform& t) { m_origin += m_basis * t.m_origin; m_basis *= t.m_basis; return *this; } /**@brief Return the inverse of this transform */ Transform inverse() const { Matrix3x3 inv = m_basis.transpose(); return Transform(inv, inv * -m_origin); } /**@brief Return the inverse of this transform times the other transform * @param t The other transform * return this.inverse() * the other */ Transform inverseTimes(const Transform& t) const; /**@brief Return the product of this transform and the other */ Transform operator*(const Transform& t) const; /**@brief Return an identity transform */ static const Transform& getIdentity() { static const Transform identityTransform(Matrix3x3::getIdentity()); return identityTransform; } void serialize(struct TransformData& dataOut) const; void serializeFloat(struct TransformFloatData& dataOut) const; void deSerialize(const struct TransformData& dataIn); void deSerializeDouble(const struct TransformDoubleData& dataIn); void deSerializeFloat(const struct TransformFloatData& dataIn); }; TF2SIMD_FORCE_INLINE Vector3 Transform::invXform(const Vector3& inVec) const { Vector3 v = inVec - m_origin; return (m_basis.transpose() * v); } TF2SIMD_FORCE_INLINE Transform Transform::inverseTimes(const Transform& t) const { Vector3 v = t.getOrigin() - m_origin; return Transform(m_basis.transposeTimes(t.m_basis), v * m_basis); } TF2SIMD_FORCE_INLINE Transform Transform::operator*(const Transform& t) const { return Transform(m_basis * t.m_basis, (*this)(t.m_origin)); } /**@brief Test if two transforms have all elements equal */ TF2SIMD_FORCE_INLINE bool operator==(const Transform& t1, const Transform& t2) { return ( t1.getBasis() == t2.getBasis() && t1.getOrigin() == t2.getOrigin() ); } ///for serialization struct TransformFloatData { Matrix3x3FloatData m_basis; Vector3FloatData m_origin; }; struct TransformDoubleData { Matrix3x3DoubleData m_basis; Vector3DoubleData m_origin; }; TF2SIMD_FORCE_INLINE void Transform::serialize(TransformData& dataOut) const { m_basis.serialize(dataOut.m_basis); m_origin.serialize(dataOut.m_origin); } TF2SIMD_FORCE_INLINE void Transform::serializeFloat(TransformFloatData& dataOut) const { m_basis.serializeFloat(dataOut.m_basis); m_origin.serializeFloat(dataOut.m_origin); } TF2SIMD_FORCE_INLINE void Transform::deSerialize(const TransformData& dataIn) { m_basis.deSerialize(dataIn.m_basis); m_origin.deSerialize(dataIn.m_origin); } TF2SIMD_FORCE_INLINE void Transform::deSerializeFloat(const TransformFloatData& dataIn) { m_basis.deSerializeFloat(dataIn.m_basis); m_origin.deSerializeFloat(dataIn.m_origin); } TF2SIMD_FORCE_INLINE void Transform::deSerializeDouble(const TransformDoubleData& dataIn) { m_basis.deSerializeDouble(dataIn.m_basis); m_origin.deSerializeDouble(dataIn.m_origin); } } #endif geometry2-0.7.5/tf2/include/tf2/LinearMath/Vector3.h000066400000000000000000000475161372352374300220210ustar00rootroot00000000000000/* Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ #ifndef TF2_VECTOR3_H #define TF2_VECTOR3_H #include "Scalar.h" #include "MinMax.h" namespace tf2 { #define Vector3Data Vector3DoubleData #define Vector3DataName "Vector3DoubleData" /**@brief tf2::Vector3 can be used to represent 3D points and vectors. * It has an un-used w component to suit 16-byte alignment when tf2::Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user * Ideally, this class should be replaced by a platform optimized TF2SIMD version that keeps the data in registers */ ATTRIBUTE_ALIGNED16(class) Vector3 { public: #if defined (__SPU__) && defined (__CELLOS_LV2__) tf2Scalar m_floats[4]; public: TF2SIMD_FORCE_INLINE const vec_float4& get128() const { return *((const vec_float4*)&m_floats[0]); } public: #else //__CELLOS_LV2__ __SPU__ #ifdef TF2_USE_SSE // _WIN32 union { __m128 mVec128; tf2Scalar m_floats[4]; }; TF2SIMD_FORCE_INLINE __m128 get128() const { return mVec128; } TF2SIMD_FORCE_INLINE void set128(__m128 v128) { mVec128 = v128; } #else tf2Scalar m_floats[4]; #endif #endif //__CELLOS_LV2__ __SPU__ public: /**@brief No initialization constructor */ TF2SIMD_FORCE_INLINE Vector3() {} /**@brief Constructor from scalars * @param x X value * @param y Y value * @param z Z value */ TF2SIMD_FORCE_INLINE Vector3(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) { m_floats[0] = x; m_floats[1] = y; m_floats[2] = z; m_floats[3] = tf2Scalar(0.); } /**@brief Add a vector to this one * @param The vector to add to this one */ TF2SIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v) { m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2]; return *this; } /**@brief Sutf2ract a vector from this one * @param The vector to sutf2ract */ TF2SIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v) { m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2]; return *this; } /**@brief Scale the vector * @param s Scale factor */ TF2SIMD_FORCE_INLINE Vector3& operator*=(const tf2Scalar& s) { m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s; return *this; } /**@brief Inversely scale the vector * @param s Scale factor to divide by */ TF2SIMD_FORCE_INLINE Vector3& operator/=(const tf2Scalar& s) { tf2FullAssert(s != tf2Scalar(0.0)); return *this *= tf2Scalar(1.0) / s; } /**@brief Return the dot product * @param v The other vector in the dot product */ TF2SIMD_FORCE_INLINE tf2Scalar dot(const Vector3& v) const { return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2]; } /**@brief Return the length of the vector squared */ TF2SIMD_FORCE_INLINE tf2Scalar length2() const { return dot(*this); } /**@brief Return the length of the vector */ TF2SIMD_FORCE_INLINE tf2Scalar length() const { return tf2Sqrt(length2()); } /**@brief Return the distance squared between the ends of this and another vector * This is symantically treating the vector like a point */ TF2SIMD_FORCE_INLINE tf2Scalar distance2(const Vector3& v) const; /**@brief Return the distance between the ends of this and another vector * This is symantically treating the vector like a point */ TF2SIMD_FORCE_INLINE tf2Scalar distance(const Vector3& v) const; /**@brief Normalize this vector * x^2 + y^2 + z^2 = 1 */ TF2SIMD_FORCE_INLINE Vector3& normalize() { return *this /= length(); } /**@brief Return a normalized version of this vector */ TF2SIMD_FORCE_INLINE Vector3 normalized() const; /**@brief Rotate this vector * @param wAxis The axis to rotate about * @param angle The angle to rotate by */ TF2SIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tf2Scalar angle ) const; /**@brief Return the angle between this and another vector * @param v The other vector */ TF2SIMD_FORCE_INLINE tf2Scalar angle(const Vector3& v) const { tf2Scalar s = tf2Sqrt(length2() * v.length2()); tf2FullAssert(s != tf2Scalar(0.0)); return tf2Acos(dot(v) / s); } /**@brief Return a vector will the absolute values of each element */ TF2SIMD_FORCE_INLINE Vector3 absolute() const { return Vector3( tf2Fabs(m_floats[0]), tf2Fabs(m_floats[1]), tf2Fabs(m_floats[2])); } /**@brief Return the cross product between this and another vector * @param v The other vector */ TF2SIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const { return Vector3( m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1], m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); } TF2SIMD_FORCE_INLINE tf2Scalar triple(const Vector3& v1, const Vector3& v2) const { return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); } /**@brief Return the axis with the smallest value * Note return values are 0,1,2 for x, y, or z */ TF2SIMD_FORCE_INLINE int minAxis() const { return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ TF2SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tf2Scalar& t) const { return Vector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, m_floats[2] + (v.m_floats[2] -m_floats[2]) * t); } /**@brief Elementwise multiply this vector by the other * @param v The other vector */ TF2SIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v) { m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2]; return *this; } /**@brief Return the x value */ TF2SIMD_FORCE_INLINE const tf2Scalar& getX() const { return m_floats[0]; } /**@brief Return the y value */ TF2SIMD_FORCE_INLINE const tf2Scalar& getY() const { return m_floats[1]; } /**@brief Return the z value */ TF2SIMD_FORCE_INLINE const tf2Scalar& getZ() const { return m_floats[2]; } /**@brief Set the x value */ TF2SIMD_FORCE_INLINE void setX(tf2Scalar x) { m_floats[0] = x;}; /**@brief Set the y value */ TF2SIMD_FORCE_INLINE void setY(tf2Scalar y) { m_floats[1] = y;}; /**@brief Set the z value */ TF2SIMD_FORCE_INLINE void setZ(tf2Scalar z) {m_floats[2] = z;}; /**@brief Set the w value */ TF2SIMD_FORCE_INLINE void setW(tf2Scalar w) { m_floats[3] = w;}; /**@brief Return the x value */ TF2SIMD_FORCE_INLINE const tf2Scalar& x() const { return m_floats[0]; } /**@brief Return the y value */ TF2SIMD_FORCE_INLINE const tf2Scalar& y() const { return m_floats[1]; } /**@brief Return the z value */ TF2SIMD_FORCE_INLINE const tf2Scalar& z() const { return m_floats[2]; } /**@brief Return the w value */ TF2SIMD_FORCE_INLINE const tf2Scalar& w() const { return m_floats[3]; } //TF2SIMD_FORCE_INLINE tf2Scalar& operator[](int i) { return (&m_floats[0])[i]; } //TF2SIMD_FORCE_INLINE const tf2Scalar& operator[](int i) const { return (&m_floats[0])[i]; } ///operator tf2Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. TF2SIMD_FORCE_INLINE operator tf2Scalar *() { return &m_floats[0]; } TF2SIMD_FORCE_INLINE operator const tf2Scalar *() const { return &m_floats[0]; } TF2SIMD_FORCE_INLINE bool operator==(const Vector3& other) const { return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0])); } TF2SIMD_FORCE_INLINE bool operator!=(const Vector3& other) const { return !(*this == other); } /**@brief Set each element to the max of the current values and the values of another Vector3 * @param other The other Vector3 to compare with */ TF2SIMD_FORCE_INLINE void setMax(const Vector3& other) { tf2SetMax(m_floats[0], other.m_floats[0]); tf2SetMax(m_floats[1], other.m_floats[1]); tf2SetMax(m_floats[2], other.m_floats[2]); tf2SetMax(m_floats[3], other.w()); } /**@brief Set each element to the min of the current values and the values of another Vector3 * @param other The other Vector3 to compare with */ TF2SIMD_FORCE_INLINE void setMin(const Vector3& other) { tf2SetMin(m_floats[0], other.m_floats[0]); tf2SetMin(m_floats[1], other.m_floats[1]); tf2SetMin(m_floats[2], other.m_floats[2]); tf2SetMin(m_floats[3], other.w()); } TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z) { m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; m_floats[3] = tf2Scalar(0.); } void getSkewSymmetricMatrix(Vector3* v0,Vector3* v1,Vector3* v2) const { v0->setValue(0. ,-z() ,y()); v1->setValue(z() ,0. ,-x()); v2->setValue(-y() ,x() ,0.); } void setZero() { setValue(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.)); } TF2SIMD_FORCE_INLINE bool isZero() const { return m_floats[0] == tf2Scalar(0) && m_floats[1] == tf2Scalar(0) && m_floats[2] == tf2Scalar(0); } TF2SIMD_FORCE_INLINE bool fuzzyZero() const { return length2() < TF2SIMD_EPSILON; } TF2SIMD_FORCE_INLINE void serialize(struct Vector3Data& dataOut) const; TF2SIMD_FORCE_INLINE void deSerialize(const struct Vector3Data& dataIn); TF2SIMD_FORCE_INLINE void serializeFloat(struct Vector3FloatData& dataOut) const; TF2SIMD_FORCE_INLINE void deSerializeFloat(const struct Vector3FloatData& dataIn); TF2SIMD_FORCE_INLINE void serializeDouble(struct Vector3DoubleData& dataOut) const; TF2SIMD_FORCE_INLINE void deSerializeDouble(const struct Vector3DoubleData& dataIn); }; /**@brief Return the sum of two vectors (Point symantics)*/ TF2SIMD_FORCE_INLINE Vector3 operator+(const Vector3& v1, const Vector3& v2) { return Vector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]); } /**@brief Return the elementwise product of two vectors */ TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3& v1, const Vector3& v2) { return Vector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]); } /**@brief Return the difference between two vectors */ TF2SIMD_FORCE_INLINE Vector3 operator-(const Vector3& v1, const Vector3& v2) { return Vector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]); } /**@brief Return the negative of the vector */ TF2SIMD_FORCE_INLINE Vector3 operator-(const Vector3& v) { return Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); } /**@brief Return the vector scaled by s */ TF2SIMD_FORCE_INLINE Vector3 operator*(const Vector3& v, const tf2Scalar& s) { return Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); } /**@brief Return the vector scaled by s */ TF2SIMD_FORCE_INLINE Vector3 operator*(const tf2Scalar& s, const Vector3& v) { return v * s; } /**@brief Return the vector inversely scaled by s */ TF2SIMD_FORCE_INLINE Vector3 operator/(const Vector3& v, const tf2Scalar& s) { tf2FullAssert(s != tf2Scalar(0.0)); return v * (tf2Scalar(1.0) / s); } /**@brief Return the vector inversely scaled by s */ TF2SIMD_FORCE_INLINE Vector3 operator/(const Vector3& v1, const Vector3& v2) { return Vector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]); } /**@brief Return the dot product between two vectors */ TF2SIMD_FORCE_INLINE tf2Scalar tf2Dot(const Vector3& v1, const Vector3& v2) { return v1.dot(v2); } /**@brief Return the distance squared between two vectors */ TF2SIMD_FORCE_INLINE tf2Scalar tf2Distance2(const Vector3& v1, const Vector3& v2) { return v1.distance2(v2); } /**@brief Return the distance between two vectors */ TF2SIMD_FORCE_INLINE tf2Scalar tf2Distance(const Vector3& v1, const Vector3& v2) { return v1.distance(v2); } /**@brief Return the angle between two vectors */ TF2SIMD_FORCE_INLINE tf2Scalar tf2Angle(const Vector3& v1, const Vector3& v2) { return v1.angle(v2); } /**@brief Return the cross product of two vectors */ TF2SIMD_FORCE_INLINE Vector3 tf2Cross(const Vector3& v1, const Vector3& v2) { return v1.cross(v2); } TF2SIMD_FORCE_INLINE tf2Scalar tf2Triple(const Vector3& v1, const Vector3& v2, const Vector3& v3) { return v1.triple(v2, v3); } /**@brief Return the linear interpolation between two vectors * @param v1 One vector * @param v2 The other vector * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ TF2SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v1, const Vector3& v2, const tf2Scalar& t) { return v1.lerp(v2, t); } TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance2(const Vector3& v) const { return (v - *this).length2(); } TF2SIMD_FORCE_INLINE tf2Scalar Vector3::distance(const Vector3& v) const { return (v - *this).length(); } TF2SIMD_FORCE_INLINE Vector3 Vector3::normalized() const { return *this / length(); } TF2SIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tf2Scalar angle ) const { // wAxis must be a unit lenght vector Vector3 o = wAxis * wAxis.dot( *this ); Vector3 x = *this - o; Vector3 y; y = wAxis.cross( *this ); return ( o + x * tf2Cos( angle ) + y * tf2Sin( angle ) ); } class tf2Vector4 : public Vector3 { public: TF2SIMD_FORCE_INLINE tf2Vector4() {} TF2SIMD_FORCE_INLINE tf2Vector4(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) : Vector3(x,y,z) { m_floats[3] = w; } TF2SIMD_FORCE_INLINE tf2Vector4 absolute4() const { return tf2Vector4( tf2Fabs(m_floats[0]), tf2Fabs(m_floats[1]), tf2Fabs(m_floats[2]), tf2Fabs(m_floats[3])); } tf2Scalar getW() const { return m_floats[3];} TF2SIMD_FORCE_INLINE int maxAxis4() const { int maxIndex = -1; tf2Scalar maxVal = tf2Scalar(-TF2_LARGE_FLOAT); if (m_floats[0] > maxVal) { maxIndex = 0; maxVal = m_floats[0]; } if (m_floats[1] > maxVal) { maxIndex = 1; maxVal = m_floats[1]; } if (m_floats[2] > maxVal) { maxIndex = 2; maxVal =m_floats[2]; } if (m_floats[3] > maxVal) { maxIndex = 3; } return maxIndex; } TF2SIMD_FORCE_INLINE int minAxis4() const { int minIndex = -1; tf2Scalar minVal = tf2Scalar(TF2_LARGE_FLOAT); if (m_floats[0] < minVal) { minIndex = 0; minVal = m_floats[0]; } if (m_floats[1] < minVal) { minIndex = 1; minVal = m_floats[1]; } if (m_floats[2] < minVal) { minIndex = 2; minVal =m_floats[2]; } if (m_floats[3] < minVal) { minIndex = 3; } return minIndex; } TF2SIMD_FORCE_INLINE int closestAxis4() const { return absolute4().maxAxis4(); } /**@brief Set x,y,z and zero w * @param x Value of x * @param y Value of y * @param z Value of z */ /* void getValue(tf2Scalar *m) const { m[0] = m_floats[0]; m[1] = m_floats[1]; m[2] =m_floats[2]; } */ /**@brief Set the values * @param x Value of x * @param y Value of y * @param z Value of z * @param w Value of w */ TF2SIMD_FORCE_INLINE void setValue(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z,const tf2Scalar& w) { m_floats[0]=x; m_floats[1]=y; m_floats[2]=z; m_floats[3]=w; } }; ///tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization TF2SIMD_FORCE_INLINE void tf2SwapScalarEndian(const tf2Scalar& sourceVal, tf2Scalar& destVal) { unsigned char* dest = (unsigned char*) &destVal; const unsigned char* src = (const unsigned char*) &sourceVal; dest[0] = src[7]; dest[1] = src[6]; dest[2] = src[5]; dest[3] = src[4]; dest[4] = src[3]; dest[5] = src[2]; dest[6] = src[1]; dest[7] = src[0]; } ///tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization TF2SIMD_FORCE_INLINE void tf2SwapVector3Endian(const Vector3& sourceVec, Vector3& destVec) { for (int i=0;i<4;i++) { tf2SwapScalarEndian(sourceVec[i],destVec[i]); } } ///tf2UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization TF2SIMD_FORCE_INLINE void tf2UnSwapVector3Endian(Vector3& vector) { Vector3 swappedVec; for (int i=0;i<4;i++) { tf2SwapScalarEndian(vector[i],swappedVec[i]); } vector = swappedVec; } TF2SIMD_FORCE_INLINE void tf2PlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q) { if (tf2Fabs(n.z()) > TF2SIMDSQRT12) { // choose p in y-z plane tf2Scalar a = n[1]*n[1] + n[2]*n[2]; tf2Scalar k = tf2RecipSqrt (a); p.setValue(0,-n[2]*k,n[1]*k); // set q = n x p q.setValue(a*k,-n[0]*p[2],n[0]*p[1]); } else { // choose p in x-y plane tf2Scalar a = n.x()*n.x() + n.y()*n.y(); tf2Scalar k = tf2RecipSqrt (a); p.setValue(-n.y()*k,n.x()*k,0); // set q = n x p q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k); } } struct Vector3FloatData { float m_floats[4]; }; struct Vector3DoubleData { double m_floats[4]; }; TF2SIMD_FORCE_INLINE void Vector3::serializeFloat(struct Vector3FloatData& dataOut) const { ///could also do a memcpy, check if it is worth it for (int i=0;i<4;i++) dataOut.m_floats[i] = float(m_floats[i]); } TF2SIMD_FORCE_INLINE void Vector3::deSerializeFloat(const struct Vector3FloatData& dataIn) { for (int i=0;i<4;i++) m_floats[i] = tf2Scalar(dataIn.m_floats[i]); } TF2SIMD_FORCE_INLINE void Vector3::serializeDouble(struct Vector3DoubleData& dataOut) const { ///could also do a memcpy, check if it is worth it for (int i=0;i<4;i++) dataOut.m_floats[i] = double(m_floats[i]); } TF2SIMD_FORCE_INLINE void Vector3::deSerializeDouble(const struct Vector3DoubleData& dataIn) { for (int i=0;i<4;i++) m_floats[i] = tf2Scalar(dataIn.m_floats[i]); } TF2SIMD_FORCE_INLINE void Vector3::serialize(struct Vector3Data& dataOut) const { ///could also do a memcpy, check if it is worth it for (int i=0;i<4;i++) dataOut.m_floats[i] = m_floats[i]; } TF2SIMD_FORCE_INLINE void Vector3::deSerialize(const struct Vector3Data& dataIn) { for (int i=0;i<4;i++) m_floats[i] = dataIn.m_floats[i]; } } #endif //TF2_VECTOR3_H geometry2-0.7.5/tf2/include/tf2/buffer_core.h000066400000000000000000000451261372352374300207240ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_BUFFER_CORE_H #define TF2_BUFFER_CORE_H #include "transform_storage.h" #include #include #include "ros/duration.h" #include "ros/time.h" //#include "geometry_msgs/TwistStamped.h" #include "geometry_msgs/TransformStamped.h" //////////////////////////backwards startup for porting //#include "tf/tf.h" #include #include #include #include namespace tf2 { typedef std::pair P_TimeAndFrameID; typedef uint32_t TransformableCallbackHandle; typedef uint64_t TransformableRequestHandle; class TimeCacheInterface; typedef boost::shared_ptr TimeCacheInterfacePtr; enum TransformableResult { TransformAvailable, TransformFailure, }; /** \brief A Class which provides coordinate transforms between any two frames in a system. * * This class provides a simple interface to allow recording and lookup of * relationships between arbitrary frames of the system. * * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames. * For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head. * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand. * libTF is designed to take care of all the intermediate steps for you. * * Internal Representation * libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame. * Frames are designated using an std::string * 0 is a frame without a parent (the top of a tree) * The positions of frames over time must be pushed in. * * All function calls which pass frame ids can potentially throw the exception tf::LookupException */ class BufferCore { public: /************* Constants ***********************/ static const int DEFAULT_CACHE_TIME = 10; //!< The default amount of time to cache data in seconds static const uint32_t MAX_GRAPH_DEPTH = 1000UL; //!< Maximum graph search depth (deeper graphs will be assumed to have loops) /** Constructor * \param interpolating Whether to interpolate, if this is false the closest value will be returned * \param cache_time How long to keep a history of transforms in nanoseconds * */ BufferCore(ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME)); virtual ~BufferCore(void); /** \brief Clear all data */ void clear(); /** \brief Add transform information to the tf data structure * \param transform The transform to store * \param authority The source of the information for this transform * \param is_static Record this transform as a static transform. It will be good across all time. (This cannot be changed after the first call.) * \return True unless an error occured */ bool setTransform(const geometry_msgs::TransformStamped& transform, const std::string & authority, bool is_static = false); /*********** Accessors *************/ /** \brief Get the transform between two frames by frame ID. * \param target_frame The frame to which data should be transformed * \param source_frame The frame where the data originated * \param time The time at which the value of the transform is desired. (0 will get the latest) * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException */ geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time) const; /** \brief Get the transform between two frames by frame ID assuming fixed frame. * \param target_frame The frame to which data should be transformed * \param target_time The time to which the data should be transformed. (0 will get the latest) * \param source_frame The frame where the data originated * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) * \param fixed_frame The frame in which to assume the transform is constant in time. * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException */ geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame) const; /* \brief Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point * \param tracking_frame The frame to track * \param observation_frame The frame from which to measure the twist * \param reference_frame The reference frame in which to express the twist * \param reference_point The reference point with which to express the twist * \param reference_point_frame The frame_id in which the reference point is expressed * \param time The time at which to get the velocity * \param duration The period over which to average * \return twist The twist output * * This will compute the average velocity on the interval * (time - duration/2, time+duration/2). If that is too close to the most * recent reading, in which case it will shift the interval up to * duration/2 to prevent extrapolation. * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException * * New in geometry 1.1 */ /* geometry_msgs::Twist lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame, const tf::Point & reference_point, const std::string& reference_point_frame, const ros::Time& time, const ros::Duration& averaging_interval) const; */ /* \brief lookup the twist of the tracking frame with respect to the observational frame * * This is a simplified version of * lookupTwist with it assumed that the reference point is the * origin of the tracking frame, and the reference frame is the * observation frame. * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException * * New in geometry 1.1 */ /* geometry_msgs::Twist lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const ros::Time& time, const ros::Duration& averaging_interval) const; */ /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param source_frame The frame from which to transform * \param time The time at which to transform * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL * \return True if the transform is possible, false otherwise */ bool canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, std::string* error_msg = NULL) const; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param target_time The time into which to transform * \param source_frame The frame from which to transform * \param source_time The time from which to transform * \param fixed_frame The frame in which to treat the transform as constant in time * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL * \return True if the transform is possible, false otherwise */ bool canTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, std::string* error_msg = NULL) const; /** \brief A way to see what frames have been cached in yaml format * Useful for debugging tools */ std::string allFramesAsYAML(double current_time) const; /** Backwards compatibility for #84 */ std::string allFramesAsYAML() const; /** \brief A way to see what frames have been cached * Useful for debugging */ std::string allFramesAsString() const; typedef boost::function TransformableCallback; /// \brief Internal use only TransformableCallbackHandle addTransformableCallback(const TransformableCallback& cb); /// \brief Internal use only void removeTransformableCallback(TransformableCallbackHandle handle); /// \brief Internal use only TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time); /// \brief Internal use only void cancelTransformableRequest(TransformableRequestHandle handle); // Tell the buffer that there are multiple threads serviciing it. // This is useful for derived classes to know if they can block or not. void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;}; // Get the state of using_dedicated_thread_ bool isUsingDedicatedThread() const { return using_dedicated_thread_;}; /* Backwards compatability section for tf::Transformer you should not use these */ /** * \brief Add a callback that happens when a new transform has arrived * * \param callback The callback, of the form void func(); * \return A boost::signals2::connection object that can be used to remove this * listener */ boost::signals2::connection _addTransformsChangedListener(boost::function callback); void _removeTransformsChangedListener(boost::signals2::connection c); /**@brief Check if a frame exists in the tree * @param frame_id_str The frame id in question */ bool _frameExists(const std::string& frame_id_str) const; /**@brief Fill the parent of a frame. * @param frame_id The frame id of the frame in question * @param parent The reference to the string to fill the parent * Returns true unless "NO_PARENT" */ bool _getParent(const std::string& frame_id, ros::Time time, std::string& parent) const; /** \brief A way to get a std::vector of available frame ids */ void _getFrameStrings(std::vector& ids) const; CompactFrameID _lookupFrameNumber(const std::string& frameid_str) const { return lookupFrameNumber(frameid_str); } CompactFrameID _lookupOrInsertFrameNumber(const std::string& frameid_str) { return lookupOrInsertFrameNumber(frameid_str); } int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const { boost::mutex::scoped_lock lock(frame_mutex_); return getLatestCommonTime(target_frame, source_frame, time, error_string); } CompactFrameID _validateFrameId(const char* function_name_arg, const std::string& frame_id) const { return validateFrameId(function_name_arg, frame_id); } /**@brief Get the duration over which this transformer will cache */ ros::Duration getCacheLength() { return cache_time_;} /** \brief Backwards compatabilityA way to see what frames have been cached * Useful for debugging */ std::string _allFramesAsDot(double current_time) const; std::string _allFramesAsDot() const; /** \brief Backwards compatabilityA way to see what frames are in a chain * Useful for debugging */ void _chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame, std::vector& output) const; private: /** \brief A way to see what frames have been cached * Useful for debugging. Use this call internally. */ std::string allFramesAsStringNoLock() const; /******************** Internal Storage ****************/ /** \brief The pointers to potential frames that the tree can be made of. * The frames will be dynamically allocated at run time when set the first time. */ typedef std::vector V_TimeCacheInterface; V_TimeCacheInterface frames_; /** \brief A mutex to protect testing and allocating new frames on the above vector. */ mutable boost::mutex frame_mutex_; /** \brief A map from string frame ids to CompactFrameID */ typedef boost::unordered_map M_StringToCompactFrameID; M_StringToCompactFrameID frameIDs_; /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */ std::vector frameIDs_reverse; /** \brief A map to lookup the most recent authority for a given frame */ std::map frame_authority_; /// How long to cache transform history ros::Duration cache_time_; typedef boost::unordered_map M_TransformableCallback; M_TransformableCallback transformable_callbacks_; uint32_t transformable_callbacks_counter_; boost::mutex transformable_callbacks_mutex_; struct TransformableRequest { ros::Time time; TransformableRequestHandle request_handle; TransformableCallbackHandle cb_handle; CompactFrameID target_id; CompactFrameID source_id; std::string target_string; std::string source_string; }; typedef std::vector V_TransformableRequest; V_TransformableRequest transformable_requests_; boost::mutex transformable_requests_mutex_; uint64_t transformable_requests_counter_; struct RemoveRequestByCallback; struct RemoveRequestByID; // Backwards compatability for tf message_filter typedef boost::signals2::signal TransformsChangedSignal; /// Signal which is fired whenever new transform data has arrived, from the thread the data arrived in TransformsChangedSignal _transforms_changed_; /************************* Internal Functions ****************************/ /** \brief An accessor to get a frame, which will throw an exception if the frame is no there. * \param frame_number The frameID of the desired Reference Frame * * This is an internal function which will get the pointer to the frame associated with the frame id * Possible Exception: tf::LookupException */ TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const; TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static); bool warnFrameId(const char* function_name_arg, const std::string& frame_id) const; CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const; /// String to number for frame lookup with dynamic allocation of new frames CompactFrameID lookupFrameNumber(const std::string& frameid_str) const; /// String to number for frame lookup with dynamic allocation of new frames CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str); ///Number to string frame lookup may throw LookupException if number invalid const std::string& lookupFrameString(CompactFrameID frame_id_num) const; void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const; /**@brief Return the latest rostime which is common across the spanning set * zero if fails to cross */ int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const; template int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const; /**@brief Traverse the transform tree. If frame_chain is not NULL, store the traversed frame tree in vector frame_chain. * */ template int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const; void testTransformableRequests(); bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, const ros::Time& time, std::string* error_msg) const; bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, const ros::Time& time, std::string* error_msg) const; //Whether it is safe to use canTransform with a timeout. (If another thread is not provided it will always timeout.) bool using_dedicated_thread_; public: friend class TestBufferCore; // For unit testing }; /** A helper class for testing internal APIs */ class TestBufferCore { public: int _walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const; const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const { return buffer.lookupFrameString(frame_id_num); } }; } #endif //TF2_CORE_H geometry2-0.7.5/tf2/include/tf2/convert.h000066400000000000000000000112031372352374300201100ustar00rootroot00000000000000/* * Copyright (c) 2013, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_CONVERT_H #define TF2_CONVERT_H #include #include #include #include namespace tf2 { /**\brief The templated function expected to be able to do a transform * * This is the method which tf2 will use to try to apply a transform for any given datatype. * \param data_in The data to be transformed. * \param data_out A reference to the output data. Note this can point to data in and the method should be mutation safe. * \param transform The transform to apply to data_in to fill data_out. * * This method needs to be implemented by client library developers */ template void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform); /**\brief Get the timestamp from data * \param t The data input. * \return The timestamp associated with the data. The lifetime of the returned * reference is bound to the lifetime of the argument. */ template const ros::Time& getTimestamp(const T& t); /**\brief Get the frame_id from data * \param t The data input. * \return The frame_id associated with the data. The lifetime of the returned * reference is bound to the lifetime of the argument. */ template const std::string& getFrameId(const T& t); /* An implementation for Stamped

datatypes */ template const ros::Time& getTimestamp(const tf2::Stamped

& t) { return t.stamp_; } /* An implementation for Stamped

datatypes */ template const std::string& getFrameId(const tf2::Stamped

& t) { return t.frame_id_; } /** Function that converts from one type to a ROS message type. It has to be * implemented by each data type in tf2_* (except ROS messages) as it is * used in the "convert" function. * \param a an object of whatever type * \return the conversion as a ROS message */ template B toMsg(const A& a); /** Function that converts from a ROS message type to another type. It has to be * implemented by each data type in tf2_* (except ROS messages) as it is used * in the "convert" function. * \param a a ROS message to convert from * \param b the object to convert to */ template void fromMsg(const A&, B& b); /** Function that converts any type to any type (messages or not). * Matching toMsg and from Msg conversion functions need to exist. * If they don't exist or do not apply (for example, if your two * classes are ROS messages), just write a specialization of the function. * \param a an object to convert from * \param b the object to convert to */ template void convert(const A& a, B& b) { //printf("In double type convert\n"); impl::Converter::value, ros::message_traits::IsMessage::value>::convert(a, b); } template void convert(const A& a1, A& a2) { //printf("In single type convert\n"); if(&a1 != &a2) a2 = a1; } } #endif //TF2_CONVERT_H geometry2-0.7.5/tf2/include/tf2/exceptions.h000066400000000000000000000074561372352374300206300ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_EXCEPTIONS_H #define TF2_EXCEPTIONS_H #include namespace tf2{ /** \brief A base class for all tf2 exceptions * This inherits from ros::exception * which inherits from std::runtime_exception */ class TransformException: public std::runtime_error { public: TransformException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; }; }; /** \brief An exception class to notify of no connection * * This is an exception class to be thrown in the case * that the Reference Frame tree is not connected between * the frames requested. */ class ConnectivityException:public TransformException { public: ConnectivityException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; }; /** \brief An exception class to notify of bad frame number * * This is an exception class to be thrown in the case that * a frame not in the graph has been attempted to be accessed. * The most common reason for this is that the frame is not * being published, or a parent frame was not set correctly * causing the tree to be broken. */ class LookupException: public TransformException { public: LookupException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; }; /** \brief An exception class to notify that the requested value would have required extrapolation beyond current limits. * */ class ExtrapolationException: public TransformException { public: ExtrapolationException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; }; /** \brief An exception class to notify that one of the arguments is invalid * * usually it's an uninitalized Quaternion (0,0,0,0) * */ class InvalidArgumentException: public TransformException { public: InvalidArgumentException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; }; /** \brief An exception class to notify that a timeout has occured * * */ class TimeoutException: public TransformException { public: TimeoutException(const std::string errorDescription) : tf2::TransformException(errorDescription) { ; }; }; } #endif //TF2_EXCEPTIONS_H geometry2-0.7.5/tf2/include/tf2/impl/000077500000000000000000000000001372352374300172235ustar00rootroot00000000000000geometry2-0.7.5/tf2/include/tf2/impl/convert.h000066400000000000000000000055551372352374300210660ustar00rootroot00000000000000/* * Copyright (c) 2013, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef TF2_IMPL_CONVERT_H #define TF2_IMPL_CONVERT_H namespace tf2 { namespace impl { template class Converter { public: template static void convert(const A& a, B& b); }; // The case where both A and B are messages should not happen: if you have two // messages that are interchangeable, well, that's against the ROS purpose: // only use one type. Worst comes to worst, specialize the original convert // function for your types. // if B == A, the templated version of convert with only one argument will be // used. // //template <> //template //inline void Converter::convert(const A& a, B& b); template <> template inline void Converter::convert(const A& a, B& b) { #ifdef _MSC_VER tf2::fromMsg(a, b); #else fromMsg(a, b); #endif } template <> template inline void Converter::convert(const A& a, B& b) { #ifdef _MSC_VER b = tf2::toMsg(a); #else b = toMsg(a); #endif } template <> template inline void Converter::convert(const A& a, B& b) { #ifdef _MSC_VER tf2::fromMsg(tf2::toMsg(a), b); #else fromMsg(toMsg(a), b); #endif } } } #endif //TF2_IMPL_CONVERT_H geometry2-0.7.5/tf2/include/tf2/impl/utils.h000066400000000000000000000105471372352374300205430ustar00rootroot00000000000000// Copyright 2014 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef TF2_IMPL_UTILS_H #define TF2_IMPL_UTILS_H #include #include #include namespace tf2 { namespace impl { /** Function needed for the generalization of toQuaternion * \param q a tf2::Quaternion * \return a copy of the same quaternion */ inline tf2::Quaternion toQuaternion(const tf2::Quaternion& q) { return q; } /** Function needed for the generalization of toQuaternion * \param q a geometry_msgs::Quaternion * \return a copy of the same quaternion as a tf2::Quaternion */ inline tf2::Quaternion toQuaternion(const geometry_msgs::Quaternion& q) { tf2::Quaternion res; fromMsg(q, res); return res; } /** Function needed for the generalization of toQuaternion * \param q a geometry_msgs::QuaternionStamped * \return a copy of the same quaternion as a tf2::Quaternion */ inline tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) { tf2::Quaternion res; fromMsg(q.quaternion, res); return res; } /** Function needed for the generalization of toQuaternion * \param t some tf2::Stamped object * \return a copy of the same quaternion as a tf2::Quaternion */ template tf2::Quaternion toQuaternion(const tf2::Stamped& t) { geometry_msgs::QuaternionStamped q = toMsg(t); return toQuaternion(q); } /** Generic version of toQuaternion. It tries to convert the argument * to a geometry_msgs::Quaternion * \param t some object * \return a copy of the same quaternion as a tf2::Quaternion */ template tf2::Quaternion toQuaternion(const T& t) { geometry_msgs::Quaternion q = toMsg(t); return toQuaternion(q); } /** The code below is blantantly copied from urdfdom_headers * only the normalization has been added. * It computes the Euler roll, pitch yaw from a tf2::Quaternion * It is equivalent to tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll); * \param q a tf2::Quaternion * \param yaw the computed yaw * \param pitch the computed pitch * \param roll the computed roll */ inline void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &roll) { double sqw; double sqx; double sqy; double sqz; sqx = q.x() * q.x(); sqy = q.y() * q.y(); sqz = q.z() * q.z(); sqw = q.w() * q.w(); // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */ if (sarg <= -0.99999) { pitch = -0.5*M_PI; roll = 0; yaw = -2 * atan2(q.y(), q.x()); } else if (sarg >= 0.99999) { pitch = 0.5*M_PI; roll = 0; yaw = 2 * atan2(q.y(), q.x()); } else { pitch = asin(sarg); roll = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz); yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz); } }; /** The code below is a simplified version of getEulerRPY that only * returns the yaw. It is mostly useful in navigation where only yaw * matters * \param q a tf2::Quaternion * \return the computed yaw */ inline double getYaw(const tf2::Quaternion& q) { double yaw; double sqw; double sqx; double sqy; double sqz; sqx = q.x() * q.x(); sqy = q.y() * q.y(); sqz = q.z() * q.z(); sqw = q.w() * q.w(); // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */ if (sarg <= -0.99999) { yaw = -2 * atan2(q.y(), q.x()); } else if (sarg >= 0.99999) { yaw = 2 * atan2(q.y(), q.x()); } else { yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz); } return yaw; }; } } #endif //TF2_IMPL_UTILS_H geometry2-0.7.5/tf2/include/tf2/time_cache.h000066400000000000000000000130161372352374300205150ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_TIME_CACHE_H #define TF2_TIME_CACHE_H #include "transform_storage.h" #include #include #include #include namespace geometry_msgs { ROS_DECLARE_MESSAGE(TransformStamped); } namespace tf2 { typedef std::pair P_TimeAndFrameID; class TimeCacheInterface { public: /** \brief Access data from the cache */ virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0)=0; //returns false if data unavailable (should be thrown as lookup exception /** \brief Insert data into the cache */ virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0)=0; /** @brief Clear the list of stored values */ virtual void clearList()=0; /** \brief Retrieve the parent at a specific time */ virtual CompactFrameID getParent(ros::Time time, std::string* error_str) = 0; /** * \brief Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no data. */ virtual P_TimeAndFrameID getLatestTimeAndParent() = 0; /// Debugging information methods /** @brief Get the length of the stored list */ virtual unsigned int getListLength()=0; /** @brief Get the latest timestamp cached */ virtual ros::Time getLatestTimestamp()=0; /** @brief Get the oldest timestamp cached */ virtual ros::Time getOldestTimestamp()=0; }; typedef boost::shared_ptr TimeCacheInterfacePtr; /** \brief A class to keep a sorted linked list in time * This builds and maintains a list of timestamped * data. And provides lookup functions to get * data out as a function of time. */ class TimeCache : public TimeCacheInterface { public: static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below. static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; //!< Maximum length of linked list, to make sure not to be able to use unlimited memory. static const int64_t DEFAULT_MAX_STORAGE_TIME = 10ULL * 1000000000LL; //!< default value of 10 seconds storage TimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME)); /// Virtual methods virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0); virtual void clearList(); virtual CompactFrameID getParent(ros::Time time, std::string* error_str); virtual P_TimeAndFrameID getLatestTimeAndParent(); /// Debugging information methods virtual unsigned int getListLength(); virtual ros::Time getLatestTimestamp(); virtual ros::Time getOldestTimestamp(); private: typedef std::deque L_TransformStorage; L_TransformStorage storage_; ros::Duration max_storage_time_; /// A helper function for getData //Assumes storage is already locked for it inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str); inline void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output); void pruneList(); }; class StaticCache : public TimeCacheInterface { public: /// Virtual methods virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0); virtual void clearList(); virtual CompactFrameID getParent(ros::Time time, std::string* error_str); virtual P_TimeAndFrameID getLatestTimeAndParent(); /// Debugging information methods virtual unsigned int getListLength(); virtual ros::Time getLatestTimestamp(); virtual ros::Time getOldestTimestamp(); private: TransformStorage storage_; }; } #endif // TF2_TIME_CACHE_H geometry2-0.7.5/tf2/include/tf2/transform_datatypes.h000066400000000000000000000060641372352374300225320ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_TRANSFORM_DATATYPES_H #define TF2_TRANSFORM_DATATYPES_H #include #include "ros/time.h" namespace tf2 { /** \brief The data type which will be cross compatable with geometry_msgs * This is the tf2 datatype equivilant of a MessageStamped */ template class Stamped : public T{ public: ros::Time stamp_; ///< The timestamp associated with this data std::string frame_id_; ///< The frame_id associated this data /** Default constructor */ Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation /** Full constructor */ Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id) : T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ; /** Copy Constructor */ Stamped(const Stamped& s): T (s), stamp_(s.stamp_), frame_id_(s.frame_id_) {} /** Copy assignment operator */ Stamped & operator=(const Stamped & rhs) { T::operator=(rhs); stamp_ = rhs.stamp_; frame_id_ = rhs.frame_id_; return *this; } /** Set the data element */ void setData(const T& input){*static_cast(this) = input;}; }; /** \brief Comparison Operator for Stamped datatypes */ template bool operator==(const Stamped &a, const Stamped &b) { return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast(a) == static_cast(b); } } #endif //TF2_TRANSFORM_DATATYPES_H geometry2-0.7.5/tf2/include/tf2/transform_storage.h000066400000000000000000000052541372352374300222000ustar00rootroot00000000000000/* * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_TRANSFORM_STORAGE_H #define TF2_TRANSFORM_STORAGE_H #include #include #include #include #include namespace geometry_msgs { ROS_DECLARE_MESSAGE(TransformStamped) } namespace tf2 { typedef uint32_t CompactFrameID; /** \brief Storage for transforms and their parent */ class TransformStorage { public: TransformStorage(); TransformStorage(const geometry_msgs::TransformStamped& data, CompactFrameID frame_id, CompactFrameID child_frame_id); TransformStorage(const TransformStorage& rhs) { *this = rhs; } TransformStorage& operator=(const TransformStorage& rhs) { #if 01 rotation_ = rhs.rotation_; translation_ = rhs.translation_; stamp_ = rhs.stamp_; frame_id_ = rhs.frame_id_; child_frame_id_ = rhs.child_frame_id_; #endif return *this; } tf2::Quaternion rotation_; tf2::Vector3 translation_; ros::Time stamp_; CompactFrameID frame_id_; CompactFrameID child_frame_id_; }; } #endif // TF2_TRANSFORM_STORAGE_H geometry2-0.7.5/tf2/include/tf2/utils.h000066400000000000000000000040341372352374300175740ustar00rootroot00000000000000// Copyright 2014 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #ifndef TF2_UTILS_H #define TF2_UTILS_H #include #include #include namespace tf2 { /** Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h * \param a the object to get data from (it represents a rotation/quaternion) * \param yaw yaw * \param pitch pitch * \param roll roll */ template void getEulerYPR(const A& a, double& yaw, double& pitch, double& roll) { tf2::Quaternion q = impl::toQuaternion(a); impl::getEulerYPR(q, yaw, pitch, roll); } /** Return the yaw of anything that can be converted to a tf2::Quaternion * The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h * This function is a specialization of getEulerYPR and is useful for its * wide-spread use in navigation * \param a the object to get data from (it represents a rotation/quaternion) * \param yaw yaw */ template double getYaw(const A& a) { tf2::Quaternion q = impl::toQuaternion(a); return impl::getYaw(q); } /** Return the identity for any type that can be converted to a tf2::Transform * \return an object of class A that is an identity transform */ template A getTransformIdentity() { tf2::Transform t; t.setIdentity(); A a; convert(t, a); return a; } } #endif //TF2_UTILS_H geometry2-0.7.5/tf2/index.rst000066400000000000000000000002631372352374300160060ustar00rootroot00000000000000tf2 ===== This is the Python API reference of the tf2 package. .. toctree:: :maxdepth: 2 tf2 Indices and tables ================== * :ref:`genindex` * :ref:`search` geometry2-0.7.5/tf2/mainpage.dox000066400000000000000000000021241372352374300164400ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2 is the second generation of the tf library. This library implements the interface defined by tf2::BufferCore. There is also a Python wrapper with the same API that class this library using CPython bindings. \section codeapi Code API The main interface is through the tf2::BufferCore interface. It uses the exceptions in exceptions.h and the Stamped datatype in transform_datatypes.h. \section conversions Conversion Interface tf2 offers a templated conversion interface for external libraries to specify conversions between tf2-specific data types and user-defined data types. Various templated functions in tf2_ros use the conversion interface to apply transformations from the tf server to these custom datatypes. The conversion interface is defined in tf2/convert.h. Some packages that implement this interface: - tf2_bullet - tf2_eigen - tf2_geometry_msgs - tf2_kdl - tf2_sensor_msgs More documentation for the conversion interface is available on the ROS Wiki. */ geometry2-0.7.5/tf2/package.xml000066400000000000000000000021541372352374300162630ustar00rootroot00000000000000 tf2 0.7.5 tf2 is the second generation of the transform library, which lets the user keep track of multiple coordinate frames over time. tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time. Tully Foote Eitan Marder-Eppstein Wim Meeussen Tully Foote BSD http://www.ros.org/wiki/tf2 catkin libconsole-bridge-dev geometry_msgs rostime tf2_msgs libconsole-bridge-dev geometry_msgs rostime tf2_msgs geometry2-0.7.5/tf2/src/000077500000000000000000000000001372352374300147335ustar00rootroot00000000000000geometry2-0.7.5/tf2/src/buffer_core.cpp000066400000000000000000001443171372352374300177320ustar00rootroot00000000000000/* * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "tf2/buffer_core.h" #include "tf2/time_cache.h" #include "tf2/exceptions.h" #include "tf2_msgs/TF2Error.h" #include #include #include "tf2/LinearMath/Transform.h" #include namespace tf2 { // Tolerance for acceptable quaternion normalization static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3; /** \brief convert Transform msg to Transform */ void transformMsgToTF2(const geometry_msgs::Transform& msg, tf2::Transform& tf2) {tf2 = tf2::Transform(tf2::Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), tf2::Vector3(msg.translation.x, msg.translation.y, msg.translation.z));} /** \brief convert Transform to Transform msg*/ void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::Transform& msg) { msg.translation.x = tf2.getOrigin().x(); msg.translation.y = tf2.getOrigin().y(); msg.translation.z = tf2.getOrigin().z(); msg.rotation.x = tf2.getRotation().x(); msg.rotation.y = tf2.getRotation().y(); msg.rotation.z = tf2.getRotation().z(); msg.rotation.w = tf2.getRotation().w(); } /** \brief convert Transform to Transform msg*/ void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id) { transformTF2ToMsg(tf2, msg.transform); msg.header.stamp = stamp; msg.header.frame_id = frame_id; msg.child_frame_id = child_frame_id; } void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::Transform& msg) { msg.translation.x = pos.x(); msg.translation.y = pos.y(); msg.translation.z = pos.z(); msg.rotation.x = orient.x(); msg.rotation.y = orient.y(); msg.rotation.z = orient.z(); msg.rotation.w = orient.w(); } void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id) { transformTF2ToMsg(orient, pos, msg.transform); msg.header.stamp = stamp; msg.header.frame_id = frame_id; msg.child_frame_id = child_frame_id; } void setIdentity(geometry_msgs::Transform& tx) { tx.translation.x = 0; tx.translation.y = 0; tx.translation.z = 0; tx.rotation.x = 0; tx.rotation.y = 0; tx.rotation.z = 0; tx.rotation.w = 1; } bool startsWithSlash(const std::string& frame_id) { if (frame_id.size() > 0) if (frame_id[0] == '/') return true; return false; } std::string stripSlash(const std::string& in) { std::string out = in; if (startsWithSlash(in)) out.erase(0,1); return out; } bool BufferCore::warnFrameId(const char* function_name_arg, const std::string& frame_id) const { if (frame_id.size() == 0) { std::stringstream ss; ss << "Invalid argument passed to "<< function_name_arg <<" in tf2 frame_ids cannot be empty"; CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str()); return true; } if (startsWithSlash(frame_id)) { std::stringstream ss; ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf2 frame_ids cannot start with a '/' like: "; CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str()); return true; } return false; } CompactFrameID BufferCore::validateFrameId(const char* function_name_arg, const std::string& frame_id) const { if (frame_id.empty()) { std::stringstream ss; ss << "Invalid argument passed to "<< function_name_arg <<" in tf2 frame_ids cannot be empty"; throw tf2::InvalidArgumentException(ss.str().c_str()); } if (startsWithSlash(frame_id)) { std::stringstream ss; ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf2 frame_ids cannot start with a '/' like: "; throw tf2::InvalidArgumentException(ss.str().c_str()); } CompactFrameID id = lookupFrameNumber(frame_id); if (id == 0) { std::stringstream ss; ss << "\"" << frame_id << "\" passed to "<< function_name_arg <<" does not exist. "; throw tf2::LookupException(ss.str().c_str()); } return id; } BufferCore::BufferCore(ros::Duration cache_time) : cache_time_(cache_time) , transformable_callbacks_counter_(0) , transformable_requests_counter_(0) , using_dedicated_thread_(false) { frameIDs_["NO_PARENT"] = 0; frames_.push_back(TimeCacheInterfacePtr()); frameIDs_reverse.push_back("NO_PARENT"); } BufferCore::~BufferCore() { } void BufferCore::clear() { //old_tf_.clear(); boost::mutex::scoped_lock lock(frame_mutex_); if ( frames_.size() > 1 ) { for (std::vector::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it) { if (*cache_it) (*cache_it)->clearList(); } } } bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_in, const std::string& authority, bool is_static) { /////BACKEARDS COMPATABILITY /* tf::StampedTransform tf_transform; tf::transformStampedMsgToTF(transform_in, tf_transform); if (!old_tf_.setTransform(tf_transform, authority)) { printf("Warning old setTransform Failed but was not caught\n"); }*/ /////// New implementation geometry_msgs::TransformStamped stripped = transform_in; stripped.header.frame_id = stripSlash(stripped.header.frame_id); stripped.child_frame_id = stripSlash(stripped.child_frame_id); bool error_exists = false; if (stripped.child_frame_id == stripped.header.frame_id) { CONSOLE_BRIDGE_logError("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id \"%s\" because they are the same", authority.c_str(), stripped.child_frame_id.c_str()); error_exists = true; } if (stripped.child_frame_id == "") { CONSOLE_BRIDGE_logError("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str()); error_exists = true; } if (stripped.header.frame_id == "") { CONSOLE_BRIDGE_logError("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", stripped.child_frame_id.c_str(), authority.c_str()); error_exists = true; } if (std::isnan(stripped.transform.translation.x) || std::isnan(stripped.transform.translation.y) || std::isnan(stripped.transform.translation.z)|| std::isnan(stripped.transform.rotation.x) || std::isnan(stripped.transform.rotation.y) || std::isnan(stripped.transform.rotation.z) || std::isnan(stripped.transform.rotation.w)) { CONSOLE_BRIDGE_logError("TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)", stripped.child_frame_id.c_str(), authority.c_str(), stripped.transform.translation.x, stripped.transform.translation.y, stripped.transform.translation.z, stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w ); error_exists = true; } bool valid = std::abs((stripped.transform.rotation.w * stripped.transform.rotation.w + stripped.transform.rotation.x * stripped.transform.rotation.x + stripped.transform.rotation.y * stripped.transform.rotation.y + stripped.transform.rotation.z * stripped.transform.rotation.z) - 1.0f) < QUATERNION_NORMALIZATION_TOLERANCE; if (!valid) { CONSOLE_BRIDGE_logError("TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of an invalid quaternion in the transform (%f %f %f %f)", stripped.child_frame_id.c_str(), authority.c_str(), stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w); error_exists = true; } if (error_exists) return false; { boost::mutex::scoped_lock lock(frame_mutex_); CompactFrameID frame_number = lookupOrInsertFrameNumber(stripped.child_frame_id); TimeCacheInterfacePtr frame = getFrame(frame_number); if (frame == NULL) frame = allocateFrame(frame_number, is_static); std::string error_string; if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number), &error_string)) { frame_authority_[frame_number] = authority; } else { CONSOLE_BRIDGE_logWarn((error_string+" for frame %s at time %lf according to authority %s").c_str(), stripped.child_frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str()); return false; } } testTransformableRequests(); return true; } TimeCacheInterfacePtr BufferCore::allocateFrame(CompactFrameID cfid, bool is_static) { TimeCacheInterfacePtr frame_ptr = frames_[cfid]; if (is_static) { frames_[cfid] = TimeCacheInterfacePtr(new StaticCache()); } else { frames_[cfid] = TimeCacheInterfacePtr(new TimeCache(cache_time_)); } return frames_[cfid]; } enum WalkEnding { Identity, TargetParentOfSource, SourceParentOfTarget, FullPath, }; // TODO for Jade: Merge walkToTopParent functions; this is now a stub to preserve ABI template int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const { return walkToTopParent(f, time, target_id, source_id, error_string, NULL); } template int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const { if (frame_chain) frame_chain->clear(); // Short circuit if zero length transform to allow lookups on non existant links if (source_id == target_id) { f.finalize(Identity, time); return tf2_msgs::TF2Error::NO_ERROR; } //If getting the latest get the latest common time if (time == ros::Time()) { int retval = getLatestCommonTime(target_id, source_id, time, error_string); if (retval != tf2_msgs::TF2Error::NO_ERROR) { return retval; } } // Walk the tree to its root from the source frame, accumulating the transform CompactFrameID frame = source_id; CompactFrameID top_parent = frame; uint32_t depth = 0; std::string extrapolation_error_string; bool extrapolation_might_have_occurred = false; while (frame != 0) { TimeCacheInterfacePtr cache = getFrame(frame); if (frame_chain) frame_chain->push_back(frame); if (!cache) { // There will be no cache for the very root of the tree top_parent = frame; break; } CompactFrameID parent = f.gather(cache, time, error_string ? &extrapolation_error_string : NULL); if (parent == 0) { // Just break out here... there may still be a path from source -> target top_parent = frame; extrapolation_might_have_occurred = true; break; } // Early out... target frame is a direct parent of the source frame if (frame == target_id) { f.finalize(TargetParentOfSource, time); return tf2_msgs::TF2Error::NO_ERROR; } f.accum(true); top_parent = frame; frame = parent; ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_string) { std::stringstream ss; ss << "The tf tree is invalid because it contains a loop." << std::endl << allFramesAsStringNoLock() << std::endl; *error_string = ss.str(); } return tf2_msgs::TF2Error::LOOKUP_ERROR; } } // Now walk to the top parent from the target frame, accumulating its transform frame = target_id; depth = 0; std::vector reverse_frame_chain; while (frame != top_parent) { TimeCacheInterfacePtr cache = getFrame(frame); if (frame_chain) reverse_frame_chain.push_back(frame); if (!cache) { break; } CompactFrameID parent = f.gather(cache, time, error_string); if (parent == 0) { if (error_string) { // optimize performance by not using stringstream char str[1000]; snprintf(str, sizeof(str), "%s, when looking up transform from frame [%s] to frame [%s]", error_string->c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str()); *error_string = str; } return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR; } // Early out... source frame is a direct parent of the target frame if (frame == source_id) { f.finalize(SourceParentOfTarget, time); if (frame_chain) { // Use the walk we just did frame_chain->swap(reverse_frame_chain); // Reverse it before returning because this is the reverse walk. std::reverse(frame_chain->begin(), frame_chain->end()); } return tf2_msgs::TF2Error::NO_ERROR; } f.accum(false); frame = parent; ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_string) { std::stringstream ss; ss << "The tf tree is invalid because it contains a loop." << std::endl << allFramesAsStringNoLock() << std::endl; *error_string = ss.str(); } return tf2_msgs::TF2Error::LOOKUP_ERROR; } } if (frame != top_parent) { if (extrapolation_might_have_occurred) { if (error_string) { // optimize performance by not using stringstream char str[1000]; snprintf(str, sizeof(str), "%s, when looking up transform from frame [%s] to frame [%s]", extrapolation_error_string.c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str()); *error_string = str; } return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR; } createConnectivityErrorString(source_id, target_id, error_string); return tf2_msgs::TF2Error::CONNECTIVITY_ERROR; } else if (frame_chain){ // append top_parent to reverse_frame_chain for easier matching/trimming reverse_frame_chain.push_back(frame); } f.finalize(FullPath, time); if (frame_chain) { // Pruning: Compare the chains starting at the parent (end) until they differ int m = reverse_frame_chain.size()-1; int n = frame_chain->size()-1; for (; m >= 0 && n >= 0; --m, --n) { if ((*frame_chain)[n] != reverse_frame_chain[m]) { break; } } // Erase all duplicate items from frame_chain if (n > 0) { // N is offset by 1 and leave the common parent for this result frame_chain->erase(frame_chain->begin() + (n + 2), frame_chain->end()); } if (m < reverse_frame_chain.size()) { for (int i = m; i >= 0; --i) { frame_chain->push_back(reverse_frame_chain[i]); } } } return tf2_msgs::TF2Error::NO_ERROR; } struct TransformAccum { TransformAccum() : source_to_top_quat(0.0, 0.0, 0.0, 1.0) , source_to_top_vec(0.0, 0.0, 0.0) , target_to_top_quat(0.0, 0.0, 0.0, 1.0) , target_to_top_vec(0.0, 0.0, 0.0) , result_quat(0.0, 0.0, 0.0, 1.0) , result_vec(0.0, 0.0, 0.0) { } CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string) { if (!cache->getData(time, st, error_string)) { return 0; } return st.frame_id_; } void accum(bool source) { if (source) { source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_; source_to_top_quat = st.rotation_ * source_to_top_quat; } else { target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_; target_to_top_quat = st.rotation_ * target_to_top_quat; } } void finalize(WalkEnding end, ros::Time _time) { switch (end) { case Identity: break; case TargetParentOfSource: result_vec = source_to_top_vec; result_quat = source_to_top_quat; break; case SourceParentOfTarget: { tf2::Quaternion inv_target_quat = target_to_top_quat.inverse(); tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); result_vec = inv_target_vec; result_quat = inv_target_quat; break; } case FullPath: { tf2::Quaternion inv_target_quat = target_to_top_quat.inverse(); tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec; result_quat = inv_target_quat * source_to_top_quat; } break; }; time = _time; } TransformStorage st; ros::Time time; tf2::Quaternion source_to_top_quat; tf2::Vector3 source_to_top_vec; tf2::Quaternion target_to_top_quat; tf2::Vector3 target_to_top_vec; tf2::Quaternion result_quat; tf2::Vector3 result_vec; }; geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time) const { boost::mutex::scoped_lock lock(frame_mutex_); if (target_frame == source_frame) { geometry_msgs::TransformStamped identity; identity.header.frame_id = target_frame; identity.child_frame_id = source_frame; identity.transform.rotation.w = 1; if (time == ros::Time()) { CompactFrameID target_id = lookupFrameNumber(target_frame); TimeCacheInterfacePtr cache = getFrame(target_id); if (cache) identity.header.stamp = cache->getLatestTimestamp(); else identity.header.stamp = time; } else identity.header.stamp = time; return identity; } //Identify case does not need to be validated above CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame); CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame); std::string error_string; TransformAccum accum; int retval = walkToTopParent(accum, time, target_id, source_id, &error_string); if (retval != tf2_msgs::TF2Error::NO_ERROR) { switch (retval) { case tf2_msgs::TF2Error::CONNECTIVITY_ERROR: throw ConnectivityException(error_string); case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR: throw ExtrapolationException(error_string); case tf2_msgs::TF2Error::LOOKUP_ERROR: throw LookupException(error_string); default: CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); assert(0); } } geometry_msgs::TransformStamped output_transform; transformTF2ToMsg(accum.result_quat, accum.result_vec, output_transform, accum.time, target_frame, source_frame); return output_transform; } geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame) const { validateFrameId("lookupTransform argument target_frame", target_frame); validateFrameId("lookupTransform argument source_frame", source_frame); validateFrameId("lookupTransform argument fixed_frame", fixed_frame); geometry_msgs::TransformStamped output; geometry_msgs::TransformStamped temp1 = lookupTransform(fixed_frame, source_frame, source_time); geometry_msgs::TransformStamped temp2 = lookupTransform(target_frame, fixed_frame, target_time); tf2::Transform tf1, tf2; transformMsgToTF2(temp1.transform, tf1); transformMsgToTF2(temp2.transform, tf2); transformTF2ToMsg(tf2*tf1, output.transform); output.header.stamp = temp2.header.stamp; output.header.frame_id = target_frame; output.child_frame_id = source_frame; return output; } /* geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const ros::Time& time, const ros::Duration& averaging_interval) const { try { geometry_msgs::Twist t; old_tf_.lookupTwist(tracking_frame, observation_frame, time, averaging_interval, t); return t; } catch (tf::LookupException& ex) { throw tf2::LookupException(ex.what()); } catch (tf::ConnectivityException& ex) { throw tf2::ConnectivityException(ex.what()); } catch (tf::ExtrapolationException& ex) { throw tf2::ExtrapolationException(ex.what()); } catch (tf::InvalidArgument& ex) { throw tf2::InvalidArgumentException(ex.what()); } } geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame, const tf2::Point & reference_point, const std::string& reference_point_frame, const ros::Time& time, const ros::Duration& averaging_interval) const { try{ geometry_msgs::Twist t; old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame, time, averaging_interval, t); return t; } catch (tf::LookupException& ex) { throw tf2::LookupException(ex.what()); } catch (tf::ConnectivityException& ex) { throw tf2::ConnectivityException(ex.what()); } catch (tf::ExtrapolationException& ex) { throw tf2::ExtrapolationException(ex.what()); } catch (tf::InvalidArgument& ex) { throw tf2::InvalidArgumentException(ex.what()); } } */ struct CanTransformAccum { CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string) { return cache->getParent(time, error_string); } void accum(bool source) { } void finalize(WalkEnding end, ros::Time _time) { } TransformStorage st; }; bool BufferCore::canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, const ros::Time& time, std::string* error_msg) const { if (target_id == 0 || source_id == 0) { if (error_msg) { if (target_id == 0) { *error_msg += std::string("target_frame: " + lookupFrameString(target_id ) + " does not exist."); } if (source_id == 0) { if (target_id == 0) { *error_msg += std::string(" "); } *error_msg += std::string("source_frame: " + lookupFrameString(source_id) + " " + lookupFrameString(source_id ) + " does not exist."); } } return false; } if (target_id == source_id) { return true; } CanTransformAccum accum; if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf2_msgs::TF2Error::NO_ERROR) { return true; } return false; } bool BufferCore::canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, const ros::Time& time, std::string* error_msg) const { boost::mutex::scoped_lock lock(frame_mutex_); return canTransformNoLock(target_id, source_id, time, error_msg); } bool BufferCore::canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, std::string* error_msg) const { // Short circuit if target_frame == source_frame if (target_frame == source_frame) return true; if (warnFrameId("canTransform argument target_frame", target_frame)) return false; if (warnFrameId("canTransform argument source_frame", source_frame)) return false; boost::mutex::scoped_lock lock(frame_mutex_); CompactFrameID target_id = lookupFrameNumber(target_frame); CompactFrameID source_id = lookupFrameNumber(source_frame); if (target_id == 0 || source_id == 0) { if (error_msg) { if (target_id == 0) { *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist."); } if (source_id == 0) { if (target_id == 0) { *error_msg += std::string(" "); } *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist."); } } return false; } return canTransformNoLock(target_id, source_id, time, error_msg); } bool BufferCore::canTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, std::string* error_msg) const { if (warnFrameId("canTransform argument target_frame", target_frame)) return false; if (warnFrameId("canTransform argument source_frame", source_frame)) return false; if (warnFrameId("canTransform argument fixed_frame", fixed_frame)) return false; boost::mutex::scoped_lock lock(frame_mutex_); CompactFrameID target_id = lookupFrameNumber(target_frame); CompactFrameID source_id = lookupFrameNumber(source_frame); CompactFrameID fixed_id = lookupFrameNumber(fixed_frame); if (target_id == 0 || source_id == 0 || fixed_id == 0) { if (error_msg) { if (target_id == 0) { *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist."); } if (source_id == 0) { if (target_id == 0) { *error_msg += std::string(" "); } *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist."); } if (source_id == 0) { if (target_id == 0 || source_id == 0) { *error_msg += std::string(" "); } *error_msg += std::string("fixed_frame: " + fixed_frame + "does not exist."); } } return false; } return canTransformNoLock(target_id, fixed_id, target_time, error_msg) && canTransformNoLock(fixed_id, source_id, source_time, error_msg); } tf2::TimeCacheInterfacePtr BufferCore::getFrame(CompactFrameID frame_id) const { if (frame_id >= frames_.size()) return TimeCacheInterfacePtr(); else { return frames_[frame_id]; } } CompactFrameID BufferCore::lookupFrameNumber(const std::string& frameid_str) const { CompactFrameID retval; M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str); if (map_it == frameIDs_.end()) { retval = CompactFrameID(0); } else retval = map_it->second; return retval; } CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string& frameid_str) { CompactFrameID retval = 0; M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str); if (map_it == frameIDs_.end()) { retval = CompactFrameID(frames_.size()); frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration frameIDs_[frameid_str] = retval; frameIDs_reverse.push_back(frameid_str); } else retval = frameIDs_[frameid_str]; return retval; } const std::string& BufferCore::lookupFrameString(CompactFrameID frame_id_num) const { if (frame_id_num >= frameIDs_reverse.size()) { std::stringstream ss; ss << "Reverse lookup of frame id " << frame_id_num << " failed!"; throw tf2::LookupException(ss.str()); } else return frameIDs_reverse[frame_id_num]; } void BufferCore::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const { if (!out) { return; } *out = std::string("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+ lookupFrameString(source_frame)+"' because they are not part of the same tree."+ "Tf has two or more unconnected trees."); } std::string BufferCore::allFramesAsString() const { boost::mutex::scoped_lock lock(frame_mutex_); return this->allFramesAsStringNoLock(); } std::string BufferCore::allFramesAsStringNoLock() const { std::stringstream mstream; TransformStorage temp; // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) ///regular transforms for (unsigned int counter = 1; counter < frames_.size(); counter ++) { TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter)); if (frame_ptr == NULL) continue; CompactFrameID frame_id_num; if( frame_ptr->getData(ros::Time(), temp)) frame_id_num = temp.frame_id_; else { frame_id_num = 0; } mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <getLatestTimestamp(); else time = ros::Time(); return tf2_msgs::TF2Error::NO_ERROR; } std::vector lct_cache; // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time // in the target is a direct parent CompactFrameID frame = source_id; P_TimeAndFrameID temp; uint32_t depth = 0; ros::Time common_time = ros::TIME_MAX; while (frame != 0) { TimeCacheInterfacePtr cache = getFrame(frame); if (!cache) { // There will be no cache for the very root of the tree break; } P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); if (latest.second == 0) { // Just break out here... there may still be a path from source -> target break; } if (!latest.first.isZero()) { common_time = std::min(latest.first, common_time); } lct_cache.push_back(latest); frame = latest.second; // Early out... target frame is a direct parent of the source frame if (frame == target_id) { time = common_time; if (time == ros::TIME_MAX) { time = ros::Time(); } return tf2_msgs::TF2Error::NO_ERROR; } ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_string) { std::stringstream ss; ss<<"The tf tree is invalid because it contains a loop." << std::endl << allFramesAsStringNoLock() << std::endl; *error_string = ss.str(); } return tf2_msgs::TF2Error::LOOKUP_ERROR; } } // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent frame = target_id; depth = 0; common_time = ros::TIME_MAX; CompactFrameID common_parent = 0; while (true) { TimeCacheInterfacePtr cache = getFrame(frame); if (!cache) { break; } P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); if (latest.second == 0) { break; } if (!latest.first.isZero()) { common_time = std::min(latest.first, common_time); } std::vector::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second)); if (it != lct_cache.end()) // found a common parent { common_parent = it->second; break; } frame = latest.second; // Early out... source frame is a direct parent of the target frame if (frame == source_id) { time = common_time; if (time == ros::TIME_MAX) { time = ros::Time(); } return tf2_msgs::TF2Error::NO_ERROR; } ++depth; if (depth > MAX_GRAPH_DEPTH) { if (error_string) { std::stringstream ss; ss<<"The tf tree is invalid because it contains a loop." << std::endl << allFramesAsStringNoLock() << std::endl; *error_string = ss.str(); } return tf2_msgs::TF2Error::LOOKUP_ERROR; } } if (common_parent == 0) { createConnectivityErrorString(source_id, target_id, error_string); return tf2_msgs::TF2Error::CONNECTIVITY_ERROR; } // Loop through the source -> root list until we hit the common parent { std::vector::iterator it = lct_cache.begin(); std::vector::iterator end = lct_cache.end(); for (; it != end; ++it) { if (!it->first.isZero()) { common_time = std::min(common_time, it->first); } if (it->second == common_parent) { break; } } } if (common_time == ros::TIME_MAX) { common_time = ros::Time(); } time = common_time; return tf2_msgs::TF2Error::NO_ERROR; } std::string BufferCore::allFramesAsYAML(double current_time) const { std::stringstream mstream; boost::mutex::scoped_lock lock(frame_mutex_); TransformStorage temp; if (frames_.size() ==1) mstream <<"{}"; mstream.precision(3); mstream.setf(std::ios::fixed,std::ios::floatfield); // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame { CompactFrameID cfid = CompactFrameID(counter); CompactFrameID frame_id_num; TimeCacheInterfacePtr cache = getFrame(cfid); if (!cache) { continue; } if(!cache->getData(ros::Time(), temp)) { continue; } frame_id_num = temp.frame_id_; std::string authority = "no recorded authority"; std::map::const_iterator it = frame_authority_.find(cfid); if (it != frame_authority_.end()) { authority = it->second; } double rate = cache->getListLength() / std::max((cache->getLatestTimestamp().toSec() - cache->getOldestTimestamp().toSec() ), 0.0001); mstream << std::fixed; //fixed point notation mstream.precision(3); //3 decimal places mstream << frameIDs_reverse[cfid] << ": " << std::endl; mstream << " parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl; mstream << " broadcaster: '" << authority << "'" << std::endl; mstream << " rate: " << rate << std::endl; mstream << " most_recent_transform: " << (cache->getLatestTimestamp()).toSec() << std::endl; mstream << " oldest_transform: " << (cache->getOldestTimestamp()).toSec() << std::endl; if ( current_time > 0 ) { mstream << " transform_delay: " << current_time - cache->getLatestTimestamp().toSec() << std::endl; } mstream << " buffer_length: " << (cache->getLatestTimestamp() - cache->getOldestTimestamp()).toSec() << std::endl; } return mstream.str(); } std::string BufferCore::allFramesAsYAML() const { return this->allFramesAsYAML(0.0); } TransformableCallbackHandle BufferCore::addTransformableCallback(const TransformableCallback& cb) { boost::mutex::scoped_lock lock(transformable_callbacks_mutex_); TransformableCallbackHandle handle = ++transformable_callbacks_counter_; while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second) { handle = ++transformable_callbacks_counter_; } return handle; } struct BufferCore::RemoveRequestByCallback { RemoveRequestByCallback(TransformableCallbackHandle handle) : handle_(handle) {} bool operator()(const TransformableRequest& req) { return req.cb_handle == handle_; } TransformableCallbackHandle handle_; }; void BufferCore::removeTransformableCallback(TransformableCallbackHandle handle) { { boost::mutex::scoped_lock lock(transformable_callbacks_mutex_); transformable_callbacks_.erase(handle); } { boost::mutex::scoped_lock lock(transformable_requests_mutex_); std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle)); } } TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time) { // shortcut if target == source if (target_frame == source_frame) { return 0; } TransformableRequest req; req.target_id = lookupFrameNumber(target_frame); req.source_id = lookupFrameNumber(source_frame); // First check if the request is already transformable. If it is, return immediately if (canTransformInternal(req.target_id, req.source_id, time, 0)) { return 0; } // Might not be transformable at all, ever (if it's too far in the past) if (req.target_id && req.source_id) { ros::Time latest_time; // TODO: This is incorrect, but better than nothing. Really we want the latest time for // any of the frames getLatestCommonTime(req.target_id, req.source_id, latest_time, 0); if (!latest_time.isZero() && time + cache_time_ < latest_time) { return 0xffffffffffffffffULL; } } req.cb_handle = handle; req.time = time; req.request_handle = ++transformable_requests_counter_; if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL) { req.request_handle = 1; } if (req.target_id == 0) { req.target_string = target_frame; } if (req.source_id == 0) { req.source_string = source_frame; } boost::mutex::scoped_lock lock(transformable_requests_mutex_); transformable_requests_.push_back(req); return req.request_handle; } struct BufferCore::RemoveRequestByID { RemoveRequestByID(TransformableRequestHandle handle) : handle_(handle) {} bool operator()(const TransformableRequest& req) { return req.request_handle == handle_; } TransformableCallbackHandle handle_; }; void BufferCore::cancelTransformableRequest(TransformableRequestHandle handle) { boost::mutex::scoped_lock lock(transformable_requests_mutex_); std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle)); } // backwards compability for tf methods boost::signals2::connection BufferCore::_addTransformsChangedListener(boost::function callback) { boost::mutex::scoped_lock lock(transformable_requests_mutex_); return _transforms_changed_.connect(callback); } void BufferCore::_removeTransformsChangedListener(boost::signals2::connection c) { boost::mutex::scoped_lock lock(transformable_requests_mutex_); c.disconnect(); } bool BufferCore::_frameExists(const std::string& frame_id_str) const { boost::mutex::scoped_lock lock(frame_mutex_); return frameIDs_.count(frame_id_str); } bool BufferCore::_getParent(const std::string& frame_id, ros::Time time, std::string& parent) const { boost::mutex::scoped_lock lock(frame_mutex_); CompactFrameID frame_number = lookupFrameNumber(frame_id); TimeCacheInterfacePtr frame = getFrame(frame_number); if (! frame) return false; CompactFrameID parent_id = frame->getParent(time, NULL); if (parent_id == 0) return false; parent = lookupFrameString(parent_id); return true; }; void BufferCore::_getFrameStrings(std::vector & vec) const { vec.clear(); boost::mutex::scoped_lock lock(frame_mutex_); TransformStorage temp; // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter ++) { vec.push_back(frameIDs_reverse[counter]); } return; } void BufferCore::testTransformableRequests() { boost::mutex::scoped_lock lock(transformable_requests_mutex_); V_TransformableRequest::iterator it = transformable_requests_.begin(); typedef boost::tuple TransformableTuple; std::vector transformables; for (; it != transformable_requests_.end();) { TransformableRequest& req = *it; // One or both of the frames may not have existed when the request was originally made. if (req.target_id == 0) { req.target_id = lookupFrameNumber(req.target_string); } if (req.source_id == 0) { req.source_id = lookupFrameNumber(req.source_string); } ros::Time latest_time; bool do_cb = false; TransformableResult result = TransformAvailable; // TODO: This is incorrect, but better than nothing. Really we want the latest time for // any of the frames getLatestCommonTime(req.target_id, req.source_id, latest_time, 0); if (!latest_time.isZero() && req.time + cache_time_ < latest_time) { do_cb = true; result = TransformFailure; } else if (canTransformInternal(req.target_id, req.source_id, req.time, 0)) { do_cb = true; result = TransformAvailable; } if (do_cb) { { boost::mutex::scoped_lock lock2(transformable_callbacks_mutex_); M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle); if (it != transformable_callbacks_.end()) { transformables.push_back(boost::make_tuple(boost::ref(it->second), req.request_handle, lookupFrameString(req.target_id), lookupFrameString(req.source_id), boost::ref(req.time), boost::ref(result))); } } if (transformable_requests_.size() > 1) { transformable_requests_[it - transformable_requests_.begin()] = transformable_requests_.back(); } transformable_requests_.erase(transformable_requests_.end() - 1); } else { ++it; } } // unlock before allowing possible user callbacks to avoid potential deadlock (#91) lock.unlock(); BOOST_FOREACH (TransformableTuple tt, transformables) { tt.get<0>()(tt.get<1>(), tt.get<2>(), tt.get<3>(), tt.get<4>(), tt.get<5>()); } // Backwards compatability callback for tf _transforms_changed_(); } std::string BufferCore::_allFramesAsDot(double current_time) const { std::stringstream mstream; mstream << "digraph G {" << std::endl; boost::mutex::scoped_lock lock(frame_mutex_); TransformStorage temp; if (frames_.size() == 1) { mstream <<"\"no tf data recieved\""; } mstream.precision(3); mstream.setf(std::ios::fixed,std::ios::floatfield); for (unsigned int counter = 1; counter < frames_.size(); counter ++) // one referenced for 0 is no frame { unsigned int frame_id_num; TimeCacheInterfacePtr counter_frame = getFrame(counter); if (!counter_frame) { continue; } if(!counter_frame->getData(ros::Time(), temp)) { continue; } else { frame_id_num = temp.frame_id_; } std::string authority = "no recorded authority"; std::map::const_iterator it = frame_authority_.find(counter); if (it != frame_authority_.end()) authority = it->second; double rate = counter_frame->getListLength() / std::max((counter_frame->getLatestTimestamp().toSec() - counter_frame->getOldestTimestamp().toSec()), 0.0001); mstream << std::fixed; //fixed point notation mstream.precision(3); //3 decimal places mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> " << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\"" //<< "Time: " << current_time.toSec() << "\\n" << "Broadcaster: " << authority << "\\n" << "Average rate: " << rate << " Hz\\n" << "Most recent transform: " << (counter_frame->getLatestTimestamp()).toSec() <<" "; if (current_time > 0) mstream << "( "<< current_time - counter_frame->getLatestTimestamp().toSec() << " sec old)"; mstream << "\\n" // << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n" // << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n" // << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n" << "Buffer length: " << (counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()).toSec() << " sec\\n" <<"\"];" < 0) { mstream << "edge [style=invis];" <" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; } continue; } if (counter_frame->getData(ros::Time(), temp)) { frame_id_num = temp.frame_id_; } else { frame_id_num = 0; } if(frameIDs_reverse[frame_id_num]=="NO_PARENT") { mstream << "edge [style=invis];" < 0) mstream << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n "; mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; } } mstream << "}"; return mstream.str(); } std::string BufferCore::_allFramesAsDot() const { return _allFramesAsDot(0.0); } void BufferCore::_chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string& fixed_frame, std::vector& output) const { std::string error_string; output.clear(); //empty vector std::stringstream mstream; boost::mutex::scoped_lock lock(frame_mutex_); TransformAccum accum; // Get source frame/time using getFrame CompactFrameID source_id = lookupFrameNumber(source_frame); CompactFrameID fixed_id = lookupFrameNumber(fixed_frame); CompactFrameID target_id = lookupFrameNumber(target_frame); std::vector source_frame_chain; int retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain); if (retval != tf2_msgs::TF2Error::NO_ERROR) { switch (retval) { case tf2_msgs::TF2Error::CONNECTIVITY_ERROR: throw ConnectivityException(error_string); case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR: throw ExtrapolationException(error_string); case tf2_msgs::TF2Error::LOOKUP_ERROR: throw LookupException(error_string); default: CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); assert(0); } } std::vector target_frame_chain; retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain); if (retval != tf2_msgs::TF2Error::NO_ERROR) { switch (retval) { case tf2_msgs::TF2Error::CONNECTIVITY_ERROR: throw ConnectivityException(error_string); case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR: throw ExtrapolationException(error_string); case tf2_msgs::TF2Error::LOOKUP_ERROR: throw LookupException(error_string); default: CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); assert(0); } } // If the two chains overlap clear the overlap if (source_frame_chain.size() > 0 && target_frame_chain.size() > 0 && source_frame_chain.back() == target_frame_chain.front()) { source_frame_chain.pop_back(); } // Join the two walks for (unsigned int i = 0; i < target_frame_chain.size(); ++i) { source_frame_chain.push_back(target_frame_chain[i]); } // Write each element of source_frame_chain as string for (unsigned int i = 0; i < source_frame_chain.size(); ++i) { output.push_back(lookupFrameString(source_frame_chain[i])); } } int TestBufferCore::_walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const { TransformAccum accum; return buffer.walkToTopParent(accum, time, target_id, source_id, error_string, frame_chain); } } // namespace tf2 geometry2-0.7.5/tf2/src/cache.cpp000066400000000000000000000221421372352374300165030ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "tf2/time_cache.h" #include "tf2/exceptions.h" #include #include #include #include #include namespace tf2 { TransformStorage::TransformStorage() { } TransformStorage::TransformStorage(const geometry_msgs::TransformStamped& data, CompactFrameID frame_id, CompactFrameID child_frame_id) : stamp_(data.header.stamp) , frame_id_(frame_id) , child_frame_id_(child_frame_id) { const geometry_msgs::Quaternion& o = data.transform.rotation; rotation_ = tf2::Quaternion(o.x, o.y, o.z, o.w); const geometry_msgs::Vector3& v = data.transform.translation; translation_ = tf2::Vector3(v.x, v.y, v.z); } TimeCache::TimeCache(ros::Duration max_storage_time) : max_storage_time_(max_storage_time) {} namespace cache { // Avoid ODR collisions https://github.com/ros/geometry2/issues/175 // hoisting these into separate functions causes an ~8% speedup. Removing calling them altogether adds another ~10% void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* error_str) { if (error_str) { char str[116]; // Text without formatting strings has 76, each timestamp has up to 20 snprintf(str, sizeof(str), "Lookup would require extrapolation at time %.09f, but only time %.09f is in the buffer", t0.toSec(), t1.toSec()); *error_str = str; } } void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str) { if (error_str) { ros::Duration tdiff = t1 - t0; char str[163]; // Text without formatting strings has 102, each timestamp has up to 20 snprintf( str, sizeof(str), "Lookup would require extrapolation %.09fs into the future. Requested time %.09f but the latest data is at time %.09f", tdiff.toSec(), t0.toSec(), t1.toSec()); *error_str = str; } } void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str) { if (error_str) { ros::Duration tdiff = t1 - t0; char str[163]; // Text without formatting strings has 102, each timestamp has up to 20 snprintf( str, sizeof(str), "Lookup would require extrapolation %.09fs into the past. Requested time %.09f but the earliest data is at time %.09f", tdiff.toSec(), t0.toSec(), t1.toSec()); *error_str = str; } } } // namespace cache bool operator>(const TransformStorage& lhs, const TransformStorage& rhs) { return lhs.stamp_ > rhs.stamp_; } uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str) { //No values stored if (storage_.empty()) { return 0; } //If time == 0 return the latest if (target_time.isZero()) { one = &storage_.front(); return 1; } // One value stored if (++storage_.begin() == storage_.end()) { TransformStorage& ts = *storage_.begin(); if (ts.stamp_ == target_time) { one = &ts; return 1; } else { cache::createExtrapolationException1(target_time, ts.stamp_, error_str); return 0; } } ros::Time latest_time = (*storage_.begin()).stamp_; ros::Time earliest_time = (*(storage_.rbegin())).stamp_; if (target_time == latest_time) { one = &(*storage_.begin()); return 1; } else if (target_time == earliest_time) { one = &(*storage_.rbegin()); return 1; } // Catch cases that would require extrapolation else if (target_time > latest_time) { cache::createExtrapolationException2(target_time, latest_time, error_str); return 0; } else if (target_time < earliest_time) { cache::createExtrapolationException3(target_time, earliest_time, error_str); return 0; } //At least 2 values stored //Find the first value less than the target value L_TransformStorage::iterator storage_it; TransformStorage storage_target_time; storage_target_time.stamp_ = target_time; storage_it = std::lower_bound( storage_.begin(), storage_.end(), storage_target_time, std::greater()); //Finally the case were somewhere in the middle Guarenteed no extrapolation :-) one = &*(storage_it); //Older two = &*(--storage_it); //Newer return 2; } void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output) { // Check for zero distance case if( two.stamp_ == one.stamp_ ) { output = two; return; } //Calculate the ratio tf2Scalar ratio = (time - one.stamp_).toSec() / (two.stamp_ - one.stamp_).toSec(); //Interpolate translation output.translation_.setInterpolate3(one.translation_, two.translation_, ratio); //Interpolate rotation output.rotation_ = slerp( one.rotation_, two.rotation_, ratio); output.stamp_ = time; output.frame_id_ = one.frame_id_; output.child_frame_id_ = one.child_frame_id_; } bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available { TransformStorage* p_temp_1; TransformStorage* p_temp_2; int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); if (num_nodes == 0) { return false; } else if (num_nodes == 1) { data_out = *p_temp_1; } else if (num_nodes == 2) { if( p_temp_1->frame_id_ == p_temp_2->frame_id_) { interpolate(*p_temp_1, *p_temp_2, time, data_out); } else { data_out = *p_temp_1; } } else { assert(0); } return true; } CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str) { TransformStorage* p_temp_1; TransformStorage* p_temp_2; int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); if (num_nodes == 0) { return 0; } return p_temp_1->frame_id_; } bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_str) { L_TransformStorage::iterator storage_it = storage_.begin(); if(storage_it != storage_.end()) { if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_) { if (error_str) { *error_str = "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)"; } return false; } } while(storage_it != storage_.end()) { if (storage_it->stamp_ <= new_data.stamp_) break; storage_it++; } if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_) { if (error_str) { *error_str = "TF_REPEATED_DATA ignoring data with redundant timestamp"; } return false; } else { storage_.insert(storage_it, new_data); } pruneList(); return true; } void TimeCache::clearList() { storage_.clear(); } unsigned int TimeCache::getListLength() { return storage_.size(); } P_TimeAndFrameID TimeCache::getLatestTimeAndParent() { if (storage_.empty()) { return std::make_pair(ros::Time(), 0); } const TransformStorage& ts = storage_.front(); return std::make_pair(ts.stamp_, ts.frame_id_); } ros::Time TimeCache::getLatestTimestamp() { if (storage_.empty()) return ros::Time(); //empty list case return storage_.front().stamp_; } ros::Time TimeCache::getOldestTimestamp() { if (storage_.empty()) return ros::Time(); //empty list case return storage_.back().stamp_; } void TimeCache::pruneList() { ros::Time latest_time = storage_.begin()->stamp_; while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time) { storage_.pop_back(); } } // namespace tf2 } geometry2-0.7.5/tf2/src/static_cache.cpp000066400000000000000000000050321372352374300200510ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "tf2/time_cache.h" #include "tf2/exceptions.h" #include "tf2/LinearMath/Transform.h" using namespace tf2; bool StaticCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available { data_out = storage_; data_out.stamp_ = time; return true; }; bool StaticCache::insertData(const TransformStorage& new_data, std::string* error_str) { storage_ = new_data; return true; }; void StaticCache::clearList() { return; }; unsigned int StaticCache::getListLength() { return 1; }; CompactFrameID StaticCache::getParent(ros::Time time, std::string* error_str) { return storage_.frame_id_; } P_TimeAndFrameID StaticCache::getLatestTimeAndParent() { return std::make_pair(ros::Time(), storage_.frame_id_); } ros::Time StaticCache::getLatestTimestamp() { return ros::Time(); }; ros::Time StaticCache::getOldestTimestamp() { return ros::Time(); }; geometry2-0.7.5/tf2/test/000077500000000000000000000000001372352374300151235ustar00rootroot00000000000000geometry2-0.7.5/tf2/test/cache_unittest.cpp000066400000000000000000000251701372352374300206360ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "tf2/LinearMath/Quaternion.h" #include #include #include std::vector values; unsigned int step = 0; void seed_rand() { values.clear(); for (unsigned int i = 0; i < 1000; i++) { int pseudo_rand = std::floor(i * M_PI); values.push_back(( pseudo_rand % 100)/50.0 - 1.0); //printf("Seeding with %f\n", values.back()); } }; double get_rand() { if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first"); if (step >= values.size()) step = 0; else step++; return values[step]; } using namespace tf2; void setIdentity(TransformStorage& stor) { stor.translation_.setValue(0.0, 0.0, 0.0); stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); } TEST(TimeCache, Repeatability) { unsigned int runs = 100; tf2::TimeCache cache; TransformStorage stor; setIdentity(stor); for ( uint64_t i = 1; i < runs ; i++ ) { stor.frame_id_ = i; stor.stamp_ = ros::Time().fromNSec(i); cache.insertData(stor); } for ( uint64_t i = 1; i < runs ; i++ ) { cache.getData(ros::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); } } TEST(TimeCache, RepeatabilityReverseInsertOrder) { unsigned int runs = 100; tf2::TimeCache cache; TransformStorage stor; setIdentity(stor); for ( int i = runs -1; i >= 0 ; i-- ) { stor.frame_id_ = i; stor.stamp_ = ros::Time().fromNSec(i); cache.insertData(stor); } for ( uint64_t i = 1; i < runs ; i++ ) { cache.getData(ros::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); } } #if 0 // jfaust: this doesn't seem to actually be testing random insertion? TEST(TimeCache, RepeatabilityRandomInsertOrder) { seed_rand(); tf2::TimeCache cache; double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8}; std::vector values (my_vals, my_vals + sizeof(my_vals)/sizeof(double)); unsigned int runs = values.size(); TransformStorage stor; setIdentity(stor); for ( uint64_t i = 0; i xvalues(2); std::vector yvalues(2); std::vector zvalues(2); uint64_t offset = 200; TransformStorage stor; setIdentity(stor); for ( uint64_t i = 1; i < runs ; i++ ) { for (uint64_t step = 0; step < 2 ; step++) { xvalues[step] = 10.0 * get_rand(); yvalues[step] = 10.0 * get_rand(); zvalues[step] = 10.0 * get_rand(); stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]); stor.frame_id_ = 2; stor.stamp_ = ros::Time().fromNSec(step * 100 + offset); cache.insertData(stor); } for (int pos = 0; pos < 100 ; pos ++) { uint64_t time = offset + pos; cache.getData(ros::Time().fromNSec(time), stor); uint64_t time_out = stor.stamp_.toNSec(); double x_out = stor.translation_.x(); double y_out = stor.translation_.y(); double z_out = stor.translation_.z(); // printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out, // xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100., // yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, // zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0); EXPECT_EQ(time, time_out); EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon); EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon); EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon); } cache.clearList(); } } /** \brief Make sure we dont' interpolate across reparented data */ TEST(TimeCache, ReparentingInterpolationProtection) { double epsilon = 1e-6; uint64_t offset = 555; seed_rand(); tf2::TimeCache cache; std::vector xvalues(2); std::vector yvalues(2); std::vector zvalues(2); TransformStorage stor; setIdentity(stor); for (uint64_t step = 0; step < 2 ; step++) { xvalues[step] = 10.0 * get_rand(); yvalues[step] = 10.0 * get_rand(); zvalues[step] = 10.0 * get_rand(); stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]); stor.frame_id_ = step + 4; stor.stamp_ = ros::Time().fromNSec(step * 100 + offset); cache.insertData(stor); } for (int pos = 0; pos < 100 ; pos ++) { EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor)); double x_out = stor.translation_.x(); double y_out = stor.translation_.y(); double z_out = stor.translation_.z(); EXPECT_NEAR(xvalues[0], x_out, epsilon); EXPECT_NEAR(yvalues[0], y_out, epsilon); EXPECT_NEAR(zvalues[0], z_out, epsilon); } } TEST(Bullet, Slerp) { uint64_t runs = 100; seed_rand(); tf2::Quaternion q1, q2; q1.setEuler(0,0,0); for (uint64_t i = 0 ; i < runs ; i++) { q2.setEuler(1.0 * get_rand(), 1.0 * get_rand(), 1.0 * get_rand()); tf2::Quaternion q3 = slerp(q1,q2,0.5); EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5); } } TEST(TimeCache, AngularInterpolation) { uint64_t runs = 100; double epsilon = 1e-6; seed_rand(); tf2::TimeCache cache; std::vector yawvalues(2); std::vector pitchvalues(2); std::vector rollvalues(2); uint64_t offset = 200; std::vector quats(2); TransformStorage stor; setIdentity(stor); for ( uint64_t i = 1; i < runs ; i++ ) { for (uint64_t step = 0; step < 2 ; step++) { yawvalues[step] = 10.0 * get_rand() / 100.0; pitchvalues[step] = 0;//10.0 * get_rand(); rollvalues[step] = 0;//10.0 * get_rand(); quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]); stor.rotation_ = quats[step]; stor.frame_id_ = 3; stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1 cache.insertData(stor); } for (int pos = 0; pos < 100 ; pos ++) { uint64_t time = offset + pos; cache.getData(ros::Time().fromNSec(time), stor); uint64_t time_out = stor.stamp_.toNSec(); tf2::Quaternion quat (stor.rotation_); //Generate a ground truth quaternion directly calling slerp tf2::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0); //Make sure the transformed one and the direct call match EXPECT_EQ(time, time_out); EXPECT_NEAR(0, angle(ground_truth, quat), epsilon); } cache.clearList(); } } TEST(TimeCache, DuplicateEntries) { TimeCache cache; TransformStorage stor; setIdentity(stor); stor.frame_id_ = 3; stor.stamp_ = ros::Time().fromNSec(1); cache.insertData(stor); cache.insertData(stor); cache.getData(ros::Time().fromNSec(1), stor); //printf(" stor is %f\n", stor.translation_.x()); EXPECT_TRUE(!std::isnan(stor.translation_.x())); EXPECT_TRUE(!std::isnan(stor.translation_.y())); EXPECT_TRUE(!std::isnan(stor.translation_.z())); EXPECT_TRUE(!std::isnan(stor.rotation_.x())); EXPECT_TRUE(!std::isnan(stor.rotation_.y())); EXPECT_TRUE(!std::isnan(stor.rotation_.z())); EXPECT_TRUE(!std::isnan(stor.rotation_.w())); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2/test/simple_tf2_core.cpp000066400000000000000000000275761372352374300207240ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include "tf2/LinearMath/Vector3.h" #include "tf2/exceptions.h" TEST(tf2, setTransformFail) { tf2::BufferCore tfc; geometry_msgs::TransformStamped st; EXPECT_FALSE(tfc.setTransform(st, "authority1")); } TEST(tf2, setTransformValid) { tf2::BufferCore tfc; geometry_msgs::TransformStamped st; st.header.frame_id = "foo"; st.header.stamp = ros::Time(1.0); st.child_frame_id = "child"; st.transform.rotation.w = 1; EXPECT_TRUE(tfc.setTransform(st, "authority1")); } TEST(tf2, setTransformInvalidQuaternion) { tf2::BufferCore tfc; geometry_msgs::TransformStamped st; st.header.frame_id = "foo"; st.header.stamp = ros::Time(1.0); st.child_frame_id = "child"; st.transform.rotation.w = 0; EXPECT_FALSE(tfc.setTransform(st, "authority1")); } TEST(tf2_lookupTransform, LookupException_Nothing_Exists) { tf2::BufferCore tfc; EXPECT_THROW(tfc.lookupTransform("a", "b", ros::Time().fromSec(1.0)), tf2::LookupException); } TEST(tf2_canTransform, Nothing_Exists) { tf2::BufferCore tfc; EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0))); std::string error_msg = std::string(); EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0), &error_msg)); ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist."); } TEST(tf2_lookupTransform, LookupException_One_Exists) { tf2::BufferCore tfc; geometry_msgs::TransformStamped st; st.header.frame_id = "foo"; st.header.stamp = ros::Time(1.0); st.child_frame_id = "child"; st.transform.rotation.w = 1; EXPECT_TRUE(tfc.setTransform(st, "authority1")); EXPECT_THROW(tfc.lookupTransform("foo", "bar", ros::Time().fromSec(1.0)), tf2::LookupException); } TEST(tf2_canTransform, One_Exists) { tf2::BufferCore tfc; geometry_msgs::TransformStamped st; st.header.frame_id = "foo"; st.header.stamp = ros::Time(1.0); st.child_frame_id = "child"; st.transform.rotation.w = 1; EXPECT_TRUE(tfc.setTransform(st, "authority1")); EXPECT_FALSE(tfc.canTransform("foo", "bar", ros::Time().fromSec(1.0))); } TEST(tf2_chainAsVector, chain_v_configuration) { tf2::BufferCore mBC; tf2::TestBufferCore tBC; geometry_msgs::TransformStamped st; st.header.stamp = ros::Time(0); st.transform.rotation.w = 1; st.header.frame_id = "a"; st.child_frame_id = "b"; mBC.setTransform(st, "authority1"); st.header.frame_id = "b"; st.child_frame_id = "c"; mBC.setTransform(st, "authority1"); st.header.frame_id = "a"; st.child_frame_id = "d"; mBC.setTransform(st, "authority1"); st.header.frame_id = "d"; st.child_frame_id = "e"; mBC.setTransform(st, "authority1"); std::vector chain; mBC._chainAsVector( "c", ros::Time(), "c", ros::Time(), "c", chain); EXPECT_EQ( 0, chain.size()); mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "c", chain); EXPECT_EQ( 3, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "c", chain); EXPECT_EQ( 3, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" ); mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "a", chain); EXPECT_EQ( 3, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "a", chain); EXPECT_EQ( 3, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" ); mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "c", chain); EXPECT_EQ( 5, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "a", chain); EXPECT_EQ( 5, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "e", chain); EXPECT_EQ( 5, chain.size()); if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); } TEST(tf2_walkToTopParent, walk_i_configuration) { tf2::BufferCore mBC; tf2::TestBufferCore tBC; geometry_msgs::TransformStamped st; st.header.stamp = ros::Time(0); st.transform.rotation.w = 1; st.header.frame_id = "a"; st.child_frame_id = "b"; mBC.setTransform(st, "authority1"); st.header.frame_id = "b"; st.child_frame_id = "c"; mBC.setTransform(st, "authority1"); st.header.frame_id = "c"; st.child_frame_id = "d"; mBC.setTransform(st, "authority1"); st.header.frame_id = "d"; st.child_frame_id = "e"; mBC.setTransform(st, "authority1"); std::vector id_chain; tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); EXPECT_EQ(5, id_chain.size() ); if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2])); if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3])); if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4])); id_chain.clear(); tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); EXPECT_EQ(5, id_chain.size() ); if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1])); if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2])); if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3])); if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4])); } TEST(tf2_walkToTopParent, walk_v_configuration) { tf2::BufferCore mBC; tf2::TestBufferCore tBC; geometry_msgs::TransformStamped st; st.header.stamp = ros::Time(0); st.transform.rotation.w = 1; // st.header.frame_id = "aaa"; // st.child_frame_id = "aa"; // mBC.setTransform(st, "authority1"); // // st.header.frame_id = "aa"; // st.child_frame_id = "a"; // mBC.setTransform(st, "authority1"); st.header.frame_id = "a"; st.child_frame_id = "b"; mBC.setTransform(st, "authority1"); st.header.frame_id = "b"; st.child_frame_id = "c"; mBC.setTransform(st, "authority1"); st.header.frame_id = "a"; st.child_frame_id = "d"; mBC.setTransform(st, "authority1"); st.header.frame_id = "d"; st.child_frame_id = "e"; mBC.setTransform(st, "authority1"); std::vector id_chain; tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain); EXPECT_EQ(5, id_chain.size()); if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1])); if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3])); if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4])); tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain); EXPECT_EQ(5, id_chain.size()); if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3])); if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4])); tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); EXPECT_EQ( id_chain.size(), 3 ); if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); EXPECT_EQ( id_chain.size(), 3 ); if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2])); tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain); EXPECT_EQ( id_chain.size(), 2 ); if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1])); tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain); EXPECT_EQ( id_chain.size(), 2 ); if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::Time::init(); //needed for ros::TIme::now() return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2/test/speed_test.cpp000066400000000000000000000200741372352374300177710ustar00rootroot00000000000000/* * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include int main(int argc, char** argv) { uint32_t num_levels = 10; if (argc > 1) { num_levels = boost::lexical_cast(argv[1]); } double time_interval = 1.0; if (argc > 2) { time_interval = boost::lexical_cast(argv[2]); } console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO); tf2::BufferCore bc; geometry_msgs::TransformStamped t; t.header.stamp = ros::Time(1); t.header.frame_id = "root"; t.child_frame_id = "0"; t.transform.translation.x = 1; t.transform.rotation.w = 1.0; bc.setTransform(t, "me"); t.header.stamp = ros::Time(2); bc.setTransform(t, "me"); for (uint32_t i = 1; i < num_levels / 2; ++i) { for (double j = time_interval; j < 2.0 + time_interval; j += time_interval) { std::stringstream parent_ss; parent_ss << (i - 1); std::stringstream child_ss; child_ss << i; t.header.stamp = ros::Time(j); t.header.frame_id = parent_ss.str(); t.child_frame_id = child_ss.str(); bc.setTransform(t, "me"); } } t.header.frame_id = "root"; std::stringstream ss; ss << num_levels/2; t.header.stamp = ros::Time(1); t.child_frame_id = ss.str(); bc.setTransform(t, "me"); t.header.stamp = ros::Time(2); bc.setTransform(t, "me"); for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i) { for (double j = time_interval; j < 2.0 + time_interval; j += time_interval) { std::stringstream parent_ss; parent_ss << (i - 1); std::stringstream child_ss; child_ss << i; t.header.stamp = ros::Time(j); t.header.frame_id = parent_ss.str(); t.child_frame_id = child_ss.str(); bc.setTransform(t, "me"); } } //logInfo_STREAM(bc.allFramesAsYAML()); std::string v_frame0 = boost::lexical_cast(num_levels - 1); std::string v_frame1 = boost::lexical_cast(num_levels/2 - 1); CONSOLE_BRIDGE_logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str()); geometry_msgs::TransformStamped out_t; const uint32_t count = 1000000; CONSOLE_BRIDGE_logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval); #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(0)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(2)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(0)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(1)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(1.5)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(2)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; //ROS_INFO_STREAM(out_t); CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(3)); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; CONSOLE_BRIDGE_logInform("canTransform at Time(3) without error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif #if 01 { ros::WallTime start = ros::WallTime::now(); std::string str; for (uint32_t i = 0; i < count; ++i) { bc.canTransform(v_frame1, v_frame0, ros::Time(3), &str); } ros::WallTime end = ros::WallTime::now(); ros::WallDuration dur = end - start; CONSOLE_BRIDGE_logInform("canTransform at Time(3) with error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif } geometry2-0.7.5/tf2/test/static_cache_test.cpp000066400000000000000000000061521372352374300213040ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include using namespace tf2; void setIdentity(TransformStorage& stor) { stor.translation_.setValue(0.0, 0.0, 0.0); stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); } TEST(StaticCache, Repeatability) { unsigned int runs = 100; tf2::StaticCache cache; TransformStorage stor; setIdentity(stor); for ( uint64_t i = 1; i < runs ; i++ ) { stor.frame_id_ = CompactFrameID(i); stor.stamp_ = ros::Time().fromNSec(i); cache.insertData(stor); cache.getData(ros::Time().fromNSec(i), stor); EXPECT_EQ(stor.frame_id_, i); EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); } } TEST(StaticCache, DuplicateEntries) { tf2::StaticCache cache; TransformStorage stor; setIdentity(stor); stor.frame_id_ = CompactFrameID(3); stor.stamp_ = ros::Time().fromNSec(1); cache.insertData(stor); cache.insertData(stor); cache.getData(ros::Time().fromNSec(1), stor); //printf(" stor is %f\n", stor.transform.translation.x); EXPECT_TRUE(!std::isnan(stor.translation_.x())); EXPECT_TRUE(!std::isnan(stor.translation_.y())); EXPECT_TRUE(!std::isnan(stor.translation_.z())); EXPECT_TRUE(!std::isnan(stor.rotation_.x())); EXPECT_TRUE(!std::isnan(stor.rotation_.y())); EXPECT_TRUE(!std::isnan(stor.rotation_.z())); EXPECT_TRUE(!std::isnan(stor.rotation_.w())); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2/test/test_transform_datatypes.cpp000066400000000000000000000052251372352374300227630ustar00rootroot00000000000000/* * Copyright (c) 2020, Open Source Robotics Foundation, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include "tf2/transform_datatypes.h" #include TEST(Stamped, assignment) { tf2::Stamped first("foobar", ros::Time(0), "my_frame_id"); tf2::Stamped second("baz", ros::Time(0), "my_frame_id"); EXPECT_NE(second, first); second = first; EXPECT_EQ(second, first); } TEST(Stamped, setData) { tf2::Stamped first("foobar", ros::Time(0), "my_frame_id"); tf2::Stamped second("baz", ros::Time(0), "my_frame_id"); EXPECT_NE(second, first); second.setData("foobar"); EXPECT_EQ(second, first); } TEST(Stamped, copy_constructor) { tf2::Stamped first("foobar", ros::Time(0), "my_frame_id"); tf2::Stamped second(first); EXPECT_EQ(second, first); } TEST(Stamped, default_constructor) { tf2::Stamped first("foobar", ros::Time(0), "my_frame_id"); tf2::Stamped second; EXPECT_NE(second, first); } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); ros::Time::init(); return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2_bullet/000077500000000000000000000000001372352374300155135ustar00rootroot00000000000000geometry2-0.7.5/tf2_bullet/CHANGELOG.rst000066400000000000000000000117431372352374300175420ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_bullet ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ * [noetic] cherry-pick Windows fixes from melodic-devel (`#450 `_) * [Windows][melodic-devel] Fix install locations (`#442 `_) * fixed install locations of tf2 * [windows][melodic] more portable fixes. (`#443 `_) * more portable fixes. * Contributors: Sean Yen 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Fix compile error missing ros/ros.h (`#400 `_) * ros/ros.h -> ros/time.h * tf2_bullet doesn't need ros.h * tf2_eigen doesn't need ros/ros.h * use find_package when pkg_check_modules doesn't work (`#364 `_) * use find_package bullet for MSVC * use find_package when pkg_check_modules fails (`#8 `_) * use find_package() when pkg_check_modules() doesn't work * Contributors: James Xu, Shane Loretz 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- 0.5.16 (2017-07-14) ------------------- * store gtest return value as int (`#229 `_) * Contributors: dhood 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- * Improve documentation * Contributors: Jackie Kay 0.5.13 (2016-03-04) ------------------- * Don't export catkin includes They only point to the temporary include in the build directory. * Contributors: Jochen Sprickerhof 0.5.12 (2015-08-05) ------------------- 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- 0.5.9 (2015-03-25) ------------------ 0.5.8 (2015-03-17) ------------------ * remove useless Makefile files * fix ODR violations * Contributors: Vincent Rabaud 0.5.7 (2014-12-23) ------------------ * fixing install rules and adding backwards compatible include with #warning * Contributors: Tully Foote 0.5.6 (2014-09-18) ------------------ 0.5.5 (2014-06-23) ------------------ 0.5.4 (2014-05-07) ------------------ 0.5.3 (2014-02-21) ------------------ 0.5.2 (2014-02-20) ------------------ 0.5.1 (2014-02-14) ------------------ 0.5.0 (2014-02-14) ------------------ 0.4.10 (2013-12-26) ------------------- 0.4.9 (2013-11-06) ------------------ * adding missing buildtool dependency on pkg-config 0.4.8 (2013-11-06) ------------------ 0.4.7 (2013-08-28) ------------------ 0.4.6 (2013-08-28) ------------------ 0.4.5 (2013-07-11) ------------------ 0.4.4 (2013-07-09) ------------------ * making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ * removing unused dependency on rostest 0.4.1 (2013-07-05) ------------------ * stripping tf2_ros dependency from tf2_bullet. Test was moved to test_tf2 0.4.0 (2013-06-27) ------------------ * moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 * Cleaning up unnecessary dependency on roscpp * converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace * Cleaning up packaging of tf2 including: removing unused nodehandle cleaning up a few dependencies and linking removing old backup of package.xml making diff minimally different from tf version of library * Restoring test packages and bullet packages. reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ 0.3.6 (2013-03-03) ------------------ 0.3.5 (2013-02-15 14:46) ------------------------ 0.3.4 (2013-02-15 13:14) ------------------------ 0.3.3 (2013-02-15 11:30) ------------------------ 0.3.2 (2013-02-15 00:42) ------------------------ 0.3.1 (2013-02-14) ------------------ 0.3.0 (2013-02-13) ------------------ * fixing groovy-devel * removing bullet and kdl related packages * catkinizing geometry-experimental * catkinizing tf2_bullet * merge tf2_cpp and tf2_py into tf2_ros * A working first version of transforming and converting between different types * Fixing tests now that Buffer creates a NodeHandle * add frame unit tests to kdl and bullet * add first regression tests for kdl and bullet tf * add btTransform transform * add bullet transforms, and create tests for bullet and kdl geometry2-0.7.5/tf2_bullet/CMakeLists.txt000066400000000000000000000016771372352374300202660ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_bullet) find_package(PkgConfig REQUIRED) set(bullet_FOUND 0) pkg_check_modules(bullet bullet) if(NOT bullet_FOUND) # windows build bullet3 doesn't come with pkg-config by default and it only comes with CMake config files # so pkg_check_modules will fail find_package(bullet REQUIRED) # https://github.com/bulletphysics/bullet3/blob/master/BulletConfig.cmake.in set(bullet_INCLUDE_DIRS "${BULLET_INCLUDE_DIRS}") endif() find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2) include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) catkin_package(INCLUDE_DIRS include ${bullet_INCLUDE_DIRS}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_bullet test/test_tf2_bullet.cpp) target_link_libraries(test_bullet ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) endif() geometry2-0.7.5/tf2_bullet/include/000077500000000000000000000000001372352374300171365ustar00rootroot00000000000000geometry2-0.7.5/tf2_bullet/include/tf2_bullet/000077500000000000000000000000001372352374300212005ustar00rootroot00000000000000geometry2-0.7.5/tf2_bullet/include/tf2_bullet/tf2_bullet.h000066400000000000000000000115241372352374300234160ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #ifndef TF2_BULLET_H #define TF2_BULLET_H #include #include #include namespace tf2 { /** \brief Convert a timestamped transform to the equivalent Bullet data type. * \param t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to a Bullet btTransform. */ inline btTransform transformToBullet(const geometry_msgs::TransformStamped& t) { return btTransform(btQuaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), btVector3(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); } /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 type. * This function is a specialization of the doTransform template defined in tf2/convert.h * \param t_in The vector to transform, as a timestamped Bullet btVector3 data type. * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); } /** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h * \param in The timestamped Bullet btVector3 to convert. * \return The vector converted to a PointStamped message. */ inline geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) { geometry_msgs::PointStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.point.x = in[0]; msg.point.y = in[1]; msg.point.z = in[2]; return msg; } /** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The PointStamped message to convert. * \param out The point converted to a timestamped Bullet Vector3. */ inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out[0] = msg.point.x; out[1] = msg.point.y; out[2] = msg.point.z; } /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Transform data type. * This function is a specialization of the doTransform template defined in tf2/convert.h * \param t_in The frame to transform, as a timestamped Bullet btTransform. * \param t_out The transformed frame, as a timestamped Bullet btTransform. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); } } // namespace #endif // TF2_BULLET_H geometry2-0.7.5/tf2_bullet/include/tf2_bullet/tf2_bullet/000077500000000000000000000000001372352374300232425ustar00rootroot00000000000000geometry2-0.7.5/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h000066400000000000000000000001731372352374300254560ustar00rootroot00000000000000#warning This header is at the wrong path you should include #include geometry2-0.7.5/tf2_bullet/mainpage.dox000066400000000000000000000013611372352374300200110ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_bullet contains functions for converting between geometry_msgs and Bullet data types. This library is an implementation of the templated conversion interface specified in tf/convert.h. It enables easy conversion from geometry_msgs Transform and Point types to the types specified by the Bullet physics engine API (see http://bulletphysics.org). See the Conversions overview wiki page for more information about datatype conversion in tf2. \section codeapi Code API This library consists of one header only, tf2_bullet/tf2_bullet.h, which consists mostly of specializations of template functions defined in tf2/convert.h. */ geometry2-0.7.5/tf2_bullet/package.xml000066400000000000000000000012311372352374300176250ustar00rootroot00000000000000 tf2_bullet 0.7.5 tf2_bullet Wim Meeussen Tully Foote BSD http://www.ros.org/wiki/tf2_bullet catkin pkg-config tf2 bullet geometry_msgs tf2 bullet geometry_msgs geometry2-0.7.5/tf2_bullet/test/000077500000000000000000000000001372352374300164725ustar00rootroot00000000000000geometry2-0.7.5/tf2_bullet/test/test_tf2_bullet.cpp000066400000000000000000000040601372352374300222770ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #include #include #include static const double EPS = 1e-3; TEST(TfBullet, ConvertVector) { btVector3 v(1,2,3); btVector3 v1 = v; tf2::convert(v1, v1); EXPECT_EQ(v, v1); btVector3 v2(3,4,5); tf2::convert(v1, v2); EXPECT_EQ(v, v2); EXPECT_EQ(v1, v2); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); int ret = RUN_ALL_TESTS(); return ret; } geometry2-0.7.5/tf2_eigen/000077500000000000000000000000001372352374300153135ustar00rootroot00000000000000geometry2-0.7.5/tf2_eigen/CHANGELOG.rst000066400000000000000000000120611372352374300173340ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_eigen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ * Cherry-picking various commits from Melodic (`#471 `_) * Revert "rework Eigen functions namespace hack" (`#436 `_) * Fixed warnings in message_filter.h (`#434 `_) the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall * Fix ambiguous call for tf2::convert on MSVC (`#444 `_) * rework ambiguous call on MSVC. * Contributors: Robert Haschke 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ * malcolm: add depends tf2 to catkin_package (`#428 `_) * Contributors: Malcolm Mielle 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Fix compile error missing ros/ros.h (`#400 `_) * ros/ros.h -> ros/time.h * tf2_bullet doesn't need ros.h * tf2_eigen doesn't need ros/ros.h * Merge pull request `#367 `_ from kejxu/add_tf2_namespace_to_avoid_name_collision * separate transform function declarations into transform_functions.h * Contributors: James Xu, Shane Loretz, Tully Foote 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ * improve comments * add Eigen::Isometry3d conversions * normalize quaternions to be in half-space w >= 0 as in tf1 * improve computation efficiency * Contributors: Robert Haschke 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ * Adds toMsg & fromMsg for Eigen Vector3 (`#294 `_) - Adds toMsg for geometry_msgs::Vector3& with dual argument syntax to avoid an overload conflict with geometry_msgs::Point& toMsg(contst Eigen::Vector3d& in) - Adds corresponding fromMsg for Eigen Vector3d and geometry_msgs::Vector3 - Fixed typos in description of fromMsg for Twist and Eigen 6x1 Matrix * Adds additional conversions for tf2, KDL, Eigen (`#292 `_) - adds non-stamped Eigen to Transform function - converts Eigen Matrix Vectors to and from geometry_msgs::Twist - adds to/from message for geometry_msgs::Pose and KDL::Frame * Contributors: Ian McMahon 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- 0.5.16 (2017-07-14) ------------------- * fix return value to prevent warnings on windows (`#237 `_) * fixing include directory order to support overlays (`#231 `_) * tf2_eigen: added support for Quaternion and QuaternionStamped (`#230 `_) * Remove an unused variable from the tf2_eigen test. (`#215 `_) * Find eigen in a much nicer way. * Switch tf2_eigen to use package.xml format 2. (`#216 `_) * Contributors: Chris Lalancette, Mikael Arguedas, Tully Foote, cwecht 0.5.15 (2017-01-24) ------------------- * fixup `#186 `_: inline template specializations (`#200 `_) * Contributors: Robert Haschke 0.5.14 (2017-01-16) ------------------- * Add tf2_eigen conversions for Pose and Point (not stamped) (`#186 `_) * tf2_eigen: added conversions for Point msg type (not timestamped) to Eigen::Vector3d * tf2_eigen: added conversions for Pose msg type (not timestamped) to Eigen::Affine3d * tf2_eigen: new functions are inline now * tf2_eigen test compiling again * tf2_eigen: added tests for Affine3d and Vector3d conversion * tf2_eigen: added redefinitions of non-stamped conversion function to make usage in tf2::convert() possible * tf2_eigen: reduced redundancy by reusing non-stamped conversion-functions in their stamped counterparts * tf2_eigen: added notes at doTransform-implementations which can not work with tf2_ros::BufferInterface::transform * tf2_eigen: fixed typos * Don't export local include dirs (`#180 `_) * Improve documentation. * Contributors: Jackie Kay, Jochen Sprickerhof, cwecht 0.5.13 (2016-03-04) ------------------- * Added missing inline * Added unit test - Testing conversion to msg forward/backward * Added eigenTotransform function * Contributors: Davide Tateo, boris-il-forte 0.5.12 (2015-08-05) ------------------- 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- * fixing CMakeLists.txt from `#97 `_ * create tf2_eigen. * Contributors: Tully Foote, koji geometry2-0.7.5/tf2_eigen/CMakeLists.txt000066400000000000000000000024461372352374300200610ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_eigen) find_package(catkin REQUIRED COMPONENTS cmake_modules geometry_msgs tf2 ) # Finding Eigen is somewhat complicated because of our need to support Ubuntu # all the way back to saucy. First we look for the Eigen3 cmake module # provided by the libeigen3-dev on newer Ubuntu. If that fails, then we # fall-back to the version provided by cmake_modules, which is a stand-in. find_package(Eigen3 QUIET) if(NOT EIGEN3_FOUND) find_package(cmake_modules REQUIRED) find_package(Eigen REQUIRED) set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) endif() # Note that eigen 3.2 (on Ubuntu Wily) only provides EIGEN3_INCLUDE_DIR, # not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former. if(NOT EIGEN3_INCLUDE_DIRS) set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) endif() include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS tf2 DEPENDS EIGEN3) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) if(CATKIN_ENABLE_TESTING) catkin_add_gtest(tf2_eigen-test test/tf2_eigen-test.cpp) target_link_libraries(tf2_eigen-test ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) endif() geometry2-0.7.5/tf2_eigen/include/000077500000000000000000000000001372352374300167365ustar00rootroot00000000000000geometry2-0.7.5/tf2_eigen/include/tf2_eigen/000077500000000000000000000000001372352374300206005ustar00rootroot00000000000000geometry2-0.7.5/tf2_eigen/include/tf2_eigen/tf2_eigen.h000066400000000000000000000531331372352374300226200ustar00rootroot00000000000000/* * Copyright (c) Koji Terada * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Koji Terada */ #ifndef TF2_EIGEN_H #define TF2_EIGEN_H #include #include #include #include #include #include namespace tf2 { /** \brief Convert a timestamped transform to the equivalent Eigen data type. * \param t The transform to convert, as a geometry_msgs Transform message. * \return The transform message converted to an Eigen Isometry3d transform. */ inline Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform& t) { return Eigen::Isometry3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z) * Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z)); } /** \brief Convert a timestamped transform to the equivalent Eigen data type. * \param t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to an Eigen Isometry3d transform. */ inline Eigen::Isometry3d transformToEigen(const geometry_msgs::TransformStamped& t) { return transformToEigen(t.transform); } /** \brief Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. * \param T The transform to convert, as an Eigen Affine3d transform. * \return The transform converted to a TransformStamped message. */ inline geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T) { geometry_msgs::TransformStamped t; t.transform.translation.x = T.translation().x(); t.transform.translation.y = T.translation().y(); t.transform.translation.z = T.translation().z(); Eigen::Quaterniond q(T.linear()); // assuming that upper 3x3 matrix is orthonormal t.transform.rotation.x = q.x(); t.transform.rotation.y = q.y(); t.transform.rotation.z = q.z(); t.transform.rotation.w = q.w(); return t; } /** \brief Convert an Eigen Isometry3d transform to the equivalent geometry_msgs message type. * \param T The transform to convert, as an Eigen Isometry3d transform. * \return The transform converted to a TransformStamped message. */ inline geometry_msgs::TransformStamped eigenToTransform(const Eigen::Isometry3d& T) { geometry_msgs::TransformStamped t; t.transform.translation.x = T.translation().x(); t.transform.translation.y = T.translation().y(); t.transform.translation.z = T.translation().z(); Eigen::Quaterniond q(T.rotation()); t.transform.rotation.x = q.x(); t.transform.rotation.y = q.y(); t.transform.rotation.z = q.z(); t.transform.rotation.w = q.w(); return t; } /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type. * This function is a specialization of the doTransform template defined in tf2/convert.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * functions rely on the existence of a time stamp and a frame id in the type which should * get transformed. * \param t_in The vector to transform, as a Eigen Vector3d data type. * \param t_out The transformed vector, as a Eigen Vector3d data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geometry_msgs::TransformStamped& transform) { t_out = Eigen::Vector3d(transformToEigen(transform) * t_in); } /** \brief Convert a Eigen Vector3d type to a Point message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped Eigen Vector3d to convert. * \return The vector converted to a Point message. */ inline geometry_msgs::Point toMsg(const Eigen::Vector3d& in) { geometry_msgs::Point msg; msg.x = in.x(); msg.y = in.y(); msg.z = in.z(); return msg; } /** \brief Convert a Point message type to a Eigen-specific Vector3d type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The Point message to convert. * \param out The point converted to a Eigen Vector3d. */ inline void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) { out.x() = msg.x; out.y() = msg.y; out.z() = msg.z; } /** \brief Convert an Eigen Vector3d type to a Vector3 message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The Eigen Vector3d to convert. * \return The vector converted to a Vector3 message. */ inline geometry_msgs::Vector3& toMsg(const Eigen::Vector3d& in, geometry_msgs::Vector3& out) { out.x = in.x(); out.y = in.y(); out.z = in.z(); return out; } /** \brief Convert a Vector3 message type to a Eigen-specific Vector3d type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The Vector3 message to convert. * \param out The vector converted to a Eigen Vector3d. */ inline void fromMsg(const geometry_msgs::Vector3& msg, Eigen::Vector3d& out) { out.x() = msg.x; out.y() = msg.y; out.z() = msg.z; } /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The vector to transform, as a timestamped Eigen Vector3d data type. * \param t_out The transformed vector, as a timestamped Eigen Vector3d data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id); } /** \brief Convert a stamped Eigen Vector3d type to a PointStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped Eigen Vector3d to convert. * \return The vector converted to a PointStamped message. */ inline geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) { geometry_msgs::PointStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.point = toMsg(static_cast(in)); return msg; } /** \brief Convert a PointStamped message type to a stamped Eigen-specific Vector3d type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The PointStamped message to convert. * \param out The point converted to a timestamped Eigen Vector3d. */ inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; fromMsg(msg.point, static_cast(out)); } /** \brief Apply a geometry_msgs Transform to an Eigen Affine3d transform. * This function is a specialization of the doTransform template defined in tf2/convert.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * function relies on the existence of a time stamp and a frame id in the type which should * get transformed. * \param t_in The frame to transform, as a Eigen Affine3d transform. * \param t_out The transformed frame, as a Eigen Affine3d transform. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const Eigen::Affine3d& t_in, Eigen::Affine3d& t_out, const geometry_msgs::TransformStamped& transform) { t_out = Eigen::Affine3d(transformToEigen(transform) * t_in); } template <> inline void doTransform(const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out, const geometry_msgs::TransformStamped& transform) { t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in); } /** \brief Convert a Eigen Quaterniond type to a Quaternion message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The Eigen Quaterniond to convert. * \return The quaternion converted to a Quaterion message. */ inline geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { geometry_msgs::Quaternion msg; msg.w = in.w(); msg.x = in.x(); msg.y = in.y(); msg.z = in.z(); return msg; } /** \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The Quaternion message to convert. * \param out The quaternion converted to a Eigen Quaterniond. */ inline void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); } /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type. * This function is a specialization of the doTransform template defined in tf2/convert.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * functions rely on the existence of a time stamp and a frame id in the type which should * get transformed. * \param t_in The vector to transform, as a Eigen Quaterniond data type. * \param t_out The transformed vector, as a Eigen Quaterniond data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template<> inline void doTransform(const Eigen::Quaterniond& t_in, Eigen::Quaterniond& t_out, const geometry_msgs::TransformStamped& transform) { Eigen::Quaterniond t; fromMsg(transform.transform.rotation, t); t_out = t.inverse() * t_in * t; } /** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped Eigen Quaterniond to convert. * \return The quaternion converted to a QuaternionStamped message. */ inline geometry_msgs::QuaternionStamped toMsg(const Stamped& in) { geometry_msgs::QuaternionStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.quaternion = toMsg(static_cast(in)); return msg; } /** \brief Convert a QuaternionStamped message type to a stamped Eigen-specific Quaterniond type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The QuaternionStamped message to convert. * \param out The quaternion converted to a timestamped Eigen Quaterniond. */ inline void fromMsg(const geometry_msgs::QuaternionStamped& msg, Stamped& out) { out.frame_id_ = msg.header.frame_id; out.stamp_ = msg.header.stamp; fromMsg(msg.quaternion, static_cast(out)); } /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The vector to transform, as a timestamped Eigen Quaterniond data type. * \param t_out The transformed vector, as a timestamped Eigen Quaterniond data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out.frame_id_ = transform.header.frame_id; t_out.stamp_ = transform.header.stamp; doTransform(static_cast(t_in), static_cast(t_out), transform); } /** \brief Convert a Eigen Affine3d transform type to a Pose message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The Eigen Affine3d to convert. * \return The Eigen transform converted to a Pose message. */ inline geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { geometry_msgs::Pose msg; msg.position.x = in.translation().x(); msg.position.y = in.translation().y(); msg.position.z = in.translation().z(); Eigen::Quaterniond q(in.linear()); msg.orientation.x = q.x(); msg.orientation.y = q.y(); msg.orientation.z = q.z(); msg.orientation.w = q.w(); if (msg.orientation.w < 0) { msg.orientation.x *= -1; msg.orientation.y *= -1; msg.orientation.z *= -1; msg.orientation.w *= -1; } return msg; } /** \brief Convert a Eigen Isometry3d transform type to a Pose message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The Eigen Isometry3d to convert. * \return The Eigen transform converted to a Pose message. */ inline geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { geometry_msgs::Pose msg; msg.position.x = in.translation().x(); msg.position.y = in.translation().y(); msg.position.z = in.translation().z(); Eigen::Quaterniond q(in.linear()); msg.orientation.x = q.x(); msg.orientation.y = q.y(); msg.orientation.z = q.z(); msg.orientation.w = q.w(); if (msg.orientation.w < 0) { msg.orientation.x *= -1; msg.orientation.y *= -1; msg.orientation.z *= -1; msg.orientation.w *= -1; } return msg; } /** \brief Convert a Pose message transform type to a Eigen Affine3d. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg The Pose message to convert. * \param out The pose converted to a Eigen Affine3d. */ inline void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { out = Eigen::Affine3d( Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) * Eigen::Quaterniond(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z)); } /** \brief Convert a Pose message transform type to a Eigen Isometry3d. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg The Pose message to convert. * \param out The pose converted to a Eigen Isometry3d. */ inline void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { out = Eigen::Isometry3d( Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) * Eigen::Quaterniond(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z)); } /** \brief Convert a Eigen 6x1 Matrix type to a Twist message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The 6x1 Eigen Matrix to convert. * \return The Eigen Matrix converted to a Twist message. */ inline geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { geometry_msgs::Twist msg; msg.linear.x = in[0]; msg.linear.y = in[1]; msg.linear.z = in[2]; msg.angular.x = in[3]; msg.angular.y = in[4]; msg.angular.z = in[5]; return msg; } /** \brief Convert a Twist message transform type to a Eigen 6x1 Matrix. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg The Twist message to convert. * \param out The twist converted to a Eigen 6x1 Matrix. */ inline void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { out[0] = msg.linear.x; out[1] = msg.linear.y; out[2] = msg.linear.z; out[3] = msg.angular.x; out[4] = msg.angular.y; out[5] = msg.angular.z; } /** \brief Apply a geometry_msgs TransformStamped to an Eigen Affine3d transform. * This function is a specialization of the doTransform template defined in tf2/convert.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * function relies on the existence of a time stamp and a frame id in the type which should * get transformed. * \param t_in The frame to transform, as a timestamped Eigen Affine3d transform. * \param t_out The transformed frame, as a timestamped Eigen Affine3d transform. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id); } /** \brief Apply a geometry_msgs TransformStamped to an Eigen Isometry transform. * This function is a specialization of the doTransform template defined in tf2/convert.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * function relies on the existence of a time stamp and a frame id in the type which should * get transformed. * \param t_in The frame to transform, as a timestamped Eigen Isometry transform. * \param t_out The transformed frame, as a timestamped Eigen Isometry transform. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id); } /** \brief Convert a stamped Eigen Affine3d transform type to a Pose message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped Eigen Affine3d to convert. * \return The Eigen transform converted to a PoseStamped message. */ inline geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) { geometry_msgs::PoseStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.pose = toMsg(static_cast(in)); return msg; } inline geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) { geometry_msgs::PoseStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.pose = toMsg(static_cast(in)); return msg; } /** \brief Convert a Pose message transform type to a stamped Eigen Affine3d. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg The PoseStamped message to convert. * \param out The pose converted to a timestamped Eigen Affine3d. */ inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; fromMsg(msg.pose, static_cast(out)); } inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; fromMsg(msg.pose, static_cast(out)); } } // namespace namespace Eigen { // This is needed to make the usage of the following conversion functions usable in tf2::convert(). // According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or // in an associated namespace of one of its arguments. The stamped versions of this conversion // functions work because they have tf2::Stamped as an argument which is the same namespace as // which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is // defined in tf2, so it take the following definitions in Eigen namespace to make them usable in // tf2::convert(). inline geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { return tf2::toMsg(in); } inline geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { return tf2::toMsg(in); } inline void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) { tf2::fromMsg(msg, out); } inline geometry_msgs::Point toMsg(const Eigen::Vector3d& in) { return tf2::toMsg(in); } inline void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { tf2::fromMsg(msg, out); } inline void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { tf2::fromMsg(msg, out); } inline geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { return tf2::toMsg(in); } inline void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { tf2::fromMsg(msg, out); } inline geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { return tf2::toMsg(in); } inline void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { tf2::fromMsg(msg, out); } } // namespace #endif // TF2_EIGEN_H geometry2-0.7.5/tf2_eigen/mainpage.dox000066400000000000000000000013621372352374300176120ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_eigen contains functions for converting between geometry_msgs and Eigen data types. This library is an implementation of the templated conversion interface specified in tf/convert.h. It enables easy conversion from geometry_msgs Transform and Point types to the types specified by the Eigen matrix algebra library (see http://eigen.tuxfamily.org). See the Conversions overview wiki page for more information about datatype conversion in tf2. \section codeapi Code API This library consists of one header only, tf2_eigen/tf2_eigen.h, which consists mostly of specializations of template functions defined in tf2/convert.h. */ geometry2-0.7.5/tf2_eigen/package.xml000066400000000000000000000007731372352374300174370ustar00rootroot00000000000000 tf2_eigen 0.7.5 tf2_eigen Koji Terada Koji Terada BSD catkin geometry_msgs tf2 cmake_modules eigen eigen geometry2-0.7.5/tf2_eigen/test/000077500000000000000000000000001372352374300162725ustar00rootroot00000000000000geometry2-0.7.5/tf2_eigen/test/tf2_eigen-test.cpp000066400000000000000000000147071372352374300216260ustar00rootroot00000000000000/* * Copyright (c) Koji Terada * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #include #include #include TEST(TfEigen, ConvertVector3dStamped) { const tf2::Stamped v(Eigen::Vector3d(1,2,3), ros::Time(5), "test"); tf2::Stamped v1; geometry_msgs::PointStamped p1; tf2::convert(v, p1); tf2::convert(p1, v1); EXPECT_EQ(v, v1); } TEST(TfEigen, ConvertVector3d) { const Eigen::Vector3d v(1,2,3); Eigen::Vector3d v1; geometry_msgs::Point p1; tf2::convert(v, p1); tf2::convert(p1, v1); EXPECT_EQ(v, v1); } TEST(TfEigen, ConvertQuaterniondStamped) { const tf2::Stamped v(Eigen::Quaterniond(1,2,3,4), ros::Time(5), "test"); tf2::Stamped v1; geometry_msgs::QuaternionStamped p1; tf2::convert(v, p1); tf2::convert(p1, v1); EXPECT_EQ(v.frame_id_, v1.frame_id_); EXPECT_EQ(v.stamp_, v1.stamp_); EXPECT_EQ(v.w(), v1.w()); EXPECT_EQ(v.x(), v1.x()); EXPECT_EQ(v.y(), v1.y()); EXPECT_EQ(v.z(), v1.z()); } TEST(TfEigen, ConvertQuaterniond) { const Eigen::Quaterniond v(1,2,3,4); Eigen::Quaterniond v1; geometry_msgs::Quaternion p1; tf2::convert(v, p1); tf2::convert(p1, v1); EXPECT_EQ(v.w(), v1.w()); EXPECT_EQ(v.x(), v1.x()); EXPECT_EQ(v.y(), v1.y()); EXPECT_EQ(v.z(), v1.z()); } TEST(TfEigen, TransformQuaterion) { const tf2::Stamped in(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitX())), ros::Time(5), "test"); const Eigen::Isometry3d iso(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY())); const Eigen::Affine3d affine(iso); const tf2::Stamped expected(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitZ())), ros::Time(10), "expected"); geometry_msgs::TransformStamped trafo = tf2::eigenToTransform(affine); trafo.header.stamp = ros::Time(10); trafo.header.frame_id = "expected"; tf2::Stamped out; tf2::doTransform(in, out, trafo); EXPECT_TRUE(out.isApprox(expected)); EXPECT_EQ(expected.stamp_, out.stamp_); EXPECT_EQ(expected.frame_id_, out.frame_id_); // the same using Isometry trafo = tf2::eigenToTransform(iso); trafo.header.stamp = ros::Time(10); trafo.header.frame_id = "expected"; tf2::doTransform(in, out, trafo); EXPECT_TRUE(out.isApprox(expected)); EXPECT_EQ(expected.stamp_, out.stamp_); EXPECT_EQ(expected.frame_id_, out.frame_id_); } TEST(TfEigen, ConvertAffine3dStamped) { const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); const tf2::Stamped v(v_nonstamped, ros::Time(42), "test_frame"); tf2::Stamped v1; geometry_msgs::PoseStamped p1; tf2::convert(v, p1); tf2::convert(p1, v1); EXPECT_EQ(v.translation(), v1.translation()); EXPECT_EQ(v.rotation(), v1.rotation()); EXPECT_EQ(v.frame_id_, v1.frame_id_); EXPECT_EQ(v.stamp_, v1.stamp_); } TEST(TfEigen, ConvertIsometry3dStamped) { const Eigen::Isometry3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); const tf2::Stamped v(v_nonstamped, ros::Time(42), "test_frame"); tf2::Stamped v1; geometry_msgs::PoseStamped p1; tf2::convert(v, p1); tf2::convert(p1, v1); EXPECT_EQ(v.translation(), v1.translation()); EXPECT_EQ(v.rotation(), v1.rotation()); EXPECT_EQ(v.frame_id_, v1.frame_id_); EXPECT_EQ(v.stamp_, v1.stamp_); } TEST(TfEigen, ConvertAffine3d) { const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); Eigen::Affine3d v1; geometry_msgs::Pose p1; tf2::convert(v, p1); tf2::convert(p1, v1); EXPECT_EQ(v.translation(), v1.translation()); EXPECT_EQ(v.rotation(), v1.rotation()); } TEST(TfEigen, ConvertIsometry3d) { const Eigen::Isometry3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); Eigen::Isometry3d v1; geometry_msgs::Pose p1; tf2::convert(v, p1); tf2::convert(p1, v1); EXPECT_EQ(v.translation(), v1.translation()); EXPECT_EQ(v.rotation(), v1.rotation()); } TEST(TfEigen, ConvertTransform) { Eigen::Matrix4d tm; double alpha = M_PI/4.0; double theta = M_PI/6.0; double gamma = M_PI/12.0; tm << cos(theta)*cos(gamma),-cos(theta)*sin(gamma),sin(theta), 1, // cos(alpha)*sin(gamma)+sin(alpha)*sin(theta)*cos(gamma),cos(alpha)*cos(gamma)-sin(alpha)*sin(theta)*sin(gamma),-sin(alpha)*cos(theta), 2, // sin(alpha)*sin(gamma)-cos(alpha)*sin(theta)*cos(gamma),cos(alpha)*sin(theta)*sin(gamma)+sin(alpha)*cos(gamma),cos(alpha)*cos(theta), 3, // 0, 0, 0, 1; Eigen::Affine3d T(tm); geometry_msgs::TransformStamped msg = tf2::eigenToTransform(T); Eigen::Affine3d Tback = tf2::transformToEigen(msg); EXPECT_TRUE(T.isApprox(Tback)); EXPECT_TRUE(tm.isApprox(Tback.matrix())); // same for Isometry Eigen::Isometry3d I(tm); msg = tf2::eigenToTransform(T); Eigen::Isometry3d Iback = tf2::transformToEigen(msg); EXPECT_TRUE(I.isApprox(Iback)); EXPECT_TRUE(tm.isApprox(Iback.matrix())); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2_geometry_msgs/000077500000000000000000000000001372352374300171105ustar00rootroot00000000000000geometry2-0.7.5/tf2_geometry_msgs/CHANGELOG.rst000066400000000000000000000172561372352374300211440ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_geometry_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ * Use list instead of set to make build reproducible (`#473 `_) * Contributors: Jochen Sprickerhof 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ * import setup from setuptools instead of distutils-core (`#449 `_) * Contributors: Alejandro Hernández Cordero 0.7.0 (2020-03-09) ------------------ * Replace kdl packages with rosdep keys (`#447 `_) * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Make kdl headers available (`#419 `_) * FIx python3 compatibility for noetic (`#416 `_) * add from STL (`#366 `_) * use ROS_DEPRECATED macro for portability (`#362 `_) * use ROS_DEPRECATED for better portability * change ROS_DEPRECATED position (`#5 `_) * Contributors: James Xu, Shane Loretz, Tully Foote 0.6.5 (2018-11-16) ------------------ * Fix python3 import error * Contributors: Timon Engelke 0.6.4 (2018-11-06) ------------------ 0.6.3 (2018-07-09) ------------------ * Changed access to Vector to prevent memory leak (`#305 `_) * Added WrenchStamped transformation (`#302 `_) * Contributors: Denis Štogl, Markus Grimm 0.6.2 (2018-05-02) ------------------ 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ * Boilerplate for Sphinx (`#284 `_) Fixes `#264 `_ * tf2_geometry_msgs added doTransform implementations for not stamped types (`#262 `_) * tf2_geometry_msgs added doTransform implementations for not stamped Point, Quaterion, Pose and Vector3 message types * New functionality to transform PoseWithCovarianceStamped messages. (`#282 `_) * New functionality to transform PoseWithCovarianceStamped messages. * Contributors: Blake Anderson, Tully Foote, cwecht 0.5.17 (2018-01-01) ------------------- 0.5.16 (2017-07-14) ------------------- * remove explicit templating to standardize on overloading. But provide backwards compatibility with deprecation. * adding unit tests for conversions * Copy transform before altering it in do_transform_vector3 [issue 233] (`#235 `_) * store gtest return value as int (`#229 `_) * Document the lifetime of the returned reference for getFrameId and getTimestamp * tf2_geometry_msgs: using tf2::Transform in doTransform-functions, marked gmTransformToKDL as deprecated * Switch tf2_geometry_msgs to use package.xml format 2 (`#217 `_) * tf2_geometry_msgs: added missing conversion functions * Contributors: Christopher Wecht, Sebastian Wagner, Tully Foote, dhood, pAIgn10 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- * Add doxygen documentation for tf2_geometry_msgs * Contributors: Jackie Kay 0.5.13 (2016-03-04) ------------------- * Add missing python_orocos_kdl dependency * make example into unit test * vector3 not affected by translation * Contributors: Daniel Claes, chapulina 0.5.12 (2015-08-05) ------------------- * Merge pull request `#112 `_ from vrabaud/getYaw Get yaw * add toMsg and fromMsg for QuaternionStamped * Contributors: Tully Foote, Vincent Rabaud 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- 0.5.9 (2015-03-25) ------------------ 0.5.8 (2015-03-17) ------------------ * remove useless Makefile files * tf2 optimizations * add conversions of type between tf2 and geometry_msgs * fix ODR violations * Contributors: Vincent Rabaud 0.5.7 (2014-12-23) ------------------ * fixing transitive dependency for kdl. Fixes `#53 `_ * Contributors: Tully Foote 0.5.6 (2014-09-18) ------------------ 0.5.5 (2014-06-23) ------------------ 0.5.4 (2014-05-07) ------------------ 0.5.3 (2014-02-21) ------------------ 0.5.2 (2014-02-20) ------------------ 0.5.1 (2014-02-14) ------------------ 0.5.0 (2014-02-14) ------------------ 0.4.10 (2013-12-26) ------------------- 0.4.9 (2013-11-06) ------------------ 0.4.8 (2013-11-06) ------------------ 0.4.7 (2013-08-28) ------------------ 0.4.6 (2013-08-28) ------------------ 0.4.5 (2013-07-11) ------------------ 0.4.4 (2013-07-09) ------------------ * making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ 0.4.1 (2013-07-05) ------------------ 0.4.0 (2013-06-27) ------------------ * moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 * Cleaning up unnecessary dependency on roscpp * converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace * Cleaning up packaging of tf2 including: removing unused nodehandle cleaning up a few dependencies and linking removing old backup of package.xml making diff minimally different from tf version of library * Restoring test packages and bullet packages. reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ 0.3.6 (2013-03-03) ------------------ 0.3.5 (2013-02-15 14:46) ------------------------ * 0.3.4 -> 0.3.5 0.3.4 (2013-02-15 13:14) ------------------------ * 0.3.3 -> 0.3.4 0.3.3 (2013-02-15 11:30) ------------------------ * 0.3.2 -> 0.3.3 0.3.2 (2013-02-15 00:42) ------------------------ * 0.3.1 -> 0.3.2 0.3.1 (2013-02-14) ------------------ * 0.3.0 -> 0.3.1 0.3.0 (2013-02-13) ------------------ * switching to version 0.3.0 * add setup.py * added setup.py etc to tf2_geometry_msgs * adding tf2 dependency to tf2_geometry_msgs * adding tf2_geometry_msgs to groovy-devel (unit tests disabled) * fixing groovy-devel * removing bullet and kdl related packages * disabling tf2_geometry_msgs due to missing kdl dependency * catkinizing geometry-experimental * catkinizing tf2_geometry_msgs * add twist, wrench and pose conversion to kdl, fix message to message conversion by adding specific conversion functions * merge tf2_cpp and tf2_py into tf2_ros * Got transform with types working in python * A working first version of transforming and converting between different types * Moving from camelCase to undescores to be in line with python style guides * Fixing tests now that Buffer creates a NodeHandle * add posestamped * import vector3stamped * add support for Vector3Stamped and PoseStamped * add support for PointStamped geometry_msgs * add regression tests for geometry_msgs point, vector and pose * Fixing missing export, compiling version of buffer_client test * add bullet transforms, and create tests for bullet and kdl * working transformations of messages * add support for PoseStamped message * test for pointstamped * add PointStamped message transform methods * transform for vector3stamped message geometry2-0.7.5/tf2_geometry_msgs/CMakeLists.txt000066400000000000000000000027531372352374300216570ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_geometry_msgs) find_package(orocos_kdl) find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2_ros tf2) find_package(Boost COMPONENTS thread REQUIRED) # Issue #53 find_library(KDL_LIBRARY REQUIRED NAMES orocos-kdl HINTS ${orocos_kdl_LIBRARY_DIRS}) catkin_package( LIBRARIES ${KDL_LIBRARY} INCLUDE_DIRS include DEPENDS orocos_kdl CATKIN_DEPENDS geometry_msgs tf2_ros tf2) include_directories(include ${catkin_INCLUDE_DIRS} ) link_directories(${orocos_kdl_LIBRARY_DIRS}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) catkin_python_setup() if(CATKIN_ENABLE_TESTING) catkin_add_gtest(test_tomsg_frommsg test/test_tomsg_frommsg.cpp) target_include_directories(test_tomsg_frommsg PUBLIC ${orocos_kdl_INCLUDE_DIRS}) target_link_libraries(test_tomsg_frommsg ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) find_package(catkin REQUIRED COMPONENTS geometry_msgs rostest tf2_ros tf2) add_executable(test_geometry_msgs EXCLUDE_FROM_ALL test/test_tf2_geometry_msgs.cpp) target_include_directories(test_geometry_msgs PUBLIC ${orocos_kdl_INCLUDE_DIRS}) target_link_libraries(test_geometry_msgs ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_python.launch) if(TARGET tests) add_dependencies(tests test_geometry_msgs) endif() endif() geometry2-0.7.5/tf2_geometry_msgs/conf.py000066400000000000000000000224501372352374300204120ustar00rootroot00000000000000# -*- coding: utf-8 -*- # # tf2_geometry_msgs documentation build configuration file, created by # sphinx-quickstart on Tue Feb 13 15:34:25 2018. # # This file is execfile()d with the current directory set to its # containing dir. # # Note that not all possible configuration values are present in this # autogenerated file. # # All configuration values have a default; values that are commented out # serve to show the default. import sys import os # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. #sys.path.insert(0, os.path.abspath('.')) # -- General configuration ------------------------------------------------ # If your documentation needs a minimal Sphinx version, state it here. #needs_sphinx = '1.0' # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ 'sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.todo', 'sphinx.ext.pngmath', 'sphinx.ext.viewcode', ] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # source_suffix = ['.rst', '.md'] source_suffix = '.rst' # The encoding of source files. #source_encoding = 'utf-8-sig' # The master toctree document. master_doc = 'index' # General information about the project. project = u'tf2_geometry_msgs' copyright = u'2018, Open Source Robotics Foundation, Inc.' author = u'Tully Foote' # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. # # The short X.Y version. version = u'0.1' # The full version, including alpha/beta/rc tags. release = u'0.1' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. # # This is also used if you do content translation via gettext catalogs. # Usually you set "language" from the command line for these cases. language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: #today = '' # Else, today_fmt is used as the format for a strftime call. #today_fmt = '%B %d, %Y' # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. exclude_patterns = ['_build'] # The reST default role (used for this markup: `text`) to use for all # documents. #default_role = None # If true, '()' will be appended to :func: etc. cross-reference text. #add_function_parentheses = True # If true, the current module name will be prepended to all description # unit titles (such as .. function::). #add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. #show_authors = False # The name of the Pygments (syntax highlighting) style to use. pygments_style = 'sphinx' # A list of ignored prefixes for module index sorting. #modindex_common_prefix = [] # If true, keep warnings as "system message" paragraphs in the built documents. #keep_warnings = False # -- Options for HTML output --------------------------------------------------- # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. html_theme = 'default' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the # documentation. #html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. #html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". #html_title = None # A shorter title for the navigation bar. Default is the same as html_title. #html_short_title = None # The name of an image file (relative to this directory) to place at the top # of the sidebar. #html_logo = None # The name of an image file (relative to this directory) to use as a favicon of # the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. #html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ['_static'] # Add any extra paths that contain custom files (such as robots.txt or # .htaccess) here, relative to this directory. These files are copied # directly to the root of the documentation. #html_extra_path = [] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. #html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. #html_use_smartypants = True # Custom sidebar templates, maps document names to template names. #html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. #html_additional_pages = {} # If false, no module index is generated. #html_domain_indices = True # If false, no index is generated. #html_use_index = True # If true, the index is split into individual pages for each letter. #html_split_index = False # If true, links to the reST sources are added to the pages. #html_show_sourcelink = True # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. #html_show_sphinx = True # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. #html_show_copyright = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. #html_use_opensearch = '' # This is the file name suffix for HTML files (e.g. ".xhtml"). #html_file_suffix = None # Language to be used for generating the HTML full-text search index. # Sphinx supports the following languages: # 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' # 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' #html_search_language = 'en' # A dictionary with options for the search language support, empty by default. # Now only 'ja' uses this config value #html_search_options = {'type': 'default'} # The name of a javascript file (relative to the configuration directory) that # implements a search results scorer. If empty, the default will be used. #html_search_scorer = 'scorer.js' # Output file base name for HTML help builder. htmlhelp_basename = 'tf2_geometry_msgsdoc' # -- Options for LaTeX output --------------------------------------------- latex_elements = { # The paper size ('letterpaper' or 'a4paper'). #'papersize': 'letterpaper', # The font size ('10pt', '11pt' or '12pt'). #'pointsize': '10pt', # Additional stuff for the LaTeX preamble. #'preamble': '', # Latex figure (float) alignment #'figure_align': 'htbp', } # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, # author, documentclass [howto, manual, or own class]). latex_documents = [ (master_doc, 'tf2_geometry_msgs.tex', u'tf2\\_geometry\\_msgs Documentation', u'Tully Foote', 'manual'), ] # The name of an image file (relative to this directory) to place at the top of # the title page. #latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. #latex_use_parts = False # If true, show page references after internal links. #latex_show_pagerefs = False # If true, show URL addresses after external links. #latex_show_urls = False # Documents to append as an appendix to all manuals. #latex_appendices = [] # If false, no module index is generated. #latex_domain_indices = True # -- Options for manual page output --------------------------------------- # One entry per manual page. List of tuples # (source start file, name, description, authors, manual section). man_pages = [ (master_doc, 'tf2_geometry_msgs', u'tf2_geometry_msgs Documentation', [author], 1) ] # If true, show URL addresses after external links. #man_show_urls = False # -- Options for Texinfo output ------------------------------------------- # Grouping the document tree into Texinfo files. List of tuples # (source start file, target name, title, author, # dir menu entry, description, category) texinfo_documents = [ (master_doc, 'tf2_geometry_msgs', u'tf2_geometry_msgs Documentation', author, 'tf2_geometry_msgs', 'One line description of project.', 'Miscellaneous'), ] # Documents to append as an appendix to all manuals. #texinfo_appendices = [] # If false, no module index is generated. #texinfo_domain_indices = True # How to display URL addresses: 'footnote', 'no', or 'inline'. #texinfo_show_urls = 'footnote' # If true, do not generate a @detailmenu in the "Top" node's menu. #texinfo_no_detailmenu = False # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = {'https://docs.python.org/': None} geometry2-0.7.5/tf2_geometry_msgs/include/000077500000000000000000000000001372352374300205335ustar00rootroot00000000000000geometry2-0.7.5/tf2_geometry_msgs/include/tf2_geometry_msgs/000077500000000000000000000000001372352374300241725ustar00rootroot00000000000000geometry2-0.7.5/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h000066400000000000000000001130611372352374300300040ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #ifndef TF2_GEOMETRY_MSGS_H #define TF2_GEOMETRY_MSGS_H #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "ros/macros.h" namespace tf2 { /** \brief Convert a TransformStamped message to a KDL frame. * \param t TransformStamped message to convert. * \return The converted KDL Frame. * \deprecated */ inline ROS_DEPRECATED KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t); inline KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) { return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); } /*************/ /** Vector3 **/ /*************/ /** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A tf2 Vector3 object. * \return The Vector3 converted to a geometry_msgs message type. */ inline geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) { geometry_msgs::Vector3 out; out.x = in.getX(); out.y = in.getY(); out.z = in.getZ(); return out; } /** \brief Convert a Vector3 message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param in A Vector3 message type. * \param out The Vector3 converted to a tf2 type. */ inline void fromMsg(const geometry_msgs::Vector3& in, tf2::Vector3& out) { out = tf2::Vector3(in.x, in.y, in.z); } /********************/ /** Vector3Stamped **/ /********************/ /** \brief Extract a timestamp from the header of a Vector message. * This function is a specialization of the getTimestamp template defined in tf2/convert.h. * \param t VectorStamped message to extract the timestamp from. * \return The timestamp of the message. The lifetime of the returned reference * is bound to the lifetime of the argument. */ template <> inline const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Vector message. * This function is a specialization of the getFrameId template defined in tf2/convert.h. * \param t VectorStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. The lifetime of the * returned reference is bound to the lifetime of the argument. */ template <> inline const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;} /** \brief Trivial "conversion" function for Vector3 message type. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A Vector3Stamped message. * \return The input argument. */ inline geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in) { return in; } /** \brief Trivial "conversion" function for Vector3 message type. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg A Vector3Stamped message. * \param out The input argument. */ inline void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out) { out = msg; } /** \brief Convert as stamped tf2 Vector3 type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in An instance of the tf2::Vector3 specialization of the tf2::Stamped template. * \return The Vector3Stamped converted to a geometry_msgs Vector3Stamped message type. */ inline geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped& in) { geometry_msgs::Vector3Stamped out; out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; out.vector.x = in.getX(); out.vector.y = in.getY(); out.vector.z = in.getZ(); return out; } /** \brief Convert a Vector3Stamped message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg A Vector3Stamped message. * \param out The Vector3Stamped converted to the equivalent tf2 type. */ inline void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out.setData(tf2::Vector3(msg.vector.x, msg.vector.y, msg.vector.z)); } /***********/ /** Point **/ /***********/ /** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A tf2 Vector3 object. * \return The Vector3 converted to a geometry_msgs message type. */ inline geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) { out.x = in.getX(); out.y = in.getY(); out.z = in.getZ(); return out; } /** \brief Convert a Vector3 message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param in A Vector3 message type. * \param out The Vector3 converted to a tf2 type. */ inline void fromMsg(const geometry_msgs::Point& in, tf2::Vector3& out) { out = tf2::Vector3(in.x, in.y, in.z); } /******************/ /** PointStamped **/ /******************/ /** \brief Extract a timestamp from the header of a Point message. * This function is a specialization of the getTimestamp template defined in tf2/convert.h. * \param t PointStamped message to extract the timestamp from. * \return The timestamp of the message. The lifetime of the returned reference * is bound to the lifetime of the argument. */ template <> inline const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Point message. * This function is a specialization of the getFrameId template defined in tf2/convert.h. * \param t PointStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. The lifetime of the * returned reference is bound to the lifetime of the argument. */ template <> inline const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;} /** \brief Trivial "conversion" function for Point message type. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A PointStamped message. * \return The input argument. */ inline geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in) { return in; } /** \brief Trivial "conversion" function for Point message type. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg A PointStamped message. * \param out The input argument. */ inline void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out) { out = msg; } /** \brief Convert as stamped tf2 Vector3 type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in An instance of the tf2::Vector3 specialization of the tf2::Stamped template. * \return The Vector3Stamped converted to a geometry_msgs PointStamped message type. */ inline geometry_msgs::PointStamped toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped & out) { out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; out.point.x = in.getX(); out.point.y = in.getY(); out.point.z = in.getZ(); return out; } /** \brief Convert a PointStamped message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg A PointStamped message. * \param out The PointStamped converted to the equivalent tf2 type. */ inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out.setData(tf2::Vector3(msg.point.x, msg.point.y, msg.point.z)); } /****************/ /** Quaternion **/ /****************/ /** \brief Convert a tf2 Quaternion type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A tf2 Quaternion object. * \return The Quaternion converted to a geometry_msgs message type. */ inline geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in) { geometry_msgs::Quaternion out; out.w = in.getW(); out.x = in.getX(); out.y = in.getY(); out.z = in.getZ(); return out; } /** \brief Convert a Quaternion message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param in A Quaternion message type. * \param out The Quaternion converted to a tf2 type. */ inline void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out) { // w at the end in the constructor out = tf2::Quaternion(in.x, in.y, in.z, in.w); } /***********************/ /** QuaternionStamped **/ /***********************/ /** \brief Extract a timestamp from the header of a Quaternion message. * This function is a specialization of the getTimestamp template defined in tf2/convert.h. * \param t QuaternionStamped message to extract the timestamp from. * \return The timestamp of the message. The lifetime of the returned reference * is bound to the lifetime of the argument. */ template <> inline const ros::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Quaternion message. * This function is a specialization of the getFrameId template defined in tf2/convert.h. * \param t QuaternionStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. The lifetime of the * returned reference is bound to the lifetime of the argument. */ template <> inline const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;} /** \brief Trivial "conversion" function for Quaternion message type. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A QuaternionStamped message. * \return The input argument. */ inline geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in) { return in; } /** \brief Trivial "conversion" function for Quaternion message type. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg A QuaternionStamped message. * \param out The input argument. */ inline void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out) { out = msg; } /** \brief Convert as stamped tf2 Quaternion type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in An instance of the tf2::Quaternion specialization of the tf2::Stamped template. * \return The QuaternionStamped converted to a geometry_msgs QuaternionStamped message type. */ inline geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) { geometry_msgs::QuaternionStamped out; out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; out.quaternion.w = in.getW(); out.quaternion.x = in.getX(); out.quaternion.y = in.getY(); out.quaternion.z = in.getZ(); return out; } template <> inline ROS_DEPRECATED geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in); //Backwards compatibility remove when forked for Lunar or newer template <> inline geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) { return toMsg(in); } /** \brief Convert a QuaternionStamped message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param in A QuaternionStamped message type. * \param out The QuaternionStamped converted to the equivalent tf2 type. */ inline void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) { out.stamp_ = in.header.stamp; out.frame_id_ = in.header.frame_id; tf2::Quaternion tmp; fromMsg(in.quaternion, tmp); out.setData(tmp); } template<> inline ROS_DEPRECATED void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out); //Backwards compatibility remove when forked for Lunar or newer template<> inline void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) { fromMsg(in, out); } /**********/ /** Pose **/ /**********/ /** \brief Convert a tf2 Transform type to an equivalent geometry_msgs Pose message. * \param in A tf2 Transform object. * \param out The Transform converted to a geometry_msgs Pose message type. */ inline geometry_msgs::Pose& toMsg(const tf2::Transform& in, geometry_msgs::Pose& out) { toMsg(in.getOrigin(), out.position); out.orientation = toMsg(in.getRotation()); return out; } /** \brief Convert a geometry_msgs Pose message to an equivalent tf2 Transform type. * \param in A Pose message. * \param out The Pose converted to a tf2 Transform type. */ inline void fromMsg(const geometry_msgs::Pose& in, tf2::Transform& out) { out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z)); // w at the end in the constructor out.setRotation(tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w)); } /*****************/ /** PoseStamped **/ /*****************/ /** \brief Extract a timestamp from the header of a Pose message. * This function is a specialization of the getTimestamp template defined in tf2/convert.h. * \param t PoseStamped message to extract the timestamp from. * \return The timestamp of the message. */ template <> inline const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Pose message. * This function is a specialization of the getFrameId template defined in tf2/convert.h. * \param t PoseStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. */ template <> inline const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;} /** \brief Trivial "conversion" function for Pose message type. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A PoseStamped message. * \return The input argument. */ inline geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in) { return in; } /** \brief Trivial "conversion" function for Pose message type. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg A PoseStamped message. * \param out The input argument. */ inline void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out) { out = msg; } /** \brief Convert as stamped tf2 Pose type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in An instance of the tf2::Pose specialization of the tf2::Stamped template. * \return The PoseStamped converted to a geometry_msgs PoseStamped message type. */ inline geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped & out) { out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; toMsg(in.getOrigin(), out.pose.position); out.pose.orientation = toMsg(in.getRotation()); return out; } /** \brief Convert a PoseStamped message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg A PoseStamped message. * \param out The PoseStamped converted to the equivalent tf2 type. */ inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; tf2::Transform tmp; fromMsg(msg.pose, tmp); out.setData(tmp); } /*******************************/ /** PoseWithCovarianceStamped **/ /*******************************/ /** \brief Extract a timestamp from the header of a PoseWithCovarianceStamped message. * This function is a specialization of the getTimestamp template defined in tf2/convert.h. * \param t PoseWithCovarianceStamped message to extract the timestamp from. * \return The timestamp of the message. */ template <> inline const ros::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a PoseWithCovarianceStamped message. * This function is a specialization of the getFrameId template defined in tf2/convert.h. * \param t PoseWithCovarianceStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. */ template <> inline const std::string& getFrameId(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.frame_id;} /** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A PoseWithCovarianceStamped message. * \return The input argument. */ inline geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCovarianceStamped& in) { return in; } /** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg A PoseWithCovarianceStamped message. * \param out The input argument. */ inline void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs::PoseWithCovarianceStamped& out) { out = msg; } /** \brief Convert as stamped tf2 PoseWithCovarianceStamped type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in An instance of the tf2::Pose specialization of the tf2::Stamped template. * \return The PoseWithCovarianceStamped converted to a geometry_msgs PoseWithCovarianceStamped message type. */ inline geometry_msgs::PoseWithCovarianceStamped toMsg(const tf2::Stamped& in, geometry_msgs::PoseWithCovarianceStamped & out) { out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; toMsg(in.getOrigin(), out.pose.pose.position); out.pose.pose.orientation = toMsg(in.getRotation()); return out; } /** \brief Convert a PoseWithCovarianceStamped message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg A PoseWithCovarianceStamped message. * \param out The PoseWithCovarianceStamped converted to the equivalent tf2 type. */ inline void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; tf2::Transform tmp; fromMsg(msg.pose, tmp); out.setData(tmp); } /***************/ /** Transform **/ /***************/ /** \brief Convert a tf2 Transform type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A tf2 Transform object. * \return The Transform converted to a geometry_msgs message type. */ inline geometry_msgs::Transform toMsg(const tf2::Transform& in) { geometry_msgs::Transform out; out.translation = toMsg(in.getOrigin()); out.rotation = toMsg(in.getRotation()); return out; } /** \brief Convert a Transform message to its equivalent tf2 representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A Transform message type. * \param out The Transform converted to a tf2 type. */ inline void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) { tf2::Vector3 v; fromMsg(in.translation, v); out.setOrigin(v); // w at the end in the constructor tf2::Quaternion q; fromMsg(in.rotation, q); out.setRotation(q); } /**********************/ /** TransformStamped **/ /**********************/ /** \brief Extract a timestamp from the header of a Transform message. * This function is a specialization of the getTimestamp template defined in tf2/convert.h. * \param t TransformStamped message to extract the timestamp from. * \return The timestamp of the message. */ template <> inline const ros::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Transform message. * This function is a specialization of the getFrameId template defined in tf2/convert.h. * \param t TransformStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. */ template <> inline const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;} /** \brief Trivial "conversion" function for Transform message type. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in A TransformStamped message. * \return The input argument. */ inline geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in) { return in; } /** \brief Convert a TransformStamped message to its equivalent tf2 representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param msg A TransformStamped message type. * \param out The TransformStamped converted to the equivalent tf2 type. */ inline void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out) { out = msg; } /** \brief Convert as stamped tf2 Transform type to its equivalent geometry_msgs representation. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in An instance of the tf2::Transform specialization of the tf2::Stamped template. * \return The tf2::Stamped converted to a geometry_msgs TransformStamped message type. */ inline geometry_msgs::TransformStamped toMsg(const tf2::Stamped& in) { geometry_msgs::TransformStamped out; out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; out.transform.translation = toMsg(in.getOrigin()); out.transform.rotation = toMsg(in.getRotation()); return out; } /** \brief Convert a TransformStamped message to its equivalent tf2 representation. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg A TransformStamped message. * \param out The TransformStamped converted to the equivalent tf2 type. */ inline void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; tf2::Transform tmp; fromMsg(msg.transform, tmp); out.setData(tmp); } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Point type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The point to transform, as a Point3 message. * \param t_out The transformed point, as a Point3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Transform t; fromMsg(transform.transform, t); tf2::Vector3 v_in; fromMsg(t_in, v_in); tf2::Vector3 v_out = t * v_in; toMsg(v_out, t_out); } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Point type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The point to transform, as a timestamped Point3 message. * \param t_out The transformed point, as a timestamped Point3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform) { doTransform(t_in.point, t_out.point, transform); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The quaternion to transform, as a Quaternion3 message. * \param t_out The transformed quaternion, as a Quaternion3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternion& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Quaternion t, q_in; fromMsg(transform.transform.rotation, t); fromMsg(t_in, q_in); tf2::Quaternion q_out = t * q_in; t_out = toMsg(q_out); } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Quaternion type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The quaternion to transform, as a timestamped Quaternion3 message. * \param t_out The transformed quaternion, as a timestamped Quaternion3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const geometry_msgs::TransformStamped& transform) { doTransform(t_in.quaternion, t_out.quaternion, transform); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The pose to transform, as a Pose3 message. * \param t_out The transformed pose, as a Pose3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Vector3 v; fromMsg(t_in.position, v); tf2::Quaternion r; fromMsg(t_in.orientation, r); tf2::Transform t; fromMsg(transform.transform, t); tf2::Transform v_out = t * tf2::Transform(r, v); toMsg(v_out, t_out); } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Pose type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The pose to transform, as a timestamped Pose3 message. * \param t_out The transformed pose, as a timestamped Pose3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform) { doTransform(t_in.pose, t_out.pose, transform); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } /** \brief Transform the covariance matrix of a PoseWithCovarianceStamped message to a new frame. * \param t_in The covariance matrix to transform. * \param transform The timestamped transform to apply, as a TransformStamped message. * \return The transformed covariance matrix. */ inline geometry_msgs::PoseWithCovariance::_covariance_type transformCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type& cov_in, const tf2::Transform& transform) { /** * To transform a covariance matrix: * * [R 0] COVARIANCE [R' 0 ] * [0 R] [0 R'] * * Where: * R is the rotation matrix (3x3). * R' is the transpose of the rotation matrix. * COVARIANCE is the 6x6 covariance matrix to be transformed. */ // get rotation matrix transpose const tf2::Matrix3x3 R_transpose = transform.getBasis().transpose(); // convert the covariance matrix into four 3x3 blocks const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2], cov_in[6], cov_in[7], cov_in[8], cov_in[12], cov_in[13], cov_in[14]); const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5], cov_in[9], cov_in[10], cov_in[11], cov_in[15], cov_in[16], cov_in[17]); const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20], cov_in[24], cov_in[25], cov_in[26], cov_in[30], cov_in[31], cov_in[32]); const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23], cov_in[27], cov_in[28], cov_in[29], cov_in[33], cov_in[34], cov_in[35]); // perform blockwise matrix multiplication const tf2::Matrix3x3 result_11 = transform.getBasis()*cov_11*R_transpose; const tf2::Matrix3x3 result_12 = transform.getBasis()*cov_12*R_transpose; const tf2::Matrix3x3 result_21 = transform.getBasis()*cov_21*R_transpose; const tf2::Matrix3x3 result_22 = transform.getBasis()*cov_22*R_transpose; // form the output geometry_msgs::PoseWithCovariance::_covariance_type output; output[0] = result_11[0][0]; output[1] = result_11[0][1]; output[2] = result_11[0][2]; output[6] = result_11[1][0]; output[7] = result_11[1][1]; output[8] = result_11[1][2]; output[12] = result_11[2][0]; output[13] = result_11[2][1]; output[14] = result_11[2][2]; output[3] = result_12[0][0]; output[4] = result_12[0][1]; output[5] = result_12[0][2]; output[9] = result_12[1][0]; output[10] = result_12[1][1]; output[11] = result_12[1][2]; output[15] = result_12[2][0]; output[16] = result_12[2][1]; output[17] = result_12[2][2]; output[18] = result_21[0][0]; output[19] = result_21[0][1]; output[20] = result_21[0][2]; output[24] = result_21[1][0]; output[25] = result_21[1][1]; output[26] = result_21[1][2]; output[30] = result_21[2][0]; output[31] = result_21[2][1]; output[32] = result_21[2][2]; output[21] = result_22[0][0]; output[22] = result_22[0][1]; output[23] = result_22[0][2]; output[27] = result_22[1][0]; output[28] = result_22[1][1]; output[29] = result_22[1][2]; output[33] = result_22[2][0]; output[34] = result_22[2][1]; output[35] = result_22[2][2]; return output; } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs PoseWithCovarianceStamped type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The pose to transform, as a timestamped PoseWithCovarianceStamped message. * \param t_out The transformed pose, as a timestamped PoseWithCovarianceStamped message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_msgs::PoseWithCovarianceStamped& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Vector3 v; fromMsg(t_in.pose.pose.position, v); tf2::Quaternion r; fromMsg(t_in.pose.pose.orientation, r); tf2::Transform t; fromMsg(transform.transform, t); tf2::Transform v_out = t * tf2::Transform(r, v); toMsg(v_out, t_out.pose.pose); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; t_out.pose.covariance = transformCovariance(t_in.pose.covariance, t); } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Transform type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The frame to transform, as a timestamped Transform3 message. * \param t_out The frame transform, as a timestamped Transform3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Transform input; fromMsg(t_in.transform, input); tf2::Transform t; fromMsg(transform.transform, t); tf2::Transform v_out = t * input; t_out.transform = toMsg(v_out); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The vector to transform, as a Vector3 message. * \param t_out The transformed vector, as a Vector3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Transform t; fromMsg(transform.transform, t); tf2::Vector3 v_out = t.getBasis() * tf2::Vector3(t_in.x, t_in.y, t_in.z); t_out.x = v_out[0]; t_out.y = v_out[1]; t_out.z = v_out[2]; } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Vector type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The vector to transform, as a timestamped Vector3 message. * \param t_out The transformed vector, as a timestamped Vector3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform) { doTransform(t_in.vector, t_out.vector, transform); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } /**********************/ /*** WrenchStamped ****/ /**********************/ template <> inline const ros::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return t.header.stamp;} template <> inline const std::string& getFrameId(const geometry_msgs::WrenchStamped& t) {return t.header.frame_id;} inline geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in) { return in; } inline void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out) { out = msg; } inline geometry_msgs::WrenchStamped toMsg(const tf2::Stamped>& in, geometry_msgs::WrenchStamped & out) { out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; out.wrench.force = toMsg(in[0]); out.wrench.torque = toMsg(in[1]); return out; } inline void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped>& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; tf2::Vector3 tmp; fromMsg(msg.wrench.force, tmp); tf2::Vector3 tmp1; fromMsg(msg.wrench.torque, tmp1); std::array tmp_array; tmp_array[0] = tmp; tmp_array[1] = tmp1; out.setData(tmp_array); } template<> inline void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out, const geometry_msgs::TransformStamped& transform) { doTransform(t_in.force, t_out.force, transform); doTransform(t_in.torque, t_out.torque, transform); } template<> inline void doTransform(const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out, const geometry_msgs::TransformStamped& transform) { doTransform(t_in.wrench, t_out.wrench, transform); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } } // namespace #endif // TF2_GEOMETRY_MSGS_H geometry2-0.7.5/tf2_geometry_msgs/index.rst000066400000000000000000000003441372352374300207520ustar00rootroot00000000000000Welcome to tf2_geometry_msgs's documentation! ============================================= Contents: .. toctree:: :maxdepth: 2 Indices and tables ================== * :ref:`genindex` * :ref:`modindex` * :ref:`search` geometry2-0.7.5/tf2_geometry_msgs/mainpage.dox000066400000000000000000000013771372352374300214150ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_geometry_msgs contains functions for converting between various geometry_msgs data types. This library is an implementation of the templated conversion interface specified in tf/convert.h. It offers conversion and transform convenience functions between various geometry_msgs data types, such as Vector, Point, Pose, Transform, Quaternion, etc. See the Conversions overview wiki page for more information about datatype conversion in tf2. \section codeapi Code API This library consists of one header only, tf2_geometry_msgs/tf2_geometry_msgs.h, which consists mostly of specializations of template functions defined in tf2/convert.h. */ geometry2-0.7.5/tf2_geometry_msgs/package.xml000066400000000000000000000013011372352374300212200ustar00rootroot00000000000000 tf2_geometry_msgs 0.7.5 tf2_geometry_msgs Wim Meeussen Tully Foote BSD http://www.ros.org/wiki/tf2_ros catkin geometry_msgs liborocos-kdl-dev tf2 tf2_ros python3-pykdl python3-pykdl ros_environment rostest geometry2-0.7.5/tf2_geometry_msgs/rosdoc.yaml000066400000000000000000000002471372352374300212700ustar00rootroot00000000000000 - builder: doxygen name: C++ API output_dir: c++ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' - builder: sphinx name: Python API output_dir: python geometry2-0.7.5/tf2_geometry_msgs/scripts/000077500000000000000000000000001372352374300205775ustar00rootroot00000000000000geometry2-0.7.5/tf2_geometry_msgs/scripts/test.py000077500000000000000000000064251372352374300221420ustar00rootroot00000000000000#!/usr/bin/env python import unittest import rospy import PyKDL import tf2_ros import tf2_geometry_msgs from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped, WrenchStamped class GeometryMsgs(unittest.TestCase): def test_transform(self): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1 t.transform.rotation.x = 1 t.header.stamp = rospy.Time(2.0) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') out = b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') self.assertEqual(out.child_frame_id, 'b') v = PointStamped() v.header.stamp = rospy.Time(2) v.header.frame_id = 'a' v.point.x = 1 v.point.y = 2 v.point.z = 3 out = b.transform(v, 'b') self.assertEqual(out.point.x, 0) self.assertEqual(out.point.y, -2) self.assertEqual(out.point.z, -3) v = PoseStamped() v.header.stamp = rospy.Time(2) v.header.frame_id = 'a' v.pose.position.x = 1 v.pose.position.y = 2 v.pose.position.z = 3 v.pose.orientation.x = 1 out = b.transform(v, 'b') self.assertEqual(out.pose.position.x, 0) self.assertEqual(out.pose.position.y, -2) self.assertEqual(out.pose.position.z, -3) # Translation shouldn't affect Vector3 t = TransformStamped() t.transform.translation.x = 1 t.transform.translation.y = 2 t.transform.translation.z = 3 t.transform.rotation.w = 1 v = Vector3Stamped() v.vector.x = 1 v.vector.y = 0 v.vector.z = 0 out = tf2_geometry_msgs.do_transform_vector3(v, t) self.assertEqual(out.vector.x, 1) self.assertEqual(out.vector.y, 0) self.assertEqual(out.vector.z, 0) # Rotate Vector3 180 deg about y t = TransformStamped() t.transform.translation.x = 1 t.transform.translation.y = 2 t.transform.translation.z = 3 t.transform.rotation.y = 1 v = Vector3Stamped() v.vector.x = 1 v.vector.y = 0 v.vector.z = 0 out = tf2_geometry_msgs.do_transform_vector3(v, t) self.assertEqual(out.vector.x, -1) self.assertEqual(out.vector.y, 0) self.assertEqual(out.vector.z, 0) v = WrenchStamped() v.wrench.force.x = 1 v.wrench.force.y = 0 v.wrench.force.z = 0 v.wrench.torque.x = 1 v.wrench.torque.y = 0 v.wrench.torque.z = 0 out = tf2_geometry_msgs.do_transform_wrench(v, t) self.assertEqual(out.wrench.force.x, -1) self.assertEqual(out.wrench.force.y, 0) self.assertEqual(out.wrench.force.z, 0) self.assertEqual(out.wrench.torque.x, -1) self.assertEqual(out.wrench.torque.y, 0) self.assertEqual(out.wrench.torque.z, 0) if __name__ == '__main__': import rosunit rospy.init_node('test_tf2_geometry_msgs_python') rosunit.unitrun("test_tf2_geometry_msgs", "test_tf2_geometry_msgs_python", GeometryMsgs) geometry2-0.7.5/tf2_geometry_msgs/setup.py000066400000000000000000000004361372352374300206250ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['tf2_geometry_msgs'], package_dir={'': 'src'}, requires=['rospy','geometry_msgs','tf2_ros','orocos_kdl'] ) setup(**d) geometry2-0.7.5/tf2_geometry_msgs/src/000077500000000000000000000000001372352374300176775ustar00rootroot00000000000000geometry2-0.7.5/tf2_geometry_msgs/src/tf2_geometry_msgs/000077500000000000000000000000001372352374300233365ustar00rootroot00000000000000geometry2-0.7.5/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py000066400000000000000000000000411372352374300254420ustar00rootroot00000000000000from .tf2_geometry_msgs import * geometry2-0.7.5/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py000066400000000000000000000114071372352374300273520ustar00rootroot00000000000000# Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped, WrenchStamped import PyKDL import rospy import tf2_ros import copy def to_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_to_msg(Vector3Stamped, to_msg_msg) tf2_ros.ConvertRegistration().add_to_msg(PoseStamped, to_msg_msg) tf2_ros.ConvertRegistration().add_to_msg(PointStamped, to_msg_msg) def from_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_from_msg(Vector3Stamped, from_msg_msg) tf2_ros.ConvertRegistration().add_from_msg(PoseStamped, from_msg_msg) tf2_ros.ConvertRegistration().add_from_msg(PointStamped, from_msg_msg) def transform_to_kdl(t): return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), PyKDL.Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)) # PointStamped def do_transform_point(point, transform): p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, point.point.y, point.point.z) res = PointStamped() res.point.x = p[0] res.point.y = p[1] res.point.z = p[2] res.header = transform.header return res tf2_ros.TransformRegistration().add(PointStamped, do_transform_point) # Vector3Stamped def do_transform_vector3(vector3, transform): transform = copy.deepcopy(transform) transform.transform.translation.x = 0; transform.transform.translation.y = 0; transform.transform.translation.z = 0; p = transform_to_kdl(transform) * PyKDL.Vector(vector3.vector.x, vector3.vector.y, vector3.vector.z) res = Vector3Stamped() res.vector.x = p[0] res.vector.y = p[1] res.vector.z = p[2] res.header = transform.header return res tf2_ros.TransformRegistration().add(Vector3Stamped, do_transform_vector3) # PoseStamped def do_transform_pose(pose, transform): f = transform_to_kdl(transform) * PyKDL.Frame(PyKDL.Rotation.Quaternion(pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w), PyKDL.Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)) res = PoseStamped() res.pose.position.x = f[(0, 3)] res.pose.position.y = f[(1, 3)] res.pose.position.z = f[(2, 3)] (res.pose.orientation.x, res.pose.orientation.y, res.pose.orientation.z, res.pose.orientation.w) = f.M.GetQuaternion() res.header = transform.header return res tf2_ros.TransformRegistration().add(PoseStamped, do_transform_pose) # WrenchStamped def do_transform_wrench(wrench, transform): force = Vector3Stamped() torque = Vector3Stamped() force.vector = wrench.wrench.force torque.vector = wrench.wrench.torque res = WrenchStamped() res.wrench.force = do_transform_vector3(force, transform).vector res.wrench.torque = do_transform_vector3(torque, transform).vector res.header = transform.header return res tf2_ros.TransformRegistration().add(WrenchStamped, do_transform_wrench) geometry2-0.7.5/tf2_geometry_msgs/test/000077500000000000000000000000001372352374300200675ustar00rootroot00000000000000geometry2-0.7.5/tf2_geometry_msgs/test/test.launch000066400000000000000000000002021372352374300222340ustar00rootroot00000000000000 geometry2-0.7.5/tf2_geometry_msgs/test/test_python.launch000066400000000000000000000002571372352374300236470ustar00rootroot00000000000000 geometry2-0.7.5/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp000066400000000000000000000317321372352374300252770ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #include #include #include #include #include #include tf2_ros::Buffer* tf_buffer; geometry_msgs::TransformStamped t; static const double EPS = 1e-3; TEST(TfGeometry, Frame) { geometry_msgs::PoseStamped v1; v1.pose.position.x = 1; v1.pose.position.y = 2; v1.pose.position.z = 3; v1.pose.orientation.x = 1; v1.header.stamp = ros::Time(2); v1.header.frame_id = "A"; // simple api geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.pose.position.x, -9, EPS); EXPECT_NEAR(v_simple.pose.position.y, 18, EPS); EXPECT_NEAR(v_simple.pose.position.z, 27, EPS); EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS); // advanced api geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS); EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS); EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS); EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS); } TEST(TfGeometry, PoseWithCovarianceStamped) { geometry_msgs::PoseWithCovarianceStamped v1; v1.pose.pose.position.x = 1; v1.pose.pose.position.y = 2; v1.pose.pose.position.z = 3; v1.pose.pose.orientation.x = 1; v1.header.stamp = ros::Time(2); v1.header.frame_id = "A"; v1.pose.covariance[0] = 1; v1.pose.covariance[7] = 1; v1.pose.covariance[14] = 1; v1.pose.covariance[21] = 1; v1.pose.covariance[28] = 1; v1.pose.covariance[35] = 1; // simple api const geometry_msgs::PoseWithCovarianceStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS); EXPECT_NEAR(v_simple.pose.pose.position.y, 18, EPS); EXPECT_NEAR(v_simple.pose.pose.position.z, 27, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS); // no rotation in this transformation, so no change to covariance EXPECT_NEAR(v_simple.pose.covariance[0], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[7], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[14], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[21], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[28], 1.0, EPS); EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS); // advanced api const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); EXPECT_NEAR(v_advanced.pose.pose.position.x, -9, EPS); EXPECT_NEAR(v_advanced.pose.pose.position.y, 18, EPS); EXPECT_NEAR(v_advanced.pose.pose.position.z, 27, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS); EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS); // no rotation in this transformation, so no change to covariance EXPECT_NEAR(v_advanced.pose.covariance[0], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[7], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[14], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[21], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[28], 1.0, EPS); EXPECT_NEAR(v_advanced.pose.covariance[35], 1.0, EPS); /** now add rotation to transform to test the effect on covariance **/ // rotate pi/2 radians about x-axis geometry_msgs::TransformStamped t_rot; t_rot.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(1,0,0), M_PI/2)); t_rot.header.stamp = ros::Time(2.0); t_rot.header.frame_id = "A"; t_rot.child_frame_id = "rotated"; tf_buffer->setTransform(t_rot, "rotation_test"); // need to put some covariance in the matrix v1.pose.covariance[1] = 1; v1.pose.covariance[6] = 1; v1.pose.covariance[12] = 1; // perform rotation const geometry_msgs::PoseWithCovarianceStamped v_rotated = tf_buffer->transform(v1, "rotated", ros::Duration(2.0)); // the covariance matrix should now be transformed EXPECT_NEAR(v_rotated.pose.covariance[0], 1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[1], 0.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[2],-1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[6], 1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[7], 1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[8], 0.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[12],-1.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[13], 0.0, EPS); EXPECT_NEAR(v_rotated.pose.covariance[14], 1.0, EPS); // set buffer back to original transform tf_buffer->setTransform(t, "test"); } TEST(TfGeometry, Transform) { geometry_msgs::TransformStamped v1; v1.transform.translation.x = 1; v1.transform.translation.y = 2; v1.transform.translation.z = 3; v1.transform.rotation.x = 1; v1.header.stamp = ros::Time(2); v1.header.frame_id = "A"; // simple api geometry_msgs::TransformStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.transform.translation.x, -9, EPS); EXPECT_NEAR(v_simple.transform.translation.y, 18, EPS); EXPECT_NEAR(v_simple.transform.translation.z, 27, EPS); EXPECT_NEAR(v_simple.transform.rotation.x, 0.0, EPS); EXPECT_NEAR(v_simple.transform.rotation.y, 0.0, EPS); EXPECT_NEAR(v_simple.transform.rotation.z, 0.0, EPS); EXPECT_NEAR(v_simple.transform.rotation.w, 1.0, EPS); // advanced api geometry_msgs::TransformStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); EXPECT_NEAR(v_advanced.transform.translation.x, -9, EPS); EXPECT_NEAR(v_advanced.transform.translation.y, 18, EPS); EXPECT_NEAR(v_advanced.transform.translation.z, 27, EPS); EXPECT_NEAR(v_advanced.transform.rotation.x, 0.0, EPS); EXPECT_NEAR(v_advanced.transform.rotation.y, 0.0, EPS); EXPECT_NEAR(v_advanced.transform.rotation.z, 0.0, EPS); EXPECT_NEAR(v_advanced.transform.rotation.w, 1.0, EPS); } TEST(TfGeometry, Vector) { geometry_msgs::Vector3Stamped v1, res; v1.vector.x = 1; v1.vector.y = 2; v1.vector.z = 3; v1.header.stamp = ros::Time(2.0); v1.header.frame_id = "A"; // simple api geometry_msgs::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.vector.x, 1, EPS); EXPECT_NEAR(v_simple.vector.y, -2, EPS); EXPECT_NEAR(v_simple.vector.z, -3, EPS); // advanced api geometry_msgs::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); EXPECT_NEAR(v_advanced.vector.x, 1, EPS); EXPECT_NEAR(v_advanced.vector.y, -2, EPS); EXPECT_NEAR(v_advanced.vector.z, -3, EPS); } TEST(TfGeometry, Point) { geometry_msgs::PointStamped v1, res; v1.point.x = 1; v1.point.y = 2; v1.point.z = 3; v1.header.stamp = ros::Time(2.0); v1.header.frame_id = "A"; // simple api geometry_msgs::PointStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.point.x, -9, EPS); EXPECT_NEAR(v_simple.point.y, 18, EPS); EXPECT_NEAR(v_simple.point.z, 27, EPS); // advanced api geometry_msgs::PointStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); EXPECT_NEAR(v_advanced.point.x, -9, EPS); EXPECT_NEAR(v_advanced.point.y, 18, EPS); EXPECT_NEAR(v_advanced.point.z, 27, EPS); } TEST(TfGeometry, doTransformPoint) { geometry_msgs::Point v1, res; v1.x = 2; v1.y = 1; v1.z = 3; geometry_msgs::TransformStamped trafo; trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); tf2::doTransform(v1, res, trafo); EXPECT_NEAR(res.x, 0, EPS); EXPECT_NEAR(res.y, 0, EPS); EXPECT_NEAR(res.z, 0, EPS); } TEST(TfGeometry, doTransformQuaterion) { geometry_msgs::Quaternion v1, res; v1.w = 1; geometry_msgs::TransformStamped trafo; trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); tf2::doTransform(v1, res, trafo); EXPECT_NEAR(res.x, trafo.transform.rotation.x, EPS); EXPECT_NEAR(res.y, trafo.transform.rotation.y, EPS); EXPECT_NEAR(res.z, trafo.transform.rotation.z, EPS); EXPECT_NEAR(res.w, trafo.transform.rotation.w, EPS); } TEST(TfGeometry, doTransformPose) { geometry_msgs::Pose v1, res; v1.position.x = 2; v1.position.y = 1; v1.position.z = 3; v1.orientation.w = 1; geometry_msgs::TransformStamped trafo; trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); tf2::doTransform(v1, res, trafo); EXPECT_NEAR(res.position.x, 0, EPS); EXPECT_NEAR(res.position.y, 0, EPS); EXPECT_NEAR(res.position.z, 0, EPS); EXPECT_NEAR(res.orientation.x, trafo.transform.rotation.x, EPS); EXPECT_NEAR(res.orientation.y, trafo.transform.rotation.y, EPS); EXPECT_NEAR(res.orientation.z, trafo.transform.rotation.z, EPS); EXPECT_NEAR(res.orientation.w, trafo.transform.rotation.w, EPS); } TEST(TfGeometry, doTransformVector3) { geometry_msgs::Vector3 v1, res; v1.x = 2; v1.y = 1; v1.z = 3; geometry_msgs::TransformStamped trafo; trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); tf2::doTransform(v1, res, trafo); EXPECT_NEAR(res.x, 1, EPS); EXPECT_NEAR(res.y, -2, EPS); EXPECT_NEAR(res.z, 3, EPS); } TEST(TfGeometry, doTransformWrench) { geometry_msgs::Wrench v1, res; v1.force.x = 2; v1.force.y = 1; v1.force.z = 3; v1.torque.x = 2; v1.torque.y = 1; v1.torque.z = 3; geometry_msgs::TransformStamped trafo; trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); tf2::doTransform(v1, res, trafo); EXPECT_NEAR(res.force.x, 1, EPS); EXPECT_NEAR(res.force.y, -2, EPS); EXPECT_NEAR(res.force.z, 3, EPS); EXPECT_NEAR(res.torque.x, 1, EPS); EXPECT_NEAR(res.torque.y, -2, EPS); EXPECT_NEAR(res.torque.z, 3, EPS); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test"); ros::NodeHandle n; tf_buffer = new tf2_ros::Buffer(); tf_buffer->setUsingDedicatedThread(true); // populate buffer t.transform.translation.x = 10; t.transform.translation.y = 20; t.transform.translation.z = 30; t.transform.rotation.x = 1; t.header.stamp = ros::Time(2.0); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); int ret = RUN_ALL_TESTS(); delete tf_buffer; return ret; } geometry2-0.7.5/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp000066400000000000000000000235611372352374300245240ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #include #include static const double EPS = 1e-6; tf2::Vector3 get_tf2_vector() { return tf2::Vector3(1.0, 2.0, 3.0); } geometry_msgs::Vector3& value_initialize(geometry_msgs::Vector3 &m1) { m1.x = 1; m1.y = 2; m1.z = 3; return m1; } std_msgs::Header& value_initialize(std_msgs::Header & h) { h.stamp = ros::Time(10); h.frame_id = "foobar"; return h; } geometry_msgs::Vector3Stamped& value_initialize(geometry_msgs::Vector3Stamped &m1) { value_initialize(m1.header); value_initialize(m1.vector); return m1; } geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1) { m1.x = 1; m1.y = 2; m1.z = 3; return m1; } geometry_msgs::PointStamped& value_initialize(geometry_msgs::PointStamped &m1) { value_initialize(m1.header); value_initialize(m1.point); return m1; } geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1) { m1.x = 0; m1.y = 0; m1.z = 0.7071067811; m1.w = 0.7071067811; return m1; } geometry_msgs::QuaternionStamped& value_initialize(geometry_msgs::QuaternionStamped &m1) { value_initialize(m1.header); value_initialize(m1.quaternion); return m1; } geometry_msgs::Pose & value_initialize(geometry_msgs::Pose & m1) { value_initialize(m1.position); value_initialize(m1.orientation); return m1; } geometry_msgs::PoseStamped& value_initialize(geometry_msgs::PoseStamped &m1) { value_initialize(m1.header); value_initialize(m1.pose); return m1; } geometry_msgs::Transform & value_initialize(geometry_msgs::Transform & m1) { value_initialize(m1.translation); value_initialize(m1.rotation); return m1; } geometry_msgs::TransformStamped& value_initialize(geometry_msgs::TransformStamped &m1) { value_initialize(m1.header); value_initialize(m1.transform); return m1; } void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2) { EXPECT_NEAR(h1.stamp.toSec(), h2.stamp.toSec(), EPS); EXPECT_STREQ(h1.frame_id.c_str(), h2.frame_id.c_str()); } /* * Vector3 */ void expect_near(const geometry_msgs::Vector3 & v1, const tf2::Vector3 & v2) { EXPECT_NEAR(v1.x, v2.x(), EPS); EXPECT_NEAR(v1.y, v2.y(), EPS); EXPECT_NEAR(v1.z, v2.z(), EPS); } void expect_near(const geometry_msgs::Vector3 & v1, const geometry_msgs::Vector3 & v2) { EXPECT_NEAR(v1.x, v2.x, EPS); EXPECT_NEAR(v1.y, v2.y, EPS); EXPECT_NEAR(v1.z, v2.z, EPS); } void expect_near(const tf2::Vector3 & v1, const tf2::Vector3 & v2) { EXPECT_NEAR(v1.x(), v2.x(), EPS); EXPECT_NEAR(v1.y(), v2.y(), EPS); EXPECT_NEAR(v1.z(), v2.z(), EPS); } void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::Vector3Stamped & p2) { expect_near(p1.header, p2.header); expect_near(p1.vector, p2.vector); } /* * Point */ void expect_near(const geometry_msgs::Point & p1, const tf2::Vector3 & v2) { EXPECT_NEAR(p1.x, v2.x(), EPS); EXPECT_NEAR(p1.y, v2.y(), EPS); EXPECT_NEAR(p1.z, v2.z(), EPS); } void expect_near(const geometry_msgs::Point & p1, const geometry_msgs::Point & v2) { EXPECT_NEAR(p1.x, v2.x, EPS); EXPECT_NEAR(p1.y, v2.y, EPS); EXPECT_NEAR(p1.z, v2.z, EPS); } void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::PointStamped & p2) { expect_near(p1.header, p2.header); expect_near(p1.point, p2.point); } /* * Quaternion */ void expect_near(const geometry_msgs::Quaternion & q1, const tf2::Quaternion & v2) { EXPECT_NEAR(q1.x, v2.x(), EPS); EXPECT_NEAR(q1.y, v2.y(), EPS); EXPECT_NEAR(q1.z, v2.z(), EPS); } void expect_near(const geometry_msgs::Quaternion & q1, const geometry_msgs::Quaternion & v2) { EXPECT_NEAR(q1.x, v2.x, EPS); EXPECT_NEAR(q1.y, v2.y, EPS); EXPECT_NEAR(q1.z, v2.z, EPS); } void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msgs::QuaternionStamped & p2) { expect_near(p1.header, p2.header); expect_near(p1.quaternion, p2.quaternion); } /* * Pose */ void expect_near(const geometry_msgs::Pose & p, const tf2::Transform & t) { expect_near(p.position, t.getOrigin()); expect_near(p.orientation, t.getRotation()); } void expect_near(const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2) { expect_near(p1.position, p2.position); expect_near(p1.orientation, p2.orientation); } void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::PoseStamped & p2) { expect_near(p1.header, p2.header); expect_near(p1.pose, p2.pose); } /* * Transform */ void expect_near(const geometry_msgs::Transform & p, const tf2::Transform & t) { expect_near(p.translation, t.getOrigin()); expect_near(p.rotation, t.getRotation()); } void expect_near(const geometry_msgs::Transform & p1, const geometry_msgs::Transform & p2) { expect_near(p1.translation, p2.translation); expect_near(p1.rotation, p2.rotation); } void expect_near(const geometry_msgs::TransformStamped & p1, const geometry_msgs::TransformStamped & p2) { expect_near(p1.header, p2.header); expect_near(p1.transform, p2.transform); } /* * Stamped templated expect_near */ template void expect_near(const tf2::Stamped& s1, const tf2::Stamped& s2) { expect_near((T)s1, (T)s2); } /********************* * Tests *********************/ TEST(tf2_geometry_msgs, Vector3) { geometry_msgs::Vector3 m1; value_initialize(m1); tf2::Vector3 v1; fromMsg(m1, v1); SCOPED_TRACE("m1 v1"); expect_near(m1, v1); geometry_msgs::Vector3 m2 = toMsg(v1); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, Point) { geometry_msgs::Point m1; value_initialize(m1); tf2::Vector3 v1; SCOPED_TRACE("m1 v1"); fromMsg(m1, v1); expect_near(m1, v1); geometry_msgs::Point m2 = toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, Quaternion) { geometry_msgs::Quaternion m1; value_initialize(m1); tf2::Quaternion q1; SCOPED_TRACE("m1 q1"); fromMsg(m1, q1); expect_near(m1, q1); geometry_msgs::Quaternion m2 = toMsg(q1); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, Pose) { geometry_msgs::Pose m1; value_initialize(m1); tf2::Transform t1; fromMsg(m1, t1); SCOPED_TRACE("m1 t1"); expect_near(m1, t1); geometry_msgs::Pose m2 = toMsg(t1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, Transform) { geometry_msgs::Transform m1; value_initialize(m1); tf2::Transform t1; fromMsg(m1, t1); SCOPED_TRACE("m1 t1"); expect_near(m1, t1); geometry_msgs::Transform m2 = toMsg(t1); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, Vector3Stamped) { geometry_msgs::Vector3Stamped m1; value_initialize(m1); tf2::Stamped v1; fromMsg(m1, v1); SCOPED_TRACE("m1 v1"); // expect_near(m1, v1); geometry_msgs::Vector3Stamped m2; m2 = toMsg(v1); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, PointStamped) { geometry_msgs::PointStamped m1; value_initialize(m1); tf2::Stamped v1; fromMsg(m1, v1); SCOPED_TRACE("m1 v1"); // expect_near(m1, v1); //TODO implement cross verification explicityly geometry_msgs::PointStamped m2; m2 = toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, QuaternionStamped) { geometry_msgs::QuaternionStamped m1; value_initialize(m1); tf2::Stamped v1; fromMsg(m1, v1); SCOPED_TRACE("m1 v1"); // expect_near(m1, v1); //TODO implement cross verification explicityly geometry_msgs::QuaternionStamped m2; m2 = tf2::toMsg(v1); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, PoseStamped) { geometry_msgs::PoseStamped m1; value_initialize(m1); tf2::Stamped v1; SCOPED_TRACE("m1 v1"); fromMsg(m1, v1); // expect_near(m1, v1); //TODO implement cross verification explicityly geometry_msgs::PoseStamped m2; m2 = tf2::toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } TEST(tf2_geometry_msgs, TransformStamped) { geometry_msgs::TransformStamped m1; value_initialize(m1); tf2::Stamped v1; fromMsg(m1, v1); SCOPED_TRACE("m1 v1"); // expect_near(m1, v1); geometry_msgs::TransformStamped m2; m2 = tf2::toMsg(v1); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); int ret = RUN_ALL_TESTS(); return ret; } geometry2-0.7.5/tf2_kdl/000077500000000000000000000000001372352374300147765ustar00rootroot00000000000000geometry2-0.7.5/tf2_kdl/CHANGELOG.rst000066400000000000000000000147401372352374300170250ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_kdl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ 0.7.2 (2020-06-08) ------------------ * fix shebang line for python3 (`#466 `_) * Contributors: Mikael Arguedas 0.7.1 (2020-05-13) ------------------ * import setup from setuptools instead of distutils-core (`#449 `_) * Contributors: Alejandro Hernández Cordero 0.7.0 (2020-03-09) ------------------ * Replace kdl packages with rosdep keys (`#447 `_) * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Make kdl headers available (`#419 `_) * Fix python3 compatibility for noetic (`#416 `_) * Merge pull request `#404 `_ from otamachan/remove-load-manifest * Python 3 compatibility: relative imports and print statement * Contributors: Shane Loretz, Tamaki Nishino, Timon Engelke, Tully Foote 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ * Adds additional conversions for tf2, KDL, Eigen (`#292 `_) - adds non-stamped Eigen to Transform function - converts Eigen Matrix Vectors to and from geometry_msgs::Twist - adds to/from message for geometry_msgs::Pose and KDL::Frame * Contributors: Ian McMahon 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- * Merge pull request `#257 `_ from delftrobotics-forks/python3 Make tf2_py python3 compatible again * Use python3 print function. * Contributors: Maarten de Vries, Tully Foote 0.5.16 (2017-07-14) ------------------- * store gtest return value as int (`#229 `_) * Find eigen in a much nicer way. * Switch tf2_kdl over to package.xml format 2. * Contributors: Chris Lalancette, dhood 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- * Add Python documentation for tf2_kdl * Document kdl * Contributors: Jackie Kay 0.5.13 (2016-03-04) ------------------- * converting python test script into unit test * Don't export catkin includes * Contributors: Jochen Sprickerhof, Tully Foote 0.5.12 (2015-08-05) ------------------- * Add kdl::Frame to TransformStamped conversion * Contributors: Paul Bovbel 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- 0.5.9 (2015-03-25) ------------------ 0.5.8 (2015-03-17) ------------------ * remove useless Makefile files * fix ODR violations * Contributors: Vincent Rabaud 0.5.7 (2014-12-23) ------------------ * fixing install rules and adding backwards compatible include with #warning * Contributors: Tully Foote 0.5.6 (2014-09-18) ------------------ 0.5.5 (2014-06-23) ------------------ 0.5.4 (2014-05-07) ------------------ 0.5.3 (2014-02-21) ------------------ * finding eigen from cmake_modules instead of from catkin * Contributors: Tully Foote 0.5.2 (2014-02-20) ------------------ * add cmake_modules dependency for eigen find_package rules * Contributors: Tully Foote 0.5.1 (2014-02-14) ------------------ 0.5.0 (2014-02-14) ------------------ 0.4.10 (2013-12-26) ------------------- 0.4.9 (2013-11-06) ------------------ 0.4.8 (2013-11-06) ------------------ 0.4.7 (2013-08-28) ------------------ 0.4.6 (2013-08-28) ------------------ 0.4.5 (2013-07-11) ------------------ 0.4.4 (2013-07-09) ------------------ * making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ 0.4.1 (2013-07-05) ------------------ 0.4.0 (2013-06-27) ------------------ * moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 * Cleaning up unnecessary dependency on roscpp * converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace * Cleaning up packaging of tf2 including: removing unused nodehandle cleaning up a few dependencies and linking removing old backup of package.xml making diff minimally different from tf version of library * Restoring test packages and bullet packages. reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ * passing unit tests 0.3.6 (2013-03-03) ------------------ * fix compilation under Oneiric 0.3.5 (2013-02-15 14:46) ------------------------ * 0.3.4 -> 0.3.5 0.3.4 (2013-02-15 13:14) ------------------------ * 0.3.3 -> 0.3.4 0.3.3 (2013-02-15 11:30) ------------------------ * 0.3.2 -> 0.3.3 0.3.2 (2013-02-15 00:42) ------------------------ * 0.3.1 -> 0.3.2 * fixed missing include export & tf2_ros dependecy 0.3.1 (2013-02-14) ------------------ * fixing version number in tf2_kdl * catkinized tf2_kdl 0.3.0 (2013-02-13) ------------------ * fixing groovy-devel * removing bullet and kdl related packages * catkinizing geometry-experimental * catkinizing tf2_kdl * fix for kdl rotaiton constrition * add twist, wrench and pose conversion to kdl, fix message to message conversion by adding specific conversion functions * merge tf2_cpp and tf2_py into tf2_ros * Got transform with types working in python * A working first version of transforming and converting between different types * Moving from camelCase to undescores to be in line with python style guides * kdl unittest passing * whitespace test * add support for PointStamped geometry_msgs * Fixing script * set transform for test * add advanced api * working to transform kdl objects with dummy buffer_core * plugin for py kdl * add regression tests for geometry_msgs point, vector and pose * add frame unit tests to kdl and bullet * add first regression tests for kdl and bullet tf * add bullet transforms, and create tests for bullet and kdl * transform for vector3stamped message * move implementation into library * add advanced api * compiling again with new design * renaming classes * compiling now * almost compiling version of template code * add test to start compiling geometry2-0.7.5/tf2_kdl/CMakeLists.txt000066400000000000000000000036301372352374300175400ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_kdl) find_package(orocos_kdl) find_package(catkin REQUIRED COMPONENTS cmake_modules tf2 tf2_ros tf2_msgs) # Finding Eigen is somewhat complicated because of our need to support Ubuntu # all the way back to saucy. First we look for the Eigen3 cmake module # provided by the libeigen3-dev on newer Ubuntu. If that fails, then we # fall-back to the version provided by cmake_modules, which is a stand-in. find_package(Eigen3 QUIET) if(NOT EIGEN3_FOUND) find_package(cmake_modules REQUIRED) find_package(Eigen REQUIRED) set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) endif() # Note that eigen 3.2 (on Ubuntu Wily) only provides EIGEN3_INCLUDE_DIR, # not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former. if(NOT EIGEN3_INCLUDE_DIRS) set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) endif() catkin_package( INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} DEPENDS EIGEN3 orocos_kdl ) catkin_python_setup() link_directories(${orocos_kdl_LIBRARY_DIRS}) include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${GTEST_INCLUDE_DIRS}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) catkin_install_python(PROGRAMS scripts/test.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS rostest tf2 tf2_ros tf2_msgs) add_executable(test_kdl EXCLUDE_FROM_ALL test/test_tf2_kdl.cpp) find_package(Threads) target_include_directories(test_kdl PUBLIC ${orocos_kdl_INCLUDE_DIRS}) target_link_libraries(test_kdl ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES} ${GTEST_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_python.launch) if(TARGET tests) add_dependencies(tests test_kdl) endif() endif() geometry2-0.7.5/tf2_kdl/conf.py000066400000000000000000000151541372352374300163030ustar00rootroot00000000000000# -*- coding: utf-8 -*- # # tf2 documentation build configuration file, created by # sphinx-quickstart on Mon Jun 1 14:21:53 2009. # # This file is execfile()d with the current directory set to its containing dir. # # Note that not all possible configuration values are present in this # autogenerated file. # # All configuration values have a default; values that are commented out # serve to show the default. import roslib #roslib.load_manifest('tf2_kdl') import sys, os # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. # sys.path.append(os.path.abspath('./src/tf2_kdl')) # -- General configuration ----------------------------------------------------- # Add any Sphinx extension module names here, as strings. They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] # The suffix of source filenames. source_suffix = '.rst' # The encoding of source files. #source_encoding = 'utf-8' # The master toctree document. master_doc = 'index' # General information about the project. project = u'tf2_kdl' copyright = u'2016, Open Source Robotics Foundation' # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. # # The short X.Y version. version = '0.5' # The full version, including alpha/beta/rc tags. release = '0.5.13' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. #language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: #today = '' # Else, today_fmt is used as the format for a strftime call. #today_fmt = '%B %d, %Y' # List of documents that shouldn't be included in the build. #unused_docs = [] # List of directories, relative to source directory, that shouldn't be searched # for source files. exclude_trees = ['_build'] exclude_patterns = ['_CHANGELOG.rst'] # The reST default role (used for this markup: `text`) to use for all documents. #default_role = None # If true, '()' will be appended to :func: etc. cross-reference text. #add_function_parentheses = True # If true, the current module name will be prepended to all description # unit titles (such as .. function::). #add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. #show_authors = False # The name of the Pygments (syntax highlighting) style to use. pygments_style = 'sphinx' # A list of ignored prefixes for module index sorting. #modindex_common_prefix = [] # -- Options for HTML output --------------------------------------------------- # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. html_theme = 'default' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the # documentation. #html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. #html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". #html_title = None # A shorter title for the navigation bar. Default is the same as html_title. #html_short_title = None # The name of an image file (relative to this directory) to place at the top # of the sidebar. #html_logo = None # The name of an image file (within the static path) to use as favicon of the # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. #html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". #html_static_path = ['_static'] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. #html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. #html_use_smartypants = True # Custom sidebar templates, maps document names to template names. #html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. #html_additional_pages = {} # If false, no module index is generated. #html_use_modindex = True # If false, no index is generated. #html_use_index = True # If true, the index is split into individual pages for each letter. #html_split_index = False # If true, links to the reST sources are added to the pages. #html_show_sourcelink = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. #html_use_opensearch = '' # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). #html_file_suffix = '' # Output file base name for HTML help builder. htmlhelp_basename = 'tfdoc' # -- Options for LaTeX output -------------------------------------------------- # The paper size ('letter' or 'a4'). #latex_paper_size = 'letter' # The font size ('10pt', '11pt' or '12pt'). #latex_font_size = '10pt' # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, author, documentclass [howto/manual]). latex_documents = [ ('index', 'tf.tex', u'stereo\\_utils Documentation', u'Tully Foote and Eitan Marder-Eppstein', 'manual'), ] # The name of an image file (relative to this directory) to place at the top of # the title page. #latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. #latex_use_parts = False # Additional stuff for the LaTeX preamble. #latex_preamble = '' # Documents to append as an appendix to all manuals. #latex_appendices = [] # If false, no module index is generated. #latex_use_modindex = True # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = { 'http://docs.python.org/': None, 'http://docs.opencv.org/3.0-last-rst/': None, 'http://docs.scipy.org/doc/numpy' : None } geometry2-0.7.5/tf2_kdl/include/000077500000000000000000000000001372352374300164215ustar00rootroot00000000000000geometry2-0.7.5/tf2_kdl/include/tf2_kdl/000077500000000000000000000000001372352374300177465ustar00rootroot00000000000000geometry2-0.7.5/tf2_kdl/include/tf2_kdl/tf2_kdl.h000066400000000000000000000274151372352374300214550ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #ifndef TF2_KDL_H #define TF2_KDL_H #include #include #include #include #include #include #include namespace tf2 { /** \brief Convert a timestamped transform to the equivalent KDL data type. * \param t The transform to convert, as a geometry_msgs TransformedStamped message. * \return The transform message converted to an KDL Frame. */ inline KDL::Frame transformToKDL(const geometry_msgs::TransformStamped& t) { return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); } /** \brief Convert an KDL Frame to the equivalent geometry_msgs message type. * \param k The transform to convert, as an KDL Frame. * \return The transform converted to a TransformStamped message. */ inline geometry_msgs::TransformStamped kdlToTransform(const KDL::Frame& k) { geometry_msgs::TransformStamped t; t.transform.translation.x = k.p.x(); t.transform.translation.y = k.p.y(); t.transform.translation.z = k.p.z(); k.M.GetQuaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w); return t; } // --------------------- // Vector // --------------------- /** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Vector type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The vector to transform, as a timestamped KDL Vector data type. * \param t_out The transformed vector, as a timestamped KDL Vector data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } /** \brief Convert a stamped KDL Vector type to a PointStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped KDL Vector to convert. * \return The vector converted to a PointStamped message. */ inline geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) { geometry_msgs::PointStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.point.x = in[0]; msg.point.y = in[1]; msg.point.z = in[2]; return msg; } /** \brief Convert a PointStamped message type to a stamped KDL-specific Vector type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The PointStamped message to convert. * \param out The point converted to a timestamped KDL Vector. */ inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out[0] = msg.point.x; out[1] = msg.point.y; out[2] = msg.point.z; } // --------------------- // Twist // --------------------- /** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Twist type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The twist to transform, as a timestamped KDL Twist data type. * \param t_out The transformed Twist, as a timestamped KDL Frame data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } /** \brief Convert a stamped KDL Twist type to a TwistStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped KDL Twist to convert. * \return The twist converted to a TwistStamped message. */ inline geometry_msgs::TwistStamped toMsg(const tf2::Stamped& in) { geometry_msgs::TwistStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.twist.linear.x = in.vel[0]; msg.twist.linear.y = in.vel[1]; msg.twist.linear.z = in.vel[2]; msg.twist.angular.x = in.rot[0]; msg.twist.angular.y = in.rot[1]; msg.twist.angular.z = in.rot[2]; return msg; } /** \brief Convert a TwistStamped message type to a stamped KDL-specific Twist type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The TwistStamped message to convert. * \param out The twist converted to a timestamped KDL Twist. */ inline void fromMsg(const geometry_msgs::TwistStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out.vel[0] = msg.twist.linear.x; out.vel[1] = msg.twist.linear.y; out.vel[2] = msg.twist.linear.z; out.rot[0] = msg.twist.angular.x; out.rot[1] = msg.twist.angular.y; out.rot[2] = msg.twist.angular.z; } // --------------------- // Wrench // --------------------- /** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Wrench type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The wrench to transform, as a timestamped KDL Wrench data type. * \param t_out The transformed Wrench, as a timestamped KDL Frame data type. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } /** \brief Convert a stamped KDL Wrench type to a WrenchStamped message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped KDL Wrench to convert. * \return The wrench converted to a WrenchStamped message. */ inline geometry_msgs::WrenchStamped toMsg(const tf2::Stamped& in) { geometry_msgs::WrenchStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.wrench.force.x = in.force[0]; msg.wrench.force.y = in.force[1]; msg.wrench.force.z = in.force[2]; msg.wrench.torque.x = in.torque[0]; msg.wrench.torque.y = in.torque[1]; msg.wrench.torque.z = in.torque[2]; return msg; } /** \brief Convert a WrenchStamped message type to a stamped KDL-specific Wrench type. * This function is a specialization of the fromMsg template defined in tf2/convert.h * \param msg The WrenchStamped message to convert. * \param out The wrench converted to a timestamped KDL Wrench. */ inline void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out.force[0] = msg.wrench.force.x; out.force[1] = msg.wrench.force.y; out.force[2] = msg.wrench.force.z; out.torque[0] = msg.wrench.torque.x; out.torque[1] = msg.wrench.torque.y; out.torque[2] = msg.wrench.torque.z; } // --------------------- // Frame // --------------------- /** \brief Apply a geometry_msgs TransformStamped to a KDL-specific Frame data type. * This function is a specialization of the doTransform template defined in tf2/convert.h. * \param t_in The frame to transform, as a timestamped KDL Frame. * \param t_out The transformed frame, as a timestamped KDL Frame. * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> inline void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) { t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); } /** \brief Convert a stamped KDL Frame type to a Pose message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped KDL Frame to convert. * \return The frame converted to a Pose message. */ inline geometry_msgs::Pose toMsg(const KDL::Frame& in) { geometry_msgs::Pose msg; msg.position.x = in.p[0]; msg.position.y = in.p[1]; msg.position.z = in.p[2]; in.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); return msg; } /** \brief Convert a Pose message type to a KDL Frame. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg The Pose message to convert. * \param out The pose converted to a KDL Frame. */ inline void fromMsg(const geometry_msgs::Pose& msg, KDL::Frame& out) { out.p[0] = msg.position.x; out.p[1] = msg.position.y; out.p[2] = msg.position.z; out.M = KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); } /** \brief Convert a stamped KDL Frame type to a Pose message. * This function is a specialization of the toMsg template defined in tf2/convert.h. * \param in The timestamped KDL Frame to convert. * \return The frame converted to a PoseStamped message. */ inline geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) { geometry_msgs::PoseStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.pose = toMsg(static_cast(in)); return msg; } /** \brief Convert a Pose message transform type to a stamped KDL Frame. * This function is a specialization of the fromMsg template defined in tf2/convert.h. * \param msg The PoseStamped message to convert. * \param out The pose converted to a timestamped KDL Frame. */ inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; fromMsg(msg.pose, static_cast(out)); } } // namespace #endif // TF2_KDL_H geometry2-0.7.5/tf2_kdl/include/tf2_kdl/tf2_kdl/000077500000000000000000000000001372352374300212735ustar00rootroot00000000000000geometry2-0.7.5/tf2_kdl/include/tf2_kdl/tf2_kdl/tf2_kdl.h000066400000000000000000000001571372352374300227740ustar00rootroot00000000000000#warning This header is at the wrong path you should include #include geometry2-0.7.5/tf2_kdl/index.rst000066400000000000000000000003631372352374300166410ustar00rootroot00000000000000tf2_kdl documentation ===================== This is the Python API reference of the tf2_kdl package. .. automodule:: tf2_kdl.tf2_kdl :members: :undoc-members: Indices and tables ================== * :ref:`genindex` * :ref:`search` geometry2-0.7.5/tf2_kdl/mainpage.dox000066400000000000000000000013751372352374300173010ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_kdl contains functions for converting between geometry_msgs and KDL data types. This library is an implementation of the templated conversion interface specified in tf/convert.h. It enables easy conversion from geometry_msgs Transform and Point types to the types specified by the Orocos KDL (Kinematics and Dynamics Library) API (see http://www.orocos.org/kdl). See the Conversions overview wiki page for more information about datatype conversion in tf2. \section codeapi Code API This library consists of one header only, tf2_kdl/tf2_kdl.h, which consists mostly of specializations of template functions defined in tf2/convert.h. */ geometry2-0.7.5/tf2_kdl/package.xml000066400000000000000000000014261372352374300171160ustar00rootroot00000000000000 tf2_kdl 0.7.5 KDL binding for tf2 Tully Foote Wim Meeussen BSD http://ros.org/wiki/tf2 catkin cmake_modules eigen eigen liborocos-kdl-dev tf2 tf2_ros ros_environment rostest geometry2-0.7.5/tf2_kdl/rosdoc.yaml000066400000000000000000000002471372352374300171560ustar00rootroot00000000000000 - builder: doxygen name: C++ API output_dir: c++ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' - builder: sphinx name: Python API output_dir: python geometry2-0.7.5/tf2_kdl/scripts/000077500000000000000000000000001372352374300164655ustar00rootroot00000000000000geometry2-0.7.5/tf2_kdl/scripts/test.py000077500000000000000000000052561372352374300200310ustar00rootroot00000000000000#!/usr/bin/env python from __future__ import print_function import unittest import rospy import PyKDL import tf2_ros import tf2_kdl from geometry_msgs.msg import TransformStamped from copy import deepcopy class KDLConversions(unittest.TestCase): def test_transform(self): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1 t.transform.rotation.x = 1 t.header.stamp = rospy.Time(2.0) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') out = b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') self.assertEqual(out.child_frame_id, 'b') v = PyKDL.Vector(1,2,3) out = b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b') self.assertEqual(out.x(), 0) self.assertEqual(out.y(), -2) self.assertEqual(out.z(), -3) f = PyKDL.Frame(PyKDL.Rotation.RPY(1,2,3), PyKDL.Vector(1,2,3)) out = b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b') print(out) self.assertEqual(out.p.x(), 0) self.assertEqual(out.p.y(), -2) self.assertEqual(out.p.z(), -3) # TODO(tfoote) check values of rotation t = PyKDL.Twist(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6)) out = b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b') self.assertEqual(out.vel.x(), 1) self.assertEqual(out.vel.y(), -8) self.assertEqual(out.vel.z(), 2) self.assertEqual(out.rot.x(), 4) self.assertEqual(out.rot.y(), -5) self.assertEqual(out.rot.z(), -6) w = PyKDL.Wrench(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6)) out = b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b') self.assertEqual(out.force.x(), 1) self.assertEqual(out.force.y(), -2) self.assertEqual(out.force.z(), -3) self.assertEqual(out.torque.x(), 4) self.assertEqual(out.torque.y(), -8) self.assertEqual(out.torque.z(), -4) def test_convert(self): v = PyKDL.Vector(1,2,3) vs = tf2_ros.Stamped(v, rospy.Time(2), 'a') vs2 = tf2_ros.convert(vs, PyKDL.Vector) self.assertEqual(vs.x(), 1) self.assertEqual(vs.y(), 2) self.assertEqual(vs.z(), 3) self.assertEqual(vs2.x(), 1) self.assertEqual(vs2.y(), 2) self.assertEqual(vs2.z(), 3) if __name__ == '__main__': import rosunit rospy.init_node('test_tf2_kdl_python') rosunit.unitrun("test_tf2_kdl", "test_tf2_kdl_python", KDLConversions) geometry2-0.7.5/tf2_kdl/setup.py000066400000000000000000000004621372352374300165120ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( ## don't do this unless you want a globally visible script # scripts=['script/test.py'], packages=['tf2_kdl'], package_dir={'': 'src'} ) setup(**d) geometry2-0.7.5/tf2_kdl/src/000077500000000000000000000000001372352374300155655ustar00rootroot00000000000000geometry2-0.7.5/tf2_kdl/src/tf2_kdl/000077500000000000000000000000001372352374300171125ustar00rootroot00000000000000geometry2-0.7.5/tf2_kdl/src/tf2_kdl/__init__.py000066400000000000000000000000271372352374300212220ustar00rootroot00000000000000from .tf2_kdl import * geometry2-0.7.5/tf2_kdl/src/tf2_kdl/tf2_kdl.py000066400000000000000000000134651372352374300210220ustar00rootroot00000000000000# Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen import PyKDL import rospy import tf2_ros from geometry_msgs.msg import PointStamped def transform_to_kdl(t): """Convert a geometry_msgs Transform message to a PyKDL Frame. :param t: The Transform message to convert. :type t: geometry_msgs.msg.TransformStamped :return: The converted PyKDL frame. :rtype: PyKDL.Frame """ return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), PyKDL.Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)) def do_transform_vector(vector, transform): """Apply a transform in the form of a geometry_msgs message to a PyKDL vector. :param vector: The PyKDL vector to transform. :type vector: PyKDL.Vector :param transform: The transform to apply. :type transform: geometry_msgs.msg.TransformStamped :return: The transformed vector. :rtype: PyKDL.Vector """ res = transform_to_kdl(transform) * vector res.header = transform.header return res tf2_ros.TransformRegistration().add(PyKDL.Vector, do_transform_vector) def to_msg_vector(vector): """Convert a PyKDL Vector to a geometry_msgs PointStamped message. :param vector: The vector to convert. :type vector: PyKDL.Vector :return: The converted vector/point. :rtype: geometry_msgs.msg.PointStamped """ msg = PointStamped() msg.header = vector.header msg.point.x = vector[0] msg.point.y = vector[1] msg.point.z = vector[2] return msg tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Vector, to_msg_vector) def from_msg_vector(msg): """Convert a PointStamped message to a stamped PyKDL Vector. :param msg: The PointStamped message to convert. :type msg: geometry_msgs.msg.PointStamped :return: The timestamped converted PyKDL vector. :rtype: PyKDL.Vector """ vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z) return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id) tf2_ros.ConvertRegistration().add_from_msg(PyKDL.Vector, from_msg_vector) def convert_vector(vector): """Convert a generic stamped triplet message to a stamped PyKDL Vector. :param vector: The message to convert. :return: The timestamped converted PyKDL vector. :rtype: PyKDL.Vector """ return tf2_ros.Stamped(PyKDL.Vector(vector), vector.header.stamp, vector.header.frame_id) tf2_ros.ConvertRegistration().add_convert((PyKDL.Vector, PyKDL.Vector), convert_vector) def do_transform_frame(frame, transform): """Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. :param frame: The PyKDL frame to transform. :type frame: PyKDL.Frame :param transform: The transform to apply. :type transform: geometry_msgs.msg.TransformStamped :return: The transformed PyKDL frame. :rtype: PyKDL.Frame """ res = transform_to_kdl(transform) * frame res.header = transform.header return res tf2_ros.TransformRegistration().add(PyKDL.Frame, do_transform_frame) def do_transform_twist(twist, transform): """Apply a transform in the form of a geometry_msgs message to a PyKDL Twist. :param twist: The PyKDL twist to transform. :type twist: PyKDL.Twist :param transform: The transform to apply. :type transform: geometry_msgs.msg.TransformStamped :return: The transformed PyKDL twist. :rtype: PyKDL.Twist """ res = transform_to_kdl(transform) * twist res.header = transform.header return res tf2_ros.TransformRegistration().add(PyKDL.Twist, do_transform_twist) # Wrench def do_transform_wrench(wrench, transform): """Apply a transform in the form of a geometry_msgs message to a PyKDL Wrench. :param wrench: The PyKDL wrench to transform. :type wrench: PyKDL.Wrench :param transform: The transform to apply. :type transform: geometry_msgs.msg.TransformStamped :return: The transformed PyKDL wrench. :rtype: PyKDL.Wrench """ res = transform_to_kdl(transform) * wrench res.header = transform.header return res tf2_ros.TransformRegistration().add(PyKDL.Wrench, do_transform_wrench) geometry2-0.7.5/tf2_kdl/test/000077500000000000000000000000001372352374300157555ustar00rootroot00000000000000geometry2-0.7.5/tf2_kdl/test/test.launch000066400000000000000000000001441372352374300201270ustar00rootroot00000000000000 geometry2-0.7.5/tf2_kdl/test/test_python.launch000066400000000000000000000002331372352374300215270ustar00rootroot00000000000000 geometry2-0.7.5/tf2_kdl/test/test_tf2_kdl.cpp000066400000000000000000000100201372352374300210360ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #include #include #include #include "tf2_ros/buffer.h" tf2_ros::Buffer* tf_buffer; static const double EPS = 1e-3; TEST(TfKDL, Frame) { tf2::Stamped v1(KDL::Frame(KDL::Rotation::RPY(M_PI, 0, 0), KDL::Vector(1,2,3)), ros::Time(2.0), "A"); // simple api KDL::Frame v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple.p[0], -9, EPS); EXPECT_NEAR(v_simple.p[1], 18, EPS); EXPECT_NEAR(v_simple.p[2], 27, EPS); double r, p, y; v_simple.M.GetRPY(r, p, y); EXPECT_NEAR(r, 0.0, EPS); EXPECT_NEAR(p, 0.0, EPS); EXPECT_NEAR(y, 0.0, EPS); // advanced api KDL::Frame v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); EXPECT_NEAR(v_advanced.p[0], -9, EPS); EXPECT_NEAR(v_advanced.p[1], 18, EPS); EXPECT_NEAR(v_advanced.p[2], 27, EPS); v_advanced.M.GetRPY(r, p, y); EXPECT_NEAR(r, 0.0, EPS); EXPECT_NEAR(p, 0.0, EPS); EXPECT_NEAR(y, 0.0, EPS); } TEST(TfKDL, Vector) { tf2::Stamped v1(KDL::Vector(1,2,3), ros::Time(2.0), "A"); // simple api KDL::Vector v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); EXPECT_NEAR(v_simple[0], -9, EPS); EXPECT_NEAR(v_simple[1], 18, EPS); EXPECT_NEAR(v_simple[2], 27, EPS); // advanced api KDL::Vector v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), "A", ros::Duration(3.0)); EXPECT_NEAR(v_advanced[0], -9, EPS); EXPECT_NEAR(v_advanced[1], 18, EPS); EXPECT_NEAR(v_advanced[2], 27, EPS); } TEST(TfKDL, ConvertVector) { tf2::Stamped v(KDL::Vector(1,2,3), ros::Time(), "my_frame"); tf2::Stamped v1 = v; tf2::convert(v1, v1); EXPECT_EQ(v, v1); tf2::Stamped v2(KDL::Vector(3,4,5), ros::Time(), "my_frame2"); tf2::convert(v1, v2); EXPECT_EQ(v, v2); EXPECT_EQ(v1, v2); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test"); ros::NodeHandle n; tf_buffer = new tf2_ros::Buffer(); // populate buffer geometry_msgs::TransformStamped t; t.transform.translation.x = 10; t.transform.translation.y = 20; t.transform.translation.z = 30; t.transform.rotation.x = 1; t.header.stamp = ros::Time(2.0); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); int retval = RUN_ALL_TESTS(); delete tf_buffer; return retval; } geometry2-0.7.5/tf2_msgs/000077500000000000000000000000001372352374300151755ustar00rootroot00000000000000geometry2-0.7.5/tf2_msgs/CHANGELOG.rst000066400000000000000000000064531372352374300172260ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Contributors: Shane Loretz 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- 0.5.16 (2017-07-14) ------------------- 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- 0.5.13 (2016-03-04) ------------------- 0.5.12 (2015-08-05) ------------------- 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- 0.5.9 (2015-03-25) ------------------ 0.5.8 (2015-03-17) ------------------ * remove useless Makefile files * Contributors: Vincent Rabaud 0.5.7 (2014-12-23) ------------------ 0.5.6 (2014-09-18) ------------------ 0.5.5 (2014-06-23) ------------------ 0.5.4 (2014-05-07) ------------------ 0.5.3 (2014-02-21) ------------------ 0.5.2 (2014-02-20) ------------------ 0.5.1 (2014-02-14) ------------------ 0.5.0 (2014-02-14) ------------------ 0.4.10 (2013-12-26) ------------------- 0.4.9 (2013-11-06) ------------------ 0.4.8 (2013-11-06) ------------------ 0.4.7 (2013-08-28) ------------------ 0.4.6 (2013-08-28) ------------------ 0.4.5 (2013-07-11) ------------------ 0.4.4 (2013-07-09) ------------------ 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ 0.4.1 (2013-07-05) ------------------ 0.4.0 (2013-06-27) ------------------ * Restoring test packages and bullet packages. reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ 0.3.6 (2013-03-03) ------------------ 0.3.5 (2013-02-15 14:46) ------------------------ * 0.3.4 -> 0.3.5 0.3.4 (2013-02-15 13:14) ------------------------ * 0.3.3 -> 0.3.4 0.3.3 (2013-02-15 11:30) ------------------------ * 0.3.2 -> 0.3.3 0.3.2 (2013-02-15 00:42) ------------------------ * 0.3.1 -> 0.3.2 0.3.1 (2013-02-14) ------------------ * 0.3.0 -> 0.3.1 0.3.0 (2013-02-13) ------------------ * switching to version 0.3.0 * removing packages with missing deps * adding include folder * adding tf2_msgs/srv/FrameGraph.srv * catkin fixes * catkinizing geometry-experimental * catkinizing tf2_msgs * Adding ROS service interface to cpp Buffer * fix tf messages dependency and name * add python transform listener * Compiling version of the buffer server * Compiling version of the buffer client * Adding a message that encapsulates errors that can be returned by tf * A fully specified version of the LookupTransform.action * Commiting so I can merge * Adding action for LookupTransform * Updating CMake to call genaction * Moving tfMessage to TFMessage to adhere to naming conventions * Copying tfMessage from tf to new tf2_msgs package * Creating a package for new tf messages geometry2-0.7.5/tf2_msgs/CMakeLists.txt000066400000000000000000000010641372352374300177360ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_msgs) find_package(catkin REQUIRED COMPONENTS message_generation geometry_msgs actionlib_msgs) find_package(Boost COMPONENTS thread REQUIRED) add_message_files(DIRECTORY msg FILES TF2Error.msg TFMessage.msg) add_service_files(DIRECTORY srv FILES FrameGraph.srv) add_action_files(DIRECTORY action FILES LookupTransform.action) generate_messages( DEPENDENCIES actionlib_msgs std_msgs geometry_msgs ) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS message_generation geometry_msgs actionlib_msgs) geometry2-0.7.5/tf2_msgs/action/000077500000000000000000000000001372352374300164525ustar00rootroot00000000000000geometry2-0.7.5/tf2_msgs/action/LookupTransform.action000066400000000000000000000004121372352374300230130ustar00rootroot00000000000000#Simple API string target_frame string source_frame time source_time duration timeout #Advanced API time target_time string fixed_frame #Whether or not to use the advanced API bool advanced --- geometry_msgs/TransformStamped transform tf2_msgs/TF2Error error --- geometry2-0.7.5/tf2_msgs/include/000077500000000000000000000000001372352374300166205ustar00rootroot00000000000000geometry2-0.7.5/tf2_msgs/include/foo000066400000000000000000000000001372352374300173140ustar00rootroot00000000000000geometry2-0.7.5/tf2_msgs/mainpage.dox000066400000000000000000000011121372352374300174650ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_msgs is ... \section codeapi Code API */ geometry2-0.7.5/tf2_msgs/msg/000077500000000000000000000000001372352374300157635ustar00rootroot00000000000000geometry2-0.7.5/tf2_msgs/msg/TF2Error.msg000066400000000000000000000003311372352374300200750ustar00rootroot00000000000000uint8 NO_ERROR = 0 uint8 LOOKUP_ERROR = 1 uint8 CONNECTIVITY_ERROR = 2 uint8 EXTRAPOLATION_ERROR = 3 uint8 INVALID_ARGUMENT_ERROR = 4 uint8 TIMEOUT_ERROR = 5 uint8 TRANSFORM_ERROR = 6 uint8 error string error_string geometry2-0.7.5/tf2_msgs/msg/TFMessage.msg000066400000000000000000000000541372352374300203100ustar00rootroot00000000000000geometry_msgs/TransformStamped[] transforms geometry2-0.7.5/tf2_msgs/package.xml000066400000000000000000000012051372352374300173100ustar00rootroot00000000000000 tf2_msgs 0.7.5 tf2_msgs Eitan Marder-Eppstein Tully Foote BSD http://www.ros.org/wiki/tf2_msgs catkin actionlib_msgs geometry_msgs message_generation actionlib_msgs geometry_msgs message_generation geometry2-0.7.5/tf2_msgs/srv/000077500000000000000000000000001372352374300160075ustar00rootroot00000000000000geometry2-0.7.5/tf2_msgs/srv/FrameGraph.srv000077500000000000000000000000261372352374300205600ustar00rootroot00000000000000--- string frame_yaml geometry2-0.7.5/tf2_py/000077500000000000000000000000001372352374300146545ustar00rootroot00000000000000geometry2-0.7.5/tf2_py/CHANGELOG.rst000066400000000000000000000117411372352374300167010ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_py ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ * avoid name collision b/t tf2_py and tf2 (`#478 `_) * Contributors: Sean Yen 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ * [noetic] cherry-pick Windows fixes from melodic-devel (`#450 `_) * [Windows][melodic-devel] Fix install locations (`#442 `_) * fixed install locations of tf2 * [windows][melodic] more portable fixes. (`#443 `_) * more portable fixes. * import setup from setuptools instead of distutils-core (`#449 `_) * Contributors: Alejandro Hernández Cordero, Sean Yen 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Merge pull request `#363 `_ from kejxu/fix_tf2_py_export use .pyd instead of .so on Windows and export symbols * limit MSVC-only change to MSVC scope (`#10 `_) * Fix the pyd extension and export the init function. * use windows counterpart for .so extension * Contributors: James Xu, Sean Yen, Shane Loretz, Tully Foote 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ * fix translation vs rotation typo Fixes `#324 `_ * Add python3.7 compatibility. * Contributors: Hans Gaiser, Tully Foote 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- * Merge pull request `#266 `_ from randoms/indigo-devel fix METH_OLDARGS is no longer supported error in python3 * Merge pull request `#260 `_ from randoms/indigo-devel fix python3 import error * Merge pull request `#257 `_ from delftrobotics-forks/python3 Make tf2_py python3 compatible again * Use string conversion from python_compat.h. * Contributors: Maarten de Vries, Tully Foote, randoms 0.5.16 (2017-07-14) ------------------- * fix memory leak calling Py_DECREF for all created PyObject * replaced dependencies on tf2_msgs_gencpp by exported dependencies * Relax strict type checks at setTransform to only check for members (`#221 `_) * expose deprecated methods in tf2_py API to support better backwards compatibility. Fixes `#206 `_ * Contributors: Christopher Wecht, Sergio Ramos, Tully Foote, alex 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- * Improve tf compatibility (`#192 `_) getLatestCommonTime() is needed to implement the TF API. See `ros/geometry#134 `_ * Add missing type checks at Python/C++ tf2 transform interface `#159 `_ (`#197 `_) * Make tf2_py compatible with python3. (`#173 `_) * tf2_py: Use PyUnicode objects for text in python3. * tf2_py: Make module initialization python3 compatible. * tf2_py: Fix type definition for python3. * tf2_py: Move and rename PyObject_BorrowAttrString. * Contributors: Maarten de Vries, Timo Röhling, alex 0.5.13 (2016-03-04) ------------------- 0.5.12 (2015-08-05) ------------------- 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- 0.5.9 (2015-03-25) ------------------ 0.5.8 (2015-03-17) ------------------ 0.5.7 (2014-12-23) ------------------ 0.5.6 (2014-09-18) ------------------ 0.5.5 (2014-06-23) ------------------ 0.5.4 (2014-05-07) ------------------ 0.5.3 (2014-02-21) ------------------ 0.5.2 (2014-02-20) ------------------ 0.5.1 (2014-02-14) ------------------ 0.5.0 (2014-02-14) ------------------ 0.4.10 (2013-12-26) ------------------- * adding support for static transforms in python listener. Fixes `#46 `_ * Contributors: Tully Foote 0.4.9 (2013-11-06) ------------------ 0.4.8 (2013-11-06) ------------------ 0.4.7 (2013-08-28) ------------------ 0.4.6 (2013-08-28) ------------------ 0.4.5 (2013-07-11) ------------------ 0.4.4 (2013-07-09) ------------------ * tf2_py: Fixes warning, implicit conversion of NULL 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ 0.4.1 (2013-07-05) ------------------ 0.4.0 (2013-06-27) ------------------ * splitting rospy dependency into tf2_py so tf2 is pure c++ library. geometry2-0.7.5/tf2_py/CMakeLists.txt000066400000000000000000000117101372352374300174140ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_py) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS rospy tf2) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) find_package(PythonLibs 2 REQUIRED) include_directories(${PYTHON_INCLUDE_PATH} ${catkin_INCLUDE_DIRS}) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html catkin_python_setup() ####################################### ## Declare ROS messages and services ## ####################################### ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) ## Generate services in the 'srv' folder # add_service_files( # FILES # Service1.srv # Service2.srv # ) ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES # std_msgs # Or other packages containing msgs # ) ################################### ## catkin specific configuration ## ################################### ## The catkin_package macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include # LIBRARIES tf2_py CATKIN_DEPENDS rospy tf2 # DEPENDS system_lib ) ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) ## Declare a cpp library # add_library(tf2_py # src/${PROJECT_NAME}/tf2_py.cpp # ) ## Declare a cpp executable # add_executable(tf2_py_node src/tf2_py_node.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes # add_dependencies(tf2_py_node tf2_py_generate_messages_cpp) ## Specify libraries to link a library or executable target against # target_link_libraries(tf2_py_node # ${catkin_LIBRARIES} # ) # Check for SSE #!!! rosbuild_check_for_sse() # Dynamic linking with tf worked OK, except for exception propagation, which failed in the unit test. # so build with the objects directly instead. link_libraries(${PYTHON_LIBRARIES}) add_library(tf2_py src/tf2_py.cpp) target_link_libraries(tf2_py ${catkin_LIBRARIES}) add_dependencies(tf2_py ${catkin_EXPORTED_TARGETS}) if(WIN32) # use .pyd extension on Windows set_target_properties(tf2_py PROPERTIES OUTPUT_NAME "_tf2" SUFFIX ".pyd") else() set_target_properties(tf2_py PROPERTIES COMPILE_FLAGS "-g -Wno-missing-field-initializers") set_target_properties(tf2_py PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") endif() set_target_properties(tf2_py PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) #!! rosbuild_add_compile_flags(tf2_py ${SSE_FLAGS}) #conditionally adds sse flags if available ############# ## Install ## ############# # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination # install(PROGRAMS # scripts/my_python_script # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark executables and/or libraries for installation # install(TARGETS tf2_py tf2_py_node # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE # ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES # # myfile1 # # myfile2 # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) install(FILES $ DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} ) ############# ## Testing ## ############# ## Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_tf2_py.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests # catkin_add_nosetests(test) geometry2-0.7.5/tf2_py/package.xml000066400000000000000000000037511372352374300167770ustar00rootroot00000000000000 tf2_py 0.7.5 The tf2_py package Tully Foote BSD http://ros.org/wiki/tf2_py catkin tf2 rospy tf2 rospy geometry2-0.7.5/tf2_py/setup.py000066400000000000000000000004101372352374300163610ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['tf2_py'], package_dir={'': 'src'}, requires=['rospy', 'geometry_msgs', 'tf2_msgs'] ) setup(**d) geometry2-0.7.5/tf2_py/src/000077500000000000000000000000001372352374300154435ustar00rootroot00000000000000geometry2-0.7.5/tf2_py/src/python_compat.h000066400000000000000000000022001372352374300204720ustar00rootroot00000000000000#ifndef TF2_PY_PYTHON_COMPAT_H #define TF2_PY_PYTHON_COMPAT_H #include #include inline PyObject *stringToPython(const std::string &input) { #if PY_MAJOR_VERSION >= 3 return PyUnicode_FromStringAndSize(input.c_str(), input.size()); #else return PyString_FromStringAndSize(input.c_str(), input.size()); #endif } inline PyObject *stringToPython(const char *input) { #if PY_MAJOR_VERSION >= 3 return PyUnicode_FromString(input); #else return PyString_FromString(input); #endif } inline std::string stringFromPython(PyObject * input) { Py_ssize_t size; #if PY_MAJOR_VERSION >= 3 const char * data; data = PyUnicode_AsUTF8AndSize(input, &size); #else char * data; PyString_AsStringAndSize(input, &data, &size); #endif return std::string(data, size); } inline PyObject *pythonImport(const std::string & name) { PyObject *py_name = stringToPython(name); PyObject *module = PyImport_Import(py_name); Py_XDECREF(py_name); return module; } inline PyObject *pythonBorrowAttrString(PyObject* o, const char *name) { PyObject *r = PyObject_GetAttrString(o, name); Py_XDECREF(r); return r; } #endif geometry2-0.7.5/tf2_py/src/tf2_py.cpp000066400000000000000000000571021372352374300173570ustar00rootroot00000000000000#include #include #include #include "python_compat.h" // Run x (a tf method, catching TF's exceptions and reraising them as Python exceptions) // #define WRAP(x) \ do { \ try \ { \ x; \ } \ catch (const tf2::ConnectivityException &e) \ { \ PyErr_SetString(tf2_connectivityexception, e.what()); \ return NULL; \ } \ catch (const tf2::LookupException &e) \ { \ PyErr_SetString(tf2_lookupexception, e.what()); \ return NULL; \ } \ catch (const tf2::ExtrapolationException &e) \ { \ PyErr_SetString(tf2_extrapolationexception, e.what()); \ return NULL; \ } \ catch (const tf2::InvalidArgumentException &e) \ { \ PyErr_SetString(tf2_invalidargumentexception, e.what()); \ return NULL; \ } \ catch (const tf2::TimeoutException &e) \ { \ PyErr_SetString(tf2_timeoutexception, e.what()); \ return NULL; \ } \ catch (const tf2::TransformException &e) \ { \ PyErr_SetString(tf2_exception, e.what()); \ return NULL; \ } \ } while (0) static PyObject *pModulerospy = NULL; static PyObject *pModulegeometrymsgs = NULL; static PyObject *tf2_exception = NULL; static PyObject *tf2_connectivityexception = NULL, *tf2_lookupexception = NULL, *tf2_extrapolationexception = NULL, *tf2_invalidargumentexception = NULL, *tf2_timeoutexception = NULL; struct buffer_core_t { PyObject_HEAD tf2::BufferCore *bc; }; static PyTypeObject buffer_core_Type = { #if PY_MAJOR_VERSION < 3 PyObject_HEAD_INIT(NULL) 0, /*size*/ # else PyVarObject_HEAD_INIT(NULL, 0) #endif "_tf2.BufferCore", /*name*/ sizeof(buffer_core_t), /*basicsize*/ }; static PyObject *transform_converter(const geometry_msgs::TransformStamped* transform) { PyObject *pclass, *pargs, *pinst = NULL; pclass = PyObject_GetAttrString(pModulegeometrymsgs, "TransformStamped"); if(pclass == NULL) { printf("Can't get geometry_msgs.msg.TransformedStamped\n"); return NULL; } pargs = Py_BuildValue("()"); if(pargs == NULL) { printf("Can't build argument list\n"); return NULL; } pinst = PyEval_CallObject(pclass, pargs); Py_DECREF(pclass); Py_DECREF(pargs); if(pinst == NULL) { printf("Can't create class\n"); return NULL; } //we need to convert the time to python PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time"); PyObject *args = Py_BuildValue("ii", transform->header.stamp.sec, transform->header.stamp.nsec); PyObject *time_obj = PyObject_CallObject(rospy_time, args); Py_DECREF(args); Py_DECREF(rospy_time); PyObject* pheader = PyObject_GetAttrString(pinst, "header"); PyObject_SetAttrString(pheader, "stamp", time_obj); Py_DECREF(time_obj); PyObject *frame_id = stringToPython(transform->header.frame_id); PyObject_SetAttrString(pheader, "frame_id", frame_id); Py_DECREF(frame_id); Py_DECREF(pheader); PyObject *ptransform = PyObject_GetAttrString(pinst, "transform"); PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation"); PyObject *protation = PyObject_GetAttrString(ptransform, "rotation"); Py_DECREF(ptransform); PyObject *child_frame_id = stringToPython(transform->child_frame_id); PyObject_SetAttrString(pinst, "child_frame_id", child_frame_id); Py_DECREF(child_frame_id); PyObject *trans_x = PyFloat_FromDouble(transform->transform.translation.x); PyObject *trans_y = PyFloat_FromDouble(transform->transform.translation.y); PyObject *trans_z = PyFloat_FromDouble(transform->transform.translation.z); PyObject_SetAttrString(ptranslation, "x", trans_x); PyObject_SetAttrString(ptranslation, "y", trans_y); PyObject_SetAttrString(ptranslation, "z", trans_z); Py_DECREF(trans_x); Py_DECREF(trans_y); Py_DECREF(trans_z); Py_DECREF(ptranslation); PyObject *rot_x = PyFloat_FromDouble(transform->transform.rotation.x); PyObject *rot_y = PyFloat_FromDouble(transform->transform.rotation.y); PyObject *rot_z = PyFloat_FromDouble(transform->transform.rotation.z); PyObject *rot_w = PyFloat_FromDouble(transform->transform.rotation.w); PyObject_SetAttrString(protation, "x", rot_x); PyObject_SetAttrString(protation, "y", rot_y); PyObject_SetAttrString(protation, "z", rot_z); PyObject_SetAttrString(protation, "w", rot_w); Py_DECREF(rot_x); Py_DECREF(rot_y); Py_DECREF(rot_z); Py_DECREF(rot_w); Py_DECREF(protation); return pinst; } static int rostime_converter(PyObject *obj, ros::Time *rt) { PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL); if (tsr == NULL) { PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration"); return 0; } else { (*rt).fromSec(PyFloat_AsDouble(tsr)); Py_DECREF(tsr); return 1; } } static int rosduration_converter(PyObject *obj, ros::Duration *rt) { PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL); if (tsr == NULL) { PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration"); return 0; } else { (*rt).fromSec(PyFloat_AsDouble(tsr)); Py_DECREF(tsr); return 1; } } static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw) { ros::Duration cache_time; cache_time.fromSec(tf2::BufferCore::DEFAULT_CACHE_TIME); if (!PyArg_ParseTuple(args, "|O&", rosduration_converter, &cache_time)) return -1; ((buffer_core_t*)self)->bc = new tf2::BufferCore(cache_time); return 0; } /* This may need to be implemented later if we decide to have it in the core static PyObject *getTFPrefix(PyObject *self, PyObject *args) { if (!PyArg_ParseTuple(args, "")) return NULL; tf::Transformer *t = ((transformer_t*)self)->t; return stringToPython(t->getTFPrefix()); } */ static PyObject *allFramesAsYAML(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; return stringToPython(bc->allFramesAsYAML()); } static PyObject *allFramesAsString(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; return stringToPython(bc->allFramesAsString()); } static PyObject *canTransformCore(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame; ros::Time time; static const char *keywords[] = { "target_frame", "source_frame", "time", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time)) return NULL; std::string error_msg; bool can_transform = bc->canTransform(target_frame, source_frame, time, &error_msg); //return PyBool_FromLong(t->canTransform(target_frame, source_frame, time)); return Py_BuildValue("bs", can_transform, error_msg.c_str()); } static PyObject *canTransformFullCore(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame, *fixed_frame; ros::Time target_time, source_time; static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords, &target_frame, rostime_converter, &target_time, &source_frame, rostime_converter, &source_time, &fixed_frame)) return NULL; std::string error_msg; bool can_transform = bc->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, &error_msg); //return PyBool_FromLong(t->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame)); return Py_BuildValue("bs", can_transform, error_msg.c_str()); } static PyObject *asListOfStrings(std::vector< std::string > los) { PyObject *r = PyList_New(los.size()); size_t i; for (i = 0; i < los.size(); i++) { PyList_SetItem(r, i, stringToPython(los[i])); } return r; } static PyObject *_chain(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame, *fixed_frame; ros::Time target_time, source_time; std::vector< std::string > output; static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords, &target_frame, rostime_converter, &target_time, &source_frame, rostime_converter, &source_time, &fixed_frame)) return NULL; WRAP(bc->_chainAsVector(target_frame, target_time, source_frame, source_time, fixed_frame, output)); return asListOfStrings(output); } static PyObject *getLatestCommonTime(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame; tf2::CompactFrameID target_id, source_id; ros::Time time; std::string error_string; if (!PyArg_ParseTuple(args, "ss", &target_frame, &source_frame)) return NULL; WRAP(target_id = bc->_validateFrameId("get_latest_common_time", target_frame)); WRAP(source_id = bc->_validateFrameId("get_latest_common_time", source_frame)); int r = bc->_getLatestCommonTime(target_id, source_id, time, &error_string); if (r == 0) { PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time"); PyObject *args = Py_BuildValue("ii", time.sec, time.nsec); PyObject *ob = PyObject_CallObject(rospy_time, args); Py_DECREF(args); Py_DECREF(rospy_time); return ob; } else { PyErr_SetString(tf2_exception, error_string.c_str()); return NULL; } } static PyObject *lookupTransformCore(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame; ros::Time time; static const char *keywords[] = { "target_frame", "source_frame", "time", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time)) return NULL; geometry_msgs::TransformStamped transform; WRAP(transform = bc->lookupTransform(target_frame, source_frame, time)); geometry_msgs::Vector3 origin = transform.transform.translation; geometry_msgs::Quaternion rotation = transform.transform.rotation; //TODO: Create a converter that will actually return a python message return Py_BuildValue("O&", transform_converter, &transform); //return Py_BuildValue("(ddd)(dddd)", // origin.x, origin.y, origin.z, // rotation.x, rotation.y, rotation.z, rotation.w); } static PyObject *lookupTransformFullCore(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *target_frame, *source_frame, *fixed_frame; ros::Time target_time, source_time; static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords, &target_frame, rostime_converter, &target_time, &source_frame, rostime_converter, &source_time, &fixed_frame)) return NULL; geometry_msgs::TransformStamped transform; WRAP(transform = bc->lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame)); geometry_msgs::Vector3 origin = transform.transform.translation; geometry_msgs::Quaternion rotation = transform.transform.rotation; //TODO: Create a converter that will actually return a python message return Py_BuildValue("O&", transform_converter, &transform); } /* static PyObject *lookupTwistCore(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *tracking_frame, *observation_frame; ros::Time time; ros::Duration averaging_interval; static const char *keywords[] = { "tracking_frame", "observation_frame", "time", "averaging_interval", NULL }; if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&", (char**)keywords, &tracking_frame, &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval)) return NULL; geometry_msgs::Twist twist; WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, time, averaging_interval)); return Py_BuildValue("(ddd)(ddd)", twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z); } static PyObject *lookupTwistFullCore(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *tracking_frame, *observation_frame, *reference_frame, *reference_point_frame; ros::Time time; ros::Duration averaging_interval; double px, py, pz; if (!PyArg_ParseTuple(args, "sss(ddd)sO&O&", &tracking_frame, &observation_frame, &reference_frame, &px, &py, &pz, &reference_point_frame, rostime_converter, &time, rosduration_converter, &averaging_interval)) return NULL; geometry_msgs::Twist twist; tf::Point pt(px, py, pz); WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, reference_frame, pt, reference_point_frame, time, averaging_interval)); return Py_BuildValue("(ddd)(ddd)", twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z); } */ static inline int checkTranslationType(PyObject* o) { PyTypeObject *translation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Vector3"); int type_check = PyObject_TypeCheck(o, translation_type); int attr_check = PyObject_HasAttrString(o, "x") && PyObject_HasAttrString(o, "y") && PyObject_HasAttrString(o, "z"); if (!type_check) { PyErr_WarnEx(PyExc_UserWarning, "translation should be of type Vector3", 1); } return attr_check; } static inline int checkRotationType(PyObject* o) { PyTypeObject *rotation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Quaternion"); int type_check = PyObject_TypeCheck(o, rotation_type); int attr_check = PyObject_HasAttrString(o, "w") && PyObject_HasAttrString(o, "x") && PyObject_HasAttrString(o, "y") && PyObject_HasAttrString(o, "z"); if (!type_check) { PyErr_WarnEx(PyExc_UserWarning, "rotation should be of type Quaternion", 1); } return attr_check; } static PyObject *setTransform(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; PyObject *py_transform; char *authority; if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority)) return NULL; geometry_msgs::TransformStamped transform; PyObject *header = pythonBorrowAttrString(py_transform, "header"); transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id")); transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id")); if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) return NULL; PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); if (!checkTranslationType(translation)) { PyErr_SetString(PyExc_TypeError, "transform.translation must have members x, y, z"); return NULL; } transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x")); transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y")); transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z")); PyObject *rotation = pythonBorrowAttrString(mtransform, "rotation"); if (!checkRotationType(rotation)) { PyErr_SetString(PyExc_TypeError, "transform.rotation must have members w, x, y, z"); return NULL; } transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x")); transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y")); transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z")); transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w")); bc->setTransform(transform, authority); Py_RETURN_NONE; } static PyObject *setTransformStatic(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; PyObject *py_transform; char *authority; if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority)) return NULL; geometry_msgs::TransformStamped transform; PyObject *header = pythonBorrowAttrString(py_transform, "header"); transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id")); transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id")); if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) return NULL; PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); if (!checkTranslationType(translation)) { PyErr_SetString(PyExc_TypeError, "transform.translation must be of type Vector3"); return NULL; } transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x")); transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y")); transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z")); PyObject *rotation = pythonBorrowAttrString(mtransform, "rotation"); if (!checkRotationType(rotation)) { PyErr_SetString(PyExc_TypeError, "transform.rotation must be of type Quaternion"); return NULL; } transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x")); transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y")); transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z")); transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w")); // only difference to above is is_static == True bc->setTransform(transform, authority, true); Py_RETURN_NONE; } static PyObject *clear(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; bc->clear(); Py_RETURN_NONE; } static PyObject *_frameExists(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; char *frame_id_str; if (!PyArg_ParseTuple(args, "s", &frame_id_str)) return NULL; return PyBool_FromLong(bc->_frameExists(frame_id_str)); } static PyObject *_getFrameStrings(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; std::vector< std::string > ids; bc->_getFrameStrings(ids); return asListOfStrings(ids); } static PyObject *_allFramesAsDot(PyObject *self, PyObject *args, PyObject *kw) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; static const char *keywords[] = { "time", NULL }; ros::Time time; if (!PyArg_ParseTupleAndKeywords(args, kw, "|O&", (char**)keywords, rostime_converter, &time)) return NULL; return stringToPython(bc->_allFramesAsDot(time.toSec())); } static struct PyMethodDef buffer_core_methods[] = { {"all_frames_as_yaml", allFramesAsYAML, METH_VARARGS}, {"all_frames_as_string", allFramesAsString, METH_VARARGS}, {"set_transform", setTransform, METH_VARARGS}, {"set_transform_static", setTransformStatic, METH_VARARGS}, {"can_transform_core", (PyCFunction)canTransformCore, METH_VARARGS | METH_KEYWORDS}, {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_VARARGS | METH_KEYWORDS}, {"_chain", (PyCFunction)_chain, METH_VARARGS | METH_KEYWORDS}, {"clear", (PyCFunction)clear, METH_VARARGS | METH_KEYWORDS}, {"_frameExists", (PyCFunction)_frameExists, METH_VARARGS}, {"_getFrameStrings", (PyCFunction)_getFrameStrings, METH_VARARGS}, {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_VARARGS | METH_KEYWORDS}, {"get_latest_common_time", (PyCFunction)getLatestCommonTime, METH_VARARGS}, {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_VARARGS | METH_KEYWORDS}, {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | METH_KEYWORDS}, //{"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS}, //{"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS}, //{"getTFPrefix", (PyCFunction)getTFPrefix, METH_VARARGS}, {NULL, NULL} }; static PyMethodDef module_methods[] = { // {"Transformer", mkTransformer, METH_VARARGS}, {0, 0, 0}, }; bool staticInit() { #if PYTHON_API_VERSION >= 1007 tf2_exception = PyErr_NewException((char*)"tf2.TransformException", NULL, NULL); tf2_connectivityexception = PyErr_NewException((char*)"tf2.ConnectivityException", tf2_exception, NULL); tf2_lookupexception = PyErr_NewException((char*)"tf2.LookupException", tf2_exception, NULL); tf2_extrapolationexception = PyErr_NewException((char*)"tf2.ExtrapolationException", tf2_exception, NULL); tf2_invalidargumentexception = PyErr_NewException((char*)"tf2.InvalidArgumentException", tf2_exception, NULL); tf2_timeoutexception = PyErr_NewException((char*)"tf2.TimeoutException", tf2_exception, NULL); #else tf2_exception = stringToPython("tf2.error"); tf2_connectivityexception = stringToPython("tf2.ConnectivityException"); tf2_lookupexception = stringToPython("tf2.LookupException"); tf2_extrapolationexception = stringToPython("tf2.ExtrapolationException"); tf2_invalidargumentexception = stringToPython("tf2.InvalidArgumentException"); tf2_timeoutexception = stringToPython("tf2.TimeoutException"); #endif pModulerospy = pythonImport("rospy"); pModulegeometrymsgs = pythonImport("geometry_msgs.msg"); if(pModulegeometrymsgs == NULL) { printf("Cannot load geometry_msgs module"); return false; } buffer_core_Type.tp_alloc = PyType_GenericAlloc; buffer_core_Type.tp_new = PyType_GenericNew; buffer_core_Type.tp_init = BufferCore_init; buffer_core_Type.tp_flags = Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE; buffer_core_Type.tp_methods = buffer_core_methods; if (PyType_Ready(&buffer_core_Type) != 0) return false; return true; } PyObject *moduleInit(PyObject *m) { PyModule_AddObject(m, "BufferCore", (PyObject *)&buffer_core_Type); PyObject *d = PyModule_GetDict(m); PyDict_SetItemString(d, "TransformException", tf2_exception); PyDict_SetItemString(d, "ConnectivityException", tf2_connectivityexception); PyDict_SetItemString(d, "LookupException", tf2_lookupexception); PyDict_SetItemString(d, "ExtrapolationException", tf2_extrapolationexception); PyDict_SetItemString(d, "InvalidArgumentException", tf2_invalidargumentexception); PyDict_SetItemString(d, "TimeoutException", tf2_timeoutexception); return m; } #if PY_MAJOR_VERSION < 3 extern "C" { ROS_HELPER_EXPORT void init_tf2() { if (!staticInit()) return; moduleInit(Py_InitModule("_tf2", module_methods)); } } #else struct PyModuleDef tf_module = { PyModuleDef_HEAD_INIT, // base "_tf2", // name NULL, // docstring -1, // state size (but we're using globals) module_methods // methods }; PyMODINIT_FUNC PyInit__tf2() { if (!staticInit()) return NULL; return moduleInit(PyModule_Create(&tf_module)); } #endif geometry2-0.7.5/tf2_py/src/tf2_py/000077500000000000000000000000001372352374300166465ustar00rootroot00000000000000geometry2-0.7.5/tf2_py/src/tf2_py/__init__.py000066400000000000000000000035511372352374300207630ustar00rootroot00000000000000#! /usr/bin/python #*********************************************************** #* Software License Agreement (BSD License) #* #* Copyright (c) 2009, Willow Garage, Inc. #* All rights reserved. #* #* Redistribution and use in source and binary forms, with or without #* modification, are permitted provided that the following conditions #* are met: #* #* * Redistributions of source code must retain the above copyright #* notice, this list of conditions and the following disclaimer. #* * Redistributions in binary form must reproduce the above #* copyright notice, this list of conditions and the following #* disclaimer in the documentation and/or other materials provided #* with the distribution. #* * Neither the name of Willow Garage, Inc. nor the names of its #* contributors may be used to endorse or promote products derived #* from this software without specific prior written permission. #* #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #* #* Author: Eitan Marder-Eppstein #*********************************************************** from __future__ import absolute_import from ._tf2 import * geometry2-0.7.5/tf2_ros/000077500000000000000000000000001372352374300150275ustar00rootroot00000000000000geometry2-0.7.5/tf2_ros/CHANGELOG.rst000066400000000000000000000403551372352374300170570ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ * Use correct frame service name in docstrings. (`#476 `_) Replaces the deprecated names {tf_frames, view_frames} -> tf2_frames * Cherry-picking various commits from Melodic (`#471 `_) * Revert "rework Eigen functions namespace hack" (`#436 `_) * Fixed warnings in message_filter.h (`#434 `_) the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall * Fix ambiguous call for tf2::convert on MSVC (`#444 `_) * rework ambiguous call on MSVC. * Contributors: Michael Grupp, Robert Haschke 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ * StatisTransformBroadcaster: simplify/modernize code * [noetic] cherry-pick Windows fixes from melodic-devel (`#450 `_) * [Windows][melodic-devel] Fix install locations (`#442 `_) * fixed install locations of tf2 * [windows][melodic] more portable fixes. (`#443 `_) * more portable fixes. * import setup from setuptools instead of distutils-core (`#449 `_) * Contributors: Alejandro Hernández Cordero, Robert Haschke, Sean Yen 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Add arguments to TransformListener constructors that accept TransportHints for the tf topic subscriber (`#438 `_) * Merge pull request `#404 `_ from otamachan/remove-load-manifest Remove roslib.load_manifest * Merge pull request `#402 `_ from rhaschke/fix-message-filter Fix message filter * resolve virtual function call in destructor * remove pending callbacks in clear() * Merge pull request `#372 `_ from lucasw/patch-1 spelling fix: seperate -> separate * Merge pull request `#369 `_ from magazino/fix-dangling-reference * Fix dangling iterator references in buffer_server.cpp * Remove some useless code from buffer_server_main.cpp (`#368 `_) * Mark check_frequency as deprecated in docstring. * Follow `#337 `_: use actionlib API in BufferClient::processGoal() * Test for equality to None with 'is' instead of '==' (`#355 `_) * added parameter to advertise tf2-frames as a service, if needed * Contributors: Daniel Ingram, Emre Sahin, JonasTietz, Lucas Walter, Michael Grupp, Robert Haschke, Shane Loretz, Tamaki Nishino, Tully Foote, toliver 0.6.5 (2018-11-16) ------------------ * Protect the time reset logic from a race condition. Fixes `#341 `_ This could incorrectly trigger a buffer clear if two concurrent callbacks were invoked. * Contributors: Tully Foote 0.6.4 (2018-11-06) ------------------ * fix(buffer-client): Use actionlib api for obtaining result Use the API provided by actionlib for waiting for result. This will improve the response time and prevent problems with custom solutions (see `#178 `_). This change makes constructor parameter check_frequency obsolute and deprecates it. * Add check to buffer_client.py to make sure result is available Related issue: `#178 `_ * Add check to reset buffer when rostime goes backwards * Fixed the value of expected_success_count\_ * Added a tf2_ros message filter unittest with multiple target frames and non-zero time tolerance * Contributors: Ewoud Pool, Jørgen Borgesen, Stephen Williams 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ * update buffer_server_name (`#296 `_) * use nodename as namespace * Update `#209 `_ to provide backwards compatibility. * Contributors: Jihoon Lee, Tully Foote 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ * tf2_ros::Buffer: canTransform can now deal with timeouts smaller than 10ms by using the hunderdth of the timeout for sleeping (`#286 `_) * More spinning to make sure the message gets through for `#129 `_ `#283 `_ * Contributors: Tully Foote, cwecht 0.5.17 (2018-01-01) ------------------- * Merge pull request `#260 `_ from randoms/indigo-devel fix python3 import error * Merge pull request `#257 `_ from delftrobotics-forks/python3 Make tf2_py python3 compatible again * Use python3 print function. * Contributors: Maarten de Vries, Tully Foote, randoms 0.5.16 (2017-07-14) ------------------- * Merge pull request `#144 `_ from clearpathrobotics/dead_lock_fix Solve a bug that causes a deadlock in MessageFilter * Clear error string if it exists from the external entry points. Fixes `#117 `_ * Make buff_size and tcp_nodelay and subscriber queue size mutable. * Remove generate_rand_vectors() from a number of tests. (`#227 `_) * Remove generate_rand_vectors() from a number of tests. * Log jump duration on backwards time jump detection. (`#234 `_) * replaced dependencies on tf2_msgs_gencpp by exported dependencies * Use new-style objects in python 2 * Solve a bug that causes a deadlock in MessageFilter * Contributors: Adel Fakih, Chris Lalancette, Christopher Wecht, Eric Wieser, Koji Terada, Stephan, Tully Foote, koji_terada 0.5.15 (2017-01-24) ------------------- * tf2_ros: add option to unregister TransformListener (`#201 `_) * Contributors: Hans-Joachim Krauch 0.5.14 (2017-01-16) ------------------- * Drop roslib.load_manifest (`#191 `_) * Adds ability to load TF from the ROS parameter server. * Code linting & reorganization * Fix indexing beyond end of array * added a static transform broadcaster in python * lots more documentation * remove BufferCore doc, add BufferClient/BufferServer doc for C++, add Buffer/BufferInterface Python documentation * Better overview for Python * Contributors: Eric Wieser, Felix Duvallet, Jackie Kay, Mikael Arguedas, Mike Purvis 0.5.13 (2016-03-04) ------------------- * fix documentation warnings * Adding tests to package * Contributors: Laurent GEORGE, Vincent Rabaud 0.5.12 (2015-08-05) ------------------- * remove annoying gcc warning This is because the roslog macro cannot have two arguments that are formatting strings: we need to concatenate them first. * break canTransform loop only for non-tiny negative time deltas (At least) with Python 2 ros.Time.now() is not necessarily monotonic and one can experience negative time deltas (usually well below 1s) on real hardware under full load. This check was originally introduced to allow for backjumps with rosbag replays, and only there it makes sense. So we'll add a small duration threshold to ignore backjumps due to non-monotonic clocks. * Contributors: Vincent Rabaud, v4hn 0.5.11 (2015-04-22) ------------------- * do not short circuit waitForTransform timeout when running inside pytf. Fixes `#102 `_ roscpp is not initialized inside pytf which means that ros::ok is not valid. This was causing the timer to abort immediately. This breaks support for pytf with respect to early breaking out of a loop re `#26 `_. This is conceptually broken in pytf, and is fixed in tf2_ros python implementation. If you want this behavior I recommend switching to the tf2 python bindings. * inject timeout information into error string for canTransform with timeout * Contributors: Tully Foote 0.5.10 (2015-04-21) ------------------- * switch to use a shared lock with upgrade instead of only a unique lock. For `#91 `_ * Update message_filter.h * filters: fix unsupported old messages with frame_id starting with '/' * Enabled tf2 documentation * make sure the messages get processed before testing the effects. Fixes `#88 `_ * allowing to use message filters with PCL types * Contributors: Brice Rebsamen, Jackie Kay, Tully Foote, Vincent Rabaud, jmtatsch 0.5.9 (2015-03-25) ------------------ * changed queue_size in Python transform boradcaster to match that in c++ * Contributors: mrath 0.5.8 (2015-03-17) ------------------ * fix deadlock `#79 `_ * break out of loop if ros is shutdown. Fixes `#26 `_ * remove useless Makefile files * Fix static broadcaster with rpy args * Contributors: Paul Bovbel, Tully Foote, Vincent Rabaud 0.5.7 (2014-12-23) ------------------ * Added 6 param transform again Yes, using Euler angles is a bad habit. But it is much more convenient if you just need a rotation by 90° somewhere to set it up in Euler angles. So I added the option to supply only the 3 angles. * Remove tf2_py dependency for Android * Contributors: Achim Königs, Gary Servin 0.5.6 (2014-09-18) ------------------ * support if canTransform(...): in python `#57 `_ * Support clearing the cache when time jumps backwards `#68 `_ * Contributors: Tully Foote 0.5.5 (2014-06-23) ------------------ 0.5.4 (2014-05-07) ------------------ * surpressing autostart on the server objects to not incur warnings * switch to boost signals2 following `ros/ros_comm#267 `_, blocking `ros/geometry#23 `_ * fix compilation with gcc 4.9 * make can_transform correctly wait * explicitly set the publish queue size for rospy * Contributors: Tully Foote, Vincent Rabaud, v4hn 0.5.3 (2014-02-21) ------------------ 0.5.2 (2014-02-20) ------------------ 0.5.1 (2014-02-14) ------------------ * adding const to MessageEvent data * Contributors: Tully Foote 0.5.0 (2014-02-14) ------------------ * TF2 uses message events to get connection header information * Contributors: Kevin Watts 0.4.10 (2013-12-26) ------------------- * adding support for static transforms in python listener. Fixes `#46 `_ * Contributors: Tully Foote 0.4.9 (2013-11-06) ------------------ 0.4.8 (2013-11-06) ------------------ * fixing pytf failing to sleep https://github.com/ros/geometry/issues/30 * moving python documentation to tf2_ros from tf2 to follow the code * Fixed static_transform_publisher duplicate check, added rostest. 0.4.7 (2013-08-28) ------------------ * fixing new conditional to cover the case that time has not progressed yet port forward of `ros/geometry#35 `_ in the python implementation * fixing new conditional to cover the case that time has not progressed yet port forward of `ros/geometry#35 `_ 0.4.6 (2013-08-28) ------------------ * patching python implementation for `#24 `_ as well * Stop waiting if time jumps backwards. fixes `#24 `_ * patch to work around uninitiaized time. `#30 https://github.com/ros/geometry/issues/30`_ * Removing unnecessary CATKIN_DEPENDS `#18 `_ 0.4.5 (2013-07-11) ------------------ * Revert "cherrypicking groovy patch for `#10 `_ into hydro" This reverts commit 296d4916706d64f719b8c1592ab60d3686f994e1. It was not starting up correctly. * fixing usage string to show quaternions and using quaternions in the test app * cherrypicking groovy patch for `#10 `_ into hydro 0.4.4 (2013-07-09) ------------------ * making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. * reviving unrun unittest and adding CATKIN_ENABLE_TESTING guards 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ 0.4.1 (2013-07-05) ------------------ * adding queue accessors lost in the new API * exposing dedicated thread logic in BufferCore and checking in Buffer * adding methods to enable backwards compatability for passing through to tf::Transformer 0.4.0 (2013-06-27) ------------------ * splitting rospy dependency into tf2_py so tf2 is pure c++ library. * moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 * Cleaning up unnecessary dependency on roscpp * converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace * fixing return by value for tranform method without preallocatoin * Cleaning up packaging of tf2 including: removing unused nodehandle cleaning up a few dependencies and linking removing old backup of package.xml making diff minimally different from tf version of library * Restoring test packages and bullet packages. reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ * Added link against catkin_LIBRARIES for tf2_ros lib, also CMakeLists.txt clean up 0.3.6 (2013-03-03) ------------------ 0.3.5 (2013-02-15 14:46) ------------------------ * 0.3.4 -> 0.3.5 0.3.4 (2013-02-15 13:14) ------------------------ * 0.3.3 -> 0.3.4 0.3.3 (2013-02-15 11:30) ------------------------ * 0.3.2 -> 0.3.3 0.3.2 (2013-02-15 00:42) ------------------------ * 0.3.1 -> 0.3.2 0.3.1 (2013-02-14) ------------------ * 0.3.0 -> 0.3.1 0.3.0 (2013-02-13) ------------------ * switching to version 0.3.0 * Merge pull request `#2 `_ from KaijenHsiao/groovy-devel added setup.py and catkin_python_setup() to tf2_ros * added setup.py and catkin_python_setup() to tf2_ros * fixing cmake target collisions * fixing catkin message dependencies * removing packages with missing deps * catkin fixes * catkinizing geometry-experimental * catkinizing tf2_ros * catching None result in buffer client before it becomes an AttributeError, raising tf2.TransformException instead * oneiric linker fixes, bump version to 0.2.3 * fix deprecated use of Header * merged faust's changes 864 and 865 into non_optimized branch: BufferCore instead of Buffer in TransformListener, and added a constructor that takes a NodeHandle. * add buffer server binary * fix compilation on 32bit * add missing file * build buffer server * TransformListener only needs a BufferCore * Add TransformListener constructor that takes a NodeHandle so you can specify a callback queue to use * Add option to use a callback queue in the message filter * move the message filter to tf2_ros * add missing std_msgs dependency * missed 2 lines in last commit * removing auto clearing from listener for it's unexpected from a library * static transform tested and working * subscriptions to tf_static unshelved * static transform publisher executable running * latching static transform publisher * cleaning out old commented code * Only query rospy.Time.now() when the timeout is greater than 0 * debug comments removed * move to tf2_ros completed. tests pass again * merge tf2_cpp and tf2_py into tf2_ros geometry2-0.7.5/tf2_ros/CMakeLists.txt000066400000000000000000000073651372352374300176020ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_ros) if(NOT ANDROID) set(TF2_PY tf2_py) endif() find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs geometry_msgs message_filters roscpp rosgraph rospy tf2 tf2_msgs ${TF2_PY} ) find_package(Boost REQUIRED COMPONENTS thread) catkin_python_setup() catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS actionlib actionlib_msgs geometry_msgs message_filters roscpp rosgraph tf2 tf2_msgs ${TF2_PY} ) include_directories(include ${catkin_INCLUDE_DIRS}) # tf2_ros library add_library(${PROJECT_NAME} src/buffer.cpp src/transform_listener.cpp src/buffer_client.cpp src/buffer_server.cpp src/transform_broadcaster.cpp src/static_transform_broadcaster.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) # buffer_server executable add_executable(${PROJECT_NAME}_buffer_server src/buffer_server_main.cpp) add_dependencies(${PROJECT_NAME}_buffer_server ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME}_buffer_server ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ) set_target_properties(${PROJECT_NAME}_buffer_server PROPERTIES OUTPUT_NAME buffer_server ) # static_transform_publisher add_executable(${PROJECT_NAME}_static_transform_publisher src/static_transform_broadcaster_program.cpp ) add_dependencies(${PROJECT_NAME}_static_transform_publisher ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME}_static_transform_publisher ${PROJECT_NAME} ${catkin_LIBRARIES} ) set_target_properties(${PROJECT_NAME}_static_transform_publisher PROPERTIES OUTPUT_NAME static_transform_publisher ) # Install library install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) # Install executable install(TARGETS ${PROJECT_NAME}_buffer_server ${PROJECT_NAME}_static_transform_publisher ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) # Tests if(CATKIN_ENABLE_TESTING) # new requirements for testing find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs geometry_msgs message_filters roscpp rosgraph rospy rostest tf2 tf2_msgs ${TF2_PY} ) # tf2_ros_test_listener add_executable(${PROJECT_NAME}_test_listener EXCLUDE_FROM_ALL test/listener_unittest.cpp) add_dependencies(${PROJECT_NAME}_test_listener ${catkin_EXPORTED_TARGETS}) add_executable(${PROJECT_NAME}_test_time_reset EXCLUDE_FROM_ALL test/time_reset_test.cpp) add_dependencies(${PROJECT_NAME}_test_time_reset ${catkin_EXPORTED_TARGETS}) add_executable(${PROJECT_NAME}_test_message_filter EXCLUDE_FROM_ALL test/message_filter_test.cpp) add_dependencies(${PROJECT_NAME}_test_message_filter ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME}_test_listener ${PROJECT_NAME} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ) target_link_libraries(${PROJECT_NAME}_test_time_reset ${PROJECT_NAME} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ) target_link_libraries(${PROJECT_NAME}_test_message_filter ${PROJECT_NAME} ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ) add_dependencies(tests ${PROJECT_NAME}_test_listener) add_dependencies(tests ${PROJECT_NAME}_test_time_reset) add_dependencies(tests ${PROJECT_NAME}_test_message_filter) add_rostest(test/transform_listener_unittest.launch) add_rostest(test/transform_listener_time_reset_test.launch) add_rostest(test/message_filter_test.launch) endif() geometry2-0.7.5/tf2_ros/doc/000077500000000000000000000000001372352374300155745ustar00rootroot00000000000000geometry2-0.7.5/tf2_ros/doc/conf.py000066400000000000000000000150221372352374300170730ustar00rootroot00000000000000# -*- coding: utf-8 -*- # # tf documentation build configuration file, created by # sphinx-quickstart on Mon Jun 1 14:21:53 2009. # # This file is execfile()d with the current directory set to its containing dir. # # Note that not all possible configuration values are present in this # autogenerated file. # # All configuration values have a default; values that are commented out # serve to show the default. import sys, os # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. #sys.path.append(os.path.abspath('.')) # -- General configuration ----------------------------------------------------- # Add any Sphinx extension module names here, as strings. They can be extensions # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] # The suffix of source filenames. source_suffix = '.rst' # The encoding of source files. #source_encoding = 'utf-8' # The master toctree document. master_doc = 'index' # General information about the project. project = u'tf' copyright = u'2009, Willow Garage, Inc.' # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. # # The short X.Y version. version = '0.1' # The full version, including alpha/beta/rc tags. release = '0.1.0' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. #language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: #today = '' # Else, today_fmt is used as the format for a strftime call. #today_fmt = '%B %d, %Y' # List of documents that shouldn't be included in the build. #unused_docs = [] # List of directories, relative to source directory, that shouldn't be searched # for source files. exclude_trees = ['_build'] # The reST default role (used for this markup: `text`) to use for all documents. #default_role = None # If true, '()' will be appended to :func: etc. cross-reference text. #add_function_parentheses = True # If true, the current module name will be prepended to all description # unit titles (such as .. function::). #add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. #show_authors = False # The name of the Pygments (syntax highlighting) style to use. pygments_style = 'sphinx' # A list of ignored prefixes for module index sorting. #modindex_common_prefix = [] # -- Options for HTML output --------------------------------------------------- # The theme to use for HTML and HTML Help pages. Major themes that come with # Sphinx are currently 'default' and 'sphinxdoc'. html_theme = 'default' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the # documentation. #html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. #html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". #html_title = None # A shorter title for the navigation bar. Default is the same as html_title. #html_short_title = None # The name of an image file (relative to this directory) to place at the top # of the sidebar. #html_logo = None # The name of an image file (within the static path) to use as favicon of the # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. #html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". #html_static_path = ['_static'] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. #html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. #html_use_smartypants = True # Custom sidebar templates, maps document names to template names. #html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. #html_additional_pages = {} # If false, no module index is generated. #html_use_modindex = True # If false, no index is generated. #html_use_index = True # If true, the index is split into individual pages for each letter. #html_split_index = False # If true, links to the reST sources are added to the pages. #html_show_sourcelink = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. #html_use_opensearch = '' # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). #html_file_suffix = '' # Output file base name for HTML help builder. htmlhelp_basename = 'tfdoc' # -- Options for LaTeX output -------------------------------------------------- # The paper size ('letter' or 'a4'). #latex_paper_size = 'letter' # The font size ('10pt', '11pt' or '12pt'). #latex_font_size = '10pt' # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, author, documentclass [howto/manual]). latex_documents = [ ('index', 'tf.tex', u'stereo\\_utils Documentation', u'Tully Foote and Eitan Marder-Eppstein', 'manual'), ] # The name of an image file (relative to this directory) to place at the top of # the title page. #latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. #latex_use_parts = False # Additional stuff for the LaTeX preamble. #latex_preamble = '' # Documents to append as an appendix to all manuals. #latex_appendices = [] # If false, no module index is generated. #latex_use_modindex = True # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = { 'http://docs.python.org/': None, 'http://docs.opencv.org/3.0-last-rst/': None, 'http://docs.scipy.org/doc/numpy' : None } autoclass_content = "both" geometry2-0.7.5/tf2_ros/doc/index.rst000066400000000000000000000036201372352374300174360ustar00rootroot00000000000000tf2_ros Overview ================ This is the Python API reference for the tf2_ros package. To broadcast transforms using ROS: - Call :meth:`rospy.init` to initialize a node. - Construct a :class:`tf2_ros.TransformBroadcaster`. - Pass a :class:`geometry_msgs.TransformStamped` message to :meth:`tf2_ros.TransformBroadcaster.sendTransform`. - Alternatively, pass a vector of :class:`geometry_msgs.TransformStamped` messages. To listen for transforms using ROS: - Construct an instance of a class that implements :class:`tf2_ros.BufferInterface`. - :class:`tf2_ros.Buffer` is the standard implementation which offers a tf2_frames service that can respond to requests with a :class:`tf2_msgs.FrameGraph`. - :class:`tf2_ros.BufferClient` uses an :class:`actionlib.SimpleActionClient` to wait for the requested transform to become available. - Pass the :class:`tf2_ros.Buffer` to the constructor of :class:`tf2_ros.TransformListener`. - Optionally, pass a :class:`ros.NodeHandle` (otherwise TransformListener will connect to the node for the process). - Optionally, specify if the TransformListener runs in its own thread or not. - Use :meth:`tf2_ros.BufferInterface.transform` to apply a transform on the tf server to an input frame. - Or, check if a transform is available with :meth:`tf2_ros.BufferInterface.can_transform`. - Then, call :meth:`tf2_ros.BufferInterface.lookup_transform` to get the transform between two frames. For more information, see the tf2 tutorials: http://wiki.ros.org/tf2/Tutorials Or, get an `overview`_ of data type conversion methods in geometry_experimental packages. See http://wiki.ros.org/tf2/Tutorials for more detailed usage. .. _overview: http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions Classes and Exceptions ====================== .. toctree:: :maxdepth: 2 tf2_ros Indices and tables ================== * :ref:`genindex` * :ref:`search` geometry2-0.7.5/tf2_ros/doc/mainpage.dox000066400000000000000000000042531372352374300200750ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_ros is the C++ ROS wrapper around the tf2 transform library. \section codeapi Code API To broadcast transforms using ROS: - Call ros::init() to initialize a node. - Construct a tf2_ros::TransformBroadcaster. - Pass a geometry_msgs::TransformStamped message to tf2_ros::TransformBroadcaster::sendTransform(). - Alternatively, pass a vector of geometry_msgs::TransformStamped messages. - Use tf2_ros::StaticTransformBroadcaster for "latching" behavior when transforms that are not expected to change. To listen for transforms using ROS: - Construct an instance of a class that implements tf2_ros::BufferInterface. - tf2_ros::Buffer is the standard implementation which offers a tf2_frames service that can respond to requests with a tf2_msgs::FrameGraph. - tf2_ros::BufferClient uses an actionlib::SimpleActionClient to wait for the requested transform to become available. - It should be used with a tf2_ros::BufferServer, which offers the corresponding actionlib::ActionSErver. - Pass the tf2_ros::Buffer to the constructor of tf2_ros::TransformListener. - Optionally, pass a ros::NodeHandle (otherwise TransformListener will connect to the node for the process). - Optionally, specify if the TransformListener runs in its own thread or not. - Use tf2_ros::BufferInterface::transform() to apply a transform on the tf server to an input frame. - Or, check if a transform is available with tf2_ros::BufferInterface::canTransform(). - Then, call tf2_ros::BufferInterface::lookupTransform() to get the transform between two frames. - Construct a tf2_ros::MessageFilter with the TransformListener to apply a transformation to incoming frames. - This is especially useful when streaming sensor data. List of exceptions thrown in this library: - tf2::LookupException - tf2::ConnectivityException - tf2::ExtrapolationException - tf2::InvalidArgumentException - tf2::TimeoutException - tf2::TransformException For more information, see the tf2 tutorials: http://wiki.ros.org/tf2/Tutorials Or, get an overview of data type conversion methods in geometry_experimental packages. */ geometry2-0.7.5/tf2_ros/doc/tf2_ros.rst000066400000000000000000000036211372352374300177060ustar00rootroot00000000000000tf_ros2 Python API ================== Exceptions ---------- .. exception:: tf2.TransformException base class for tf exceptions. Because :exc:`tf2.TransformException` is the base class for other exceptions, you can catch all tf exceptions by writing:: try: # do some tf2 work except tf2.TransformException: print "some tf2 exception happened" .. exception:: tf2.ConnectivityException subclass of :exc:`TransformException`. Raised when that the fixed_frame tree is not connected between the frames requested. .. exception:: tf2.LookupException subclass of :exc:`TransformException`. Raised when a tf method has attempted to access a frame, but the frame is not in the graph. The most common reason for this is that the frame is not being published, or a parent frame was not set correctly causing the tree to be broken. .. exception:: tf2.ExtrapolationException subclass of :exc:`TransformException` Raised when a tf method would have required extrapolation beyond current limits. .. exception:: tf2.InvalidArgumentException subclass of :exc:`TransformException`. Raised when the arguments to the method are called improperly formed. An example of why this might be raised is if an argument is nan. .. autoexception:: tf2_ros.buffer_interface.TypeException .. autoexception:: tf2_ros.buffer_interface.NotImplementedException BufferInterface --------------- .. autoclass:: tf2_ros.buffer_interface.BufferInterface :members: Buffer ------ .. autoclass:: tf2_ros.buffer.Buffer :members: BufferClient ------------ .. autoclass:: tf2_ros.buffer_client.BufferClient :members: TransformBroadcaster -------------------- .. autoclass:: tf2_ros.transform_broadcaster.TransformBroadcaster :members: TransformListener ----------------- .. autoclass:: tf2_ros.transform_listener.TransformListener :members: geometry2-0.7.5/tf2_ros/include/000077500000000000000000000000001372352374300164525ustar00rootroot00000000000000geometry2-0.7.5/tf2_ros/include/tf2_ros/000077500000000000000000000000001372352374300200305ustar00rootroot00000000000000geometry2-0.7.5/tf2_ros/include/tf2_ros/buffer.h000066400000000000000000000153431372352374300214600ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #ifndef TF2_ROS_BUFFER_H #define TF2_ROS_BUFFER_H #include #include #include #include #include namespace tf2_ros { /** \brief Standard implementation of the tf2_ros::BufferInterface abstract data type. * * Inherits tf2_ros::BufferInterface and tf2::BufferCore. * Stores known frames and optionally offers a ROS service, "tf2_frames", which responds to client requests * with a response containing a tf2_msgs::FrameGraph representing the relationship of known frames. */ class Buffer: public BufferInterface, public tf2::BufferCore { public: using tf2::BufferCore::lookupTransform; using tf2::BufferCore::canTransform; /** * @brief Constructor for a Buffer object * @param cache_time How long to keep a history of transforms * @param debug Whether to advertise the tf2_frames service that exposes debugging information from the buffer * @return */ Buffer(ros::Duration cache_time = ros::Duration(BufferCore::DEFAULT_CACHE_TIME), bool debug = false); /** \brief Get the transform between two frames by frame ID. * \param target_frame The frame to which data should be transformed * \param source_frame The frame where the data originated * \param time The time at which the value of the transform is desired. (0 will get the latest) * \param timeout How long to block before failing * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException */ virtual geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout) const; /** \brief Get the transform between two frames by frame ID assuming fixed frame. * \param target_frame The frame to which data should be transformed * \param target_time The time to which the data should be transformed. (0 will get the latest) * \param source_frame The frame where the data originated * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) * \param fixed_frame The frame in which to assume the transform is constant in time. * \param timeout How long to block before failing * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException */ virtual geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout) const; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param source_frame The frame from which to transform * \param target_time The time at which to transform * \param timeout How long to block before failing * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL * \return True if the transform is possible, false otherwise */ virtual bool canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& target_time, const ros::Duration timeout, std::string* errstr = NULL) const; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param target_time The time into which to transform * \param source_frame The frame from which to transform * \param source_time The time from which to transform * \param fixed_frame The frame in which to treat the transform as constant in time * \param timeout How long to block before failing * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL * \return True if the transform is possible, false otherwise */ virtual bool canTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr = NULL) const; private: bool getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res) ; // conditionally error if dedicated_thread unset. bool checkAndErrorDedicatedThreadPresent(std::string* errstr) const; ros::ServiceServer frames_server_; }; // class static const std::string threading_error = "Do not call canTransform or lookupTransform with a timeout unless you are using another thread for populating data. Without a dedicated thread it will always timeout. If you have a separate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance."; } // namespace #endif // TF2_ROS_BUFFER_H geometry2-0.7.5/tf2_ros/include/tf2_ros/buffer_client.h000066400000000000000000000160161372352374300230140ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef TF2_ROS_BUFFER_CLIENT_H_ #define TF2_ROS_BUFFER_CLIENT_H_ #include #include #include namespace tf2_ros { /** \brief Action client-based implementation of the tf2_ros::BufferInterface abstract data type. * * BufferClient uses actionlib to coordinate waiting for available transforms. * * You can use this class with a tf2_ros::BufferServer and tf2_ros::TransformListener in a separate process. */ class BufferClient : public BufferInterface { public: typedef actionlib::SimpleActionClient LookupActionClient; /** \brief BufferClient constructor * \param ns The namespace in which to look for a BufferServer * \param check_frequency Deprecated, not used anymore * \param timeout_padding The amount of time to allow passed the desired timeout on the client side for communication lag */ BufferClient(std::string ns, double check_frequency = 10.0, ros::Duration timeout_padding = ros::Duration(2.0)); /** \brief Get the transform between two frames by frame ID. * \param target_frame The frame to which data should be transformed * \param source_frame The frame where the data originated * \param time The time at which the value of the transform is desired. (0 will get the latest) * \param timeout How long to block before failing * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException */ virtual geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout = ros::Duration(0.0)) const; /** \brief Get the transform between two frames by frame ID assuming fixed frame. * \param target_frame The frame to which data should be transformed * \param target_time The time to which the data should be transformed. (0 will get the latest) * \param source_frame The frame where the data originated * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) * \param fixed_frame The frame in which to assume the transform is constant in time. * \param timeout How long to block before failing * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException */ virtual geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout = ros::Duration(0.0)) const; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param source_frame The frame from which to transform * \param time The time at which to transform * \param timeout How long to block before failing * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL * \return True if the transform is possible, false otherwise */ virtual bool canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout = ros::Duration(0.0), std::string* errstr = NULL) const; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param target_time The time into which to transform * \param source_frame The frame from which to transform * \param source_time The time from which to transform * \param fixed_frame The frame in which to treat the transform as constant in time * \param timeout How long to block before failing * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL * \return True if the transform is possible, false otherwise */ virtual bool canTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout = ros::Duration(0.0), std::string* errstr = NULL) const; /** \brief Block until the action server is ready to respond to requests. * \param timeout Time to wait for the server. * \return True if the server is ready, false otherwise. */ bool waitForServer(const ros::Duration& timeout = ros::Duration(0)) { return client_.waitForServer(timeout); } private: geometry_msgs::TransformStamped processGoal(const tf2_msgs::LookupTransformGoal& goal) const; geometry_msgs::TransformStamped processResult(const tf2_msgs::LookupTransformResult& result) const; mutable LookupActionClient client_; double check_frequency_; ros::Duration timeout_padding_; }; }; #endif geometry2-0.7.5/tf2_ros/include/tf2_ros/buffer_interface.h000066400000000000000000000316301372352374300234750ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #ifndef TF2_ROS_BUFFER_INTERFACE_H #define TF2_ROS_BUFFER_INTERFACE_H #include #include #include #include #include #include namespace tf2_ros { /** \brief Abstract interface for wrapping tf2::BufferCore in a ROS-based API. * Implementations include tf2_ros::Buffer and tf2_ros::BufferClient. */ class BufferInterface { public: /** \brief Get the transform between two frames by frame ID. * \param target_frame The frame to which data should be transformed * \param source_frame The frame where the data originated * \param time The time at which the value of the transform is desired. (0 will get the latest) * \param timeout How long to block before failing * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException */ virtual geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout) const = 0; /** \brief Get the transform between two frames by frame ID assuming fixed frame. * \param target_frame The frame to which data should be transformed * \param target_time The time to which the data should be transformed. (0 will get the latest) * \param source_frame The frame where the data originated * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) * \param fixed_frame The frame in which to assume the transform is constant in time. * \param timeout How long to block before failing * \return The transform between the frames * * Possible exceptions tf2::LookupException, tf2::ConnectivityException, * tf2::ExtrapolationException, tf2::InvalidArgumentException */ virtual geometry_msgs::TransformStamped lookupTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout) const = 0; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param source_frame The frame from which to transform * \param time The time at which to transform * \param timeout How long to block before failing * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL * \return True if the transform is possible, false otherwise */ virtual bool canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout, std::string* errstr = NULL) const = 0; /** \brief Test if a transform is possible * \param target_frame The frame into which to transform * \param target_time The time into which to transform * \param source_frame The frame from which to transform * \param source_time The time from which to transform * \param fixed_frame The frame in which to treat the transform as constant in time * \param timeout How long to block before failing * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL * \return True if the transform is possible, false otherwise */ virtual bool canTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr = NULL) const = 0; /** \brief Transform an input into the target frame. * This function is templated and can take as input any valid mathematical object that tf knows * how to apply a transform to, by way of the templated math conversions interface. * For example, the template type could be a Transform, Pose, Vector, or Quaternion message * type (as defined in geometry_msgs). * \tparam T The type of the object to transform. * \param in The object to transform * \param out The transformed output, preallocated by the caller. * \param target_frame The string identifer for the frame to transform into. * \param timeout How long to wait for the target frame. Default value is zero (no blocking). */ template T& transform(const T& in, T& out, const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const { // do the transform tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout)); return out; } /** \brief Transform an input into the target frame. * This function is templated and can take as input any valid mathematical object that tf knows * how to apply a transform to, by way of the templated math conversions interface. * For example, the template type could be a Transform, Pose, Vector, or Quaternion message * type (as defined in geometry_msgs). * \tparam T The type of the object to transform. * \param in The object to transform. * \param target_frame The string identifer for the frame to transform into. * \param timeout How long to wait for the target frame. Default value is zero (no blocking). * \return The transformed output. */ template T transform(const T& in, const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const { T out; return transform(in, out, target_frame, timeout); } /** \brief Transform an input into the target frame and convert to a specified output type. * It is templated on two types: the type of the input object and the type of the * transformed output. * For example, the template types could be Transform, Pose, Vector, or Quaternion messages * type (as defined in geometry_msgs). * The function will calculate the transformation and then convert the result into the * specified output type. * Compilation will fail if a known conversion does not exist bewteen the two template * parameters. * \tparam A The type of the object to transform. * \tparam B The type of the transformed output. * \param in The object to transform * \param out The transformed output, converted to the specified type. * \param target_frame The string identifer for the frame to transform into. * \param timeout How long to wait for the target frame. Default value is zero (no blocking). * \return The transformed output, converted to the specified type. */ template B& transform(const A& in, B& out, const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const { A copy = transform(in, target_frame, timeout); tf2::convert(copy, out); return out; } /** \brief Transform an input into the target frame (advanced). * This function is templated and can take as input any valid mathematical object that tf knows * how to apply a transform to, by way of the templated math conversions interface. * For example, the template type could be a Transform, Pose, Vector, or Quaternion message * type (as defined in geometry_msgs). * This function follows the advanced API, which allows transforming between different time * points, and specifying a fixed frame that does not varying in time. * \tparam T The type of the object to transform. * \param in The object to transform * \param out The transformed output, preallocated by the caller. * \param target_frame The string identifer for the frame to transform into. * \param target_time The time into which to transform * \param fixed_frame The frame in which to treat the transform as constant in time. * \param timeout How long to wait for the target frame. Default value is zero (no blocking). */ template T& transform(const T& in, T& out, const std::string& target_frame, const ros::Time& target_time, const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const { // do the transform tf2::doTransform(in, out, lookupTransform(target_frame, target_time, tf2::getFrameId(in), tf2::getTimestamp(in), fixed_frame, timeout)); return out; } /** \brief Transform an input into the target frame (advanced). * This function is templated and can take as input any valid mathematical object that tf knows * how to apply a transform to, by way of the templated math conversions interface. * For example, the template type could be a Transform, Pose, Vector, or Quaternion message * type (as defined in geometry_msgs). * This function follows the advanced API, which allows transforming between different time * points, and specifying a fixed frame that does not varying in time. * \tparam T The type of the object to transform. * \param in The object to transform * \param target_frame The string identifer for the frame to transform into. * \param target_time The time into which to transform * \param fixed_frame The frame in which to treat the transform as constant in time. * \param timeout How long to wait for the target frame. Default value is zero (no blocking). * \return The transformed output. */ template T transform(const T& in, const std::string& target_frame, const ros::Time& target_time, const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const { T out; return transform(in, out, target_frame, target_time, fixed_frame, timeout); } /** \brief Transform an input into the target frame and convert to a specified output type (advanced). * It is templated on two types: the type of the input object and the type of the * transformed output. * For example, the template type could be a Transform, Pose, Vector, or Quaternion message * type (as defined in geometry_msgs). * The function will calculate the transformation and then convert the result into the * specified output type. * Compilation will fail if a known conversion does not exist bewteen the two template * parameters. * This function follows the advanced API, which allows transforming between different time * points, and specifying a fixed frame that does not varying in time. * \tparam A The type of the object to transform. * \tparam B The type of the transformed output. * \param in The object to transform * \param out The transformed output, converted to the specified output type. * \param target_frame The string identifer for the frame to transform into. * \param target_time The time into which to transform * \param fixed_frame The frame in which to treat the transform as constant in time. * \param timeout How long to wait for the target frame. Default value is zero (no blocking). * \return The transformed output, converted to the specified output type. */ template B& transform(const A& in, B& out, const std::string& target_frame, const ros::Time& target_time, const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const { // do the transform A copy = transform(in, target_frame, target_time, fixed_frame, timeout); tf2::convert(copy, out); return out; } }; // class } // namespace #endif // TF2_ROS_BUFFER_INTERFACE_H geometry2-0.7.5/tf2_ros/include/tf2_ros/buffer_server.h000066400000000000000000000070541372352374300230460ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef TF2_ROS_BUFFER_SERVER_H_ #define TF2_ROS_BUFFER_SERVER_H_ #include #include #include #include namespace tf2_ros { /** \brief Action server for the actionlib-based implementation of tf2_ros::BufferInterface. * * Use this class with a tf2_ros::TransformListener in the same process. * You can use this class with a tf2_ros::BufferClient in a different process. */ class BufferServer { private: typedef actionlib::ActionServer LookupTransformServer; typedef LookupTransformServer::GoalHandle GoalHandle; struct GoalInfo { GoalHandle handle; ros::Time end_time; }; public: /** \brief Constructor * \param buffer The Buffer that this BufferServer will wrap. * \param ns The namespace in which to look for action clients. * \param auto_start Pass argument to the constructor of the ActionServer. * \param check_period How often to check for changes to known transforms (via a timer event). */ BufferServer(const Buffer& buffer, const std::string& ns, bool auto_start = true, ros::Duration check_period = ros::Duration(0.01)); /** \brief Start the action server. */ void start(); private: void goalCB(GoalHandle gh); void cancelCB(GoalHandle gh); void checkTransforms(const ros::TimerEvent& e); bool canTransform(GoalHandle gh); geometry_msgs::TransformStamped lookupTransform(GoalHandle gh); const Buffer& buffer_; LookupTransformServer server_; std::list active_goals_; boost::mutex mutex_; ros::Timer check_timer_; }; } #endif geometry2-0.7.5/tf2_ros/include/tf2_ros/message_filter.h000066400000000000000000000571101372352374300231760ustar00rootroot00000000000000/* * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Josh Faust */ #ifndef TF2_ROS_MESSAGE_FILTER_H #define TF2_ROS_MESSAGE_FILTER_H #include #include #include #include #include #include #include #include #include #include #include #include #include #define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \ ROS_DEBUG_NAMED("message_filter", std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), getTargetFramesString().c_str(), __VA_ARGS__) #define TF2_ROS_MESSAGEFILTER_WARN(fmt, ...) \ ROS_WARN_NAMED("message_filter", std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), getTargetFramesString().c_str(), __VA_ARGS__) namespace tf2_ros { namespace filter_failure_reasons { enum FilterFailureReason { /// The message buffer overflowed, and this message was pushed off the back of the queue, but the reason it was unable to be transformed is unknown. Unknown, /// The timestamp on the message is more than the cache length earlier than the newest data in the transform cache OutTheBack, /// The frame_id on the message is empty EmptyFrameID, }; } typedef filter_failure_reasons::FilterFailureReason FilterFailureReason; class MessageFilterBase { public: typedef std::vector V_string; virtual ~MessageFilterBase(){} virtual void clear() = 0; virtual void setTargetFrame(const std::string& target_frame) = 0; virtual void setTargetFrames(const V_string& target_frames) = 0; virtual void setTolerance(const ros::Duration& tolerance) = 0; }; /** * \brief Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available * * The callbacks used in this class are of the same form as those used by roscpp's message callbacks. * * MessageFilter is templated on a message type. * * \section example_usage Example Usage * * If you want to hook a MessageFilter into a ROS topic: \verbatim message_filters::Subscriber sub(node_handle_, "topic", 10); tf::MessageFilter tf_filter(sub, tf_listener_, "/map", 10); tf_filter.registerCallback(&MyClass::myCallback, this); \endverbatim */ template class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter { public: typedef boost::shared_ptr MConstPtr; typedef ros::MessageEvent MEvent; typedef boost::function FailureCallback; typedef boost::signals2::signal FailureSignal; // If you hit this assert your message does not have a header, or does not have the HasHeader trait defined for it // Actually, we need to check that the message has a header, or that it // has the FrameId and Stamp traits. However I don't know how to do that // so simply commenting out for now. //ROS_STATIC_ASSERT(ros::message_traits::HasHeader::value); /** * \brief Constructor * * \param bc The tf2::BufferCore this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param nh The NodeHandle whose callback queue we should add callbacks to */ MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh) : bc_(bc) , queue_size_(queue_size) , callback_queue_(nh.getCallbackQueue()) { init(); setTargetFrame(target_frame); } /** * \brief Constructor * * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. * \param bc The tf2::BufferCore this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param nh The NodeHandle whose callback queue we should add callbacks to */ template MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh) : bc_(bc) , queue_size_(queue_size) , callback_queue_(nh.getCallbackQueue()) { init(); setTargetFrame(target_frame); connectInput(f); } /** * \brief Constructor * * \param bc The tf2::BufferCore this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param cbqueue The callback queue to add callbacks to. If NULL, callbacks will happen from whatever thread either * a) add() is called, which will generally be when the previous filter in the chain outputs a message, or * b) tf2::BufferCore::setTransform() is called */ MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue) : bc_(bc) , queue_size_(queue_size) , callback_queue_(cbqueue) { init(); setTargetFrame(target_frame); } /** * \brief Constructor * * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. * \param bc The tf2::BufferCore this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param cbqueue The callback queue to add callbacks to. If NULL, callbacks will happen from whatever thread either * a) add() is called, which will generally be when the previous filter in the chain outputs a message, or * b) tf2::BufferCore::setTransform() is called */ template MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue) : bc_(bc) , queue_size_(queue_size) , callback_queue_(cbqueue) { init(); setTargetFrame(target_frame); connectInput(f); } /** * \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first. */ template void connectInput(F& f) { message_connection_.disconnect(); message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this); } /** * \brief Destructor */ ~MessageFilter() { message_connection_.disconnect(); MessageFilter::clear(); TF2_ROS_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu", (long long unsigned int)successful_transform_count_, (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_, (long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_); } /** * \brief Set the frame you need to be able to transform to before getting a message callback */ void setTargetFrame(const std::string& target_frame) { V_string frames; frames.push_back(target_frame); setTargetFrames(frames); } /** * \brief Set the frames you need to be able to transform to before getting a message callback */ void setTargetFrames(const V_string& target_frames) { boost::mutex::scoped_lock frames_lock(target_frames_mutex_); target_frames_.resize(target_frames.size()); std::transform(target_frames.begin(), target_frames.end(), target_frames_.begin(), this->stripSlash); expected_success_count_ = target_frames_.size() * (time_tolerance_.isZero() ? 1 : 2); std::stringstream ss; for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) { ss << *it << " "; } target_frames_string_ = ss.str(); } /** * \brief Get the target frames as a string for debugging */ std::string getTargetFramesString() { boost::mutex::scoped_lock lock(target_frames_mutex_); return target_frames_string_; }; /** * \brief Set the required tolerance for the notifier to return true */ void setTolerance(const ros::Duration& tolerance) { boost::mutex::scoped_lock lock(target_frames_mutex_); time_tolerance_ = tolerance; expected_success_count_ = target_frames_.size() * (time_tolerance_.isZero() ? 1 : 2); } /** * \brief Clear any messages currently in the queue */ void clear() { boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_); TF2_ROS_MESSAGEFILTER_DEBUG("%s", "Cleared"); bc_.removeTransformableCallback(callback_handle_); callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, _1, _2, _3, _4, _5)); messages_.clear(); message_count_ = 0; // remove pending callbacks in callback queue as well if (callback_queue_) callback_queue_->removeByID((uint64_t)this); warned_about_empty_frame_id_ = false; } void add(const MEvent& evt) { if (target_frames_.empty()) { return; } namespace mt = ros::message_traits; const MConstPtr& message = evt.getMessage(); std::string frame_id = stripSlash(mt::FrameId::value(*message)); ros::Time stamp = mt::TimeStamp::value(*message); if (frame_id.empty()) { messageDropped(evt, filter_failure_reasons::EmptyFrameID); return; } // iterate through the target frames and add requests for each of them MessageInfo info; info.handles.reserve(expected_success_count_); { V_string target_frames_copy; // Copy target_frames_ to avoid deadlock from #79 { boost::mutex::scoped_lock frames_lock(target_frames_mutex_); target_frames_copy = target_frames_; } V_string::iterator it = target_frames_copy.begin(); V_string::iterator end = target_frames_copy.end(); for (; it != end; ++it) { const std::string& target_frame = *it; tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp); if (handle == 0xffffffffffffffffULL) // never transformable { messageDropped(evt, filter_failure_reasons::OutTheBack); return; } else if (handle == 0) { ++info.success_count; } else { info.handles.push_back(handle); } if (!time_tolerance_.isZero()) { handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp + time_tolerance_); if (handle == 0xffffffffffffffffULL) // never transformable { messageDropped(evt, filter_failure_reasons::OutTheBack); return; } else if (handle == 0) { ++info.success_count; } else { info.handles.push_back(handle); } } } } // We can transform already if (info.success_count == expected_success_count_) { messageReady(evt); } else { boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_); // If this message is about to push us past our queue size, erase the oldest message if (queue_size_ != 0 && message_count_ + 1 > queue_size_) { ++dropped_message_count_; const MessageInfo& front = messages_.front(); TF2_ROS_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_, (mt::FrameId::value(*front.event.getMessage())).c_str(), mt::TimeStamp::value(*front.event.getMessage()).toSec()); V_TransformableRequestHandle::const_iterator it = front.handles.begin(); V_TransformableRequestHandle::const_iterator end = front.handles.end(); for (; it != end; ++it) { bc_.cancelTransformableRequest(*it); } messageDropped(front.event, filter_failure_reasons::Unknown); messages_.pop_front(); --message_count_; } // Add the message to our list info.event = evt; messages_.push_back(info); ++message_count_; } TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_); ++incoming_message_count_; } /** * \brief Manually add a message into this filter. * \note If the message (or any other messages in the queue) are immediately transformable this will immediately call through to the output callback, possibly * multiple times */ void add(const MConstPtr& message) { boost::shared_ptr > header(new std::map); (*header)["callerid"] = "unknown"; ros::WallTime n = ros::WallTime::now(); ros::Time t(n.sec, n.nsec); add(MEvent(message, header, t)); } /** * \brief Register a callback to be called when a message is about to be dropped * \param callback The callback to call */ message_filters::Connection registerFailureCallback(const FailureCallback& callback) { boost::mutex::scoped_lock lock(failure_signal_mutex_); return message_filters::Connection(boost::bind(&MessageFilter::disconnectFailure, this, _1), failure_signal_.connect(callback)); } virtual void setQueueSize( uint32_t new_queue_size ) { queue_size_ = new_queue_size; } virtual uint32_t getQueueSize() { return queue_size_; } private: void init() { message_count_ = 0; successful_transform_count_ = 0; failed_out_the_back_count_ = 0; transform_message_count_ = 0; incoming_message_count_ = 0; dropped_message_count_ = 0; time_tolerance_ = ros::Duration(0.0); warned_about_empty_frame_id_ = false; expected_success_count_ = 1; callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, _1, _2, _3, _4, _5)); } void transformable(tf2::TransformableRequestHandle request_handle, const std::string& /* target_frame */, const std::string& /* source_frame */, ros::Time /* time */, tf2::TransformableResult result) { namespace mt = ros::message_traits; boost::upgrade_lock< boost::shared_mutex > lock(messages_mutex_); // find the message this request is associated with typename L_MessageInfo::iterator msg_it = messages_.begin(); typename L_MessageInfo::iterator msg_end = messages_.end(); for (; msg_it != msg_end; ++msg_it) { MessageInfo& info = *msg_it; V_TransformableRequestHandle::const_iterator handle_it = std::find(info.handles.begin(), info.handles.end(), request_handle); if (handle_it != info.handles.end()) { // found msg_it ++info.success_count; break; } } if (msg_it == msg_end) { return; } const MessageInfo& info = *msg_it; if (info.success_count < expected_success_count_) { return; } bool can_transform = true; const MConstPtr& message = info.event.getMessage(); std::string frame_id = stripSlash(mt::FrameId::value(*message)); ros::Time stamp = mt::TimeStamp::value(*message); if (result == tf2::TransformAvailable) { boost::mutex::scoped_lock frames_lock(target_frames_mutex_); // make sure we can still perform all the necessary transforms typename V_string::iterator it = target_frames_.begin(); typename V_string::iterator end = target_frames_.end(); for (; it != end; ++it) { const std::string& target = *it; if (!bc_.canTransform(target, frame_id, stamp)) { can_transform = false; break; } if (!time_tolerance_.isZero()) { if (!bc_.canTransform(target, frame_id, stamp + time_tolerance_)) { can_transform = false; break; } } } } else { can_transform = false; } // We will be mutating messages now, require unique lock boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock); if (can_transform) { TF2_ROS_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1); ++successful_transform_count_; messageReady(info.event); } else { ++dropped_message_count_; TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1); messageDropped(info.event, filter_failure_reasons::Unknown); } messages_.erase(msg_it); --message_count_; } /** * \brief Callback that happens when we receive a message on the message topic */ void incomingMessage(const ros::MessageEvent& evt) { add(evt); } void checkFailures() { if (next_failure_warning_.isZero()) { next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(15); } if (ros::WallTime::now() >= next_failure_warning_) { if (incoming_message_count_ - message_count_ == 0) { return; } double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_); if (dropped_pct > 0.95) { TF2_ROS_MESSAGEFILTER_WARN("Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct*100, ROSCONSOLE_DEFAULT_NAME); next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(60); if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5) { TF2_ROS_MESSAGEFILTER_WARN(" The majority of dropped messages were due to messages growing older than the TF cache time. The last message's timestamp was: %f, and the last frame_id was: %s", last_out_the_back_stamp_.toSec(), last_out_the_back_frame_.c_str()); } } } } struct CBQueueCallback : public ros::CallbackInterface { CBQueueCallback(MessageFilter* filter, const MEvent& event, bool success, FilterFailureReason reason) : event_(event) , filter_(filter) , reason_(reason) , success_(success) {} virtual CallResult call() { if (success_) { filter_->signalMessage(event_); } else { filter_->signalFailure(event_, reason_); } return Success; } private: MEvent event_; MessageFilter* filter_; FilterFailureReason reason_; bool success_; }; void messageDropped(const MEvent& evt, FilterFailureReason reason) { if (callback_queue_) { ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason)); callback_queue_->addCallback(cb, (uint64_t)this); } else { signalFailure(evt, reason); } } void messageReady(const MEvent& evt) { if (callback_queue_) { ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, true, filter_failure_reasons::Unknown)); callback_queue_->addCallback(cb, (uint64_t)this); } else { this->signalMessage(evt); } } void disconnectFailure(const message_filters::Connection& c) { boost::mutex::scoped_lock lock(failure_signal_mutex_); c.getBoostConnection().disconnect(); } void signalFailure(const MEvent& evt, FilterFailureReason reason) { boost::mutex::scoped_lock lock(failure_signal_mutex_); failure_signal_(evt.getMessage(), reason); } static std::string stripSlash(const std::string& in) { if ( !in.empty() && (in[0] == '/')) { std::string out = in; out.erase(0, 1); return out; } return in; } tf2::BufferCore& bc_; ///< The Transformer used to determine if transformation data is available V_string target_frames_; ///< The frames we need to be able to transform to before a message is ready std::string target_frames_string_; boost::mutex target_frames_mutex_; ///< A mutex to protect access to the target_frames_ list and target_frames_string. uint32_t queue_size_; ///< The maximum number of messages we queue up tf2::TransformableCallbackHandle callback_handle_; typedef std::vector V_TransformableRequestHandle; struct MessageInfo { MessageInfo() : success_count(0) {} MEvent event; V_TransformableRequestHandle handles; uint32_t success_count; }; typedef std::list L_MessageInfo; L_MessageInfo messages_; uint32_t message_count_; ///< The number of messages in the list. Used because \.size() may have linear cost boost::shared_mutex messages_mutex_; ///< The mutex used for locking message list operations uint32_t expected_success_count_; bool warned_about_empty_frame_id_; uint64_t successful_transform_count_; uint64_t failed_out_the_back_count_; uint64_t transform_message_count_; uint64_t incoming_message_count_; uint64_t dropped_message_count_; ros::Time last_out_the_back_stamp_; std::string last_out_the_back_frame_; ros::WallTime next_failure_warning_; ros::Duration time_tolerance_; ///< Provide additional tolerance on time for messages which are stamped but can have associated duration message_filters::Connection message_connection_; FailureSignal failure_signal_; boost::mutex failure_signal_mutex_; ros::CallbackQueueInterface* callback_queue_; }; } // namespace tf2 #endif geometry2-0.7.5/tf2_ros/include/tf2_ros/static_transform_broadcaster.h000066400000000000000000000056061372352374300261430ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_ROS_STATICTRANSFORMBROADCASTER_H #define TF2_ROS_STATICTRANSFORMBROADCASTER_H #include "ros/ros.h" #include "geometry_msgs/TransformStamped.h" #include "tf2_msgs/TFMessage.h" namespace tf2_ros { /** \brief This class provides an easy way to publish coordinate frame transform information. * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the * necessary data needed for each message. */ class StaticTransformBroadcaster{ public: /** \brief Constructor (needs a ros::Node reference) */ StaticTransformBroadcaster(); /** \brief Send a TransformStamped message * The stamped data structure includes frame_id, and time, and parent_id already. */ void sendTransform(const geometry_msgs::TransformStamped & transform) { sendTransform(std::vector({transform})); } /** \brief Send a vector of TransformStamped messages * The stamped data structure includes frame_id, and time, and parent_id already. */ void sendTransform(const std::vector & transforms); private: /// Internal reference to ros::Node ros::NodeHandle node_; ros::Publisher publisher_; tf2_msgs::TFMessage net_message_; }; } #endif //TF_STATICTRANSFORMBROADCASTER_H geometry2-0.7.5/tf2_ros/include/tf2_ros/transform_broadcaster.h000066400000000000000000000061411372352374300245670ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_ROS_TRANSFORMBROADCASTER_H #define TF2_ROS_TRANSFORMBROADCASTER_H #include "ros/ros.h" #include "geometry_msgs/TransformStamped.h" namespace tf2_ros { /** \brief This class provides an easy way to publish coordinate frame transform information. * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the * necessary data needed for each message. */ class TransformBroadcaster{ public: /** \brief Constructor (needs a ros::Node reference) */ TransformBroadcaster(); /** \brief Send a StampedTransform * The stamped data structure includes frame_id, and time, and parent_id already. */ // void sendTransform(const StampedTransform & transform); /** \brief Send a vector of StampedTransforms * The stamped data structure includes frame_id, and time, and parent_id already. */ //void sendTransform(const std::vector & transforms); /** \brief Send a TransformStamped message * The stamped data structure includes frame_id, and time, and parent_id already. */ void sendTransform(const geometry_msgs::TransformStamped & transform); /** \brief Send a vector of TransformStamped messages * The stamped data structure includes frame_id, and time, and parent_id already. */ void sendTransform(const std::vector & transforms); private: /// Internal reference to ros::Node ros::NodeHandle node_; ros::Publisher publisher_; }; } #endif //TF_TRANSFORMBROADCASTER_H geometry2-0.7.5/tf2_ros/include/tf2_ros/transform_listener.h000066400000000000000000000066011372352374300241240ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #ifndef TF2_ROS_TRANSFORMLISTENER_H #define TF2_ROS_TRANSFORMLISTENER_H #include "std_msgs/Empty.h" #include "tf2_msgs/TFMessage.h" #include "ros/ros.h" #include "ros/callback_queue.h" #include "tf2_ros/buffer.h" #include "boost/thread.hpp" namespace tf2_ros{ /** \brief This class provides an easy way to request and receive coordinate frame transform information. */ class TransformListener { public: /**@brief Constructor for transform listener */ TransformListener(tf2::BufferCore& buffer, bool spin_thread = true, ros::TransportHints transport_hints = ros::TransportHints()); TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread = true, ros::TransportHints transport_hints = ros::TransportHints()); ~TransformListener(); private: /// Initialize this transform listener, subscribing, advertising services, etc. void init(); void initWithThread(); /// Callback function for ros message subscription void subscription_callback(const ros::MessageEvent& msg_evt); void static_subscription_callback(const ros::MessageEvent& msg_evt); void subscription_callback_impl(const ros::MessageEvent& msg_evt, bool is_static); ros::CallbackQueue tf_message_callback_queue_; boost::thread* dedicated_listener_thread_; ros::NodeHandle node_; ros::Subscriber message_subscriber_tf_; ros::Subscriber message_subscriber_tf_static_; tf2::BufferCore& buffer_; bool using_dedicated_thread_; ros::TransportHints transport_hints_; ros::Time last_update_; void dedicatedListenerThread() { while (using_dedicated_thread_) { tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01)); } }; }; } #endif //TF_TRANSFORMLISTENER_H geometry2-0.7.5/tf2_ros/package.xml000066400000000000000000000027071372352374300171520ustar00rootroot00000000000000 tf2_ros 0.7.5 This package contains the ROS bindings for the tf2 library, for both Python and C++. Eitan Marder-Eppstein Wim Meeussen Tully Foote BSD http://www.ros.org/wiki/tf2_ros catkin actionlib actionlib_msgs geometry_msgs message_filters roscpp rosgraph rospy std_msgs tf2 tf2_msgs tf2_py xmlrpcpp actionlib actionlib_msgs geometry_msgs message_filters roscpp rosgraph rospy std_msgs tf2 tf2_msgs tf2_py xmlrpcpp rostest geometry2-0.7.5/tf2_ros/rosdoc.yaml000066400000000000000000000002771372352374300172120ustar00rootroot00000000000000 - builder: doxygen name: C++ API output_dir: c++ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' - builder: sphinx name: Python API output_dir: python sphinx_root_dir: doc geometry2-0.7.5/tf2_ros/setup.py000066400000000000000000000005001372352374300165340ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['tf2_ros'], package_dir={'': 'src'}, requires=['rospy', 'actionlib', 'actionlib_msgs', 'tf2_msgs', 'tf2_py', 'geometry_msgs'] ) setup(**d) geometry2-0.7.5/tf2_ros/src/000077500000000000000000000000001372352374300156165ustar00rootroot00000000000000geometry2-0.7.5/tf2_ros/src/buffer.cpp000066400000000000000000000154571372352374300176070ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Wim Meeussen */ #include "tf2_ros/buffer.h" #include #include namespace tf2_ros { static const double CAN_TRANSFORM_POLLING_SCALE = 0.01; Buffer::Buffer(ros::Duration cache_time, bool debug) : BufferCore(cache_time) { if(debug && !ros::service::exists("~tf2_frames", false)) { ros::NodeHandle n("~"); frames_server_ = n.advertiseService("tf2_frames", &Buffer::getFrames, this); } } geometry_msgs::TransformStamped Buffer::lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout) const { canTransform(target_frame, source_frame, time, timeout); return lookupTransform(target_frame, source_frame, time); } geometry_msgs::TransformStamped Buffer::lookupTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout) const { canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); return lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame); } /** This is a workaround for the case that we're running inside of rospy and ros::Time is not initialized inside the c++ instance. This makes the system fall back to Wall time if not initialized. */ ros::Time now_fallback_to_wall() { try { return ros::Time::now(); } catch (ros::TimeNotInitializedException ex) { ros::WallTime wt = ros::WallTime::now(); return ros::Time(wt.sec, wt.nsec); } } /** This is a workaround for the case that we're running inside of rospy and ros::Time is not initialized inside the c++ instance. This makes the system fall back to Wall time if not initialized. https://github.com/ros/geometry/issues/30 */ void sleep_fallback_to_wall(const ros::Duration& d) { try { d.sleep(); } catch (ros::TimeNotInitializedException ex) { ros::WallDuration wd = ros::WallDuration(d.sec, d.nsec); wd.sleep(); } } void conditionally_append_timeout_info(std::string * errstr, const ros::Time& start_time, const ros::Duration& timeout) { if (errstr) { std::stringstream ss; ss << ". canTransform returned after "<< (now_fallback_to_wall() - start_time).toSec() \ <<" timeout was " << timeout.toSec() << "."; (*errstr) += ss.str(); } } bool Buffer::canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout, std::string* errstr) const { // Clear the errstr before populating it if it's valid. if (errstr) { errstr->clear(); } if (!checkAndErrorDedicatedThreadPresent(errstr)) return false; // poll for transform if timeout is set ros::Time start_time = now_fallback_to_wall(); const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE; while (now_fallback_to_wall() < start_time + timeout && !canTransform(target_frame, source_frame, time) && (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf) { sleep_fallback_to_wall(sleep_duration); } bool retval = canTransform(target_frame, source_frame, time, errstr); conditionally_append_timeout_info(errstr, start_time, timeout); return retval; } bool Buffer::canTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const { // Clear the errstr before populating it if it's valid. if (errstr) { errstr->clear(); } if (!checkAndErrorDedicatedThreadPresent(errstr)) return false; // poll for transform if timeout is set ros::Time start_time = now_fallback_to_wall(); const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE; while (now_fallback_to_wall() < start_time + timeout && !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) && (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf) { sleep_fallback_to_wall(sleep_duration); } bool retval = canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, errstr); conditionally_append_timeout_info(errstr, start_time, timeout); return retval; } bool Buffer::getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res) { res.frame_yaml = allFramesAsYAML(); return true; } bool Buffer::checkAndErrorDedicatedThreadPresent(std::string* error_str) const { if (isUsingDedicatedThread()) return true; if (error_str) *error_str = tf2_ros::threading_error; ROS_ERROR("%s", tf2_ros::threading_error.c_str()); return false; } } geometry2-0.7.5/tf2_ros/src/buffer_client.cpp000066400000000000000000000140131372352374300211300ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #include namespace tf2_ros { BufferClient::BufferClient(std::string ns, double check_frequency, ros::Duration timeout_padding): client_(ns), check_frequency_(check_frequency), timeout_padding_(timeout_padding) { } geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout) const { //populate the goal message tf2_msgs::LookupTransformGoal goal; goal.target_frame = target_frame; goal.source_frame = source_frame; goal.source_time = time; goal.timeout = timeout; goal.advanced = false; return processGoal(goal); } geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout) const { //populate the goal message tf2_msgs::LookupTransformGoal goal; goal.target_frame = target_frame; goal.source_frame = source_frame; goal.source_time = source_time; goal.timeout = timeout; goal.target_time = target_time; goal.fixed_frame = fixed_frame; goal.advanced = true; return processGoal(goal); } geometry_msgs::TransformStamped BufferClient::processGoal(const tf2_msgs::LookupTransformGoal& goal) const { client_.sendGoal(goal); //this shouldn't happen, but could in rare cases where the server hangs if(!client_.waitForResult(goal.timeout + timeout_padding_)) { //make sure to cancel the goal the server is pursuing client_.cancelGoal(); throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server."); } if(client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server."); //process the result for errors and return it return processResult(*client_.getResult()); } geometry_msgs::TransformStamped BufferClient::processResult(const tf2_msgs::LookupTransformResult& result) const { //if there's no error, then we'll just return the transform if(result.error.error != result.error.NO_ERROR){ //otherwise, we'll have to throw the appropriate exception if(result.error.error == result.error.LOOKUP_ERROR) throw tf2::LookupException(result.error.error_string); if(result.error.error == result.error.CONNECTIVITY_ERROR) throw tf2::ConnectivityException(result.error.error_string); if(result.error.error == result.error.EXTRAPOLATION_ERROR) throw tf2::ExtrapolationException(result.error.error_string); if(result.error.error == result.error.INVALID_ARGUMENT_ERROR) throw tf2::InvalidArgumentException(result.error.error_string); if(result.error.error == result.error.TIMEOUT_ERROR) throw tf2::TimeoutException(result.error.error_string); throw tf2::TransformException(result.error.error_string); } return result.transform; } bool BufferClient::canTransform(const std::string& target_frame, const std::string& source_frame, const ros::Time& time, const ros::Duration timeout, std::string* errstr) const { try { lookupTransform(target_frame, source_frame, time, timeout); return true; } catch(tf2::TransformException& ex) { if(errstr) { errstr->clear(); *errstr = ex.what(); } return false; } } bool BufferClient::canTransform(const std::string& target_frame, const ros::Time& target_time, const std::string& source_frame, const ros::Time& source_time, const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const { try { lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); return true; } catch(tf2::TransformException& ex) { if(errstr) { errstr->clear(); *errstr = ex.what(); } return false; } } }; geometry2-0.7.5/tf2_ros/src/buffer_server.cpp000066400000000000000000000171071372352374300211670ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Eitan Marder-Eppstein *********************************************************************/ #include namespace tf2_ros { BufferServer::BufferServer(const Buffer& buffer, const std::string& ns, bool auto_start, ros::Duration check_period): buffer_(buffer), server_(ros::NodeHandle(), ns, boost::bind(&BufferServer::goalCB, this, _1), boost::bind(&BufferServer::cancelCB, this, _1), auto_start) { ros::NodeHandle n; check_timer_ = n.createTimer(check_period, boost::bind(&BufferServer::checkTransforms, this, _1)); } void BufferServer::checkTransforms(const ros::TimerEvent& e) { (void) e; //Unused boost::mutex::scoped_lock l(mutex_); for(std::list::iterator it = active_goals_.begin(); it != active_goals_.end();) { GoalInfo& info = *it; //we want to lookup a transform if the time on the goal //has expired, or a transform is available if(canTransform(info.handle) || info.end_time < ros::Time::now()) { tf2_msgs::LookupTransformResult result; //try to populate the result, catching exceptions if they occur try { result.transform = lookupTransform(info.handle); } catch (tf2::ConnectivityException &ex) { result.error.error = result.error.CONNECTIVITY_ERROR; result.error.error_string = ex.what(); } catch (tf2::LookupException &ex) { result.error.error = result.error.LOOKUP_ERROR; result.error.error_string = ex.what(); } catch (tf2::ExtrapolationException &ex) { result.error.error = result.error.EXTRAPOLATION_ERROR; result.error.error_string = ex.what(); } catch (tf2::InvalidArgumentException &ex) { result.error.error = result.error.INVALID_ARGUMENT_ERROR; result.error.error_string = ex.what(); } catch (tf2::TimeoutException &ex) { result.error.error = result.error.TIMEOUT_ERROR; result.error.error_string = ex.what(); } catch (tf2::TransformException &ex) { result.error.error = result.error.TRANSFORM_ERROR; result.error.error_string = ex.what(); } //make sure to pass the result to the client //even failed transforms are considered a success //since the request was successfully processed info.handle.setSucceeded(result); it = active_goals_.erase(it); } else ++it; } } void BufferServer::cancelCB(GoalHandle gh) { boost::mutex::scoped_lock l(mutex_); //we need to find the goal in the list and remove it... also setting it as canceled //if its not in the list, we won't do anything since it will have already been set //as completed for(std::list::iterator it = active_goals_.begin(); it != active_goals_.end();) { GoalInfo& info = *it; if(info.handle == gh) { info.handle.setCanceled(); it = active_goals_.erase(it); return; } else ++it; } } void BufferServer::goalCB(GoalHandle gh) { //we'll accept all goals we get gh.setAccepted(); //if the transform isn't immediately available, we'll push it onto our list to check //along with the time that the goal will end GoalInfo goal_info; goal_info.handle = gh; goal_info.end_time = ros::Time::now() + gh.getGoal()->timeout; //we can do a quick check here to see if the transform is valid //we'll also do this if the end time has been reached if(canTransform(gh) || goal_info.end_time <= ros::Time::now()) { tf2_msgs::LookupTransformResult result; try { result.transform = lookupTransform(gh); } catch (tf2::ConnectivityException &ex) { result.error.error = result.error.CONNECTIVITY_ERROR; result.error.error_string = ex.what(); } catch (tf2::LookupException &ex) { result.error.error = result.error.LOOKUP_ERROR; result.error.error_string = ex.what(); } catch (tf2::ExtrapolationException &ex) { result.error.error = result.error.EXTRAPOLATION_ERROR; result.error.error_string = ex.what(); } catch (tf2::InvalidArgumentException &ex) { result.error.error = result.error.INVALID_ARGUMENT_ERROR; result.error.error_string = ex.what(); } catch (tf2::TimeoutException &ex) { result.error.error = result.error.TIMEOUT_ERROR; result.error.error_string = ex.what(); } catch (tf2::TransformException &ex) { result.error.error = result.error.TRANSFORM_ERROR; result.error.error_string = ex.what(); } gh.setSucceeded(result); return; } boost::mutex::scoped_lock l(mutex_); active_goals_.push_back(goal_info); } bool BufferServer::canTransform(GoalHandle gh) { const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal(); //check whether we need to used the advanced or simple api if(!goal->advanced) return buffer_.canTransform(goal->target_frame, goal->source_frame, goal->source_time); return buffer_.canTransform(goal->target_frame, goal->target_time, goal->source_frame, goal->source_time, goal->fixed_frame); } geometry_msgs::TransformStamped BufferServer::lookupTransform(GoalHandle gh) { const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal(); //check whether we need to used the advanced or simple api if(!goal->advanced) return buffer_.lookupTransform(goal->target_frame, goal->source_frame, goal->source_time); return buffer_.lookupTransform(goal->target_frame, goal->target_time, goal->source_frame, goal->source_time, goal->fixed_frame); } void BufferServer::start() { server_.start(); } }; geometry2-0.7.5/tf2_ros/src/buffer_server_main.cpp000066400000000000000000000051721372352374300221720ustar00rootroot00000000000000/********************************************************************* * * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * * Author: Wim Meeussen *********************************************************************/ #include #include #include int main(int argc, char** argv) { ros::init(argc, argv, "tf_buffer"); ros::NodeHandle nh; double buffer_size; nh.param("buffer_size", buffer_size, 120.0); bool publish_frame_service; nh.param("publish_frame_service", publish_frame_service, false); // Legacy behavior re: #209 bool use_node_namespace; nh.param("use_node_namespace", use_node_namespace, false); std::string node_name; if (use_node_namespace) { node_name = ros::this_node::getName(); } else { node_name = "tf2_buffer_server"; } tf2_ros::Buffer buffer_core(ros::Duration(buffer_size), publish_frame_service); tf2_ros::TransformListener listener(buffer_core); tf2_ros::BufferServer buffer_server(buffer_core, node_name , false); buffer_server.start(); ros::spin(); } geometry2-0.7.5/tf2_ros/src/static_transform_broadcaster.cpp000066400000000000000000000047601372352374300242640ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "ros/ros.h" #include "tf2_msgs/TFMessage.h" #include "tf2_ros/static_transform_broadcaster.h" #include namespace tf2_ros { StaticTransformBroadcaster::StaticTransformBroadcaster() { publisher_ = node_.advertise("/tf_static", 100, true); }; void StaticTransformBroadcaster::sendTransform(const std::vector & msgtf) { for (const geometry_msgs::TransformStamped& input : msgtf) { auto predicate = [&input](const geometry_msgs::TransformStamped existing) { return input.child_frame_id == existing.child_frame_id; }; auto existing = std::find_if(net_message_.transforms.begin(), net_message_.transforms.end(), predicate); if (existing != net_message_.transforms.end()) *existing = input; else net_message_.transforms.push_back(input); } publisher_.publish(net_message_); } } geometry2-0.7.5/tf2_ros/src/static_transform_broadcaster_program.cpp000066400000000000000000000143601372352374300260100ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "tf2_ros/static_transform_broadcaster.h" bool validateXmlRpcTf(XmlRpc::XmlRpcValue tf_data) { // Validate a TF stored in XML RPC format: ensures the appropriate fields // exist. Note this does not check data types. return tf_data.hasMember("child_frame_id") && tf_data.hasMember("header") && tf_data["header"].hasMember("frame_id") && tf_data.hasMember("transform") && tf_data["transform"].hasMember("translation") && tf_data["transform"]["translation"].hasMember("x") && tf_data["transform"]["translation"].hasMember("y") && tf_data["transform"]["translation"].hasMember("z") && tf_data["transform"].hasMember("rotation") && tf_data["transform"]["rotation"].hasMember("x") && tf_data["transform"]["rotation"].hasMember("y") && tf_data["transform"]["rotation"].hasMember("z") && tf_data["transform"]["rotation"].hasMember("w"); }; int main(int argc, char ** argv) { //Initialize ROS ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName); tf2_ros::StaticTransformBroadcaster broadcaster; geometry_msgs::TransformStamped msg; if(argc == 10) { msg.transform.translation.x = atof(argv[1]); msg.transform.translation.y = atof(argv[2]); msg.transform.translation.z = atof(argv[3]); msg.transform.rotation.x = atof(argv[4]); msg.transform.rotation.y = atof(argv[5]); msg.transform.rotation.z = atof(argv[6]); msg.transform.rotation.w = atof(argv[7]); msg.header.stamp = ros::Time::now(); msg.header.frame_id = argv[8]; msg.child_frame_id = argv[9]; } else if (argc == 9) { msg.transform.translation.x = atof(argv[1]); msg.transform.translation.y = atof(argv[2]); msg.transform.translation.z = atof(argv[3]); tf2::Quaternion quat; quat.setRPY(atof(argv[6]), atof(argv[5]), atof(argv[4])); msg.transform.rotation.x = quat.x(); msg.transform.rotation.y = quat.y(); msg.transform.rotation.z = quat.z(); msg.transform.rotation.w = quat.w(); msg.header.stamp = ros::Time::now(); msg.header.frame_id = argv[7]; msg.child_frame_id = argv[8]; } else if (argc == 2) { const std::string param_name = argv[1]; ROS_INFO_STREAM("Looking for TF in parameter: " << param_name); XmlRpc::XmlRpcValue tf_data; if (!ros::param::has(param_name) || !ros::param::get(param_name, tf_data)) { ROS_FATAL_STREAM("Could not read TF from parameter server: " << param_name); return -1; } // Check that all required members are present & of the right type. if (!validateXmlRpcTf(tf_data)) { ROS_FATAL_STREAM("Could not validate XmlRpcC for TF data: " << tf_data); return -1; } msg.transform.translation.x = (double) tf_data["transform"]["translation"]["x"]; msg.transform.translation.y = (double) tf_data["transform"]["translation"]["y"]; msg.transform.translation.z = (double) tf_data["transform"]["translation"]["z"]; msg.transform.rotation.x = (double) tf_data["transform"]["rotation"]["x"]; msg.transform.rotation.y = (double) tf_data["transform"]["rotation"]["y"]; msg.transform.rotation.z = (double) tf_data["transform"]["rotation"]["z"]; msg.transform.rotation.w = (double) tf_data["transform"]["rotation"]["w"]; msg.header.stamp = ros::Time::now(); msg.header.frame_id = (std::string) tf_data["header"]["frame_id"]; msg.child_frame_id = (std::string) tf_data["child_frame_id"]; } else { printf("A command line utility for manually sending a transform.\n"); //printf("It will periodicaly republish the given transform. \n"); printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \n"); printf("OR \n"); printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \n"); printf("OR \n"); printf("Usage: static_transform_publisher /param_name \n"); printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n"); printf("of the child_frame_id. \n"); ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments"); return -1; } // Checks: frames should not be the same. if (msg.header.frame_id == msg.child_frame_id) { ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", msg.header.frame_id.c_str(), msg.child_frame_id.c_str()); return 1; } broadcaster.sendTransform(msg); ROS_INFO("Spinning until killed publishing %s to %s", msg.header.frame_id.c_str(), msg.child_frame_id.c_str()); ros::spin(); return 0; }; geometry2-0.7.5/tf2_ros/src/tf2_ros/000077500000000000000000000000001372352374300171745ustar00rootroot00000000000000geometry2-0.7.5/tf2_ros/src/tf2_ros/__init__.py000066400000000000000000000040601372352374300213050ustar00rootroot00000000000000#! /usr/bin/python #*********************************************************** #* Software License Agreement (BSD License) #* #* Copyright (c) 2009, Willow Garage, Inc. #* All rights reserved. #* #* Redistribution and use in source and binary forms, with or without #* modification, are permitted provided that the following conditions #* are met: #* #* * Redistributions of source code must retain the above copyright #* notice, this list of conditions and the following disclaimer. #* * Redistributions in binary form must reproduce the above #* copyright notice, this list of conditions and the following #* disclaimer in the documentation and/or other materials provided #* with the distribution. #* * Neither the name of Willow Garage, Inc. nor the names of its #* contributors may be used to endorse or promote products derived #* from this software without specific prior written permission. #* #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #* #* Author: Eitan Marder-Eppstein #*********************************************************** from __future__ import absolute_import from tf2_py import * from .buffer_interface import * from .buffer import * from .buffer_client import * from .transform_listener import * from .transform_broadcaster import * from .static_transform_broadcaster import * geometry2-0.7.5/tf2_ros/src/tf2_ros/buffer.py000066400000000000000000000176341372352374300210320ustar00rootroot00000000000000# Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen import rospy import tf2_py as tf2 import tf2_ros from tf2_msgs.srv import FrameGraph, FrameGraphResponse import rosgraph.masterapi class Buffer(tf2.BufferCore, tf2_ros.BufferInterface): """ Standard implementation of the :class:`tf2_ros.BufferInterface` abstract data type. Inherits from :class:`tf2_ros.buffer_interface.BufferInterface` and :class:`tf2.BufferCore`. Stores known frames and optionally offers a ROS service, "tf2_frames", which responds to client requests with a response containing a :class:`tf2_msgs.FrameGraph` representing the relationship of known frames. """ def __init__(self, cache_time = None, debug = True): """ .. function:: __init__(cache_time = None, debug = True) Constructor. :param cache_time: (Optional) How long to retain past information in BufferCore. :param debug: (Optional) If true, check if another tf2_frames service has been advertised. """ if cache_time != None: tf2.BufferCore.__init__(self, cache_time) else: tf2.BufferCore.__init__(self) tf2_ros.BufferInterface.__init__(self) if debug: #Check to see if the service has already been advertised in this node try: m = rosgraph.masterapi.Master(rospy.get_name()) m.lookupService('~tf2_frames') except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure): self.frame_server = rospy.Service('~tf2_frames', FrameGraph, self.__get_frames) def __get_frames(self, req): return FrameGraphResponse(self.all_frames_as_yaml()) def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): """ Get the transform from the source frame to the target frame. :param target_frame: Name of the frame to transform into. :param source_frame: Name of the input frame. :param time: The time at which to get the transform. (0 will get the latest) :param timeout: (Optional) Time to wait for the target frame to become available. :return: The transform between the frames. :rtype: :class:`geometry_msgs.msg.TransformStamped` """ self.can_transform(target_frame, source_frame, time, timeout) return self.lookup_transform_core(target_frame, source_frame, time) def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): """ Get the transform from the source frame to the target frame using the advanced API. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: (Optional) Time to wait for the target frame to become available. :return: The transform between the frames. :rtype: :class:`geometry_msgs.msg.TransformStamped` """ self.can_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout) return self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame) def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0), return_debug_tuple=False): """ Check if a transform from the source frame to the target frame is possible. :param target_frame: Name of the frame to transform into. :param source_frame: Name of the input frame. :param time: The time at which to get the transform. (0 will get the latest) :param timeout: (Optional) Time to wait for the target frame to become available. :param return_debug_type: (Optional) If true, return a tuple representing debug information. :return: True if the transform is possible, false otherwise. :rtype: bool """ if timeout != rospy.Duration(0.0): start_time = rospy.Time.now() r= rospy.Rate(20) while (rospy.Time.now() < start_time + timeout and not self.can_transform_core(target_frame, source_frame, time)[0] and (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them r.sleep() core_result = self.can_transform_core(target_frame, source_frame, time) if return_debug_tuple: return core_result return core_result[0] def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0), return_debug_tuple=False): """ Check if a transform from the source frame to the target frame is possible (advanced API). Must be implemented by a subclass of BufferInterface. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: (Optional) Time to wait for the target frame to become available. :param return_debug_type: (Optional) If true, return a tuple representing debug information. :return: True if the transform is possible, false otherwise. :rtype: bool """ if timeout != rospy.Duration(0.0): start_time = rospy.Time.now() r= rospy.Rate(20) while (rospy.Time.now() < start_time + timeout and not self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0] and (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them r.sleep() core_result = self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame) if return_debug_tuple: return core_result return core_result[0] geometry2-0.7.5/tf2_ros/src/tf2_ros/buffer_client.py000066400000000000000000000223331372352374300223600ustar00rootroot00000000000000#! /usr/bin/python #*********************************************************** #* Software License Agreement (BSD License) #* #* Copyright (c) 2009, Willow Garage, Inc. #* All rights reserved. #* #* Redistribution and use in source and binary forms, with or without #* modification, are permitted provided that the following conditions #* are met: #* #* * Redistributions of source code must retain the above copyright #* notice, this list of conditions and the following disclaimer. #* * Redistributions in binary form must reproduce the above #* copyright notice, this list of conditions and the following #* disclaimer in the documentation and/or other materials provided #* with the distribution. #* * Neither the name of Willow Garage, Inc. nor the names of its #* contributors may be used to endorse or promote products derived #* from this software without specific prior written permission. #* #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #* #* Author: Eitan Marder-Eppstein #*********************************************************** import rospy import actionlib import tf2_py as tf2 import tf2_ros from tf2_msgs.msg import LookupTransformAction, LookupTransformGoal from actionlib_msgs.msg import GoalStatus class BufferClient(tf2_ros.BufferInterface): """ Action client-based implementation of BufferInterface. """ def __init__(self, ns, check_frequency = None, timeout_padding = rospy.Duration.from_sec(2.0)): """ .. function:: __init__(ns, check_frequency = None, timeout_padding = rospy.Duration.from_sec(2.0)) Constructor. :param ns: The namespace in which to look for a BufferServer. :param check_frequency: How frequently to check for updates to known transforms. :param timeout_padding: A constant timeout to add to blocking calls. """ tf2_ros.BufferInterface.__init__(self) self.client = actionlib.SimpleActionClient(ns, LookupTransformAction) self.timeout_padding = timeout_padding if check_frequency is not None: rospy.logwarn('Argument check_frequency is deprecated and should not be used.') def wait_for_server(self, timeout = rospy.Duration()): """ Block until the action server is ready to respond to requests. :param timeout: Time to wait for the server. :return: True if the server is ready, false otherwise. :rtype: bool """ return self.client.wait_for_server(timeout) # lookup, simple api def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): """ Get the transform from the source frame to the target frame. :param target_frame: Name of the frame to transform into. :param source_frame: Name of the input frame. :param time: The time at which to get the transform. (0 will get the latest) :param timeout: (Optional) Time to wait for the target frame to become available. :return: The transform between the frames. :rtype: :class:`geometry_msgs.msg.TransformStamped` """ goal = LookupTransformGoal() goal.target_frame = target_frame; goal.source_frame = source_frame; goal.source_time = time; goal.timeout = timeout; goal.advanced = False; return self.__process_goal(goal) # lookup, advanced api def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): """ Get the transform from the source frame to the target frame using the advanced API. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: (Optional) Time to wait for the target frame to become available. :return: The transform between the frames. :rtype: :class:`geometry_msgs.msg.TransformStamped` """ goal = LookupTransformGoal() goal.target_frame = target_frame; goal.source_frame = source_frame; goal.source_time = source_time; goal.timeout = timeout; goal.target_time = target_time; goal.fixed_frame = fixed_frame; goal.advanced = True; return self.__process_goal(goal) # can, simple api def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): """ Check if a transform from the source frame to the target frame is possible. :param target_frame: Name of the frame to transform into. :param source_frame: Name of the input frame. :param time: The time at which to get the transform. (0 will get the latest) :param timeout: (Optional) Time to wait for the target frame to become available. :param return_debug_type: (Optional) If true, return a tuple representing debug information. :return: True if the transform is possible, false otherwise. :rtype: bool """ try: self.lookup_transform(target_frame, source_frame, time, timeout) return True except tf2.TransformException: return False # can, advanced api def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): """ Check if a transform from the source frame to the target frame is possible (advanced API). Must be implemented by a subclass of BufferInterface. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: (Optional) Time to wait for the target frame to become available. :param return_debug_type: (Optional) If true, return a tuple representing debug information. :return: True if the transform is possible, false otherwise. :rtype: bool """ try: self.lookup_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout) return True except tf2.TransformException: return False def __process_goal(self, goal): self.client.send_goal(goal) if not self.client.wait_for_result(goal.timeout + self.timeout_padding): #This shouldn't happen, but could in rare cases where the server hangs raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server") if self.client.get_state() != GoalStatus.SUCCEEDED: raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.") return self.__process_result(self.client.get_result()) def __process_result(self, result): if not result: raise tf2.TransformException("The BufferServer returned None for result! Something is likely wrong with the server.") if not result.error: raise tf2.TransformException("The BufferServer returned None for result.error! Something is likely wrong with the server.") if result.error.error != result.error.NO_ERROR: if result.error.error == result.error.LOOKUP_ERROR: raise tf2.LookupException(result.error.error_string) if result.error.error == result.error.CONNECTIVITY_ERROR: raise tf2.ConnectivityException(result.error.error_string) if result.error.error == result.error.EXTRAPOLATION_ERROR: raise tf2.ExtrapolationException(result.error.error_string) if result.error.error == result.error.INVALID_ARGUMENT_ERROR: raise tf2.InvalidArgumentException(result.error.error_string) if result.error.error == result.error.TIMEOUT_ERROR: raise tf2.TimeoutException(result.error.error_string) raise tf2.TransformException(result.error.error_string) return result.transform geometry2-0.7.5/tf2_ros/src/tf2_ros/buffer_interface.py000066400000000000000000000254531372352374300230500ustar00rootroot00000000000000# Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen from __future__ import print_function import rospy import tf2_py as tf2 import tf2_ros from copy import deepcopy from std_msgs.msg import Header class BufferInterface: """ Abstract interface for wrapping the Python bindings for the tf2 library in a ROS-based convenience API. Implementations include :class:tf2_ros.buffer.Buffer and :class:tf2_ros.buffer_client.BufferClient. """ def __init__(self): self.registration = tf2_ros.TransformRegistration() # transform, simple api def transform(self, object_stamped, target_frame, timeout=rospy.Duration(0.0), new_type = None): """ Transform an input into the target frame. The input must be a known transformable type (by way of the tf2 data type conversion interface). If new_type is not None, the type specified must have a valid conversion from the input type, else the function will raise an exception. :param object_stamped: The timestamped object the transform. :param target_frame: Name of the frame to transform the input into. :param timeout: (Optional) Time to wait for the target frame to become available. :param new_type: (Optional) Type to convert the object to. :return: The transformed, timestamped output, possibly converted to a new type. """ do_transform = self.registration.get(type(object_stamped)) res = do_transform(object_stamped, self.lookup_transform(target_frame, object_stamped.header.frame_id, object_stamped.header.stamp, timeout)) if not new_type: return res return convert(res, new_type) # transform, advanced api def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=rospy.Duration(0.0), new_type = None): """ Transform an input into the target frame (advanced API). The input must be a known transformable type (by way of the tf2 data type conversion interface). If new_type is not None, the type specified must have a valid conversion from the input type, else the function will raise an exception. This function follows the advanced API, which allows tranforming between different time points, as well as specifying a frame to be considered fixed in time. :param object_stamped: The timestamped object the transform. :param target_frame: Name of the frame to transform the input into. :param target_time: Time to transform the input into. :param fixed_frame: Name of the frame to consider constant in time. :param timeout: (Optional) Time to wait for the target frame to become available. :param new_type: (Optional) Type to convert the object to. :return: The transformed, timestamped output, possibly converted to a new type. """ do_transform = self.registration.get(type(object_stamped)) res = do_transform(object_stamped, self.lookup_transform_full(target_frame, target_time, object_stamped.header.frame_id, object_stamped.header.stamp, fixed_frame, timeout)) if not new_type: return res return convert(res, new_type) def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): """ Get the transform from the source frame to the target frame. Must be implemented by a subclass of BufferInterface. :param target_frame: Name of the frame to transform into. :param source_frame: Name of the input frame. :param time: The time at which to get the transform. (0 will get the latest) :param timeout: (Optional) Time to wait for the target frame to become available. :return: The transform between the frames. :rtype: :class:`geometry_msgs.msg.TransformStamped` """ raise NotImplementedException() def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): """ Get the transform from the source frame to the target frame using the advanced API. Must be implemented by a subclass of BufferInterface. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: (Optional) Time to wait for the target frame to become available. :return: The transform between the frames. :rtype: :class:`geometry_msgs.msg.TransformStamped` """ raise NotImplementedException() # can, simple api def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): """ Check if a transform from the source frame to the target frame is possible. Must be implemented by a subclass of BufferInterface. :param target_frame: Name of the frame to transform into. :param source_frame: Name of the input frame. :param time: The time at which to get the transform. (0 will get the latest) :param timeout: (Optional) Time to wait for the target frame to become available. :return: True if the transform is possible, false otherwise. :rtype: bool """ raise NotImplementedException() # can, advanced api def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): """ Check if a transform from the source frame to the target frame is possible (advanced API). Must be implemented by a subclass of BufferInterface. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: (Optional) Time to wait for the target frame to become available. :return: True if the transform is possible, false otherwise. :rtype: bool """ raise NotImplementedException() def Stamped(obj, stamp, frame_id): obj.header = Header(frame_id=frame_id, stamp=stamp) return obj class TypeException(Exception): """ Raised when an unexpected type is received while registering a transform in :class:`tf2_ros.buffer_interface.BufferInterface`. """ def __init__(self, errstr): self.errstr = errstr class NotImplementedException(Exception): """ Raised when can_transform or lookup_transform is not implemented in a subclass of :class:`tf2_ros.buffer_interface.BufferInterface`. """ def __init__(self): self.errstr = 'CanTransform or LookupTransform not implemented' class TransformRegistration(): __type_map = {} def print_me(self): print(TransformRegistration.__type_map) def add(self, key, callback): TransformRegistration.__type_map[key] = callback def get(self, key): if not key in TransformRegistration.__type_map: raise TypeException('Type %s if not loaded or supported'% str(key)) else: return TransformRegistration.__type_map[key] class ConvertRegistration(): __to_msg_map = {} __from_msg_map = {} __convert_map = {} def add_from_msg(self, key, callback): ConvertRegistration.__from_msg_map[key] = callback def add_to_msg(self, key, callback): ConvertRegistration.__to_msg_map[key] = callback def add_convert(self, key, callback): ConvertRegistration.__convert_map[key] = callback def get_from_msg(self, key): if not key in ConvertRegistration.__from_msg_map: raise TypeException('Type %s if not loaded or supported'% str(key)) else: return ConvertRegistration.__from_msg_map[key] def get_to_msg(self, key): if not key in ConvertRegistration.__to_msg_map: raise TypeException('Type %s if not loaded or supported'%str(key)) else: return ConvertRegistration.__to_msg_map[key] def get_convert(self, key): if not key in ConvertRegistration.__convert_map: raise TypeException("Type %s if not loaded or supported" % str(key)) else: return ConvertRegistration.__convert_map[key] def convert(a, b_type): c = ConvertRegistration() #check if an efficient conversion function between the types exists try: f = c.get_convert((type(a), b_type)) print("efficient copy") return f(a) except TypeException: if type(a) == b_type: print("deep copy") return deepcopy(a) f_to = c.get_to_msg(type(a)) f_from = c.get_from_msg(b_type) print("message copy") return f_from(f_to(a)) geometry2-0.7.5/tf2_ros/src/tf2_ros/static_transform_broadcaster.py000066400000000000000000000042001372352374300254750ustar00rootroot00000000000000# Software License Agreement (BSD License) # # Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of the Willow Garage nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import rospy from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped class StaticTransformBroadcaster(object): """ :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic. """ def __init__(self): self.pub_tf = rospy.Publisher("/tf_static", TFMessage, queue_size=100, latch=True) def sendTransform(self, transform): if not isinstance(transform, list): transform = [transform] self.pub_tf.publish(TFMessage(transform)) geometry2-0.7.5/tf2_ros/src/tf2_ros/transform_broadcaster.py000066400000000000000000000044351372352374300241400ustar00rootroot00000000000000# Software License Agreement (BSD License) # # Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above # copyright notice, this list of conditions and the following # disclaimer in the documentation and/or other materials provided # with the distribution. # * Neither the name of the Willow Garage nor the names of its # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import rospy from tf2_msgs.msg import TFMessage from geometry_msgs.msg import TransformStamped class TransformBroadcaster: """ :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. """ def __init__(self): self.pub_tf = rospy.Publisher("/tf", TFMessage, queue_size=100) def sendTransform(self, transform): """ Send a transform, or a list of transforms, to the Buffer associated with this TransformBroadcaster. :param transform: A transform or list of transforms to send. """ if not isinstance(transform, list): transform = [transform] self.pub_tf.publish(TFMessage(transform)) geometry2-0.7.5/tf2_ros/src/tf2_ros/transform_listener.py000066400000000000000000000112731372352374300234720ustar00rootroot00000000000000# Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen import threading import rospy import tf2_ros from tf2_msgs.msg import TFMessage class TransformListener(): """ :class:`TransformListener` is a convenient way to listen for coordinate frame transformation info. This class takes an object that instantiates the :class:`BufferInterface` interface, to which it propagates changes to the tf frame graph. """ def __init__(self, buffer, queue_size=None, buff_size=65536, tcp_nodelay=False): """ .. function:: __init__(buffer) Constructor. :param buffer: The buffer to propagate changes to when tf info updates. :param queue_size (int) - maximum number of messages to receive at a time. This will generally be 1 or None (infinite, default). buff_size should be increased if this parameter is set as incoming data still needs to sit in the incoming buffer before being discarded. Setting queue_size buff_size to a non-default value affects all subscribers to this topic in this process. :param buff_size (int) - incoming message buffer size in bytes. If queue_size is set, this should be set to a number greater than the queue_size times the average message size. Setting buff_size to a non-default value affects all subscribers to this topic in this process. :param tcp_nodelay (bool) - if True, request TCP_NODELAY from publisher. Use of this option is not generally recommended in most cases as it is better to rely on timestamps in message data. Setting tcp_nodelay to True enables TCP_NODELAY for all subscribers in the same python process. """ self.buffer = buffer self.last_update = rospy.Time.now() self.last_update_lock = threading.Lock() self.tf_sub = rospy.Subscriber("/tf", TFMessage, self.callback, queue_size=queue_size, buff_size=buff_size, tcp_nodelay=tcp_nodelay) self.tf_static_sub = rospy.Subscriber("/tf_static", TFMessage, self.static_callback, queue_size=queue_size, buff_size=buff_size, tcp_nodelay=tcp_nodelay) def __del__(self): self.unregister() def unregister(self): """ Unregisters all tf subscribers. """ self.tf_sub.unregister() self.tf_static_sub.unregister() def check_for_reset(self): # Lock to prevent different threads racing on this test and update. # https://github.com/ros/geometry2/issues/341 with self.last_update_lock: now = rospy.Time.now() if now < self.last_update: rospy.logwarn("Detected jump back in time of %fs. Clearing TF buffer." % (self.last_update - now).to_sec()) self.buffer.clear() self.last_update = now def callback(self, data): self.check_for_reset() who = data._connection_header.get('callerid', "default_authority") for transform in data.transforms: self.buffer.set_transform(transform, who) def static_callback(self, data): self.check_for_reset() who = data._connection_header.get('callerid', "default_authority") for transform in data.transforms: self.buffer.set_transform_static(transform, who) geometry2-0.7.5/tf2_ros/src/transform_broadcaster.cpp000066400000000000000000000045411372352374300227120ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "ros/ros.h" #include "tf2_msgs/TFMessage.h" #include "tf2_ros/transform_broadcaster.h" namespace tf2_ros { TransformBroadcaster::TransformBroadcaster() { publisher_ = node_.advertise("/tf", 100); }; void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf) { std::vector v1; v1.push_back(msgtf); sendTransform(v1); } void TransformBroadcaster::sendTransform(const std::vector & msgtf) { tf2_msgs::TFMessage message; for (std::vector::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it) { message.transforms.push_back(*it); } publisher_.publish(message); } } geometry2-0.7.5/tf2_ros/src/transform_listener.cpp000066400000000000000000000122011372352374300222360ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ /** \author Tully Foote */ #include "tf2_ros/transform_listener.h" using namespace tf2_ros; TransformListener::TransformListener(tf2::BufferCore& buffer, bool spin_thread, ros::TransportHints transport_hints): dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false), transport_hints_(transport_hints) { if (spin_thread) initWithThread(); else init(); } TransformListener::TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread, ros::TransportHints transport_hints) : dedicated_listener_thread_(NULL) , node_(nh) , buffer_(buffer) , using_dedicated_thread_(false) , transport_hints_(transport_hints) { if (spin_thread) initWithThread(); else init(); } TransformListener::~TransformListener() { using_dedicated_thread_ = false; if (dedicated_listener_thread_) { dedicated_listener_thread_->join(); delete dedicated_listener_thread_; } } void TransformListener::init() { message_subscriber_tf_ = node_.subscribe("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1), ros::VoidConstPtr(), transport_hints_); ///\todo magic number message_subscriber_tf_static_ = node_.subscribe("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, _1)); ///\todo magic number } void TransformListener::initWithThread() { using_dedicated_thread_ = true; ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1), ros::VoidPtr(), &tf_message_callback_queue_); ///\todo magic number ops_tf.transport_hints = transport_hints_; message_subscriber_tf_ = node_.subscribe(ops_tf); ros::SubscribeOptions ops_tf_static = ros::SubscribeOptions::create("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, _1), ros::VoidPtr(), &tf_message_callback_queue_); ///\todo magic number message_subscriber_tf_static_ = node_.subscribe(ops_tf_static); dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this)); //Tell the buffer we have a dedicated thread to enable timeouts buffer_.setUsingDedicatedThread(true); } void TransformListener::subscription_callback(const ros::MessageEvent& msg_evt) { subscription_callback_impl(msg_evt, false); } void TransformListener::static_subscription_callback(const ros::MessageEvent& msg_evt) { subscription_callback_impl(msg_evt, true); } void TransformListener::subscription_callback_impl(const ros::MessageEvent& msg_evt, bool is_static) { ros::Time now = ros::Time::now(); if(now < last_update_){ ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing TF buffer."); buffer_.clear(); } last_update_ = now; const tf2_msgs::TFMessage& msg_in = *(msg_evt.getConstMessage()); std::string authority = msg_evt.getPublisherName(); // lookup the authority for (unsigned int i = 0; i < msg_in.transforms.size(); i++) { try { buffer_.setTransform(msg_in.transforms[i], authority, is_static); } catch (tf2::TransformException& ex) { ///\todo Use error reporting std::string temp = ex.what(); ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str()); } } }; geometry2-0.7.5/tf2_ros/test/000077500000000000000000000000001372352374300160065ustar00rootroot00000000000000geometry2-0.7.5/tf2_ros/test/listener_unittest.cpp000066400000000000000000000041451372352374300223020ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include using namespace tf2; TEST(tf2_ros_transform, transform_listener) { tf2_ros::Buffer buffer; tf2_ros::TransformListener tfl(buffer); } TEST(tf2_ros_transform, transform_listener_transport_hints) { tf2_ros::Buffer buffer; tf2_ros::TransformListener tfl(buffer, true, ros::TransportHints().tcpNoDelay()); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "transform_listener_unittest"); return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2_ros/test/message_filter_test.cpp000066400000000000000000000111451372352374300225440ustar00rootroot00000000000000/* * Copyright (c) 2014, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include #include #include #include #include #include void spin_for_a_second() { ros::spinOnce(); for (uint8_t i = 0; i < 10; ++i) { std::this_thread::sleep_for(std::chrono::microseconds(100)); ros::spinOnce(); } } bool filter_callback_fired = false; void filter_callback(const geometry_msgs::PointStamped& msg) { filter_callback_fired = true; } TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) { ros::NodeHandle nh; message_filters::Subscriber sub; sub.subscribe(nh, "point", 10); tf2_ros::Buffer buffer; tf2_ros::TransformListener tfl(buffer); tf2_ros::MessageFilter filter(buffer, "map", 10, nh); filter.connectInput(sub); filter.registerCallback(&filter_callback); // Register multiple target frames std::vector frames; frames.push_back("odom"); frames.push_back("map"); filter.setTargetFrames(frames); // Set a non-zero time tolerance filter.setTolerance(ros::Duration(1, 0)); // Publish static transforms so the frame transformations will always be valid tf2_ros::StaticTransformBroadcaster tfb; geometry_msgs::TransformStamped map_to_odom; map_to_odom.header.stamp = ros::Time(0, 0); map_to_odom.header.frame_id = "map"; map_to_odom.child_frame_id = "odom"; map_to_odom.transform.translation.x = 0.0; map_to_odom.transform.translation.y = 0.0; map_to_odom.transform.translation.z = 0.0; map_to_odom.transform.rotation.x = 0.0; map_to_odom.transform.rotation.y = 0.0; map_to_odom.transform.rotation.z = 0.0; map_to_odom.transform.rotation.w = 1.0; tfb.sendTransform(map_to_odom); geometry_msgs::TransformStamped odom_to_base; odom_to_base.header.stamp = ros::Time(0, 0); odom_to_base.header.frame_id = "odom"; odom_to_base.child_frame_id = "base"; odom_to_base.transform.translation.x = 0.0; odom_to_base.transform.translation.y = 0.0; odom_to_base.transform.translation.z = 0.0; odom_to_base.transform.rotation.x = 0.0; odom_to_base.transform.rotation.y = 0.0; odom_to_base.transform.rotation.z = 0.0; odom_to_base.transform.rotation.w = 1.0; tfb.sendTransform(odom_to_base); // Publish a Point message in the "base" frame ros::Publisher pub = nh.advertise("point", 10); geometry_msgs::PointStamped point; point.header.stamp = ros::Time::now(); point.header.frame_id = "base"; pub.publish(point); // make sure it arrives spin_for_a_second(); // The filter callback should have been fired because all required transforms are available ASSERT_TRUE(filter_callback_fired); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "tf2_ros_message_filter"); return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2_ros/test/message_filter_test.launch000066400000000000000000000001601372352374300232270ustar00rootroot00000000000000 geometry2-0.7.5/tf2_ros/test/time_reset_test.cpp000066400000000000000000000066541372352374300217240ustar00rootroot00000000000000/* * Copyright (c) 2014, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include using namespace tf2; void spin_for_a_second() { ros::spinOnce(); for (uint8_t i = 0; i < 10; ++i) { std::this_thread::sleep_for(std::chrono::microseconds(100)); ros::spinOnce(); } } TEST(tf2_ros_transform_listener, time_backwards) { tf2_ros::Buffer buffer; tf2_ros::TransformListener tfl(buffer); tf2_ros::TransformBroadcaster tfb; ros::NodeHandle nh = ros::NodeHandle(); ros::Publisher clock = nh.advertise("/clock", 5); rosgraph_msgs::Clock c; c.clock = ros::Time(100); clock.publish(c); // basic test ASSERT_FALSE(buffer.canTransform("foo", "bar", ros::Time(101, 0))); // set the transform geometry_msgs::TransformStamped msg; msg.header.stamp = ros::Time(100, 0); msg.header.frame_id = "foo"; msg.child_frame_id = "bar"; msg.transform.rotation.x = 1.0; tfb.sendTransform(msg); msg.header.stamp = ros::Time(102, 0); tfb.sendTransform(msg); // make sure it arrives spin_for_a_second(); // verify it's been set ASSERT_TRUE(buffer.canTransform("foo", "bar", ros::Time(101, 0))); c.clock = ros::Time(90); clock.publish(c); // make sure it arrives spin_for_a_second(); //Send another message to trigger clock test on an unrelated frame msg.header.stamp = ros::Time(110, 0); msg.header.frame_id = "foo2"; msg.child_frame_id = "bar2"; tfb.sendTransform(msg); // make sure it arrives spin_for_a_second(); //verify the data's been cleared ASSERT_FALSE(buffer.canTransform("foo", "bar", ros::Time(101, 0))); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "transform_listener_backwards_reset"); return RUN_ALL_TESTS(); } geometry2-0.7.5/tf2_ros/test/transform_listener_time_reset_test.launch000066400000000000000000000002461372352374300264030ustar00rootroot00000000000000 geometry2-0.7.5/tf2_ros/test/transform_listener_unittest.launch000066400000000000000000000001601372352374300250560ustar00rootroot00000000000000 geometry2-0.7.5/tf2_sensor_msgs/000077500000000000000000000000001372352374300165665ustar00rootroot00000000000000geometry2-0.7.5/tf2_sensor_msgs/CHANGELOG.rst000066400000000000000000000062361372352374300206160ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_sensor_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ * Use list instead of set to make build reproducible (`#473 `_) * Contributors: Jochen Sprickerhof 0.7.2 (2020-06-08) ------------------ 0.7.1 (2020-05-13) ------------------ * import setup from setuptools instead of distutils-core (`#449 `_) * Contributors: Alejandro Hernández Cordero 0.7.0 (2020-03-09) ------------------ * Replace kdl packages with rosdep keys (`#447 `_) * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Merge pull request `#378 `_ from peci1/tf2_sensor_msgs_isometry Affine->Isometry * Python 3 compatibility: relative imports and print statement * Contributors: Martin Pecka, Shane Loretz, Timon Engelke, Tully Foote 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- * Merge pull request `#257 `_ from delftrobotics-forks/python3 Make tf2_py python3 compatible again * Use python3 print function. * Contributors: Maarten de Vries, Tully Foote 0.5.16 (2017-07-14) ------------------- * Fix do_transform_cloud for multi-channelled pointcloud2. (`#241 `_) * store gtest return value as int (`#229 `_) * Document the lifetime of the returned reference for getFrameId and getTimestamp * Find eigen in a much nicer way. * Switch tf2_sensor_msgs over to package format 2. * Contributors: Atsushi Watanabe, Chris Lalancette, dhood 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- 0.5.13 (2016-03-04) ------------------- * add missing Python runtime dependency * fix wrong comment * Adding tests to package * Fixing do_transform_cloud for python The previous code was not used at all (it was a mistake in the __init_\_.py so the do_transform_cloud was not available to the python users). The python code need some little correction (e.g there is no method named read_cloud but it's read_points for instance, and as we are in python we can't use the same trick as in c++ when we got an immutable) * Contributors: Laurent GEORGE, Vincent Rabaud 0.5.12 (2015-08-05) ------------------- 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- 0.5.9 (2015-03-25) ------------------ 0.5.8 (2015-03-17) ------------------ * ODR violation fixes and more conversions * Fix keeping original pointcloud header in transformed pointcloud * Contributors: Paul Bovbel, Tully Foote, Vincent Rabaud 0.5.7 (2014-12-23) ------------------ * add support for transforming sensor_msgs::PointCloud2 * Contributors: Vincent Rabaud geometry2-0.7.5/tf2_sensor_msgs/CMakeLists.txt000066400000000000000000000032531372352374300213310ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_sensor_msgs) find_package(catkin REQUIRED COMPONENTS cmake_modules sensor_msgs tf2_ros tf2) find_package(Boost COMPONENTS thread REQUIRED) # Finding Eigen is somewhat complicated because of our need to support Ubuntu # all the way back to saucy. First we look for the Eigen3 cmake module # provided by the libeigen3-dev on newer Ubuntu. If that fails, then we # fall-back to the version provided by cmake_modules, which is a stand-in. find_package(Eigen3 QUIET) if(NOT EIGEN3_FOUND) find_package(cmake_modules REQUIRED) find_package(Eigen REQUIRED) set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) endif() # Note that eigen 3.2 (on Ubuntu Wily) only provides EIGEN3_INCLUDE_DIR, # not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former. if(NOT EIGEN3_INCLUDE_DIRS) set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) endif() catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS sensor_msgs tf2_ros tf2 DEPENDS EIGEN3 ) include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) catkin_python_setup() if(CATKIN_ENABLE_TESTING) catkin_add_nosetests(test/test_tf2_sensor_msgs.py) find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs rostest tf2_ros tf2) add_executable(test_tf2_sensor_msgs_cpp EXCLUDE_FROM_ALL test/test_tf2_sensor_msgs.cpp) target_link_libraries(test_tf2_sensor_msgs_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) if(TARGET tests) add_dependencies(tests test_tf2_sensor_msgs_cpp) endif() add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) endif() geometry2-0.7.5/tf2_sensor_msgs/include/000077500000000000000000000000001372352374300202115ustar00rootroot00000000000000geometry2-0.7.5/tf2_sensor_msgs/include/tf2_sensor_msgs/000077500000000000000000000000001372352374300233265ustar00rootroot00000000000000geometry2-0.7.5/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h000066400000000000000000000106131372352374300266150ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef TF2_SENSOR_MSGS_H #define TF2_SENSOR_MSGS_H #include #include #include #include #include namespace tf2 { /********************/ /** PointCloud2 **/ /********************/ /** \brief Extract a timestamp from the header of a PointCloud2 message. * This function is a specialization of the getTimestamp template defined in tf2/convert.h. * \param t PointCloud2 message to extract the timestamp from. * \return The timestamp of the message. The lifetime of the returned reference * is bound to the lifetime of the argument. */ template <> inline const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;} /** \brief Extract a frame ID from the header of a PointCloud2 message. * This function is a specialization of the getFrameId template defined in tf2/convert.h. * \param t PointCloud2 message to extract the frame ID from. * \return A string containing the frame ID of the message. The lifetime of the * returned reference is bound to the lifetime of the argument. */ template <> inline const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;} // this method needs to be implemented by client library developers template <> inline void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in) { p_out = p_in; p_out.header = t_in.header; Eigen::Transform t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y, t_in.transform.translation.z) * Eigen::Quaternion( t_in.transform.rotation.w, t_in.transform.rotation.x, t_in.transform.rotation.y, t_in.transform.rotation.z); sensor_msgs::PointCloud2ConstIterator x_in(p_in, "x"); sensor_msgs::PointCloud2ConstIterator y_in(p_in, "y"); sensor_msgs::PointCloud2ConstIterator z_in(p_in, "z"); sensor_msgs::PointCloud2Iterator x_out(p_out, "x"); sensor_msgs::PointCloud2Iterator y_out(p_out, "y"); sensor_msgs::PointCloud2Iterator z_out(p_out, "z"); Eigen::Vector3f point; for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) { point = t * Eigen::Vector3f(*x_in, *y_in, *z_in); *x_out = point.x(); *y_out = point.y(); *z_out = point.z(); } } inline sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in) { return in; } inline void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out) { out = msg; } } // namespace #endif // TF2_SENSOR_MSGS_H geometry2-0.7.5/tf2_sensor_msgs/package.xml000066400000000000000000000015061372352374300207050ustar00rootroot00000000000000 tf2_sensor_msgs 0.7.5 Small lib to transform sensor_msgs with tf. Most notably, PointCloud2 Vincent Rabaud Vincent Rabaud BSD http://www.ros.org/wiki/tf2_ros catkin cmake_modules eigen sensor_msgs tf2 tf2_ros python3-pykdl rospy eigen rostest geometry_msgs geometry2-0.7.5/tf2_sensor_msgs/setup.py000066400000000000000000000004321372352374300202770ustar00rootroot00000000000000#!/usr/bin/env python from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( packages=['tf2_sensor_msgs'], package_dir={'': 'src'}, requires=['rospy','sensor_msgs','tf2_ros','orocos_kdl'] ) setup(**d) geometry2-0.7.5/tf2_sensor_msgs/src/000077500000000000000000000000001372352374300173555ustar00rootroot00000000000000geometry2-0.7.5/tf2_sensor_msgs/src/tf2_sensor_msgs/000077500000000000000000000000001372352374300224725ustar00rootroot00000000000000geometry2-0.7.5/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py000066400000000000000000000000371372352374300246030ustar00rootroot00000000000000from .tf2_sensor_msgs import * geometry2-0.7.5/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py000066400000000000000000000053471372352374300261720ustar00rootroot00000000000000# Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from sensor_msgs.msg import PointCloud2 from sensor_msgs.point_cloud2 import read_points, create_cloud import PyKDL import rospy import tf2_ros def to_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_to_msg(PointCloud2, to_msg_msg) def from_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg) def transform_to_kdl(t): return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w), PyKDL.Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)) # PointStamped def do_transform_cloud(cloud, transform): t_kdl = transform_to_kdl(transform) points_out = [] for p_in in read_points(cloud): p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2]) points_out.append((p_out[0], p_out[1], p_out[2]) + p_in[3:]) res = create_cloud(transform.header, cloud.fields, points_out) return res tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud) geometry2-0.7.5/tf2_sensor_msgs/test/000077500000000000000000000000001372352374300175455ustar00rootroot00000000000000geometry2-0.7.5/tf2_sensor_msgs/test/test.launch000066400000000000000000000002011372352374300217110ustar00rootroot00000000000000 geometry2-0.7.5/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp000066400000000000000000000077061372352374300244370ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include #include #include tf2_ros::Buffer* tf_buffer; static const double EPS = 1e-3; TEST(Tf2Sensor, PointCloud2) { sensor_msgs::PointCloud2 cloud; sensor_msgs::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); modifier.resize(1); sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); *iter_x = 1; *iter_y = 2; *iter_z = 3; cloud.header.stamp = ros::Time(2); cloud.header.frame_id = "A"; // simple api sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0)); sensor_msgs::PointCloud2Iterator iter_x_after(cloud_simple, "x"); sensor_msgs::PointCloud2Iterator iter_y_after(cloud_simple, "y"); sensor_msgs::PointCloud2Iterator iter_z_after(cloud_simple, "z"); EXPECT_NEAR(*iter_x_after, -9, EPS); EXPECT_NEAR(*iter_y_after, 18, EPS); EXPECT_NEAR(*iter_z_after, 27, EPS); // advanced api sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0), "A", ros::Duration(3.0)); sensor_msgs::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); sensor_msgs::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); sensor_msgs::PointCloud2Iterator iter_z_advanced(cloud_advanced, "z"); EXPECT_NEAR(*iter_x_advanced, -9, EPS); EXPECT_NEAR(*iter_y_advanced, 18, EPS); EXPECT_NEAR(*iter_z_advanced, 27, EPS); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test"); ros::NodeHandle n; tf_buffer = new tf2_ros::Buffer(); // populate buffer geometry_msgs::TransformStamped t; t.transform.translation.x = 10; t.transform.translation.y = 20; t.transform.translation.z = 30; t.transform.rotation.x = 1; t.transform.rotation.y = 0; t.transform.rotation.z = 0; t.transform.rotation.w = 0; t.header.stamp = ros::Time(2.0); t.header.frame_id = "A"; t.child_frame_id = "B"; tf_buffer->setTransform(t, "test"); int ret = RUN_ALL_TESTS(); delete tf_buffer; return ret; } geometry2-0.7.5/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py000066400000000000000000000114651372352374300243020ustar00rootroot00000000000000#!/usr/bin/env python from __future__ import print_function import unittest import struct import tf2_sensor_msgs from sensor_msgs import point_cloud2 from sensor_msgs.msg import PointField from tf2_ros import TransformStamped import copy ## A sample python unit test class PointCloudConversions(unittest.TestCase): def setUp(self): self.point_cloud_in = point_cloud2.PointCloud2() self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1)] self.point_cloud_in.point_step = 4 * 3 self.point_cloud_in.height = 1 # we add two points (with x, y, z to the cloud) self.point_cloud_in.width = 2 self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width points = [1, 2, 0, 10, 20, 30] self.point_cloud_in.data = struct.pack('%sf' % len(points), *points) self.transform_translate_xyz_300 = TransformStamped() self.transform_translate_xyz_300.transform.translation.x = 300 self.transform_translate_xyz_300.transform.translation.y = 300 self.transform_translate_xyz_300.transform.translation.z = 300 self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)]) def test_simple_transform(self): old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300) k = 300 expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)] new_points = list(point_cloud2.read_points(point_cloud_transformed)) print("new_points are %s" % new_points) assert(expected_coordinates == new_points) assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud ## A simple unit test for tf2_sensor_msgs.do_transform_cloud (multi channel version) class PointCloudConversionsMultichannel(unittest.TestCase): TRANSFORM_OFFSET_DISTANCE = 300 def setUp(self): self.point_cloud_in = point_cloud2.PointCloud2() self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('index', 12, PointField.INT32, 1)] self.point_cloud_in.point_step = 4 * 4 self.point_cloud_in.height = 1 # we add two points (with x, y, z to the cloud) self.point_cloud_in.width = 2 self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)] for point in self.points: self.point_cloud_in.data += struct.pack('3fi', *point) self.transform_translate_xyz_300 = TransformStamped() self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w assert(list(point_cloud2.read_points(self.point_cloud_in)) == self.points) def test_simple_transform_multichannel(self): old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300) expected_coordinates = [] for point in self.points: expected_coordinates += [( point[0] + self.TRANSFORM_OFFSET_DISTANCE, point[1] + self.TRANSFORM_OFFSET_DISTANCE, point[2] + self.TRANSFORM_OFFSET_DISTANCE, point[3] # index channel must be kept same )] new_points = list(point_cloud2.read_points(point_cloud_transformed)) print("new_points are %s" % new_points) assert(expected_coordinates == new_points) assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud if __name__ == '__main__': import rosunit rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions) rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMultichannel) geometry2-0.7.5/tf2_tools/000077500000000000000000000000001372352374300153645ustar00rootroot00000000000000geometry2-0.7.5/tf2_tools/CHANGELOG.rst000066400000000000000000000127321372352374300174120ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 0.7.5 (2020-09-01) ------------------ 0.7.4 (2020-09-01) ------------------ 0.7.3 (2020-08-25) ------------------ 0.7.2 (2020-06-08) ------------------ * fix shebang line for python3 (`#466 `_) * Contributors: Mikael Arguedas 0.7.1 (2020-05-13) ------------------ 0.7.0 (2020-03-09) ------------------ * Bump CMake version to avoid CMP0048 warning (`#445 `_) * Merge pull request `#377 `_ from InstitutMaupertuis/melodic-devel Allow to choose output precision in echo * Merge pull request `#373 `_ from mikaelarguedas/yaml_safe_load use yaml.safe_load instead of deprecated yaml.load * Python 3 compatibility: relative imports and print statement * Contributors: Mikael Arguedas, Shane Loretz, Timon Engelke, Tully Foote, Victor Lamoine 0.6.5 (2018-11-16) ------------------ 0.6.4 (2018-11-06) ------------------ 0.6.3 (2018-07-09) ------------------ 0.6.2 (2018-05-02) ------------------ * Tf2 tools echo (`#289 `_) * tf2_tools echo is working but not yet printing the rotation `#287 `_ * install echo.py * Added quaternion output but importing from tf1 for euler_from_quaternion seems wrong (`#222 `_) so not doing that yet. Also made count exit after n counts even if exceptions occurred, also printing time of lookup for exceptions `#287 `_ * Fixed time query option, also changing message text to be more clear `#287 `_ * Added bsd license, code from transform3d transformations.py `#287 `_ * Get rid of tabs * docstring for each function * Contributors: Lucas Walter 0.6.1 (2018-03-21) ------------------ 0.6.0 (2018-03-21) ------------------ 0.5.17 (2018-01-01) ------------------- * Merge pull request `#268 `_ from smnogar/indigo-devel Fixed for cases of non-standard python install * Contributors: Steve Nogar, Tully Foote 0.5.16 (2017-07-14) ------------------- 0.5.15 (2017-01-24) ------------------- 0.5.14 (2017-01-16) ------------------- * Remove old load_manifest from view_frames (`#182 `_) * Contributors: Jochen Sprickerhof 0.5.13 (2016-03-04) ------------------- * casted el to string in view_frames * Contributors: g_gemignani 0.5.12 (2015-08-05) ------------------- 0.5.11 (2015-04-22) ------------------- 0.5.10 (2015-04-21) ------------------- 0.5.9 (2015-03-25) ------------------ 0.5.8 (2015-03-17) ------------------ * remove useless Makefile files * Contributors: Vincent Rabaud 0.5.7 (2014-12-23) ------------------ 0.5.6 (2014-09-18) ------------------ 0.5.5 (2014-06-23) ------------------ 0.5.4 (2014-05-07) ------------------ 0.5.3 (2014-02-21) ------------------ 0.5.2 (2014-02-20) ------------------ 0.5.1 (2014-02-14) ------------------ 0.5.0 (2014-02-14) ------------------ 0.4.10 (2013-12-26) ------------------- 0.4.9 (2013-11-06) ------------------ 0.4.8 (2013-11-06) ------------------ * updating install rule for view_frames.py fixes `#44 `_ 0.4.7 (2013-08-28) ------------------ 0.4.6 (2013-08-28) ------------------ 0.4.5 (2013-07-11) ------------------ 0.4.4 (2013-07-09) ------------------ 0.4.3 (2013-07-05) ------------------ 0.4.2 (2013-07-05) ------------------ 0.4.1 (2013-07-05) ------------------ 0.4.0 (2013-06-27) ------------------ * splitting rospy dependency into tf2_py so tf2 is pure c++ library. * Restoring test packages and bullet packages. reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ 0.3.6 (2013-03-03) ------------------ 0.3.5 (2013-02-15 14:46) ------------------------ * 0.3.4 -> 0.3.5 0.3.4 (2013-02-15 13:14) ------------------------ * 0.3.3 -> 0.3.4 0.3.3 (2013-02-15 11:30) ------------------------ * 0.3.2 -> 0.3.3 0.3.2 (2013-02-15 00:42) ------------------------ * 0.3.1 -> 0.3.2 0.3.1 (2013-02-14) ------------------ * 0.3.0 -> 0.3.1 0.3.0 (2013-02-13) ------------------ * switching to version 0.3.0 * removing packages with missing deps * catkinizing geometry-experimental * catkinizing tf2_tools * strip out rx dependencies * Some fixes to make things work with rxbag * Threading ns list * merge tf2_cpp and tf2_py into tf2_ros * Now catching exceptions correctly with echo * Working version of tf echo * Making sure to clear details when switching frames * Changing file format to tf * First cut at loading, saving, and exporting support * tf frame viewer is now an rxbag plugin * Can now connect to any node in the system that has a tf2 buffer * Now populates namespaces as well * Now populates a frame list on the fly * Got the GUI set up for a bunch of features, now just have to implement the backend of them * Persistent service call to speed things up. Also, coloring on click * Adding a first version of frame_viewer * Adding xdot as a dep in prep for frame_viewer * working view frames * call new service * new version of view_frames in new tf2_tools package geometry2-0.7.5/tf2_tools/CMakeLists.txt000066400000000000000000000006311372352374300201240ustar00rootroot00000000000000cmake_minimum_required(VERSION 3.0.2) project(tf2_tools) find_package(catkin REQUIRED COMPONENTS tf2 tf2_msgs tf2_ros ) catkin_package( CATKIN_DEPENDS tf2 tf2_msgs tf2_ros) install(PROGRAMS scripts/view_frames.py scripts/echo.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) geometry2-0.7.5/tf2_tools/mainpage.dox000066400000000000000000000011131372352374300176550ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_tools is ... \section codeapi Code API */ geometry2-0.7.5/tf2_tools/package.xml000066400000000000000000000011101372352374300174720ustar00rootroot00000000000000 tf2_tools 0.7.5 tf2_tools Wim Meeussen Tully Foote BSD http://www.ros.org/wiki/tf2_tools catkin tf2_msgs tf2 tf2_ros tf2_msgs tf2 tf2_ros geometry2-0.7.5/tf2_tools/scripts/000077500000000000000000000000001372352374300170535ustar00rootroot00000000000000geometry2-0.7.5/tf2_tools/scripts/echo.py000077500000000000000000000251221372352374300203500ustar00rootroot00000000000000#!/usr/bin/env python3 # tf2 echo code Copyright (c) 2018, Lucas Walter # transformations.py code Copyright (c) 2006-2017, Christoph Gohlke # transformations.py code Copyright (c) 2006-2017, The Regents of the University of California # Produced at the Laboratory for Fluorescence Dynamics # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the copyright holders nor the names of any # contributors may be used to endorse or promote products derived # from this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from __future__ import print_function import argparse import math import numpy import rospy import sys import tf2_py as tf2 import tf2_ros from geometry_msgs.msg import TransformStamped # https://github.com/ros/geometry2/issues/222 # from tf import transformations """ The following euler conversion functions are from https://github.com/matthew-brett/transforms3d which adapted it from transformations.py, it is needed here until transforms3d is available as a dependency. They are for internal use only. """ # epsilon for testing whether a number is close to zero _EPS = numpy.finfo(float).eps * 4.0 # axis sequences for Euler angles _NEXT_AXIS = [1, 2, 0, 1] # TODO(lucasw) if sxyz works then eliminate the other possibilities # map axes strings to/from tuples of inner axis, parity, repetition, frame _AXES2TUPLE = { 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} def _euler_from_matrix(matrix, axes='sxyz'): """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" try: firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] except (AttributeError, KeyError): _TUPLE2AXES[axes] # validation firstaxis, parity, repetition, frame = axes i = firstaxis j = _NEXT_AXIS[i+parity] k = _NEXT_AXIS[i-parity+1] M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] if repetition: sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) if sy > _EPS: ax = math.atan2( M[i, j], M[i, k]) ay = math.atan2( sy, M[i, i]) az = math.atan2( M[j, i], -M[k, i]) else: ax = math.atan2(-M[j, k], M[j, j]) ay = math.atan2( sy, M[i, i]) az = 0.0 else: cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) if cy > _EPS: ax = math.atan2( M[k, j], M[k, k]) ay = math.atan2(-M[k, i], cy) az = math.atan2( M[j, i], M[i, i]) else: ax = math.atan2(-M[j, k], M[j, j]) ay = math.atan2(-M[k, i], cy) az = 0.0 if parity: ax, ay, az = -ax, -ay, -az if frame: ax, az = az, ax return ax, ay, az def _quaternion_matrix(quaternion): """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" q = numpy.array(quaternion, dtype=numpy.float64, copy=True) n = numpy.dot(q, q) if n < _EPS: return numpy.identity(4) q *= math.sqrt(2.0 / n) q = numpy.outer(q, q) return numpy.array([ [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], [ 0.0, 0.0, 0.0, 1.0]]) def _euler_from_quaternion(quaternion, axes='sxyz'): """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" return _euler_from_matrix(_quaternion_matrix(quaternion), axes) def _euler_from_quaternion_msg(quaternion): # the above code is from transform3 which changed convention from old transformations.py # from xyzw to wxyz # return transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w]) return _euler_from_quaternion([quaternion.w, quaternion.x, quaternion.y, quaternion.z]) class Echo(): def __init__(self, args): self.tf_buffer = tf2_ros.Buffer(cache_time=args.cache_time) self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.args = args self.count = 0 self.timer = rospy.Timer(rospy.Duration(1.0 / self.args.rate), self.lookup) def lookup(self, event): self.count += 1 if self.args.limit: if self.count > self.args.limit: # TODO(lucasw) is there a better method to stop the spin()? rospy.signal_shutdown("tf echo finished") return cur_time = rospy.Time.now() # If the transform is from tf_static the ts.header.stamp will be 0.0 # when offset == 0 or lookup_time is rospy.Time() if self.args.time: lookup_time = rospy.Time(self.args.time) elif self.args.offset: # If the transform is static this will always work lookup_time = cur_time + rospy.Duration(self.args.offset) else: # Get the most recent transform lookup_time = rospy.Time() try: ts = self.tf_buffer.lookup_transform(self.args.source_frame, self.args.target_frame, lookup_time) except tf2.LookupException as ex: msg = "At time {}, (current time {}) ".format(lookup_time.to_sec(), cur_time.to_sec()) rospy.logerr(msg + str(ex)) return except tf2.ExtrapolationException as ex: msg = "(current time {}) ".format(cur_time.to_sec()) rospy.logerr(msg + str(ex)) return # The old tf1 static_transform_publisher (which published into /tf, not /tf_static) # publishes transforms 0.5 seconds into future so the cur_time and header stamp # will be identical. msg = "At time {}, (current time {})".format(ts.header.stamp.to_sec(), cur_time.to_sec()) xyz = ts.transform.translation msg += "\n- Translation: [{:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(xyz.x, xyz.y, xyz.z, p=self.args.precision) quat = ts.transform.rotation msg += "- Rotation: in Quaternion [{:.{p}f}, {:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(quat.x, quat.y, quat.z, quat.w, p=self.args.precision) # TODO(lucasw) need to get quaternion to euler from somewhere, but not tf1 # or a dependency that isn't in Ubuntu or ros repos euler = _euler_from_quaternion_msg(quat) msg += " in RPY (radian) " msg += "[{:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(euler[0], euler[1], euler[2], p=self.args.precision) msg += " in RPY (degree) " msg += "[{:.{p}f}, {:.{p}f}, {:.{p}f}]".format(math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2]), p=self.args.precision) print(msg) def positive_float(x): x = float(x) if x <= 0.0: raise argparse.ArgumentTypeError("{} must be > 0.0".format(x)) return x def positive_int(x): x = int(x) if x <= 0: raise argparse.ArgumentTypeError("{} must be > 0".format(x)) return x if __name__ == '__main__': rospy.init_node("echo") other_args = rospy.myargv(argv=sys.argv) precision=3 try: precision = rospy.get_param('~precision') rospy.loginfo("Precision default value was overriden, new value: %d", precision) except KeyError: pass parser = argparse.ArgumentParser() parser.add_argument("source_frame") # parent parser.add_argument("target_frame") # child parser.add_argument("-r", "--rate", help="update rate, must be > 0.0", default=1.0, type=positive_float) parser.add_argument("-c", "--cache_time", help="length of tf buffer cache in seconds", type=positive_float) parser.add_argument("-o", "--offset", help="offset the lookup from current time, ignored if using -t", type=float) parser.add_argument("-t", "--time", help="fixed time to do the lookup", type=float) parser.add_argument("-l", "--limit", help="lookup fixed number of times", type=positive_int) parser.add_argument("-p", "--precision", help="output precision", default=precision, type=positive_int) args = parser.parse_args(other_args[1:]) # Remove first arg echo = Echo(args) rospy.spin() geometry2-0.7.5/tf2_tools/scripts/view_frames.py000077500000000000000000000064411372352374300217440ustar00rootroot00000000000000#!/usr/bin/env python3 # Copyright (c) 2008, Willow Garage, Inc. # All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen import rospy import tf2_py as tf2 import yaml import subprocess from tf2_msgs.srv import FrameGraph import tf2_ros def main(): rospy.init_node('view_frames') # listen to tf for 5 seconds rospy.loginfo('Listening to tf data during 5 seconds...') rospy.sleep(0.00001) buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(buffer) rospy.sleep(5.0) rospy.loginfo('Generating graph in frames.pdf file...') rospy.wait_for_service('~tf2_frames') srv = rospy.ServiceProxy('~tf2_frames', FrameGraph) data = yaml.safe_load(srv().frame_yaml) with open('frames.gv', 'w') as f: f.write(generate_dot(data)) subprocess.Popen('dot -Tpdf frames.gv -o frames.pdf'.split(' ')).communicate() def generate_dot(data): if len(data) == 0: return 'digraph G { "No tf data received" }' dot = 'digraph G {\n' for el in data: map = data[el] dot += '"'+map['parent']+'" -> "'+str(el)+'"' dot += '[label=" ' dot += 'Broadcaster: '+map['broadcaster']+'\\n' dot += 'Average rate: '+str(map['rate'])+'\\n' dot += 'Buffer length: '+str(map['buffer_length'])+'\\n' dot += 'Most recent transform: '+str(map['most_recent_transform'])+'\\n' dot += 'Oldest transform: '+str(map['oldest_transform'])+'\\n' dot += '"];\n' if not map['parent'] in data: root = map['parent'] dot += 'edge [style=invis];\n' dot += ' subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";\n' dot += '"Recorded at time: '+str(rospy.Time.now().to_sec())+'"[ shape=plaintext ] ;\n' dot += '}->"'+root+'";\n}' return dot if __name__ == '__main__': main()