pax_global_header00006660000000000000000000000064125604655760014532gustar00rootroot0000000000000052 comment=c5893ee1299c1bb0bdf13c0695398c2bb5fbceff ros-geometry-experimental-0.5.12/000077500000000000000000000000001256046557600167465ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/.gitignore000066400000000000000000000000141256046557600207310ustar00rootroot00000000000000*~ *.pyc \#*ros-geometry-experimental-0.5.12/geometry_experimental/000077500000000000000000000000001256046557600233565ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/geometry_experimental/CHANGELOG.rst000066400000000000000000000037611256046557600254060ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package geometry_experimental ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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) ------------------- 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) ------------------ * removing test_tf2 from metapackage and update description 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 `_ * updating for new metapackage specification 0.3.6 (2013-03-03) ------------------ 0.3.5 (2013-02-15 14:46) ------------------------ * 0.3.4 -> 0.3.5 * fixing adding new packages to metapackage 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 * renaming geometry_experimental meta package in order to comply with catkin naming convention ros-geometry-experimental-0.5.12/geometry_experimental/CMakeLists.txt000066400000000000000000000001701256046557600261140ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(geometry_experimental) find_package(catkin REQUIRED) catkin_metapackage() ros-geometry-experimental-0.5.12/geometry_experimental/package.xml000066400000000000000000000016061256046557600254760ustar00rootroot00000000000000 geometry_experimental 0.5.12 The second generation Transform Library in ros. This metapackage is deprecated, but is kept for backwards compatability. Tully Foote Eitan Marder-Eppstein Wim Meeussen Tully Foote BSD LGPL http://www.ros.org/wiki/geometry_experimental catkin tf2 tf2_bullet tf2_geometry_msgs tf2_kdl tf2_msgs tf2_ros tf2_tools ros-geometry-experimental-0.5.12/test_tf2/000077500000000000000000000000001256046557600205005ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/test_tf2/CHANGELOG.rst000066400000000000000000000147571256046557600225370ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package test_tf2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/test_tf2/CMakeLists.txt000066400000000000000000000043531256046557600232450ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) if(NOT CATKIN_ENABLE_TESTING) return() endif() project(test_tf2) find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs) find_package(Boost REQUIRED COMPONENTS thread) find_package(orocos_kdl REQUIRED) include_directories(${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) link_directories(${orocos_kdl_LIBRARY_DIRS}) catkin_package() 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()ros-geometry-experimental-0.5.12/test_tf2/mainpage.dox000066400000000000000000000011121256046557600227700ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b test_tf2 is ... \section codeapi Code API */ ros-geometry-experimental-0.5.12/test_tf2/package.xml000066400000000000000000000023601256046557600226160ustar00rootroot00000000000000 test_tf2 0.5.12 tf2 unit tests Tully Foote Eitan Marder-Eppstein Tully Foote BSD LGPL http://www.ros.org/wiki/geometry_experimental catkin rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs rosunit ros-geometry-experimental-0.5.12/test_tf2/test/000077500000000000000000000000001256046557600214575ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/test_tf2/test/buffer_client_tester.launch000066400000000000000000000005021256046557600270450ustar00rootroot00000000000000 ros-geometry-experimental-0.5.12/test_tf2/test/buffer_core_test.cpp000066400000000000000000003044701256046557600255130ustar00rootroot00000000000000/* * 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 "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "rostest/permuter.h" void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); }; void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) { seed_rand(); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; } } 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.header.frame_id = frame_prefix + frames[i-1]; if (i > 1) ts.child_frame_id = frame_prefix + frames[i]; else ts.child_frame_id = frames[i]; // connect first frame 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 = 0.0/0.0; 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(); } ros-geometry-experimental-0.5.12/test_tf2/test/static_publisher.launch000066400000000000000000000005261256046557600262220ustar00rootroot00000000000000 ros-geometry-experimental-0.5.12/test_tf2/test/test_buffer_client.cpp000066400000000000000000000073571256046557600260450ustar00rootroot00000000000000/********************************************************************* * * 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(); } ros-geometry-experimental-0.5.12/test_tf2/test/test_buffer_client.py000077500000000000000000000063211256046557600257040ustar00rootroot00000000000000#! /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 #*********************************************************** PKG = 'test_tf2' import roslib; roslib.load_manifest(PKG) 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) ros-geometry-experimental-0.5.12/test_tf2/test/test_buffer_server.cpp000066400000000000000000000042031256046557600260600ustar00rootroot00000000000000/********************************************************************* * * 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(); } ros-geometry-experimental-0.5.12/test_tf2/test/test_convert.cpp000066400000000000000000000071451256046557600247110ustar00rootroot00000000000000/********************************************************************* * * 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 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); } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ros-geometry-experimental-0.5.12/test_tf2/test/test_convert.py000077500000000000000000000047621256046557600245640ustar00rootroot00000000000000#! /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 #*********************************************************** PKG = 'test_tf2' import roslib; roslib.load_manifest(PKG) 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) ros-geometry-experimental-0.5.12/test_tf2/test/test_message_filter.cpp000066400000000000000000000242451256046557600262220ustar00rootroot00000000000000/* * 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; } ros-geometry-experimental-0.5.12/test_tf2/test/test_static_publisher.cpp000066400000000000000000000115331256046557600265710ustar00rootroot00000000000000/* * 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 #include "rostest/permuter.h" #include "tf2_ros/transform_listener.h" TEST(StaticTranformPublsher, 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(StaticTranformPublsher, 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(StaticTranformPublsher, 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(StaticTranformPublsher, 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))); }; int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "tf_unittest"); return RUN_ALL_TESTS(); } ros-geometry-experimental-0.5.12/test_tf2/test/test_tf2_bullet.cpp000066400000000000000000000070761256046557600252760ustar00rootroot00000000000000/* * 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"); bool ret = RUN_ALL_TESTS(); delete tf_buffer; return ret; } ros-geometry-experimental-0.5.12/test_tf2/test/test_tf2_bullet.launch000066400000000000000000000001551256046557600257550ustar00rootroot00000000000000 ros-geometry-experimental-0.5.12/test_tf2/test/test_utils.cpp000066400000000000000000000047161256046557600243720ustar00rootroot00000000000000// 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(); } ros-geometry-experimental-0.5.12/tf2/000077500000000000000000000000001256046557600174415ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2/CHANGELOG.rst000066400000000000000000000361151256046557600214700ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2 ^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2/CMakeLists.txt000066400000000000000000000034601256046557600222040ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(tf2) find_package(console_bridge REQUIRED) find_package(catkin REQUIRED COMPONENTS geometry_msgs rostime tf2_msgs) find_package(Boost REQUIRED COMPONENTS signals 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}) include_directories (src/bt) # 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 tf2_msgs_gencpp) install(TARGETS tf2 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) 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 tf2_msgs_gencpp) 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 tf2_msgs_gencpp) catkin_add_gtest(test_simple test/simple_tf2_core.cpp) target_link_libraries(test_simple tf2 ${console_bridge_LIBRARIES}) add_dependencies(test_simple tf2_msgs_gencpp) 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 tf2_msgs_gencpp) endif() ros-geometry-experimental-0.5.12/tf2/include/000077500000000000000000000000001256046557600210645ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2/include/tf2/000077500000000000000000000000001256046557600215575ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2/include/tf2/LinearMath/000077500000000000000000000000001256046557600236035ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2/include/tf2/LinearMath/Matrix3x3.h000066400000000000000000000517221256046557600255650ustar00rootroot00000000000000/* 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" 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 */ void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated)) { 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*/ __attribute__((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 ros-geometry-experimental-0.5.12/tf2/include/tf2/LinearMath/MinMax.h000066400000000000000000000032611256046557600251470ustar00rootroot00000000000000/* 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/LinearMath/QuadWord.h000066400000000000000000000137061256046557600255110ustar00rootroot00000000000000/* 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/LinearMath/Quaternion.h000066400000000000000000000373561256046557600261170ustar00rootroot00000000000000/* 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" 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 */ Quaternion(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated)) { #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 */ void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated)) { 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 of rotation represented by this quaternion */ tf2Scalar getAngle() const { tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]); return s; } /**@brief Return the angle of rotation represented by this quaternion along the shortest path*/ tf2Scalar getAngleShortestPath() const { tf2Scalar s; if (dot(*this) < 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/LinearMath/Scalar.h000066400000000000000000000321751256046557600251710ustar00rootroot00000000000000/* 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/LinearMath/Transform.h000066400000000000000000000206371256046557600257370ustar00rootroot00000000000000/* 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/LinearMath/Vector3.h000066400000000000000000000475021256046557600253110ustar00rootroot00000000000000/* 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; unsigned char* src = (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 ros-geometry-experimental-0.5.12/tf2/include/tf2/buffer_core.h000066400000000000000000000441451256046557600242210ustar00rootroot00000000000000/* * 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; //!< The default amount of time to cache data in seconds /** 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_; }; }; #endif //TF2_CORE_H ros-geometry-experimental-0.5.12/tf2/include/tf2/convert.h000066400000000000000000000107351256046557600234160ustar00rootroot00000000000000/* * 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. */ 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. */ 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/exceptions.h000066400000000000000000000074561256046557600241250ustar00rootroot00000000000000/* * 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/impl/000077500000000000000000000000001256046557600225205ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2/include/tf2/impl/convert.h000066400000000000000000000053111256046557600243510ustar00rootroot00000000000000/* * 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) { fromMsg(a, b); } template <> template inline void Converter::convert(const A& a, B& b) { b = toMsg(a); } template <> template inline void Converter::convert(const A& a, B& b) { fromMsg(toMsg(a), b); } } } #endif //TF2_IMPL_CONVERT_H ros-geometry-experimental-0.5.12/tf2/include/tf2/impl/utils.h000066400000000000000000000106771256046557600240440ustar00rootroot00000000000000// 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, geometry_msgs::QuaternionStamped>(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 ros-geometry-experimental-0.5.12/tf2/include/tf2/time_cache.h000066400000000000000000000127131256046557600240150ustar00rootroot00000000000000/* * 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 #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)=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 = 1ULL * 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); 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::list 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); 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/transform_datatypes.h000066400000000000000000000055671256046557600260360ustar00rootroot00000000000000/* * 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_) {} /** 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/transform_storage.h000066400000000000000000000052551256046557600254760ustar00rootroot00000000000000/* * 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 ros-geometry-experimental-0.5.12/tf2/include/tf2/utils.h000066400000000000000000000040341256046557600230710ustar00rootroot00000000000000// 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 ros-geometry-experimental-0.5.12/tf2/index.rst000066400000000000000000000002631256046557600213030ustar00rootroot00000000000000tf2 ===== This is the Python API reference of the tf2 package. .. toctree:: :maxdepth: 2 tf2 Indices and tables ================== * :ref:`genindex` * :ref:`search` ros-geometry-experimental-0.5.12/tf2/mainpage.dox000066400000000000000000000006211256046557600217350ustar00rootroot00000000000000/** \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. \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. */ ros-geometry-experimental-0.5.12/tf2/package.xml000066400000000000000000000021551256046557600215610ustar00rootroot00000000000000 tf2 0.5.12 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 ros-geometry-experimental-0.5.12/tf2/src/000077500000000000000000000000001256046557600202305ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2/src/buffer_core.cpp000066400000000000000000001341521256046557600232230ustar00rootroot00000000000000/* * 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" namespace tf2 { /** \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"; 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: "; 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) { 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 == "") { 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 == "") { 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)) { 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; } 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); if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number))) { frame_authority_[frame_number] = authority; } else { logWarn("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained", 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, &extrapolation_error_string); 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) { std::stringstream ss; ss << *error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]"; *error_string = ss.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) { frame_chain->swap(reverse_frame_chain); } 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) { std::stringstream ss; ss << extrapolation_error_string << ", when looking up transform from frame [" << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) << "]"; *error_string = ss.str(); } return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR; } createConnectivityErrorString(source_id, target_id, error_string); return tf2_msgs::TF2Error::CONNECTIVITY_ERROR; } 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) frame_chain->erase(frame_chain->begin() + (n-1), 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: 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) { 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); 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; return canTransform(target_frame, fixed_frame, target_time) && canTransform(fixed_frame, source_frame, 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_); V_TransformableRequest::iterator it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle)); transformable_requests_.erase(it, transformable_requests_.end()); } } 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_); V_TransformableRequest::iterator it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle)); if (it != transformable_requests_.end()) { transformable_requests_.erase(it, transformable_requests_.end()); } } // 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(); 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()) { const TransformableCallback& cb = it->second; cb(req.request_handle, lookupFrameString(req.target_id), lookupFrameString(req.source_id), req.time, 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 detadlock (#91) lock.unlock(); // 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: logError("Unknown error code: %d", retval); assert(0); } } if (source_time != target_time) { 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: logError("Unknown error code: %d", retval); assert(0); } } int m = target_frame_chain.size()-1; int n = source_frame_chain.size()-1; for (; m >= 0 && n >= 0; --m, --n) { if (source_frame_chain[n] != target_frame_chain[m]) break; } // Erase all duplicate items from frame_chain if (n > 0) source_frame_chain.erase(source_frame_chain.begin() + (n-1), source_frame_chain.end()); if (m < target_frame_chain.size()) { for (unsigned int i = 0; i <= m; ++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])); } } } // namespace tf2 ros-geometry-experimental-0.5.12/tf2/src/cache.cpp000066400000000000000000000176621256046557600220130ustar00rootroot00000000000000/* * 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 using 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) {} // 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) { std::stringstream ss; ss << "Lookup would require extrapolation at time " << t0 << ", but only time " << t1 << " is in the buffer"; *error_str = ss.str(); } } void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str) { if (error_str) { std::stringstream ss; ss << "Lookup would require extrapolation into the future. Requested time " << t0 << " but the latest data is at time " << t1; *error_str = ss.str(); } } void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str) { if (error_str) { std::stringstream ss; ss << "Lookup would require extrapolation into the past. Requested time " << t0 << " but the earliest data is at time " << t1; *error_str = ss.str(); } } uint8_t TimeCache::findClosest(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 { 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) { createExtrapolationException2(target_time, latest_time, error_str); return 0; } else if (target_time < earliest_time) { 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 = storage_.begin(); while(storage_it != storage_.end()) { if (storage_it->stamp_ <= target_time) break; storage_it++; } //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.toSec() - one.stamp_.toSec()) / (two.stamp_.toSec() - one.stamp_.toSec()); //Interpolate translation output.translation_.setInterpolate3(one.translation_, two.translation_, ratio); //Interpolate rotation output.rotation_ = slerp( one.rotation_, two.rotation_, ratio); output.stamp_ = one.stamp_; output.frame_id_ = one.frame_id_; output.child_frame_id_ = one.child_frame_id_; } bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available { 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) { L_TransformStorage::iterator storage_it = storage_.begin(); if(storage_it != storage_.end()) { if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_) { return false; } } while(storage_it != storage_.end()) { if (storage_it->stamp_ <= new_data.stamp_) break; storage_it++; } 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(); } } ros-geometry-experimental-0.5.12/tf2/src/static_cache.cpp000066400000000000000000000050021256046557600233430ustar00rootroot00000000000000/* * 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) { 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(); }; ros-geometry-experimental-0.5.12/tf2/test/000077500000000000000000000000001256046557600204205ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2/test/cache_unittest.cpp000066400000000000000000000247451256046557600241420ustar00rootroot00000000000000/* * 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/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 ++) { 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(); // 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_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 ++) { EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor)); //get the transform for the position 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_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(); } ros-geometry-experimental-0.5.12/tf2/test/simple_tf2_core.cpp000066400000000000000000000077741256046557600242170ustar00rootroot00000000000000/* * 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 "tf2/LinearMath/Vector3.h" #include "tf2/exceptions.h" void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); }; void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) { seed_rand(); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; } } 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_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))); } 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))); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::Time::init(); //needed for ros::TIme::now() return RUN_ALL_TESTS(); } ros-geometry-experimental-0.5.12/tf2/test/speed_test.cpp000066400000000000000000000154331256046557600232710ustar00rootroot00000000000000/* * 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]); } 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 (uint32_t j = 1; j < 3; ++j) { 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 (uint32_t j = 1; j < 3; ++j) { 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); logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str()); geometry_msgs::TransformStamped out_t; const uint32_t count = 1000000; logInform("Doing %d %d-level tests", count, num_levels); #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); 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); 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); 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); 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); 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); 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); 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); logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); } #endif } ros-geometry-experimental-0.5.12/tf2/test/static_cache_test.cpp000066400000000000000000000062001256046557600245730ustar00rootroot00000000000000/* * 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 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(); } ros-geometry-experimental-0.5.12/tf2_bullet/000077500000000000000000000000001256046557600210105ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_bullet/CHANGELOG.rst000066400000000000000000000060011256046557600230260ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_bullet ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2_bullet/CMakeLists.txt000066400000000000000000000012051256046557600235460ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(tf2_bullet) find_package(PkgConfig REQUIRED) set(bullet_FOUND 0) pkg_check_modules(bullet REQUIRED bullet) 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} ${catkin_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() ros-geometry-experimental-0.5.12/tf2_bullet/include/000077500000000000000000000000001256046557600224335ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_bullet/include/tf2_bullet/000077500000000000000000000000001256046557600244755ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_bullet/include/tf2_bullet/tf2_bullet.h000066400000000000000000000065551256046557600267230ustar00rootroot00000000000000/* * 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 { 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)); } // this method needs to be implemented by client library developers 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); } //convert to vector 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; } 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; } // this method needs to be implemented by client library developers 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 ros-geometry-experimental-0.5.12/tf2_bullet/include/tf2_bullet/tf2_bullet/000077500000000000000000000000001256046557600265375ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h000066400000000000000000000001731256046557600307530ustar00rootroot00000000000000#warning This header is at the wrong path you should include #include ros-geometry-experimental-0.5.12/tf2_bullet/mainpage.dox000066400000000000000000000011111256046557600232770ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_kdl is ... \section codeapi Code API */ ros-geometry-experimental-0.5.12/tf2_bullet/package.xml000066400000000000000000000012321256046557600231230ustar00rootroot00000000000000 tf2_bullet 0.5.12 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 ros-geometry-experimental-0.5.12/tf2_bullet/test/000077500000000000000000000000001256046557600217675ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_bullet/test/test_tf2_bullet.cpp000066400000000000000000000041061256046557600255750ustar00rootroot00000000000000/* * 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 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); bool ret = RUN_ALL_TESTS(); return ret; } ros-geometry-experimental-0.5.12/tf2_eigen/000077500000000000000000000000001256046557600206105ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_eigen/CHANGELOG.rst000066400000000000000000000005571256046557600226400ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_eigen ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2_eigen/CMakeLists.txt000066400000000000000000000011761256046557600233550ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(tf2_eigen) find_package(catkin REQUIRED COMPONENTS cmake_modules geometry_msgs tf2 ) find_package(Eigen REQUIRED) include_directories(${EIGEN_INCLUDE_DIRS}) add_definitions(${EIGEN_DEFINITIONS}) include_directories(include) catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} DEPENDS eigen) 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() ros-geometry-experimental-0.5.12/tf2_eigen/include/000077500000000000000000000000001256046557600222335ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_eigen/include/tf2_eigen/000077500000000000000000000000001256046557600240755ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_eigen/include/tf2_eigen/tf2_eigen.h000066400000000000000000000103331256046557600261100ustar00rootroot00000000000000/* * 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 namespace tf2 { Eigen::Affine3d transformToEigen(const geometry_msgs::TransformStamped& t) { return Eigen::Affine3d(Eigen::Translation3d(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z) * Eigen::Quaterniond(t.transform.rotation.w, t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z)); } // this method needs to be implemented by client library developers template <> 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); } //convert to vector message 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.x(); msg.point.y = in.y(); msg.point.z = in.z(); return msg; } void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out.x() = msg.point.x; out.y() = msg.point.y; out.z() = msg.point.z; } // this method needs to be implemented by client library developers template <> 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); } //convert to pose message 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.position.x = in.translation().x(); msg.pose.position.y = in.translation().y(); msg.pose.position.z = in.translation().z(); msg.pose.orientation.x = Eigen::Quaterniond(in.rotation()).x(); msg.pose.orientation.y = Eigen::Quaterniond(in.rotation()).y(); msg.pose.orientation.z = Eigen::Quaterniond(in.rotation()).z(); msg.pose.orientation.w = Eigen::Quaterniond(in.rotation()).w(); return msg; } void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out.setData(Eigen::Affine3d(Eigen::Translation3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) * Eigen::Quaterniond(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z))); } } // namespace #endif // TF2_EIGEN_H ros-geometry-experimental-0.5.12/tf2_eigen/package.xml000066400000000000000000000011051256046557600227220ustar00rootroot00000000000000 tf2_eigen 0.5.12 tf2_eigen Koji Terada Koji Terada BSD catkin eigen geometry_msgs tf2 cmake_modules geometry_msgs tf2 cmake_modules ros-geometry-experimental-0.5.12/tf2_eigen/test/000077500000000000000000000000001256046557600215675ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_eigen/test/tf2_eigen-test.cpp000066400000000000000000000035551256046557600251220ustar00rootroot00000000000000/* * 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 #include static const double EPS = 1e-3; TEST(TfEigen, ConvertVector) { Eigen::Vector3d v(1,2,3); Eigen::Vector3d v1 = v; tf2::convert(v1, v1); EXPECT_EQ(v, v1); Eigen::Vector3d 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); bool ret = RUN_ALL_TESTS(); return ret; } ros-geometry-experimental-0.5.12/tf2_geometry_msgs/000077500000000000000000000000001256046557600224055ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_geometry_msgs/CHANGELOG.rst000066400000000000000000000101031256046557600244210ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_geometry_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2_geometry_msgs/CMakeLists.txt000066400000000000000000000021351256046557600251460ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) 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) 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_link_libraries(test_geometry_msgs ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) if(TARGET tests) add_dependencies(tests test_geometry_msgs) endif() endif() ros-geometry-experimental-0.5.12/tf2_geometry_msgs/include/000077500000000000000000000000001256046557600240305ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_geometry_msgs/include/tf2_geometry_msgs/000077500000000000000000000000001256046557600274675ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h000066400000000000000000000265461256046557600333140ustar00rootroot00000000000000/* * 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 namespace tf2 { 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)); } /********************/ /** Vector3Stamped **/ /********************/ // method to extract timestamp from object template <> inline const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;} // method to extract frame id from object template <> inline const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform) { KDL::Vector v_out = gmTransformToKDL(transform).M * KDL::Vector(t_in.vector.x, t_in.vector.y, t_in.vector.z); t_out.vector.x = v_out[0]; t_out.vector.y = v_out[1]; t_out.vector.z = v_out[2]; t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } inline geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in) { return in; } inline void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out) { out = msg; } /******************/ /** PointStamped **/ /******************/ // method to extract timestamp from object template <> inline const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;} // method to extract frame id from object template <> inline const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform) { KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.point.x, t_in.point.y, t_in.point.z); t_out.point.x = v_out[0]; t_out.point.y = v_out[1]; t_out.point.z = v_out[2]; t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } inline geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in) { return in; } inline void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out) { out = msg; } /*****************/ /** PoseStamped **/ /*****************/ // method to extract timestamp from object template <> inline const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;} // method to extract frame id from object template <> inline const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform) { KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z); KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.orientation.x, t_in.pose.orientation.y, t_in.pose.orientation.z, t_in.pose.orientation.w); KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); t_out.pose.position.x = v_out.p[0]; t_out.pose.position.y = v_out.p[1]; t_out.pose.position.z = v_out.p[2]; v_out.M.GetQuaternion(t_out.pose.orientation.x, t_out.pose.orientation.y, t_out.pose.orientation.z, t_out.pose.orientation.w); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } inline geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in) { return in; } inline void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out) { out = msg; } /****************/ /** Quaternion **/ /****************/ 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; } 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 **/ /***********************/ // method to extract timestamp from object template <> inline const ros::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;} // method to extract frame id from object template <> inline const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const geometry_msgs::TransformStamped& transform) { tf2::Quaternion q_out = tf2::Quaternion(transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w)* tf2::Quaternion(t_in.quaternion.x, t_in.quaternion.y, t_in.quaternion.z, t_in.quaternion.w); t_out.quaternion = toMsg(q_out); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } inline geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in) { return in; } inline void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out) { out = msg; } template <> 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 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); } /**********************/ /** TransformStamped **/ /**********************/ // method to extract timestamp from object template <> inline const ros::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;} // method to extract frame id from object template <> inline const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;} // this method needs to be implemented by client library developers template <> inline void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform) { KDL::Vector v(t_in.transform.translation.x, t_in.transform.translation.y, t_in.transform.translation.z); KDL::Rotation r = KDL::Rotation::Quaternion(t_in.transform.rotation.x, t_in.transform.rotation.y, t_in.transform.rotation.z, t_in.transform.rotation.w); KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); t_out.transform.translation.x = v_out.p[0]; t_out.transform.translation.y = v_out.p[1]; t_out.transform.translation.z = v_out.p[2]; v_out.M.GetQuaternion(t_out.transform.rotation.x, t_out.transform.rotation.y, t_out.transform.rotation.z, t_out.transform.rotation.w); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } inline geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in) { return in; } inline void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out) { out = msg; } /***************/ /** Transform **/ /***************/ inline geometry_msgs::Transform toMsg(const tf2::Transform& in) { geometry_msgs::Transform out; out.translation.x = in.getOrigin().getX(); out.translation.y = in.getOrigin().getY(); out.translation.z = in.getOrigin().getZ(); out.rotation = toMsg(in.getRotation()); return out; } inline void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) { out.setOrigin(tf2::Vector3(in.translation.x, in.translation.y, in.translation.z)); // w at the end in the constructor out.setRotation(tf2::Quaternion(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w)); } /**********/ /** Pose **/ /**********/ inline /** This section is about converting */ void toMsg(const tf2::Transform& in, geometry_msgs::Pose& out ) { out.position.x = in.getOrigin().getX(); out.position.y = in.getOrigin().getY(); out.position.z = in.getOrigin().getZ(); out.orientation = toMsg(in.getRotation()); } 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)); } } // namespace #endif // TF2_GEOMETRY_MSGS_H ros-geometry-experimental-0.5.12/tf2_geometry_msgs/mainpage.dox000066400000000000000000000011231256046557600246770ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_geometry_msgs is ... \section codeapi Code API */ ros-geometry-experimental-0.5.12/tf2_geometry_msgs/package.xml000066400000000000000000000013531256046557600245240ustar00rootroot00000000000000 tf2_geometry_msgs 0.5.12 tf2_geometry_msgs Wim Meeussen Tully Foote BSD http://www.ros.org/wiki/tf2_ros catkin geometry_msgs tf2_ros tf2 orocos_kdl geometry_msgs tf2_ros tf2 orocos_kdl rostest ros-geometry-experimental-0.5.12/tf2_geometry_msgs/scripts/000077500000000000000000000000001256046557600240745ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_geometry_msgs/scripts/test.py000077500000000000000000000022651256046557600254350ustar00rootroot00000000000000#!/usr/bin/python import roslib; roslib.load_manifest('tf2_geometry_msgs') import rospy import PyKDL import tf2_ros import tf2_geometry_msgs from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped def main(): 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') print b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0)) 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 print b.transform(v, 'b') v = Vector3Stamped() v.header.stamp = rospy.Time(2) v.header.frame_id = 'a' v.vector.x = 1 v.vector.y = 2 v.vector.z = 3 print b.transform(v, 'b') 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 print b.transform(v, 'b') if __name__ == '__main__': rospy.init_node('wim') main() ros-geometry-experimental-0.5.12/tf2_geometry_msgs/setup.py000066400000000000000000000004421256046557600241170ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core 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) ros-geometry-experimental-0.5.12/tf2_geometry_msgs/src/000077500000000000000000000000001256046557600231745ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_geometry_msgs/src/tf2_geometry_msgs/000077500000000000000000000000001256046557600266335ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py000066400000000000000000000000401256046557600307360ustar00rootroot00000000000000from tf2_geometry_msgs import * ros-geometry-experimental-0.5.12/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py000066400000000000000000000101271256046557600326450ustar00rootroot00000000000000# 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 import PyKDL import rospy import tf2_ros 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): 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.p[0] res.pose.position.y = f.p[1] res.pose.position.z = f.p[2] (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) ros-geometry-experimental-0.5.12/tf2_geometry_msgs/test/000077500000000000000000000000001256046557600233645ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_geometry_msgs/test/test.launch000066400000000000000000000002021256046557600255310ustar00rootroot00000000000000 ros-geometry-experimental-0.5.12/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp000066400000000000000000000117731256046557600305770ustar00rootroot00000000000000/* * 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; 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, 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); } 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"); bool ret = RUN_ALL_TESTS(); delete tf_buffer; return ret; } ros-geometry-experimental-0.5.12/tf2_kdl/000077500000000000000000000000001256046557600202735ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_kdl/CHANGELOG.rst000066400000000000000000000077761256046557600223350ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_kdl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2_kdl/CMakeLists.txt000066400000000000000000000021361256046557600230350ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(tf2_kdl) find_package(orocos_kdl) find_package(catkin REQUIRED COMPONENTS cmake_modules tf2 tf2_ros tf2_msgs) find_package(Eigen REQUIRED) catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} DEPENDS Eigen orocos_kdl ) catkin_python_setup() link_directories(${orocos_kdl_LIBRARY_DIRS}) include_directories(include ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ${GTEST_INCLUDE_DIRS}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) install(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_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) if(TARGET tests) add_dependencies(tests test_kdl) endif() endif() ros-geometry-experimental-0.5.12/tf2_kdl/include/000077500000000000000000000000001256046557600217165ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_kdl/include/tf2_kdl/000077500000000000000000000000001256046557600232435ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_kdl/include/tf2_kdl/tf2_kdl.h000066400000000000000000000163261256046557600247510ustar00rootroot00000000000000/* * 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 namespace tf2 { 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)); } 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 // --------------------- // this method needs to be implemented by client library developers 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); } //convert to vector 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; } 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 // --------------------- // this method needs to be implemented by client library developers 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); } //convert to twist 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; } 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 // --------------------- // this method needs to be implemented by client library developers 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); } //convert to wrench 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; } 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 // --------------------- // this method needs to be implemented by client library developers 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); } //convert to pose 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.position.x = in.p[0]; msg.pose.position.y = in.p[1]; msg.pose.position.z = in.p[2]; in.M.GetQuaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w); return msg; } inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; out.p[0] = msg.pose.position.x; out.p[1] = msg.pose.position.y; out.p[2] = msg.pose.position.z; out.M = KDL::Rotation::Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w); } } // namespace #endif // TF2_KDL_H ros-geometry-experimental-0.5.12/tf2_kdl/include/tf2_kdl/tf2_kdl/000077500000000000000000000000001256046557600245705ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_kdl/include/tf2_kdl/tf2_kdl/tf2_kdl.h000066400000000000000000000001571256046557600262710ustar00rootroot00000000000000#warning This header is at the wrong path you should include #include ros-geometry-experimental-0.5.12/tf2_kdl/mainpage.dox000066400000000000000000000011111256046557600225620ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_kdl is ... \section codeapi Code API */ ros-geometry-experimental-0.5.12/tf2_kdl/package.xml000066400000000000000000000015621256046557600224140ustar00rootroot00000000000000 tf2_kdl 0.5.12 KDL binding for tf2 Tully Foote Wim Meeussen BSD http://ros.org/wiki/tf2 catkin cmake_modules eigen orocos_kdl tf2 tf2_ros cmake_modules eigen orocos_kdl tf2 tf2_ros rostest ros-geometry-experimental-0.5.12/tf2_kdl/scripts/000077500000000000000000000000001256046557600217625ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_kdl/scripts/test.py000077500000000000000000000024041256046557600233160ustar00rootroot00000000000000#!/usr/bin/python import roslib; roslib.load_manifest('tf2_kdl') import rospy import PyKDL import tf2_ros import tf2_kdl from geometry_msgs.msg import TransformStamped from copy import deepcopy def main(): 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') print b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0)) v = PyKDL.Vector(1,2,3) print b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b') f = PyKDL.Frame(PyKDL.Rotation.RPY(1,2,3), PyKDL.Vector(1,2,3)) print b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b') t = PyKDL.Twist(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6)) print b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b') w = PyKDL.Wrench(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6)) print b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b') def convert(): v = PyKDL.Vector(1,2,3) vs = tf2_ros.Stamped(v, rospy.Time(2), 'a') vs2 = tf2_ros.convert(vs, PyKDL.Vector) vs2[1] = 100 print vs2 print v if __name__ == '__main__': rospy.init_node('wim') main() convert() ros-geometry-experimental-0.5.12/tf2_kdl/setup.py000066400000000000000000000004661256046557600220130ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core 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) ros-geometry-experimental-0.5.12/tf2_kdl/src/000077500000000000000000000000001256046557600210625ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_kdl/src/tf2_kdl/000077500000000000000000000000001256046557600224075ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_kdl/src/tf2_kdl/__init__.py000066400000000000000000000000261256046557600245160ustar00rootroot00000000000000from tf2_kdl import * ros-geometry-experimental-0.5.12/tf2_kdl/src/tf2_kdl/tf2_kdl.py000066400000000000000000000072241256046557600243130ustar00rootroot00000000000000# 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 roslib; roslib.load_manifest('tf2_kdl') import PyKDL import rospy import tf2_ros from geometry_msgs.msg import PointStamped 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)) # Vector def do_transform_vector(vector, transform): 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): 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): 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): 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) # Frame def do_transform_frame(frame, transform): res = transform_to_kdl(transform) * frame res.header = transform.header return res tf2_ros.TransformRegistration().add(PyKDL.Frame, do_transform_frame) # Twist def do_transform_twist(twist, transform): 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): res = transform_to_kdl(transform) * wrench res.header = transform.header return res tf2_ros.TransformRegistration().add(PyKDL.Wrench, do_transform_wrench) ros-geometry-experimental-0.5.12/tf2_kdl/test/000077500000000000000000000000001256046557600212525ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_kdl/test/test.launch000066400000000000000000000001441256046557600234240ustar00rootroot00000000000000 ros-geometry-experimental-0.5.12/tf2_kdl/test/test_tf2_kdl.cpp000066400000000000000000000100211256046557600243340ustar00rootroot00000000000000/* * 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"); bool retval = RUN_ALL_TESTS(); delete tf_buffer; return retval; } ros-geometry-experimental-0.5.12/tf2_msgs/000077500000000000000000000000001256046557600204725ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_msgs/CHANGELOG.rst000066400000000000000000000050071256046557600225150ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2_msgs/CMakeLists.txt000066400000000000000000000010641256046557600232330ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) 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) ros-geometry-experimental-0.5.12/tf2_msgs/action/000077500000000000000000000000001256046557600217475ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_msgs/action/LookupTransform.action000066400000000000000000000004121256046557600263100ustar00rootroot00000000000000#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 --- ros-geometry-experimental-0.5.12/tf2_msgs/include/000077500000000000000000000000001256046557600221155ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_msgs/include/foo000066400000000000000000000000001256046557600226110ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_msgs/mainpage.dox000066400000000000000000000011121256046557600227620ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_msgs is ... \section codeapi Code API */ ros-geometry-experimental-0.5.12/tf2_msgs/msg/000077500000000000000000000000001256046557600212605ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_msgs/msg/TF2Error.msg000066400000000000000000000003311256046557600233720ustar00rootroot00000000000000uint8 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 ros-geometry-experimental-0.5.12/tf2_msgs/msg/TFMessage.msg000066400000000000000000000000541256046557600236050ustar00rootroot00000000000000geometry_msgs/TransformStamped[] transforms ros-geometry-experimental-0.5.12/tf2_msgs/package.xml000066400000000000000000000012061256046557600226060ustar00rootroot00000000000000 tf2_msgs 0.5.12 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 ros-geometry-experimental-0.5.12/tf2_msgs/srv/000077500000000000000000000000001256046557600213045ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_msgs/srv/FrameGraph.srv000077500000000000000000000000261256046557600240550ustar00rootroot00000000000000--- string frame_yaml ros-geometry-experimental-0.5.12/tf2_py/000077500000000000000000000000001256046557600201515ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_py/CHANGELOG.rst000066400000000000000000000024371256046557600222000ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_py ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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. ros-geometry-experimental-0.5.12/tf2_py/CMakeLists.txt000066400000000000000000000114101256046557600227060ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) 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 tf2_msgs_gencpp) set_target_properties(tf2_py PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") set_target_properties(tf2_py PROPERTIES COMPILE_FLAGS "-g -Wno-missing-field-initializers") 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} ) #!! 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 ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/_tf2.so 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) ros-geometry-experimental-0.5.12/tf2_py/package.xml000066400000000000000000000037521256046557600222750ustar00rootroot00000000000000 tf2_py 0.5.12 The tf2_py package Tully Foote BSD http://ros.org/wiki/tf2_py catkin tf2 rospy tf2 rospy ros-geometry-experimental-0.5.12/tf2_py/setup.py000066400000000000000000000004141256046557600216620ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core 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) ros-geometry-experimental-0.5.12/tf2_py/src/000077500000000000000000000000001256046557600207405ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_py/src/tf2_py.cpp000066400000000000000000000511141256046557600226510ustar00rootroot00000000000000#include #include #include // 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 = { PyObject_HEAD_INIT(&PyType_Type) 0, /*size*/ "_tf2.BufferCore", /*name*/ sizeof(buffer_core_t), /*basicsize*/ }; static PyObject *PyObject_BorrowAttrString(PyObject* o, const char *name) { PyObject *r = PyObject_GetAttrString(o, name); if (r != NULL) Py_DECREF(r); return r; } 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_SetAttrString(pheader, "frame_id", PyString_FromString((transform->header.frame_id).c_str())); 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_SetAttrString(pinst, "child_frame_id", PyString_FromString((transform->child_frame_id).c_str())); PyObject_SetAttrString(ptranslation, "x", PyFloat_FromDouble(transform->transform.translation.x)); PyObject_SetAttrString(ptranslation, "y", PyFloat_FromDouble(transform->transform.translation.y)); PyObject_SetAttrString(ptranslation, "z", PyFloat_FromDouble(transform->transform.translation.z)); Py_DECREF(ptranslation); PyObject_SetAttrString(protation, "x", PyFloat_FromDouble(transform->transform.rotation.x)); PyObject_SetAttrString(protation, "y", PyFloat_FromDouble(transform->transform.rotation.y)); PyObject_SetAttrString(protation, "z", PyFloat_FromDouble(transform->transform.rotation.z)); PyObject_SetAttrString(protation, "w", PyFloat_FromDouble(transform->transform.rotation.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 PyString_FromString(t->getTFPrefix().c_str()); } */ static PyObject *allFramesAsYAML(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; return PyString_FromString(bc->allFramesAsYAML().c_str()); } static PyObject *allFramesAsString(PyObject *self, PyObject *args) { tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; return PyString_FromString(bc->allFramesAsString().c_str()); } 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()); } /* Debugging stuff that may need to be implemented later 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, PyString_FromString(los[i].c_str())); } return r; } static PyObject *chain(PyObject *self, PyObject *args, PyObject *kw) { tf::Transformer *t = ((transformer_t*)self)->t; 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(t->chainAsVector(target_frame, target_time, source_frame, source_time, fixed_frame, output)); return asListOfStrings(output); } static PyObject *getLatestCommonTime(PyObject *self, PyObject *args, PyObject *kw) { tf::Transformer *t = ((transformer_t*)self)->t; char *source, *dest; std::string error_string; ros::Time time; if (!PyArg_ParseTuple(args, "ss", &source, &dest)) return NULL; int r = t->getLatestCommonTime(source, dest, 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(tf_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 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 = PyObject_BorrowAttrString(py_transform, "header"); transform.child_frame_id = PyString_AsString(PyObject_BorrowAttrString(py_transform, "child_frame_id")); transform.header.frame_id = PyString_AsString(PyObject_BorrowAttrString(header, "frame_id")); if (rostime_converter(PyObject_BorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) return NULL; PyObject *mtransform = PyObject_BorrowAttrString(py_transform, "transform"); PyObject *translation = PyObject_BorrowAttrString(mtransform, "translation"); transform.transform.translation.x = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "x")); transform.transform.translation.y = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "y")); transform.transform.translation.z = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "z")); PyObject *rotation = PyObject_BorrowAttrString(mtransform, "rotation"); transform.transform.rotation.x = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "x")); transform.transform.rotation.y = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "y")); transform.transform.rotation.z = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "z")); transform.transform.rotation.w = PyFloat_AsDouble(PyObject_BorrowAttrString(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 = PyObject_BorrowAttrString(py_transform, "header"); transform.child_frame_id = PyString_AsString(PyObject_BorrowAttrString(py_transform, "child_frame_id")); transform.header.frame_id = PyString_AsString(PyObject_BorrowAttrString(header, "frame_id")); if (rostime_converter(PyObject_BorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) return NULL; PyObject *mtransform = PyObject_BorrowAttrString(py_transform, "transform"); PyObject *translation = PyObject_BorrowAttrString(mtransform, "translation"); transform.transform.translation.x = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "x")); transform.transform.translation.y = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "y")); transform.transform.translation.z = PyFloat_AsDouble(PyObject_BorrowAttrString(translation, "z")); PyObject *rotation = PyObject_BorrowAttrString(mtransform, "rotation"); transform.transform.rotation.x = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "x")); transform.transform.rotation.y = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "y")); transform.transform.rotation.z = PyFloat_AsDouble(PyObject_BorrowAttrString(rotation, "z")); transform.transform.rotation.w = PyFloat_AsDouble(PyObject_BorrowAttrString(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; } /* May come back eventually, but not in public api right now static PyObject *frameExists(PyObject *self, PyObject *args) { tf::Transformer *t = ((transformer_t*)self)->t; char *frame_id_str; if (!PyArg_ParseTuple(args, "s", &frame_id_str)) return NULL; return PyBool_FromLong(t->frameExists(frame_id_str)); } static PyObject *getFrameStrings(PyObject *self, PyObject *args) { tf::Transformer *t = ((transformer_t*)self)->t; std::vector< std::string > ids; t->getFrameStrings(ids); return asListOfStrings(ids); } */ 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_KEYWORDS}, {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_KEYWORDS}, //{"chain", (PyCFunction)chain, METH_KEYWORDS}, {"clear", (PyCFunction)clear, METH_KEYWORDS}, //{"frameExists", (PyCFunction)frameExists, METH_VARARGS}, //{"getFrameStrings", (PyCFunction)getFrameStrings, METH_VARARGS}, //{"getLatestCommonTime", (PyCFunction)getLatestCommonTime, METH_VARARGS}, {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_KEYWORDS}, {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_KEYWORDS}, //{"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_KEYWORDS}, //{"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS}, //{"getTFPrefix", (PyCFunction)getTFPrefix, METH_VARARGS}, {NULL, NULL} }; static PyMethodDef module_methods[] = { // {"Transformer", mkTransformer, METH_VARARGS}, {0, 0, 0}, }; extern "C" void init_tf2() { PyObject *item, *m, *d; #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 = PyString_FromString("tf2.error"); tf2_connectivityexception = PyString_FromString("tf2.ConnectivityException"); tf2_lookupexception = PyString_FromString("tf2.LookupException"); tf2_extrapolationexception = PyString_FromString("tf2.ExtrapolationException"); tf2_invalidargumentexception = PyString_FromString("tf2.InvalidArgumentException"); tf2_timeoutexception = PyString_FromString("tf2.TimeoutException"); #endif pModulerospy = PyImport_Import(item= PyString_FromString("rospy")); Py_DECREF(item); pModulegeometrymsgs = PyImport_ImportModule("geometry_msgs.msg"); if(pModulegeometrymsgs == NULL) { printf("Cannot load geometry_msgs module"); return; } 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; m = Py_InitModule("_tf2", module_methods); PyModule_AddObject(m, "BufferCore", (PyObject *)&buffer_core_Type); 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); } ros-geometry-experimental-0.5.12/tf2_py/src/tf2_py/000077500000000000000000000000001256046557600221435ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_py/src/tf2_py/__init__.py000066400000000000000000000035021256046557600242540ustar00rootroot00000000000000#! /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 _tf2 import * ros-geometry-experimental-0.5.12/tf2_ros/000077500000000000000000000000001256046557600203245ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_ros/CHANGELOG.rst000066400000000000000000000222441256046557600223510ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2_ros/CMakeLists.txt000066400000000000000000000061261256046557600230710ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) 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} tf2_msgs_gencpp) 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 tf2_msgs_gencpp) 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 tf2_msgs_gencpp) 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 rules install(TARGETS ${PROJECT_NAME} ${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 tf2_msgs_gencpp) add_executable(${PROJECT_NAME}_test_time_reset EXCLUDE_FROM_ALL test/time_reset_test.cpp) add_dependencies(${PROJECT_NAME}_test_time_reset tf2_msgs_gencpp) 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} ) add_dependencies(tests ${PROJECT_NAME}_test_listener) add_dependencies(tests ${PROJECT_NAME}_test_time_reset) add_rostest(test/transform_listener_unittest.launch) add_rostest(test/transform_listener_time_reset_test.launch) endif() ros-geometry-experimental-0.5.12/tf2_ros/conf.py000066400000000000000000000150361256046557600216300ustar00rootroot00000000000000# -*- 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 roslib roslib.load_manifest('tf') 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 } ros-geometry-experimental-0.5.12/tf2_ros/include/000077500000000000000000000000001256046557600217475ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/000077500000000000000000000000001256046557600233255ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/buffer.h000066400000000000000000000146441256046557600247600ustar00rootroot00000000000000/* * 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 { // extend the BufferInterface class and BufferCore class 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 view_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 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 seperate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance."; } // namespace #endif // TF2_ROS_BUFFER_H ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/buffer_client.h000066400000000000000000000150761256046557600263160ustar00rootroot00000000000000/********************************************************************* * * 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 { 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 The frequency in Hz to check whether the BufferServer has completed a request * \param timeout_paddind 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; 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 ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/buffer_interface.h000066400000000000000000000166531256046557600270020ustar00rootroot00000000000000/* * 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 { // extend the TFCore class and the TFCpp class 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; // Transform, simple api, with pre-allocation 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; } // transform, simple api, no pre-allocation 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); } //transform, simple api, different types, pre-allocation 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; } // Transform, advanced api, with pre-allocation 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; } // transform, advanced api, no pre-allocation 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); } // Transform, advanced api, different types, with pre-allocation 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 ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/buffer_server.h000066400000000000000000000056121256046557600263410ustar00rootroot00000000000000/********************************************************************* * * 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 { class BufferServer { private: typedef actionlib::ActionServer LookupTransformServer; typedef LookupTransformServer::GoalHandle GoalHandle; struct GoalInfo { GoalHandle handle; ros::Time end_time; }; public: BufferServer(const Buffer& buffer, const std::string& ns, bool auto_start = true, ros::Duration check_period = ros::Duration(0.01)); 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 ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/message_filter.h000066400000000000000000000601621256046557600264740ustar00rootroot00000000000000/* * 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 tf The tf::Transformer this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param nh The NodeHandle 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 tf The tf::Transformer this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param nh The NodeHandle 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 tf The tf::Transformer this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param 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 tf The tf::Transformer this filter should use * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param 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(); 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() ? 0 : 1); 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() ? 0 : 1); } /** * \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; 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 { // 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_) { // While we're using the reference keep a shared lock on the messages. boost::shared_lock< boost::shared_mutex > shared_lock(messages_mutex_); ++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); // Unlock the shared lock and get a unique lock. Upgradeable lock is used in transformable. // There can only be one upgrade lock. It's important the cancelTransformableRequest not deadlock with transformable. // They both require the transformable_requests_mutex_ in BufferCore. shared_lock.unlock(); // There is a very slight race condition if an older message arrives in this gap. boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_); messages_.pop_front(); --message_count_; } // Add the message to our list info.event = evt; // Lock access to the messages_ before modifying them. boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_); 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 ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/static_transform_broadcaster.h000066400000000000000000000054631256046557600314410ustar00rootroot00000000000000/* * 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); /** \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 ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/transform_broadcaster.h000066400000000000000000000061411256046557600300640ustar00rootroot00000000000000/* * 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 ros-geometry-experimental-0.5.12/tf2_ros/include/tf2_ros/transform_listener.h000066400000000000000000000061461256046557600274250ustar00rootroot00000000000000/* * 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{ class TransformListener { public: /**@brief Constructor for transform listener */ TransformListener(tf2::BufferCore& buffer, bool spin_thread = true); TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread = true); ~TransformListener(); private: /// Initialize this transform listener, subscribing, advertising services, etc. void init(); void initWithThread(); /// Callback function for ros message subscriptoin 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::Time last_update_; void dedicatedListenerThread() { while (using_dedicated_thread_) { tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01)); } }; }; } #endif //TF_TRANSFORMLISTENER_H ros-geometry-experimental-0.5.12/tf2_ros/index.rst000066400000000000000000000002771256046557600221730ustar00rootroot00000000000000tf2_ros ===== This is the Python API reference of the tf2_ros package. .. toctree:: :maxdepth: 2 tf2_ros Indices and tables ================== * :ref:`genindex` * :ref:`search` ros-geometry-experimental-0.5.12/tf2_ros/mainpage.dox000066400000000000000000000011111256046557600226130ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_cpp is ... \section codeapi Code API */ ros-geometry-experimental-0.5.12/tf2_ros/package.xml000066400000000000000000000026071256046557600224460ustar00rootroot00000000000000 tf2_ros 0.5.12 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 actionlib actionlib_msgs geometry_msgs message_filters roscpp rosgraph rospy std_msgs tf2 tf2_msgs tf2_py rostestg ros-geometry-experimental-0.5.12/tf2_ros/rosdoc.yaml000066400000000000000000000002731256046557600225030ustar00rootroot00000000000000 - builder: rosmake - builder: doxygen name: C++ API output_dir: c++ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' - builder: sphinx name: Python API output_dir: python ros-geometry-experimental-0.5.12/tf2_ros/setup.py000066400000000000000000000005041256046557600220350ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core 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) ros-geometry-experimental-0.5.12/tf2_ros/src/000077500000000000000000000000001256046557600211135ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_ros/src/buffer.cpp000066400000000000000000000146141256046557600230760ustar00rootroot00000000000000/* * 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 { 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 { if (!checkAndErrorDedicatedThreadPresent(errstr)) return false; // poll for transform if timeout is set ros::Time start_time = now_fallback_to_wall(); 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(ros::Duration(0.01)); } 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 { if (!checkAndErrorDedicatedThreadPresent(errstr)) return false; // poll for transform if timeout is set ros::Time start_time = now_fallback_to_wall(); 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(ros::Duration(0.01)); } 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; } } ros-geometry-experimental-0.5.12/tf2_ros/src/buffer_client.cpp000066400000000000000000000142511256046557600244310ustar00rootroot00000000000000/********************************************************************* * * 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); ros::Rate r(check_frequency_); bool timed_out = false; ros::Time start_time = ros::Time::now(); while(ros::ok() && !client_.getState().isDone() && !timed_out) { timed_out = ros::Time::now() > start_time + goal.timeout + timeout_padding_; r.sleep(); } //this shouldn't happen, but could in rare cases where the server hangs if(timed_out) { //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 = 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 = ex.what(); return false; } } }; ros-geometry-experimental-0.5.12/tf2_ros/src/buffer_server.cpp000066400000000000000000000170601256046557600244620ustar00rootroot00000000000000/********************************************************************* * * 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) { 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 it = active_goals_.erase(it); info.handle.setSucceeded(result); } 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) { it = active_goals_.erase(it); info.handle.setCanceled(); 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(); } }; ros-geometry-experimental-0.5.12/tf2_ros/src/buffer_server_main.cpp000066400000000000000000000050131256046557600254610ustar00rootroot00000000000000/********************************************************************* * * 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 class MyClass { public: MyClass() {} MyClass(double d) {} }; class BlankClass { public: BlankClass() {} }; 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); // WIM: this works fine: tf2_ros::Buffer buffer_core(ros::Duration(buffer_size+0)); // WTF?? tf2_ros::TransformListener listener(buffer_core); tf2_ros::BufferServer buffer_server(buffer_core, "tf2_buffer_server", false); buffer_server.start(); // But you should probably read this instead: // http://www.informit.com/guides/content.aspx?g=cplusplus&seqNum=439 ros::spin(); } ros-geometry-experimental-0.5.12/tf2_ros/src/static_transform_broadcaster.cpp000066400000000000000000000053651256046557600275630ustar00rootroot00000000000000/* * 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" namespace tf2_ros { StaticTransformBroadcaster::StaticTransformBroadcaster() { publisher_ = node_.advertise("/tf_static", 100, true); }; void StaticTransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf) { std::vector v1; v1.push_back(msgtf); sendTransform(v1); } void StaticTransformBroadcaster::sendTransform(const std::vector & msgtf) { for (std::vector::const_iterator it_in = msgtf.begin(); it_in != msgtf.end(); ++it_in) { bool match_found = false; for (std::vector::iterator it_msg = net_message_.transforms.begin(); it_msg != net_message_.transforms.end(); ++it_msg) { if (it_in->child_frame_id == it_msg->child_frame_id) { *it_msg = *it_in; match_found = true; break; } } if (! match_found) net_message_.transforms.push_back(*it_in); } publisher_.publish(net_message_); } } ros-geometry-experimental-0.5.12/tf2_ros/src/static_transform_broadcaster_program.cpp000066400000000000000000000104251256046557600313030ustar00rootroot00000000000000/* * 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" int main(int argc, char ** argv) { //Initialize ROS ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName); tf2_ros::StaticTransformBroadcaster broadcaster; if(argc == 10) { if (strcmp(argv[8], argv[9]) == 0) { ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]); return 1; } geometry_msgs::TransformStamped msg; 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]; 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; } else if (argc == 9) { if (strcmp(argv[7], argv[8]) == 0) { ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]); return 1; } geometry_msgs::TransformStamped msg; 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]; 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; } 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("\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; } }; ros-geometry-experimental-0.5.12/tf2_ros/src/tf2_ros/000077500000000000000000000000001256046557600224715ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_ros/src/tf2_ros/__init__.py000066400000000000000000000037311256046557600246060ustar00rootroot00000000000000#! /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 tf2_py import * from buffer_interface import * from buffer import * from buffer_client import * from transform_listener import * from transform_broadcaster import * ros-geometry-experimental-0.5.12/tf2_ros/src/tf2_ros/buffer.py000066400000000000000000000114101256046557600243110ustar00rootroot00000000000000# 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 roslib; roslib.load_manifest('tf2_ros') 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): def __init__(self, cache_time = None, debug = True): 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()) # lookup, simple api def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): self.can_transform(target_frame, source_frame, time, timeout) return self.lookup_transform_core(target_frame, source_frame, time) # lookup, advanced api def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): 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) # can, simple api def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0), return_debug_tuple=False): 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] # can, advanced api def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0), return_debug_tuple=False): 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] ros-geometry-experimental-0.5.12/tf2_ros/src/tf2_ros/buffer_client.py000066400000000000000000000146411256046557600256600ustar00rootroot00000000000000#! /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 #*********************************************************** PKG = 'tf2_ros' import roslib; roslib.load_manifest(PKG) 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): def __init__(self, ns, check_frequency = 10.0, timeout_padding = rospy.Duration.from_sec(2.0)): tf2_ros.BufferInterface.__init__(self) self.client = actionlib.SimpleActionClient(ns, LookupTransformAction) self.check_frequency = check_frequency self.timeout_padding = timeout_padding def wait_for_server(self, timeout = rospy.Duration()): return self.client.wait_for_server(timeout) # lookup, simple api def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): 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)): 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)): 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)): try: self.lookup_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout) return True except tf2.TransformException: return False def __is_done(self, state): if state == GoalStatus.REJECTED or state == GoalStatus.ABORTED or \ state == GoalStatus.RECALLED or state == GoalStatus.PREEMPTED or \ state == GoalStatus.SUCCEEDED or state == GoalStatus.LOST: return True return False def __process_goal(self, goal): self.client.send_goal(goal) r = rospy.Rate(self.check_frequency) timed_out = False start_time = rospy.Time.now() while not rospy.is_shutdown() and not self.__is_done(self.client.get_state()) and not timed_out: if rospy.Time.now() > start_time + goal.timeout + self.timeout_padding: timed_out = True r.sleep() #This shouldn't happen, but could in rare cases where the server hangs if timed_out: self.client.cancel_goal() 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 result == None or result.error == None: raise tf2.TransformException("The BufferServer returned None for result or 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 ros-geometry-experimental-0.5.12/tf2_ros/src/tf2_ros/buffer_interface.py000066400000000000000000000140451256046557600263400ustar00rootroot00000000000000# 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 roslib; roslib.load_manifest('tf2_ros') import rospy import tf2_py as tf2 import tf2_ros from copy import deepcopy from std_msgs.msg import Header class BufferInterface: 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): 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 new_type == None: 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): 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 new_type == None: return res return convert(res, new_type) # lookup, simple api def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): raise NotImplementedException() # lookup, advanced api def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): raise NotImplementedException() # can, simple api def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): 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)): raise NotImplementedException() def Stamped(obj, stamp, frame_id): obj.header = Header(frame_id=frame_id, stamp=stamp) return obj class TypeException(Exception): def __init__(self, errstr): self.errstr = errstr class NotImplementedException(Exception): 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)) ros-geometry-experimental-0.5.12/tf2_ros/src/tf2_ros/transform_broadcaster.py000066400000000000000000000041231256046557600274270ustar00rootroot00000000000000# 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): if not isinstance(transform, list): transform = [transform] self.pub_tf.publish(TFMessage(transform)) ros-geometry-experimental-0.5.12/tf2_ros/src/tf2_ros/transform_listener.py000066400000000000000000000052031256046557600267630ustar00rootroot00000000000000# 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 roslib; roslib.load_manifest('tf2_ros') import rospy import rospy import tf2_ros import threading from tf2_msgs.msg import TFMessage class TransformListener(): def __init__(self, buffer): self.listenerthread = TransformListenerThread(buffer) self.listenerthread.setDaemon(True) self.listenerthread.start() class TransformListenerThread(threading.Thread): def __init__(self, buffer): self.buffer = buffer threading.Thread.__init__(self) def run(self): rospy.Subscriber("/tf", TFMessage, self.callback) rospy.Subscriber("/tf_static", TFMessage, self.static_callback) rospy.spin() def callback(self, data): who = data._connection_header.get('callerid', "default_authority") for transform in data.transforms: self.buffer.set_transform(transform, who) def static_callback(self, data): who = data._connection_header.get('callerid', "default_authority") for transform in data.transforms: self.buffer.set_transform_static(transform, who) ros-geometry-experimental-0.5.12/tf2_ros/src/transform_broadcaster.cpp000066400000000000000000000045411256046557600262070ustar00rootroot00000000000000/* * 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); } } ros-geometry-experimental-0.5.12/tf2_ros/src/transform_listener.cpp000066400000000000000000000115471256046557600255470ustar00rootroot00000000000000/* * 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): dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false) { if (spin_thread) initWithThread(); else init(); } TransformListener::TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread) : dedicated_listener_thread_(NULL) , node_(nh) , buffer_(buffer) , using_dedicated_thread_(false) { 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)); ///\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 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("Detected jump back in time. 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()); } } }; ros-geometry-experimental-0.5.12/tf2_ros/test/000077500000000000000000000000001256046557600213035ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_ros/test/listener_unittest.cpp000066400000000000000000000051471256046557600256020ustar00rootroot00000000000000/* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include void seed_rand() { //Seed random number generator with current microseond count timeval temp_time_struct; gettimeofday(&temp_time_struct,NULL); srand(temp_time_struct.tv_usec); }; void generate_rand_vectors(double scale, uint64_t runs, std::vector& xvalues, std::vector& yvalues, std::vector&zvalues) { seed_rand(); for ( uint64_t i = 0; i < runs ; i++ ) { xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX; } } using namespace tf2; TEST(tf2_ros_transform, transform_listener) { tf2_ros::Buffer buffer; tf2_ros::TransformListener tfl(buffer); } int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "transform_listener_unittest"); return RUN_ALL_TESTS(); } ros-geometry-experimental-0.5.12/tf2_ros/test/time_reset_test.cpp000066400000000000000000000064041256046557600252120ustar00rootroot00000000000000/* * 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 using namespace tf2; 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 ros::spinOnce(); sleep(1); // 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 ros::spinOnce(); sleep(1); //Send anoterh 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 ros::spinOnce(); sleep(1); //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(); } ros-geometry-experimental-0.5.12/tf2_ros/test/transform_listener_time_reset_test.launch000066400000000000000000000002461256046557600317000ustar00rootroot00000000000000 ros-geometry-experimental-0.5.12/tf2_ros/test/transform_listener_unittest.launch000066400000000000000000000001601256046557600303530ustar00rootroot00000000000000 ros-geometry-experimental-0.5.12/tf2_ros/tf2_ros.rst000066400000000000000000000151201256046557600224330ustar00rootroot00000000000000tf2 ---------------- .. currentmodule:: tf2 .. exception:: 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:: ConnectivityException subclass of :exc:`TransformException`. Raised when that the fixed_frame tree is not connected between the frames requested. .. exception:: 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:: ExtrapolationException subclass of :exc:`TransformException` Raised when a tf method would have required extrapolation beyond current limits. .. exception:: 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. BufferCore ----------- .. class:: tf2.BufferCore(cache_time = rospy.Duration(10)) :param cache_time: how long the buffer should retain transformation information in the past. The BufferCore object is the core of tf2. It maintains a time-varying graph of transforms, and permits asynchronous graph modification and queries: .. doctest:: >>> import rospy >>> import tf2 >>> import geometry_msgs.msg >>> t = tf2.BufferCore(rospy.Duration(10.0)) >>> t.getFrameStrings() [] >>> m = geometry_msgs.msg.TransformStamped() >>> m.header.frame_id = 'THISFRAME' >>> m.child_frame_id = 'CHILD' >>> m.transform.translation.x = 2.71828183 >>> m.transform.rotation.w = 1.0 >>> t.setTransform(m) >>> t.getFrameStrings() ['/CHILD', '/THISFRAME'] >>> t.lookupTransform('THISFRAME', 'CHILD', rospy.Time(0)) ((2.71828183, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) >>> t.lookupTransform('CHILD', 'THISFRAME', rospy.Time(0)) ((-2.71828183, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)) The transformer refers to frames using strings, and represents transformations using translation (x, y, z) and quaternions (x, y, z, w) expressed as Python a :class:`tuple`. Transformer also does not mandate any particular linear algebra library. Transformer does not handle ROS messages directly; the only ROS type it uses is `rospy.Time() `_. To use tf with ROS messages, see :class:`TransformerROS` and :class:`TransformListener`. .. method:: allFramesAsYAML() -> string Returns a string representing all frames, intended for debugging tools. .. method:: allFramesAsString() -> string Returns a human-readable string representing all frames .. method:: setTransform(transform, authority = "") :param transform: transform object, see below :param authority: string giving authority for this transformation. Adds a new transform to the Transformer graph. transform is an object with the following structure:: header stamp time stamp, rospy.Time frame_id string, parent frame child_frame_id string, child frame transform translation x float y float z float rotation x float y float z float w float These members exactly match those of a ROS TransformStamped message. .. method:: canTransform(target_frame, source_frame, time) -> bool :param target_frame: transformation target frame in tf, string :param source_frame: transformation source frame in tf, string :param time: time of the transformation, use ``rospy.Time()`` to indicate present time. Returns True if the Transformer can determine the transform from source_frame to target_frame at time. .. method:: canTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame) -> bool Extended version of :meth:`canTransform`. :param target_time: The time at which to transform into the target_frame. :param source_time: The time at which to transform from the source frame. :param fixed_frame: The frame in which to jump from source_time to target_time. .. method:: clear() -> None Clear all transformations from the buffer. .. method:: lookupTransform(target_frame, source_frame, time) -> geometry_msgs.msg.TransformStamped :param target_frame: transformation target frame in tf, string :param source_frame: transformation source frame in tf, string :param time: time of the transformation, use ``rospy.Time()`` to indicate most recent common time. :returns: position as a translation (x, y, z) and orientation as a quaternion (x, y, z, w) :raises: :exc:`tf2.ConnectivityException`, :exc:`tf2.LookupException`, or :exc:`tf2.ExtrapolationException`, or :exc:`tf2.InvalidArgumentException` Returns the transform from source_frame to target_frame at time. Raises one of the exceptions if the transformation is not possible. Note that a time of zero means latest common time, so:: t.lookupTransform("a", "b", rospy.Time()) will return the transform from "a" to "b" at the latest time available in all transforms between "a" and "b". .. method:: lookupTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame) -> geometry_msgs.msg.TransformStamped :param target_frame: transformation target frame in tf, string :param target_time: time of transformation in target_frame, a :class:`rospy.Time` :param source_frame: transformation source frame in tf, string :param source_time: time of transformation in target_frame, a :class:`rospy.Time` :param fixed_frame: reference frame common to both target_frame and source_frame. :raises: :exc:`tf2.ConnectivityException`, :exc:`tf2.LookupException`, or :exc:`tf2.ExtrapolationException`, or :exc:`tf2.InvalidArgumentException` Extended version of :meth:`lookupTransform`. ros-geometry-experimental-0.5.12/tf2_sensor_msgs/000077500000000000000000000000001256046557600220635ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_sensor_msgs/CHANGELOG.rst000066400000000000000000000011401256046557600241000ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_sensor_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2_sensor_msgs/CMakeLists.txt000066400000000000000000000016761256046557600246350ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(tf2_sensor_msgs) find_package(catkin REQUIRED COMPONENTS cmake_modules sensor_msgs tf2_ros tf2) find_package(Boost COMPONENTS thread REQUIRED) find_package(Eigen) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS sensor_msgs tf2_ros tf2 DEPENDS Eigen ) include_directories(include ${catkin_INCLUDE_DIRS} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) catkin_python_setup() if(CATKIN_ENABLE_TESTING) find_package(catkin REQUIRED COMPONENTS sensor_msgs rostest tf2_ros tf2) include_directories(${EIGEN_INCLUDE_DIRS}) 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() ros-geometry-experimental-0.5.12/tf2_sensor_msgs/include/000077500000000000000000000000001256046557600235065ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_sensor_msgs/include/tf2_sensor_msgs/000077500000000000000000000000001256046557600266235ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h000066400000000000000000000074031256046557600321150ustar00rootroot00000000000000/* * 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 **/ /********************/ // method to extract timestamp from object template <> inline const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;} // method to extract frame id from object 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 ros-geometry-experimental-0.5.12/tf2_sensor_msgs/package.xml000066400000000000000000000015511256046557600242020ustar00rootroot00000000000000 tf2_sensor_msgs 0.5.12 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_ros tf2 cmake_modules eigen sensor_msgs tf2_ros tf2 rostest ros-geometry-experimental-0.5.12/tf2_sensor_msgs/setup.py000066400000000000000000000004361256046557600236000ustar00rootroot00000000000000#!/usr/bin/env python from distutils.core 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) ros-geometry-experimental-0.5.12/tf2_sensor_msgs/src/000077500000000000000000000000001256046557600226525ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_sensor_msgs/src/tf2_sensor_msgs/000077500000000000000000000000001256046557600257675ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py000066400000000000000000000000401256046557600300720ustar00rootroot00000000000000from tf2_geometry_msgs import * ros-geometry-experimental-0.5.12/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py000066400000000000000000000053161256046557600314630ustar00rootroot00000000000000# 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 import PyKDL import rospy import tf2_ros from sensor_msgs.point_cloud2 import read_cloud 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): res = cloud.copy() t_kdl = transform_to_kdl(transform) for p_in, p_out in [ read_cloud(cloud), read_cloud(res) ]: p = t_kdl * PyKDL.Vector(p_in.x, p_in.y, p_in.z) p_out.x = p[0] p_out.y = p[1] p_out.z = p[2] res.header = transform.header return res tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud) ros-geometry-experimental-0.5.12/tf2_sensor_msgs/test/000077500000000000000000000000001256046557600230425ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_sensor_msgs/test/test.launch000066400000000000000000000002011256046557600252060ustar00rootroot00000000000000 ros-geometry-experimental-0.5.12/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp000066400000000000000000000077071256046557600277350ustar00rootroot00000000000000/* * 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"); bool ret = RUN_ALL_TESTS(); delete tf_buffer; return ret; } ros-geometry-experimental-0.5.12/tf2_tools/000077500000000000000000000000001256046557600206615ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_tools/CHANGELOG.rst000066400000000000000000000056301256046557600227060ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package tf2_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 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 ros-geometry-experimental-0.5.12/tf2_tools/CMakeLists.txt000066400000000000000000000006111256046557600234170ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) 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 DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) ros-geometry-experimental-0.5.12/tf2_tools/mainpage.dox000066400000000000000000000011131256046557600231520ustar00rootroot00000000000000/** \mainpage \htmlinclude manifest.html \b tf2_tools is ... \section codeapi Code API */ ros-geometry-experimental-0.5.12/tf2_tools/package.xml000066400000000000000000000011111256046557600227700ustar00rootroot00000000000000 tf2_tools 0.5.12 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 ros-geometry-experimental-0.5.12/tf2_tools/scripts/000077500000000000000000000000001256046557600223505ustar00rootroot00000000000000ros-geometry-experimental-0.5.12/tf2_tools/scripts/view_frames.py000077500000000000000000000065031256046557600252400ustar00rootroot00000000000000#!/usr/bin/python # 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 roslib; roslib.load_manifest('tf2_tools') 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.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']+'" -> "'+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()