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

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

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

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

& t) + { + return t.frame_id_; + } + +/** Function that converts from one type to a ROS message type. It has to be + * implemented by each data type in tf2_* (except ROS messages) as it is + * used in the "convert" function. + * \param a an object of whatever type + * \return the conversion as a ROS message + */ +template + B toMsg(const A& a); + +/** Function that converts from a ROS message type to another type. It has to be + * implemented by each data type in tf2_* (except ROS messages) as it is used + * in the "convert" function. + * \param a a ROS message to convert from + * \param b the object to convert to + */ +template + void fromMsg(const A&, B& b); + +/** Function that converts any type to any type (messages or not). + * Matching toMsg and from Msg conversion functions need to exist. + * If they don't exist or do not apply (for example, if your two + * classes are ROS messages), just write a specialization of the function. + * \param a an object to convert from + * \param b the object to convert to + */ +template + void convert(const A& a, B& b) + { + //printf("In double type convert\n"); + impl::Converter::value, ros::message_traits::IsMessage::value>::convert(a, b); + } + +template + void convert(const A& a1, A& a2) + { + //printf("In single type convert\n"); + if(&a1 != &a2) + a2 = a1; + } + + +} + +#endif //TF2_CONVERT_H diff --git a/src/geometry2/tf2/include/tf2/exceptions.h b/src/geometry2/tf2/include/tf2/exceptions.h new file mode 100644 index 0000000..662fa38 --- /dev/null +++ b/src/geometry2/tf2/include/tf2/exceptions.h @@ -0,0 +1,110 @@ +/* + * 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 diff --git a/src/geometry2/tf2/include/tf2/impl/convert.h b/src/geometry2/tf2/include/tf2/impl/convert.h new file mode 100644 index 0000000..6b68ccd --- /dev/null +++ b/src/geometry2/tf2/include/tf2/impl/convert.h @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2013, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TF2_IMPL_CONVERT_H +#define TF2_IMPL_CONVERT_H + +namespace tf2 { +namespace impl { + +template +class Converter { +public: + template + static void convert(const A& a, B& b); +}; + +// The case where both A and B are messages should not happen: if you have two +// messages that are interchangeable, well, that's against the ROS purpose: +// only use one type. Worst comes to worst, specialize the original convert +// function for your types. +// if B == A, the templated version of convert with only one argument will be +// used. +// +//template <> +//template +//inline void Converter::convert(const A& a, B& b); + +template <> +template +inline void Converter::convert(const A& a, B& b) +{ +#ifdef _MSC_VER + tf2::fromMsg(a, b); +#else + fromMsg(a, b); +#endif +} + +template <> +template +inline void Converter::convert(const A& a, B& b) +{ +#ifdef _MSC_VER + b = tf2::toMsg(a); +#else + b = toMsg(a); +#endif +} + +template <> +template +inline void Converter::convert(const A& a, B& b) +{ +#ifdef _MSC_VER + tf2::fromMsg(tf2::toMsg(a), b); +#else + fromMsg(toMsg(a), b); +#endif +} + +} +} + +#endif //TF2_IMPL_CONVERT_H diff --git a/src/geometry2/tf2/include/tf2/impl/utils.h b/src/geometry2/tf2/include/tf2/impl/utils.h new file mode 100644 index 0000000..5b545e5 --- /dev/null +++ b/src/geometry2/tf2/include/tf2/impl/utils.h @@ -0,0 +1,153 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TF2_IMPL_UTILS_H +#define TF2_IMPL_UTILS_H + +#include +#include +#include + +namespace tf2 { +namespace impl { + +/** Function needed for the generalization of toQuaternion + * \param q a tf2::Quaternion + * \return a copy of the same quaternion + */ +inline +tf2::Quaternion toQuaternion(const tf2::Quaternion& q) { + return q; + } + +/** Function needed for the generalization of toQuaternion + * \param q a geometry_msgs::Quaternion + * \return a copy of the same quaternion as a tf2::Quaternion + */ +inline +tf2::Quaternion toQuaternion(const geometry_msgs::Quaternion& q) { + tf2::Quaternion res; + fromMsg(q, res); + return res; + } + +/** Function needed for the generalization of toQuaternion + * \param q a geometry_msgs::QuaternionStamped + * \return a copy of the same quaternion as a tf2::Quaternion + */ +inline +tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) { + tf2::Quaternion res; + fromMsg(q.quaternion, res); + return res; + } + +/** Function needed for the generalization of toQuaternion + * \param t some tf2::Stamped object + * \return a copy of the same quaternion as a tf2::Quaternion + */ +template + tf2::Quaternion toQuaternion(const tf2::Stamped& t) { + geometry_msgs::QuaternionStamped q = toMsg(t); + return toQuaternion(q); + } + +/** Generic version of toQuaternion. It tries to convert the argument + * to a geometry_msgs::Quaternion + * \param t some object + * \return a copy of the same quaternion as a tf2::Quaternion + */ +template + tf2::Quaternion toQuaternion(const T& t) { + geometry_msgs::Quaternion q = toMsg(t); + return toQuaternion(q); + } + +/** The code below is blantantly copied from urdfdom_headers + * only the normalization has been added. + * It computes the Euler roll, pitch yaw from a tf2::Quaternion + * It is equivalent to tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll); + * \param q a tf2::Quaternion + * \param yaw the computed yaw + * \param pitch the computed pitch + * \param roll the computed roll + */ +inline +void getEulerYPR(const tf2::Quaternion& q, double &yaw, double &pitch, double &roll) +{ + double sqw; + double sqx; + double sqy; + double sqz; + + sqx = q.x() * q.x(); + sqy = q.y() * q.y(); + sqz = q.z() * q.z(); + sqw = q.w() * q.w(); + + // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ + double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */ + if (sarg <= -0.99999) { + pitch = -0.5*M_PI; + roll = 0; + yaw = -2 * atan2(q.y(), q.x()); + } else if (sarg >= 0.99999) { + pitch = 0.5*M_PI; + roll = 0; + yaw = 2 * atan2(q.y(), q.x()); + } else { + pitch = asin(sarg); + roll = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz); + yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz); + } +}; + +/** The code below is a simplified version of getEulerRPY that only + * returns the yaw. It is mostly useful in navigation where only yaw + * matters + * \param q a tf2::Quaternion + * \return the computed yaw + */ +inline +double getYaw(const tf2::Quaternion& q) +{ + double yaw; + + double sqw; + double sqx; + double sqy; + double sqz; + + sqx = q.x() * q.x(); + sqy = q.y() * q.y(); + sqz = q.z() * q.z(); + sqw = q.w() * q.w(); + + // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ + double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */ + + if (sarg <= -0.99999) { + yaw = -2 * atan2(q.y(), q.x()); + } else if (sarg >= 0.99999) { + yaw = 2 * atan2(q.y(), q.x()); + } else { + yaw = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz); + } + return yaw; +}; + +} +} + +#endif //TF2_IMPL_UTILS_H diff --git a/src/geometry2/tf2/include/tf2/time_cache.h b/src/geometry2/tf2/include/tf2/time_cache.h new file mode 100644 index 0000000..8ce9258 --- /dev/null +++ b/src/geometry2/tf2/include/tf2/time_cache.h @@ -0,0 +1,162 @@ +/* + * 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 = 10ULL * 1000000000LL; //!< default value of 10 seconds storage + + TimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME)); + + + /// Virtual methods + + virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); + virtual bool insertData(const TransformStorage& new_data); + virtual void clearList(); + virtual CompactFrameID getParent(ros::Time time, std::string* error_str); + virtual P_TimeAndFrameID getLatestTimeAndParent(); + + /// Debugging information methods + virtual unsigned int getListLength(); + virtual ros::Time getLatestTimestamp(); + virtual ros::Time getOldestTimestamp(); + + +private: + typedef std::deque L_TransformStorage; + L_TransformStorage storage_; + + ros::Duration max_storage_time_; + + + /// A helper function for getData + //Assumes storage is already locked for it + inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str); + + inline void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output); + + + void pruneList(); + + + +}; + +class StaticCache : public TimeCacheInterface +{ + public: + /// Virtual methods + + virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception + virtual bool insertData(const TransformStorage& new_data); + 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 diff --git a/src/geometry2/tf2/include/tf2/transform_datatypes.h b/src/geometry2/tf2/include/tf2/transform_datatypes.h new file mode 100644 index 0000000..e5ea9f9 --- /dev/null +++ b/src/geometry2/tf2/include/tf2/transform_datatypes.h @@ -0,0 +1,74 @@ +/* + * 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 diff --git a/src/geometry2/tf2/include/tf2/transform_storage.h b/src/geometry2/tf2/include/tf2/transform_storage.h new file mode 100644 index 0000000..7856bb8 --- /dev/null +++ b/src/geometry2/tf2/include/tf2/transform_storage.h @@ -0,0 +1,86 @@ +/* + * 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 + diff --git a/src/geometry2/tf2/include/tf2/utils.h b/src/geometry2/tf2/include/tf2/utils.h new file mode 100644 index 0000000..54805cc --- /dev/null +++ b/src/geometry2/tf2/include/tf2/utils.h @@ -0,0 +1,66 @@ +// 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 diff --git a/src/geometry2/tf2/index.rst b/src/geometry2/tf2/index.rst new file mode 100644 index 0000000..5001a33 --- /dev/null +++ b/src/geometry2/tf2/index.rst @@ -0,0 +1,15 @@ +tf2 +===== + +This is the Python API reference of the tf2 package. + +.. toctree:: + :maxdepth: 2 + + tf2 + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` diff --git a/src/geometry2/tf2/mainpage.dox b/src/geometry2/tf2/mainpage.dox new file mode 100644 index 0000000..2cba60c --- /dev/null +++ b/src/geometry2/tf2/mainpage.dox @@ -0,0 +1,36 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf2 is the second generation of the tf library. + +This library implements the interface defined by tf2::BufferCore. + +There is also a Python wrapper with the same API that class this library using CPython bindings. + +\section codeapi Code API + +The main interface is through the tf2::BufferCore interface. + +It uses the exceptions in exceptions.h and the Stamped datatype +in transform_datatypes.h. + +\section conversions Conversion Interface + +tf2 offers a templated conversion interface for external libraries to specify conversions between +tf2-specific data types and user-defined data types. Various templated functions in tf2_ros use the +conversion interface to apply transformations from the tf server to these custom datatypes. + +The conversion interface is defined in tf2/convert.h. + +Some packages that implement this interface: + +- tf2_bullet +- tf2_eigen +- tf2_geometry_msgs +- tf2_kdl +- tf2_sensor_msgs + +More documentation for the conversion interface is available on the ROS Wiki. + +*/ diff --git a/src/geometry2/tf2/package.xml b/src/geometry2/tf2/package.xml new file mode 100644 index 0000000..65bc8b7 --- /dev/null +++ b/src/geometry2/tf2/package.xml @@ -0,0 +1,34 @@ + + tf2 + 0.6.7 + + 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 + + + + diff --git a/src/geometry2/tf2/src/buffer_core.cpp b/src/geometry2/tf2/src/buffer_core.cpp new file mode 100644 index 0000000..75b827d --- /dev/null +++ b/src/geometry2/tf2/src/buffer_core.cpp @@ -0,0 +1,1656 @@ +/* + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Tully Foote */ + +#include "tf2/buffer_core.h" +#include "tf2/time_cache.h" +#include "tf2/exceptions.h" +#include "tf2_msgs/TF2Error.h" + +#include +#include +#include "tf2/LinearMath/Transform.h" +#include + +namespace tf2 +{ + +// Tolerance for acceptable quaternion normalization +static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3; + +/** \brief convert Transform msg to Transform */ +void transformMsgToTF2(const geometry_msgs::Transform& msg, tf2::Transform& tf2) +{tf2 = tf2::Transform(tf2::Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), tf2::Vector3(msg.translation.x, msg.translation.y, msg.translation.z));} + +/** \brief convert Transform to Transform msg*/ +void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::Transform& msg) +{ + msg.translation.x = tf2.getOrigin().x(); + msg.translation.y = tf2.getOrigin().y(); + msg.translation.z = tf2.getOrigin().z(); + msg.rotation.x = tf2.getRotation().x(); + msg.rotation.y = tf2.getRotation().y(); + msg.rotation.z = tf2.getRotation().z(); + msg.rotation.w = tf2.getRotation().w(); +} + +/** \brief convert Transform to Transform msg*/ +void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id) +{ + transformTF2ToMsg(tf2, msg.transform); + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + msg.child_frame_id = child_frame_id; +} + +void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::Transform& msg) +{ + msg.translation.x = pos.x(); + msg.translation.y = pos.y(); + msg.translation.z = pos.z(); + msg.rotation.x = orient.x(); + msg.rotation.y = orient.y(); + msg.rotation.z = orient.z(); + msg.rotation.w = orient.w(); +} + +void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id) +{ + transformTF2ToMsg(orient, pos, msg.transform); + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + msg.child_frame_id = child_frame_id; +} + +void setIdentity(geometry_msgs::Transform& tx) +{ + tx.translation.x = 0; + tx.translation.y = 0; + tx.translation.z = 0; + tx.rotation.x = 0; + tx.rotation.y = 0; + tx.rotation.z = 0; + tx.rotation.w = 1; +} + +bool startsWithSlash(const std::string& frame_id) +{ + if (frame_id.size() > 0) + if (frame_id[0] == '/') + return true; + return false; +} + +std::string stripSlash(const std::string& in) +{ + std::string out = in; + if (startsWithSlash(in)) + out.erase(0,1); + return out; +} + + +bool BufferCore::warnFrameId(const char* function_name_arg, const std::string& frame_id) const +{ + if (frame_id.size() == 0) + { + std::stringstream ss; + ss << "Invalid argument passed to "<< function_name_arg <<" in tf2 frame_ids cannot be empty"; + CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str()); + return true; + } + + if (startsWithSlash(frame_id)) + { + std::stringstream ss; + ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf2 frame_ids cannot start with a '/' like: "; + CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str()); + return true; + } + + return false; +} + +CompactFrameID BufferCore::validateFrameId(const char* function_name_arg, const std::string& frame_id) const +{ + if (frame_id.empty()) + { + std::stringstream ss; + ss << "Invalid argument passed to "<< function_name_arg <<" in tf2 frame_ids cannot be empty"; + throw tf2::InvalidArgumentException(ss.str().c_str()); + } + + if (startsWithSlash(frame_id)) + { + std::stringstream ss; + ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf2 frame_ids cannot start with a '/' like: "; + throw tf2::InvalidArgumentException(ss.str().c_str()); + } + + CompactFrameID id = lookupFrameNumber(frame_id); + if (id == 0) + { + std::stringstream ss; + ss << "\"" << frame_id << "\" passed to "<< function_name_arg <<" does not exist. "; + throw tf2::LookupException(ss.str().c_str()); + } + + return id; +} + +BufferCore::BufferCore(ros::Duration cache_time) +: cache_time_(cache_time) +, transformable_callbacks_counter_(0) +, transformable_requests_counter_(0) +, using_dedicated_thread_(false) +{ + frameIDs_["NO_PARENT"] = 0; + frames_.push_back(TimeCacheInterfacePtr()); + frameIDs_reverse.push_back("NO_PARENT"); +} + +BufferCore::~BufferCore() +{ + +} + +void BufferCore::clear() +{ + //old_tf_.clear(); + + + boost::mutex::scoped_lock lock(frame_mutex_); + if ( frames_.size() > 1 ) + { + for (std::vector::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it) + { + if (*cache_it) + (*cache_it)->clearList(); + } + } + +} + +bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_in, const std::string& authority, bool is_static) +{ + + /////BACKEARDS COMPATABILITY + /* tf::StampedTransform tf_transform; + tf::transformStampedMsgToTF(transform_in, tf_transform); + if (!old_tf_.setTransform(tf_transform, authority)) + { + printf("Warning old setTransform Failed but was not caught\n"); + }*/ + + /////// New implementation + geometry_msgs::TransformStamped stripped = transform_in; + stripped.header.frame_id = stripSlash(stripped.header.frame_id); + stripped.child_frame_id = stripSlash(stripped.child_frame_id); + + + bool error_exists = false; + if (stripped.child_frame_id == stripped.header.frame_id) + { + CONSOLE_BRIDGE_logError("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id \"%s\" because they are the same", authority.c_str(), stripped.child_frame_id.c_str()); + error_exists = true; + } + + if (stripped.child_frame_id == "") + { + CONSOLE_BRIDGE_logError("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str()); + error_exists = true; + } + + if (stripped.header.frame_id == "") + { + CONSOLE_BRIDGE_logError("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", stripped.child_frame_id.c_str(), authority.c_str()); + error_exists = true; + } + + if (std::isnan(stripped.transform.translation.x) || std::isnan(stripped.transform.translation.y) || std::isnan(stripped.transform.translation.z)|| + std::isnan(stripped.transform.rotation.x) || std::isnan(stripped.transform.rotation.y) || std::isnan(stripped.transform.rotation.z) || std::isnan(stripped.transform.rotation.w)) + { + CONSOLE_BRIDGE_logError("TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)", + stripped.child_frame_id.c_str(), authority.c_str(), + stripped.transform.translation.x, stripped.transform.translation.y, stripped.transform.translation.z, + stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w + ); + error_exists = true; + } + + bool valid = std::abs((stripped.transform.rotation.w * stripped.transform.rotation.w + + stripped.transform.rotation.x * stripped.transform.rotation.x + + stripped.transform.rotation.y * stripped.transform.rotation.y + + stripped.transform.rotation.z * stripped.transform.rotation.z) - 1.0f) < QUATERNION_NORMALIZATION_TOLERANCE; + + if (!valid) + { + CONSOLE_BRIDGE_logError("TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of an invalid quaternion in the transform (%f %f %f %f)", + stripped.child_frame_id.c_str(), authority.c_str(), + stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w); + error_exists = true; + } + + if (error_exists) + return false; + + { + boost::mutex::scoped_lock lock(frame_mutex_); + CompactFrameID frame_number = lookupOrInsertFrameNumber(stripped.child_frame_id); + TimeCacheInterfacePtr frame = getFrame(frame_number); + if (frame == NULL) + frame = allocateFrame(frame_number, is_static); + + if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number))) + { + frame_authority_[frame_number] = authority; + } + else + { + CONSOLE_BRIDGE_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) + { + // Use the walk we just did + frame_chain->swap(reverse_frame_chain); + // Reverse it before returning because this is the reverse walk. + std::reverse(frame_chain->begin(), frame_chain->end()); + } + return tf2_msgs::TF2Error::NO_ERROR; + } + + f.accum(false); + + frame = parent; + + ++depth; + if (depth > MAX_GRAPH_DEPTH) + { + if (error_string) + { + std::stringstream ss; + ss << "The tf tree is invalid because it contains a loop." << std::endl + << allFramesAsStringNoLock() << std::endl; + *error_string = ss.str(); + } + return tf2_msgs::TF2Error::LOOKUP_ERROR; + } + } + + if (frame != top_parent) + { + if (extrapolation_might_have_occurred) + { + if (error_string) + { + 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; + } + else if (frame_chain){ + // append top_parent to reverse_frame_chain for easier matching/trimming + reverse_frame_chain.push_back(frame); + } + + f.finalize(FullPath, time); + if (frame_chain) + { + // Pruning: Compare the chains starting at the parent (end) until they differ + int m = reverse_frame_chain.size()-1; + int n = frame_chain->size()-1; + for (; m >= 0 && n >= 0; --m, --n) + { + if ((*frame_chain)[n] != reverse_frame_chain[m]) + { + break; + } + } + // Erase all duplicate items from frame_chain + if (n > 0) + { + // N is offset by 1 and leave the common parent for this result + frame_chain->erase(frame_chain->begin() + (n + 2), frame_chain->end()); + } + if (m < reverse_frame_chain.size()) + { + for (int i = m; i >= 0; --i) + { + frame_chain->push_back(reverse_frame_chain[i]); + } + } + } + + return tf2_msgs::TF2Error::NO_ERROR; +} + + + +struct TransformAccum +{ + TransformAccum() + : source_to_top_quat(0.0, 0.0, 0.0, 1.0) + , source_to_top_vec(0.0, 0.0, 0.0) + , target_to_top_quat(0.0, 0.0, 0.0, 1.0) + , target_to_top_vec(0.0, 0.0, 0.0) + , result_quat(0.0, 0.0, 0.0, 1.0) + , result_vec(0.0, 0.0, 0.0) + { + } + + CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string) + { + if (!cache->getData(time, st, error_string)) + { + return 0; + } + + return st.frame_id_; + } + + void accum(bool source) + { + if (source) + { + source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_; + source_to_top_quat = st.rotation_ * source_to_top_quat; + } + else + { + target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_; + target_to_top_quat = st.rotation_ * target_to_top_quat; + } + } + + void finalize(WalkEnding end, ros::Time _time) + { + switch (end) + { + case Identity: + break; + case TargetParentOfSource: + result_vec = source_to_top_vec; + result_quat = source_to_top_quat; + break; + case SourceParentOfTarget: + { + tf2::Quaternion inv_target_quat = target_to_top_quat.inverse(); + tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); + result_vec = inv_target_vec; + result_quat = inv_target_quat; + break; + } + case FullPath: + { + tf2::Quaternion inv_target_quat = target_to_top_quat.inverse(); + tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec); + + result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec; + result_quat = inv_target_quat * source_to_top_quat; + } + break; + }; + + time = _time; + } + + TransformStorage st; + ros::Time time; + tf2::Quaternion source_to_top_quat; + tf2::Vector3 source_to_top_vec; + tf2::Quaternion target_to_top_quat; + tf2::Vector3 target_to_top_vec; + + tf2::Quaternion result_quat; + tf2::Vector3 result_vec; +}; + +geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame, + const std::string& source_frame, + const ros::Time& time) const +{ + boost::mutex::scoped_lock lock(frame_mutex_); + + if (target_frame == source_frame) { + geometry_msgs::TransformStamped identity; + identity.header.frame_id = target_frame; + identity.child_frame_id = source_frame; + identity.transform.rotation.w = 1; + + if (time == ros::Time()) + { + CompactFrameID target_id = lookupFrameNumber(target_frame); + TimeCacheInterfacePtr cache = getFrame(target_id); + if (cache) + identity.header.stamp = cache->getLatestTimestamp(); + else + identity.header.stamp = time; + } + else + identity.header.stamp = time; + + return identity; + } + + //Identify case does not need to be validated above + CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame); + CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame); + + std::string error_string; + TransformAccum accum; + int retval = walkToTopParent(accum, time, target_id, source_id, &error_string); + if (retval != tf2_msgs::TF2Error::NO_ERROR) + { + switch (retval) + { + case tf2_msgs::TF2Error::CONNECTIVITY_ERROR: + throw ConnectivityException(error_string); + case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR: + throw ExtrapolationException(error_string); + case tf2_msgs::TF2Error::LOOKUP_ERROR: + throw LookupException(error_string); + default: + CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + assert(0); + } + } + + geometry_msgs::TransformStamped output_transform; + transformTF2ToMsg(accum.result_quat, accum.result_vec, output_transform, accum.time, target_frame, source_frame); + return output_transform; +} + + +geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame, + const ros::Time& target_time, + const std::string& source_frame, + const ros::Time& source_time, + const std::string& fixed_frame) const +{ + validateFrameId("lookupTransform argument target_frame", target_frame); + validateFrameId("lookupTransform argument source_frame", source_frame); + validateFrameId("lookupTransform argument fixed_frame", fixed_frame); + + geometry_msgs::TransformStamped output; + geometry_msgs::TransformStamped temp1 = lookupTransform(fixed_frame, source_frame, source_time); + geometry_msgs::TransformStamped temp2 = lookupTransform(target_frame, fixed_frame, target_time); + + tf2::Transform tf1, tf2; + transformMsgToTF2(temp1.transform, tf1); + transformMsgToTF2(temp2.transform, tf2); + transformTF2ToMsg(tf2*tf1, output.transform); + output.header.stamp = temp2.header.stamp; + output.header.frame_id = target_frame; + output.child_frame_id = source_frame; + return output; +} + + + +/* +geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, + const std::string& observation_frame, + const ros::Time& time, + const ros::Duration& averaging_interval) const +{ + try + { + geometry_msgs::Twist t; + old_tf_.lookupTwist(tracking_frame, observation_frame, + time, averaging_interval, t); + return t; + } + catch (tf::LookupException& ex) + { + throw tf2::LookupException(ex.what()); + } + catch (tf::ConnectivityException& ex) + { + throw tf2::ConnectivityException(ex.what()); + } + catch (tf::ExtrapolationException& ex) + { + throw tf2::ExtrapolationException(ex.what()); + } + catch (tf::InvalidArgument& ex) + { + throw tf2::InvalidArgumentException(ex.what()); + } +} + +geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, + const std::string& observation_frame, + const std::string& reference_frame, + const tf2::Point & reference_point, + const std::string& reference_point_frame, + const ros::Time& time, + const ros::Duration& averaging_interval) const +{ + try{ + geometry_msgs::Twist t; + old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame, + time, averaging_interval, t); + return t; + } + catch (tf::LookupException& ex) + { + throw tf2::LookupException(ex.what()); + } + catch (tf::ConnectivityException& ex) + { + throw tf2::ConnectivityException(ex.what()); + } + catch (tf::ExtrapolationException& ex) + { + throw tf2::ExtrapolationException(ex.what()); + } + catch (tf::InvalidArgument& ex) + { + throw tf2::InvalidArgumentException(ex.what()); + } +} +*/ + +struct CanTransformAccum +{ + CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string) + { + return cache->getParent(time, error_string); + } + + void accum(bool source) + { + } + + void finalize(WalkEnding end, ros::Time _time) + { + } + + TransformStorage st; +}; + +bool BufferCore::canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, + const ros::Time& time, std::string* error_msg) const +{ + if (target_id == 0 || source_id == 0) + { + if (error_msg) + { + if (target_id == 0) + { + *error_msg += std::string("target_frame: " + lookupFrameString(target_id ) + " does not exist."); + } + if (source_id == 0) + { + if (target_id == 0) + { + *error_msg += std::string(" "); + } + *error_msg += std::string("source_frame: " + lookupFrameString(source_id) + " " + lookupFrameString(source_id ) + " does not exist."); + } + } + return false; + } + + if (target_id == source_id) + { + return true; + } + + CanTransformAccum accum; + if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf2_msgs::TF2Error::NO_ERROR) + { + return true; + } + + return false; +} + +bool BufferCore::canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, + const ros::Time& time, std::string* error_msg) const +{ + boost::mutex::scoped_lock lock(frame_mutex_); + return canTransformNoLock(target_id, source_id, time, error_msg); +} + +bool BufferCore::canTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, std::string* error_msg) const +{ + // Short circuit if target_frame == source_frame + if (target_frame == source_frame) + return true; + + if (warnFrameId("canTransform argument target_frame", target_frame)) + return false; + if (warnFrameId("canTransform argument source_frame", source_frame)) + return false; + + boost::mutex::scoped_lock lock(frame_mutex_); + + CompactFrameID target_id = lookupFrameNumber(target_frame); + CompactFrameID source_id = lookupFrameNumber(source_frame); + + if (target_id == 0 || source_id == 0) + { + if (error_msg) + { + if (target_id == 0) + { + *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist."); + } + if (source_id == 0) + { + if (target_id == 0) + { + *error_msg += std::string(" "); + } + *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist."); + } + } + return false; + } + return canTransformNoLock(target_id, source_id, time, error_msg); +} + +bool BufferCore::canTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, std::string* error_msg) const +{ + if (warnFrameId("canTransform argument target_frame", target_frame)) + return false; + if (warnFrameId("canTransform argument source_frame", source_frame)) + return false; + if (warnFrameId("canTransform argument fixed_frame", fixed_frame)) + return false; + + boost::mutex::scoped_lock lock(frame_mutex_); + CompactFrameID target_id = lookupFrameNumber(target_frame); + CompactFrameID source_id = lookupFrameNumber(source_frame); + CompactFrameID fixed_id = lookupFrameNumber(fixed_frame); + + if (target_id == 0 || source_id == 0 || fixed_id == 0) + { + if (error_msg) + { + if (target_id == 0) + { + *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist."); + } + if (source_id == 0) + { + if (target_id == 0) + { + *error_msg += std::string(" "); + } + *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist."); + } + if (source_id == 0) + { + if (target_id == 0 || source_id == 0) + { + *error_msg += std::string(" "); + } + *error_msg += std::string("fixed_frame: " + fixed_frame + "does not exist."); + } + } + return false; + } + return canTransformNoLock(target_id, fixed_id, target_time, error_msg) && canTransformNoLock(fixed_id, source_id, source_time, error_msg); +} + + +tf2::TimeCacheInterfacePtr BufferCore::getFrame(CompactFrameID frame_id) const +{ + if (frame_id >= frames_.size()) + return TimeCacheInterfacePtr(); + else + { + return frames_[frame_id]; + } +} + +CompactFrameID BufferCore::lookupFrameNumber(const std::string& frameid_str) const +{ + CompactFrameID retval; + M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str); + if (map_it == frameIDs_.end()) + { + retval = CompactFrameID(0); + } + else + retval = map_it->second; + return retval; +} + +CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string& frameid_str) +{ + CompactFrameID retval = 0; + M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str); + if (map_it == frameIDs_.end()) + { + retval = CompactFrameID(frames_.size()); + frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration + frameIDs_[frameid_str] = retval; + frameIDs_reverse.push_back(frameid_str); + } + else + retval = frameIDs_[frameid_str]; + + return retval; +} + +const std::string& BufferCore::lookupFrameString(CompactFrameID frame_id_num) const +{ + if (frame_id_num >= frameIDs_reverse.size()) + { + std::stringstream ss; + ss << "Reverse lookup of frame id " << frame_id_num << " failed!"; + throw tf2::LookupException(ss.str()); + } + else + return frameIDs_reverse[frame_id_num]; +} + +void BufferCore::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const +{ + if (!out) + { + return; + } + *out = std::string("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+ + lookupFrameString(source_frame)+"' because they are not part of the same tree."+ + "Tf has two or more unconnected trees."); +} + +std::string BufferCore::allFramesAsString() const +{ + boost::mutex::scoped_lock lock(frame_mutex_); + return this->allFramesAsStringNoLock(); +} + +std::string BufferCore::allFramesAsStringNoLock() const +{ + std::stringstream mstream; + + TransformStorage temp; + + // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) + + ///regular transforms + for (unsigned int counter = 1; counter < frames_.size(); counter ++) + { + TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter)); + if (frame_ptr == NULL) + continue; + CompactFrameID frame_id_num; + if( frame_ptr->getData(ros::Time(), temp)) + frame_id_num = temp.frame_id_; + else + { + frame_id_num = 0; + } + mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <getLatestTimestamp(); + else + time = ros::Time(); + return tf2_msgs::TF2Error::NO_ERROR; + } + + std::vector lct_cache; + + // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time + // in the target is a direct parent + CompactFrameID frame = source_id; + P_TimeAndFrameID temp; + uint32_t depth = 0; + ros::Time common_time = ros::TIME_MAX; + while (frame != 0) + { + TimeCacheInterfacePtr cache = getFrame(frame); + + if (!cache) + { + // There will be no cache for the very root of the tree + break; + } + + P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); + + if (latest.second == 0) + { + // Just break out here... there may still be a path from source -> target + break; + } + + if (!latest.first.isZero()) + { + common_time = std::min(latest.first, common_time); + } + + lct_cache.push_back(latest); + + frame = latest.second; + + // Early out... target frame is a direct parent of the source frame + if (frame == target_id) + { + time = common_time; + if (time == ros::TIME_MAX) + { + time = ros::Time(); + } + return tf2_msgs::TF2Error::NO_ERROR; + } + + ++depth; + if (depth > MAX_GRAPH_DEPTH) + { + if (error_string) + { + std::stringstream ss; + ss<<"The tf tree is invalid because it contains a loop." << std::endl + << allFramesAsStringNoLock() << std::endl; + *error_string = ss.str(); + } + return tf2_msgs::TF2Error::LOOKUP_ERROR; + } + } + + // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent + frame = target_id; + depth = 0; + common_time = ros::TIME_MAX; + CompactFrameID common_parent = 0; + while (true) + { + TimeCacheInterfacePtr cache = getFrame(frame); + + if (!cache) + { + break; + } + + P_TimeAndFrameID latest = cache->getLatestTimeAndParent(); + + if (latest.second == 0) + { + break; + } + + if (!latest.first.isZero()) + { + common_time = std::min(latest.first, common_time); + } + + std::vector::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second)); + if (it != lct_cache.end()) // found a common parent + { + common_parent = it->second; + break; + } + + frame = latest.second; + + // Early out... source frame is a direct parent of the target frame + if (frame == source_id) + { + time = common_time; + if (time == ros::TIME_MAX) + { + time = ros::Time(); + } + return tf2_msgs::TF2Error::NO_ERROR; + } + + ++depth; + if (depth > MAX_GRAPH_DEPTH) + { + if (error_string) + { + std::stringstream ss; + ss<<"The tf tree is invalid because it contains a loop." << std::endl + << allFramesAsStringNoLock() << std::endl; + *error_string = ss.str(); + } + return tf2_msgs::TF2Error::LOOKUP_ERROR; + } + } + + if (common_parent == 0) + { + createConnectivityErrorString(source_id, target_id, error_string); + return tf2_msgs::TF2Error::CONNECTIVITY_ERROR; + } + + // Loop through the source -> root list until we hit the common parent + { + std::vector::iterator it = lct_cache.begin(); + std::vector::iterator end = lct_cache.end(); + for (; it != end; ++it) + { + if (!it->first.isZero()) + { + common_time = std::min(common_time, it->first); + } + + if (it->second == common_parent) + { + break; + } + } + } + + if (common_time == ros::TIME_MAX) + { + common_time = ros::Time(); + } + + time = common_time; + return tf2_msgs::TF2Error::NO_ERROR; +} + +std::string BufferCore::allFramesAsYAML(double current_time) const +{ + std::stringstream mstream; + boost::mutex::scoped_lock lock(frame_mutex_); + + TransformStorage temp; + + if (frames_.size() ==1) + mstream <<"{}"; + + mstream.precision(3); + mstream.setf(std::ios::fixed,std::ios::floatfield); + + // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it) + for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame + { + CompactFrameID cfid = CompactFrameID(counter); + CompactFrameID frame_id_num; + TimeCacheInterfacePtr cache = getFrame(cfid); + if (!cache) + { + continue; + } + + if(!cache->getData(ros::Time(), temp)) + { + continue; + } + + frame_id_num = temp.frame_id_; + + std::string authority = "no recorded authority"; + std::map::const_iterator it = frame_authority_.find(cfid); + if (it != frame_authority_.end()) { + authority = it->second; + } + + double rate = cache->getListLength() / std::max((cache->getLatestTimestamp().toSec() - + cache->getOldestTimestamp().toSec() ), 0.0001); + + mstream << std::fixed; //fixed point notation + mstream.precision(3); //3 decimal places + mstream << frameIDs_reverse[cfid] << ": " << std::endl; + mstream << " parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl; + mstream << " broadcaster: '" << authority << "'" << std::endl; + mstream << " rate: " << rate << std::endl; + mstream << " most_recent_transform: " << (cache->getLatestTimestamp()).toSec() << std::endl; + mstream << " oldest_transform: " << (cache->getOldestTimestamp()).toSec() << std::endl; + if ( current_time > 0 ) { + mstream << " transform_delay: " << current_time - cache->getLatestTimestamp().toSec() << std::endl; + } + mstream << " buffer_length: " << (cache->getLatestTimestamp() - cache->getOldestTimestamp()).toSec() << std::endl; + } + + return mstream.str(); +} + +std::string BufferCore::allFramesAsYAML() const +{ + return this->allFramesAsYAML(0.0); +} + +TransformableCallbackHandle BufferCore::addTransformableCallback(const TransformableCallback& cb) +{ + boost::mutex::scoped_lock lock(transformable_callbacks_mutex_); + TransformableCallbackHandle handle = ++transformable_callbacks_counter_; + while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second) + { + handle = ++transformable_callbacks_counter_; + } + + return handle; +} + +struct BufferCore::RemoveRequestByCallback +{ + RemoveRequestByCallback(TransformableCallbackHandle handle) + : handle_(handle) + {} + + bool operator()(const TransformableRequest& req) + { + return req.cb_handle == handle_; + } + + TransformableCallbackHandle handle_; +}; + +void BufferCore::removeTransformableCallback(TransformableCallbackHandle handle) +{ + { + boost::mutex::scoped_lock lock(transformable_callbacks_mutex_); + transformable_callbacks_.erase(handle); + } + + { + boost::mutex::scoped_lock lock(transformable_requests_mutex_); + 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(); + + typedef boost::tuple TransformableTuple; + std::vector transformables; + + for (; it != transformable_requests_.end();) + { + TransformableRequest& req = *it; + + // One or both of the frames may not have existed when the request was originally made. + if (req.target_id == 0) + { + req.target_id = lookupFrameNumber(req.target_string); + } + + if (req.source_id == 0) + { + req.source_id = lookupFrameNumber(req.source_string); + } + + ros::Time latest_time; + bool do_cb = false; + TransformableResult result = TransformAvailable; + // TODO: This is incorrect, but better than nothing. Really we want the latest time for + // any of the frames + getLatestCommonTime(req.target_id, req.source_id, latest_time, 0); + if (!latest_time.isZero() && req.time + cache_time_ < latest_time) + { + do_cb = true; + result = TransformFailure; + } + else if (canTransformInternal(req.target_id, req.source_id, req.time, 0)) + { + do_cb = true; + result = TransformAvailable; + } + + if (do_cb) + { + { + boost::mutex::scoped_lock lock2(transformable_callbacks_mutex_); + M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle); + if (it != transformable_callbacks_.end()) + { + transformables.push_back(boost::make_tuple(boost::ref(it->second), + req.request_handle, + lookupFrameString(req.target_id), + lookupFrameString(req.source_id), + boost::ref(req.time), + boost::ref(result))); + } + } + + if (transformable_requests_.size() > 1) + { + transformable_requests_[it - transformable_requests_.begin()] = transformable_requests_.back(); + } + + transformable_requests_.erase(transformable_requests_.end() - 1); + } + else + { + ++it; + } + } + + // unlock before allowing possible user callbacks to avoid potential deadlock (#91) + lock.unlock(); + + BOOST_FOREACH (TransformableTuple tt, transformables) + { + tt.get<0>()(tt.get<1>(), tt.get<2>(), tt.get<3>(), tt.get<4>(), tt.get<5>()); + } + + // Backwards compatability callback for tf + _transforms_changed_(); +} + + +std::string BufferCore::_allFramesAsDot(double current_time) const +{ + std::stringstream mstream; + mstream << "digraph G {" << std::endl; + boost::mutex::scoped_lock lock(frame_mutex_); + + TransformStorage temp; + + if (frames_.size() == 1) { + mstream <<"\"no tf data recieved\""; + } + mstream.precision(3); + mstream.setf(std::ios::fixed,std::ios::floatfield); + + for (unsigned int counter = 1; counter < frames_.size(); counter ++) // one referenced for 0 is no frame + { + unsigned int frame_id_num; + TimeCacheInterfacePtr counter_frame = getFrame(counter); + if (!counter_frame) { + continue; + } + if(!counter_frame->getData(ros::Time(), temp)) { + continue; + } else { + frame_id_num = temp.frame_id_; + } + std::string authority = "no recorded authority"; + std::map::const_iterator it = frame_authority_.find(counter); + if (it != frame_authority_.end()) + authority = it->second; + + double rate = counter_frame->getListLength() / std::max((counter_frame->getLatestTimestamp().toSec() - + counter_frame->getOldestTimestamp().toSec()), 0.0001); + + mstream << std::fixed; //fixed point notation + mstream.precision(3); //3 decimal places + mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> " + << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\"" + //<< "Time: " << current_time.toSec() << "\\n" + << "Broadcaster: " << authority << "\\n" + << "Average rate: " << rate << " Hz\\n" + << "Most recent transform: " << (counter_frame->getLatestTimestamp()).toSec() <<" "; + if (current_time > 0) + mstream << "( "<< current_time - counter_frame->getLatestTimestamp().toSec() << " sec old)"; + mstream << "\\n" + // << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n" + // << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n" + // << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n" + << "Buffer length: " << (counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()).toSec() << " sec\\n" + <<"\"];" < 0) { + mstream << "edge [style=invis];" <" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; + } + continue; + } + if (counter_frame->getData(ros::Time(), temp)) { + frame_id_num = temp.frame_id_; + } else { + frame_id_num = 0; + } + + if(frameIDs_reverse[frame_id_num]=="NO_PARENT") + { + mstream << "edge [style=invis];" < 0) + mstream << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n "; + mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl; + } + } + mstream << "}"; + return mstream.str(); +} + +std::string BufferCore::_allFramesAsDot() const +{ + return _allFramesAsDot(0.0); +} + +void BufferCore::_chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string& fixed_frame, std::vector& output) const +{ + std::string error_string; + + output.clear(); //empty vector + + std::stringstream mstream; + boost::mutex::scoped_lock lock(frame_mutex_); + + TransformAccum accum; + + // Get source frame/time using getFrame + CompactFrameID source_id = lookupFrameNumber(source_frame); + CompactFrameID fixed_id = lookupFrameNumber(fixed_frame); + CompactFrameID target_id = lookupFrameNumber(target_frame); + + std::vector source_frame_chain; + int retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain); + + if (retval != tf2_msgs::TF2Error::NO_ERROR) + { + switch (retval) + { + case tf2_msgs::TF2Error::CONNECTIVITY_ERROR: + throw ConnectivityException(error_string); + case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR: + throw ExtrapolationException(error_string); + case tf2_msgs::TF2Error::LOOKUP_ERROR: + throw LookupException(error_string); + default: + CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + assert(0); + } + } + + std::vector target_frame_chain; + retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain); + + if (retval != tf2_msgs::TF2Error::NO_ERROR) + { + switch (retval) + { + case tf2_msgs::TF2Error::CONNECTIVITY_ERROR: + throw ConnectivityException(error_string); + case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR: + throw ExtrapolationException(error_string); + case tf2_msgs::TF2Error::LOOKUP_ERROR: + throw LookupException(error_string); + default: + CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + assert(0); + } + } + // If the two chains overlap clear the overlap + if (source_frame_chain.size() > 0 && target_frame_chain.size() > 0 && + source_frame_chain.back() == target_frame_chain.front()) + { + source_frame_chain.pop_back(); + } + // Join the two walks + for (unsigned int i = 0; i < target_frame_chain.size(); ++i) + { + source_frame_chain.push_back(target_frame_chain[i]); + } + + + // Write each element of source_frame_chain as string + for (unsigned int i = 0; i < source_frame_chain.size(); ++i) + { + output.push_back(lookupFrameString(source_frame_chain[i])); + } +} + +int TestBufferCore::_walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector *frame_chain) const +{ + TransformAccum accum; + return buffer.walkToTopParent(accum, time, target_id, source_id, error_string, frame_chain); +} + +} // namespace tf2 diff --git a/src/geometry2/tf2/src/cache.cpp b/src/geometry2/tf2/src/cache.cpp new file mode 100644 index 0000000..edbbc76 --- /dev/null +++ b/src/geometry2/tf2/src/cache.cpp @@ -0,0 +1,313 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Tully Foote */ + +#include "tf2/time_cache.h" +#include "tf2/exceptions.h" + +#include +#include +#include +#include +#include + +namespace tf2 { + +TransformStorage::TransformStorage() +{ +} + +TransformStorage::TransformStorage(const geometry_msgs::TransformStamped& data, CompactFrameID frame_id, + CompactFrameID child_frame_id) +: stamp_(data.header.stamp) +, frame_id_(frame_id) +, child_frame_id_(child_frame_id) +{ + const geometry_msgs::Quaternion& o = data.transform.rotation; + rotation_ = tf2::Quaternion(o.x, o.y, o.z, o.w); + const geometry_msgs::Vector3& v = data.transform.translation; + translation_ = tf2::Vector3(v.x, v.y, v.z); +} + +TimeCache::TimeCache(ros::Duration max_storage_time) +: max_storage_time_(max_storage_time) +{} + +namespace cache { // Avoid ODR collisions https://github.com/ros/geometry2/issues/175 +// hoisting these into separate functions causes an ~8% speedup. Removing calling them altogether adds another ~10% +void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* error_str) +{ + if (error_str) + { + 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(); + } +} +} // namespace cache + +bool operator>(const TransformStorage& lhs, const TransformStorage& rhs) +{ + return lhs.stamp_ > rhs.stamp_; +} + +uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str) +{ + //No values stored + if (storage_.empty()) + { + return 0; + } + + //If time == 0 return the latest + if (target_time.isZero()) + { + one = &storage_.front(); + return 1; + } + + // One value stored + if (++storage_.begin() == storage_.end()) + { + TransformStorage& ts = *storage_.begin(); + if (ts.stamp_ == target_time) + { + one = &ts; + return 1; + } + else + { + cache::createExtrapolationException1(target_time, ts.stamp_, error_str); + return 0; + } + } + + ros::Time latest_time = (*storage_.begin()).stamp_; + ros::Time earliest_time = (*(storage_.rbegin())).stamp_; + + if (target_time == latest_time) + { + one = &(*storage_.begin()); + return 1; + } + else if (target_time == earliest_time) + { + one = &(*storage_.rbegin()); + return 1; + } + // Catch cases that would require extrapolation + else if (target_time > latest_time) + { + cache::createExtrapolationException2(target_time, latest_time, error_str); + return 0; + } + else if (target_time < earliest_time) + { + cache::createExtrapolationException3(target_time, earliest_time, error_str); + return 0; + } + + //At least 2 values stored + //Find the first value less than the target value + L_TransformStorage::iterator storage_it; + TransformStorage storage_target_time; + storage_target_time.stamp_ = target_time; + + storage_it = std::lower_bound( + storage_.begin(), + storage_.end(), + storage_target_time, std::greater()); + + //Finally the case were somewhere in the middle Guarenteed no extrapolation :-) + one = &*(storage_it); //Older + two = &*(--storage_it); //Newer + return 2; + + +} + +void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output) +{ + // Check for zero distance case + if( two.stamp_ == one.stamp_ ) + { + output = two; + return; + } + //Calculate the ratio + tf2Scalar ratio = (time - one.stamp_).toSec() / (two.stamp_ - one.stamp_).toSec(); + + //Interpolate translation + output.translation_.setInterpolate3(one.translation_, two.translation_, ratio); + + //Interpolate rotation + output.rotation_ = slerp( one.rotation_, two.rotation_, ratio); + + output.stamp_ = time; + output.frame_id_ = one.frame_id_; + output.child_frame_id_ = one.child_frame_id_; +} + +bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available +{ + TransformStorage* p_temp_1; + TransformStorage* p_temp_2; + + int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); + if (num_nodes == 0) + { + return false; + } + else if (num_nodes == 1) + { + data_out = *p_temp_1; + } + else if (num_nodes == 2) + { + if( p_temp_1->frame_id_ == p_temp_2->frame_id_) + { + interpolate(*p_temp_1, *p_temp_2, time, data_out); + } + else + { + data_out = *p_temp_1; + } + } + else + { + assert(0); + } + + return true; +} + +CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str) +{ + TransformStorage* p_temp_1; + TransformStorage* p_temp_2; + + int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str); + if (num_nodes == 0) + { + return 0; + } + + return p_temp_1->frame_id_; +} + +bool TimeCache::insertData(const TransformStorage& new_data) +{ + 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(); + } + +} // namespace tf2 +} diff --git a/src/geometry2/tf2/src/static_cache.cpp b/src/geometry2/tf2/src/static_cache.cpp new file mode 100644 index 0000000..cb588c5 --- /dev/null +++ b/src/geometry2/tf2/src/static_cache.cpp @@ -0,0 +1,80 @@ +/* + * 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(); +}; + diff --git a/src/geometry2/tf2/test/cache_unittest.cpp b/src/geometry2/tf2/test/cache_unittest.cpp new file mode 100644 index 0000000..2077791 --- /dev/null +++ b/src/geometry2/tf2/test/cache_unittest.cpp @@ -0,0 +1,414 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include "tf2/LinearMath/Quaternion.h" +#include + +#include + +#include + +std::vector values; +unsigned int step = 0; + +void seed_rand() +{ + values.clear(); + for (unsigned int i = 0; i < 1000; i++) + { + int pseudo_rand = std::floor(i * M_PI); + values.push_back(( pseudo_rand % 100)/50.0 - 1.0); + //printf("Seeding with %f\n", values.back()); + } +}; + + +double get_rand() +{ + if (values.size() == 0) throw std::runtime_error("you need to call seed_rand first"); + if (step >= values.size()) + step = 0; + else + step++; + return values[step]; +} + +using namespace tf2; + + +void setIdentity(TransformStorage& stor) +{ + stor.translation_.setValue(0.0, 0.0, 0.0); + stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); +} + +TEST(TimeCache, Repeatability) +{ + unsigned int runs = 100; + + tf2::TimeCache cache; + + TransformStorage stor; + setIdentity(stor); + + for ( uint64_t i = 1; i < runs ; i++ ) + { + stor.frame_id_ = i; + stor.stamp_ = ros::Time().fromNSec(i); + + cache.insertData(stor); + } + + for ( uint64_t i = 1; i < runs ; i++ ) + + { + cache.getData(ros::Time().fromNSec(i), stor); + EXPECT_EQ(stor.frame_id_, i); + EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); + } + +} + +TEST(TimeCache, RepeatabilityReverseInsertOrder) +{ + unsigned int runs = 100; + + tf2::TimeCache cache; + + TransformStorage stor; + setIdentity(stor); + + for ( int i = runs -1; i >= 0 ; i-- ) + { + stor.frame_id_ = i; + stor.stamp_ = ros::Time().fromNSec(i); + + cache.insertData(stor); + } + for ( uint64_t i = 1; i < runs ; i++ ) + + { + cache.getData(ros::Time().fromNSec(i), stor); + EXPECT_EQ(stor.frame_id_, i); + EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); + } + +} + +#if 0 // jfaust: this doesn't seem to actually be testing random insertion? +TEST(TimeCache, RepeatabilityRandomInsertOrder) +{ + + seed_rand(); + + tf2::TimeCache cache; + double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8}; + std::vector values (my_vals, my_vals + sizeof(my_vals)/sizeof(double)); + unsigned int runs = values.size(); + + TransformStorage stor; + setIdentity(stor); + for ( uint64_t i = 0; i xvalues(2); + std::vector yvalues(2); + std::vector zvalues(2); + + uint64_t offset = 200; + + TransformStorage stor; + setIdentity(stor); + + for ( uint64_t i = 1; i < runs ; i++ ) + { + + for (uint64_t step = 0; step < 2 ; step++) + { + xvalues[step] = 10.0 * get_rand(); + yvalues[step] = 10.0 * get_rand(); + zvalues[step] = 10.0 * get_rand(); + + stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]); + stor.frame_id_ = 2; + stor.stamp_ = ros::Time().fromNSec(step * 100 + offset); + cache.insertData(stor); + } + + for (int pos = 0; pos < 100 ; pos ++) + { + uint64_t time = offset + pos; + cache.getData(ros::Time().fromNSec(time), stor); + uint64_t time_out = stor.stamp_.toNSec(); + double x_out = stor.translation_.x(); + double y_out = stor.translation_.y(); + double z_out = stor.translation_.z(); + // printf("pose %d, %f %f %f, expected %f %f %f\n", pos, x_out, y_out, z_out, + // xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100., + // yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, + // zvalues[0] + (xvalues[1] - zvalues[0]) * (double)pos/100.0); + EXPECT_EQ(time, time_out); + EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon); + EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon); + EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon); + } + + + cache.clearList(); + } + + +} + +/** \brief Make sure we dont' interpolate across reparented data */ +TEST(TimeCache, ReparentingInterpolationProtection) +{ + double epsilon = 1e-6; + uint64_t offset = 555; + + seed_rand(); + + tf2::TimeCache cache; + std::vector xvalues(2); + std::vector yvalues(2); + std::vector zvalues(2); + + TransformStorage stor; + setIdentity(stor); + + for (uint64_t step = 0; step < 2 ; step++) + { + xvalues[step] = 10.0 * get_rand(); + yvalues[step] = 10.0 * get_rand(); + zvalues[step] = 10.0 * get_rand(); + + stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]); + stor.frame_id_ = step + 4; + stor.stamp_ = ros::Time().fromNSec(step * 100 + offset); + cache.insertData(stor); + } + + for (int pos = 0; pos < 100 ; pos ++) + { + EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor)); + double x_out = stor.translation_.x(); + double y_out = stor.translation_.y(); + double z_out = stor.translation_.z(); + EXPECT_NEAR(xvalues[0], x_out, epsilon); + EXPECT_NEAR(yvalues[0], y_out, epsilon); + EXPECT_NEAR(zvalues[0], z_out, epsilon); + } +} + +TEST(Bullet, Slerp) +{ + + uint64_t runs = 100; + seed_rand(); + + tf2::Quaternion q1, q2; + q1.setEuler(0,0,0); + + for (uint64_t i = 0 ; i < runs ; i++) + { + q2.setEuler(1.0 * get_rand(), + 1.0 * get_rand(), + 1.0 * get_rand()); + + + tf2::Quaternion q3 = slerp(q1,q2,0.5); + + EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5); + } + +} + + +TEST(TimeCache, AngularInterpolation) +{ + uint64_t runs = 100; + double epsilon = 1e-6; + seed_rand(); + + tf2::TimeCache cache; + std::vector yawvalues(2); + std::vector pitchvalues(2); + std::vector rollvalues(2); + uint64_t offset = 200; + + std::vector quats(2); + + TransformStorage stor; + setIdentity(stor); + + for ( uint64_t i = 1; i < runs ; i++ ) + { + + for (uint64_t step = 0; step < 2 ; step++) + { + yawvalues[step] = 10.0 * get_rand() / 100.0; + pitchvalues[step] = 0;//10.0 * get_rand(); + rollvalues[step] = 0;//10.0 * get_rand(); + quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]); + stor.rotation_ = quats[step]; + stor.frame_id_ = 3; + stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1 + cache.insertData(stor); + } + + for (int pos = 0; pos < 100 ; pos ++) + { + uint64_t time = offset + pos; + cache.getData(ros::Time().fromNSec(time), stor); + uint64_t time_out = stor.stamp_.toNSec(); + tf2::Quaternion quat (stor.rotation_); + + //Generate a ground truth quaternion directly calling slerp + tf2::Quaternion ground_truth = quats[0].slerp(quats[1], pos/100.0); + + //Make sure the transformed one and the direct call match + EXPECT_EQ(time, time_out); + EXPECT_NEAR(0, angle(ground_truth, quat), epsilon); + + } + + cache.clearList(); + } + + +} + +TEST(TimeCache, DuplicateEntries) +{ + + TimeCache cache; + + TransformStorage stor; + setIdentity(stor); + stor.frame_id_ = 3; + stor.stamp_ = ros::Time().fromNSec(1); + + cache.insertData(stor); + + cache.insertData(stor); + + + cache.getData(ros::Time().fromNSec(1), stor); + + //printf(" stor is %f\n", stor.translation_.x()); + EXPECT_TRUE(!std::isnan(stor.translation_.x())); + EXPECT_TRUE(!std::isnan(stor.translation_.y())); + EXPECT_TRUE(!std::isnan(stor.translation_.z())); + EXPECT_TRUE(!std::isnan(stor.rotation_.x())); + EXPECT_TRUE(!std::isnan(stor.rotation_.y())); + EXPECT_TRUE(!std::isnan(stor.rotation_.z())); + EXPECT_TRUE(!std::isnan(stor.rotation_.w())); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/src/geometry2/tf2/test/simple_tf2_core.cpp b/src/geometry2/tf2/test/simple_tf2_core.cpp new file mode 100644 index 0000000..79ea4d7 --- /dev/null +++ b/src/geometry2/tf2/test/simple_tf2_core.cpp @@ -0,0 +1,320 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include "tf2/LinearMath/Vector3.h" +#include "tf2/exceptions.h" + +TEST(tf2, setTransformFail) +{ + tf2::BufferCore tfc; + geometry_msgs::TransformStamped st; + EXPECT_FALSE(tfc.setTransform(st, "authority1")); + +} + +TEST(tf2, setTransformValid) +{ + tf2::BufferCore tfc; + geometry_msgs::TransformStamped st; + st.header.frame_id = "foo"; + st.header.stamp = ros::Time(1.0); + st.child_frame_id = "child"; + st.transform.rotation.w = 1; + EXPECT_TRUE(tfc.setTransform(st, "authority1")); + +} + +TEST(tf2, setTransformInvalidQuaternion) +{ + tf2::BufferCore tfc; + geometry_msgs::TransformStamped st; + st.header.frame_id = "foo"; + st.header.stamp = ros::Time(1.0); + st.child_frame_id = "child"; + st.transform.rotation.w = 0; + EXPECT_FALSE(tfc.setTransform(st, "authority1")); + +} + +TEST(tf2_lookupTransform, LookupException_Nothing_Exists) +{ + tf2::BufferCore tfc; + EXPECT_THROW(tfc.lookupTransform("a", "b", ros::Time().fromSec(1.0)), tf2::LookupException); + +} + +TEST(tf2_canTransform, Nothing_Exists) +{ + tf2::BufferCore tfc; + EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0))); + + std::string error_msg = std::string(); + EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0), &error_msg)); + ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist."); + +} + +TEST(tf2_lookupTransform, LookupException_One_Exists) +{ + tf2::BufferCore tfc; + geometry_msgs::TransformStamped st; + st.header.frame_id = "foo"; + st.header.stamp = ros::Time(1.0); + st.child_frame_id = "child"; + st.transform.rotation.w = 1; + EXPECT_TRUE(tfc.setTransform(st, "authority1")); + EXPECT_THROW(tfc.lookupTransform("foo", "bar", ros::Time().fromSec(1.0)), tf2::LookupException); + +} + +TEST(tf2_canTransform, One_Exists) +{ + tf2::BufferCore tfc; + geometry_msgs::TransformStamped st; + st.header.frame_id = "foo"; + st.header.stamp = ros::Time(1.0); + st.child_frame_id = "child"; + st.transform.rotation.w = 1; + EXPECT_TRUE(tfc.setTransform(st, "authority1")); + EXPECT_FALSE(tfc.canTransform("foo", "bar", ros::Time().fromSec(1.0))); +} + +TEST(tf2_chainAsVector, chain_v_configuration) +{ + tf2::BufferCore mBC; + tf2::TestBufferCore tBC; + + geometry_msgs::TransformStamped st; + st.header.stamp = ros::Time(0); + st.transform.rotation.w = 1; + + st.header.frame_id = "a"; + st.child_frame_id = "b"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "b"; + st.child_frame_id = "c"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "a"; + st.child_frame_id = "d"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "d"; + st.child_frame_id = "e"; + mBC.setTransform(st, "authority1"); + + std::vector chain; + + + mBC._chainAsVector( "c", ros::Time(), "c", ros::Time(), "c", chain); + EXPECT_EQ( 0, chain.size()); + + mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "c", chain); + EXPECT_EQ( 3, chain.size()); + if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" ); + if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); + if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); + + mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "c", chain); + EXPECT_EQ( 3, chain.size()); + if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" ); + if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); + if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" ); + + mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "a", chain); + EXPECT_EQ( 3, chain.size()); + if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" ); + if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); + if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); + + mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "a", chain); + EXPECT_EQ( 3, chain.size()); + if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" ); + if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" ); + if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" ); + + mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "c", chain); + + EXPECT_EQ( 5, chain.size()); + if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); + if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); + if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); + if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); + if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); + + mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "a", chain); + + EXPECT_EQ( 5, chain.size()); + if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); + if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); + if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); + if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); + if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); + + mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "e", chain); + + EXPECT_EQ( 5, chain.size()); + if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" ); + if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "d" ); + if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" ); + if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" ); + if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" ); +} + +TEST(tf2_walkToTopParent, walk_i_configuration) +{ + tf2::BufferCore mBC; + tf2::TestBufferCore tBC; + + geometry_msgs::TransformStamped st; + st.header.stamp = ros::Time(0); + st.transform.rotation.w = 1; + + st.header.frame_id = "a"; + st.child_frame_id = "b"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "b"; + st.child_frame_id = "c"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "c"; + st.child_frame_id = "d"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "d"; + st.child_frame_id = "e"; + mBC.setTransform(st, "authority1"); + + std::vector id_chain; + tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); + + EXPECT_EQ(5, id_chain.size() ); + if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); + if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); + if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2])); + if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3])); + if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4])); + + id_chain.clear(); + tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); + + EXPECT_EQ(5, id_chain.size() ); + if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0])); + if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1])); + if( id_chain.size() >= 3 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[2])); + if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3])); + if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4])); + +} + +TEST(tf2_walkToTopParent, walk_v_configuration) +{ + tf2::BufferCore mBC; + tf2::TestBufferCore tBC; + + geometry_msgs::TransformStamped st; + st.header.stamp = ros::Time(0); + st.transform.rotation.w = 1; + + // st.header.frame_id = "aaa"; + // st.child_frame_id = "aa"; + // mBC.setTransform(st, "authority1"); + // + // st.header.frame_id = "aa"; + // st.child_frame_id = "a"; + // mBC.setTransform(st, "authority1"); + + st.header.frame_id = "a"; + st.child_frame_id = "b"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "b"; + st.child_frame_id = "c"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "a"; + st.child_frame_id = "d"; + mBC.setTransform(st, "authority1"); + + st.header.frame_id = "d"; + st.child_frame_id = "e"; + mBC.setTransform(st, "authority1"); + + std::vector id_chain; + tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain); + + EXPECT_EQ(5, id_chain.size()); + if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0])); + if( id_chain.size() >= 2 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[1])); + if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); + if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3])); + if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4])); + + tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain); + EXPECT_EQ(5, id_chain.size()); + if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); + if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); + if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); + if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3])); + if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4])); + + tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain); + EXPECT_EQ( id_chain.size(), 3 ); + if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); + if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); + if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2])); + + tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain); + EXPECT_EQ( id_chain.size(), 3 ); + if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0])); + if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); + if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2])); + + tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain); + EXPECT_EQ( id_chain.size(), 2 ); + if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0])); + if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1])); + + tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain); + EXPECT_EQ( id_chain.size(), 2 ); + if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0])); + if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1])); +} + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::Time::init(); //needed for ros::TIme::now() + return RUN_ALL_TESTS(); +} diff --git a/src/geometry2/tf2/test/speed_test.cpp b/src/geometry2/tf2/test/speed_test.cpp new file mode 100644 index 0000000..c63ca18 --- /dev/null +++ b/src/geometry2/tf2/test/speed_test.cpp @@ -0,0 +1,225 @@ +/* + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include + +#include + +int main(int argc, char** argv) +{ + uint32_t num_levels = 10; + if (argc > 1) + { + num_levels = boost::lexical_cast(argv[1]); + } + double time_interval = 1.0; + if (argc > 2) + { + time_interval = boost::lexical_cast(argv[2]); + } + + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO); + + tf2::BufferCore bc; + geometry_msgs::TransformStamped t; + t.header.stamp = ros::Time(1); + t.header.frame_id = "root"; + t.child_frame_id = "0"; + t.transform.translation.x = 1; + t.transform.rotation.w = 1.0; + bc.setTransform(t, "me"); + t.header.stamp = ros::Time(2); + bc.setTransform(t, "me"); + + for (uint32_t i = 1; i < num_levels / 2; ++i) + { + for (double j = time_interval; j < 2.0 + time_interval; j += time_interval) + { + std::stringstream parent_ss; + parent_ss << (i - 1); + std::stringstream child_ss; + child_ss << i; + + t.header.stamp = ros::Time(j); + t.header.frame_id = parent_ss.str(); + t.child_frame_id = child_ss.str(); + bc.setTransform(t, "me"); + } + } + + t.header.frame_id = "root"; + std::stringstream ss; + ss << num_levels/2; + t.header.stamp = ros::Time(1); + t.child_frame_id = ss.str(); + bc.setTransform(t, "me"); + t.header.stamp = ros::Time(2); + bc.setTransform(t, "me"); + + for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i) + { + for (double j = time_interval; j < 2.0 + time_interval; j += time_interval) + { + std::stringstream parent_ss; + parent_ss << (i - 1); + std::stringstream child_ss; + child_ss << i; + + t.header.stamp = ros::Time(j); + t.header.frame_id = parent_ss.str(); + t.child_frame_id = child_ss.str(); + bc.setTransform(t, "me"); + } + } + + //logInfo_STREAM(bc.allFramesAsYAML()); + + std::string v_frame0 = boost::lexical_cast(num_levels - 1); + std::string v_frame1 = boost::lexical_cast(num_levels/2 - 1); + CONSOLE_BRIDGE_logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str()); + geometry_msgs::TransformStamped out_t; + + const uint32_t count = 1000000; + CONSOLE_BRIDGE_logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval); + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(0)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + //ROS_INFO_STREAM(out_t); + CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + //ROS_INFO_STREAM(out_t); + CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + //ROS_INFO_STREAM(out_t); + CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(2)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + //ROS_INFO_STREAM(out_t); + CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + bc.canTransform(v_frame1, v_frame0, ros::Time(0)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + //ROS_INFO_STREAM(out_t); + CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + bc.canTransform(v_frame1, v_frame0, ros::Time(1)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + //ROS_INFO_STREAM(out_t); + CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + bc.canTransform(v_frame1, v_frame0, ros::Time(1.5)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + //ROS_INFO_STREAM(out_t); + CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif + +#if 01 + { + ros::WallTime start = ros::WallTime::now(); + for (uint32_t i = 0; i < count; ++i) + { + bc.canTransform(v_frame1, v_frame0, ros::Time(2)); + } + ros::WallTime end = ros::WallTime::now(); + ros::WallDuration dur = end - start; + //ROS_INFO_STREAM(out_t); + CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count); + } +#endif +} diff --git a/src/geometry2/tf2/test/static_cache_test.cpp b/src/geometry2/tf2/test/static_cache_test.cpp new file mode 100644 index 0000000..f7a4622 --- /dev/null +++ b/src/geometry2/tf2/test/static_cache_test.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include + +#include + +using namespace tf2; + + +void setIdentity(TransformStorage& stor) +{ + stor.translation_.setValue(0.0, 0.0, 0.0); + stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0); +} + +TEST(StaticCache, Repeatability) +{ + unsigned int runs = 100; + + tf2::StaticCache cache; + + TransformStorage stor; + setIdentity(stor); + + for ( uint64_t i = 1; i < runs ; i++ ) + { + stor.frame_id_ = CompactFrameID(i); + stor.stamp_ = ros::Time().fromNSec(i); + + cache.insertData(stor); + + + cache.getData(ros::Time().fromNSec(i), stor); + EXPECT_EQ(stor.frame_id_, i); + EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i)); + + } +} + +TEST(StaticCache, DuplicateEntries) +{ + + tf2::StaticCache cache; + + TransformStorage stor; + setIdentity(stor); + stor.frame_id_ = CompactFrameID(3); + stor.stamp_ = ros::Time().fromNSec(1); + + cache.insertData(stor); + + cache.insertData(stor); + + + cache.getData(ros::Time().fromNSec(1), stor); + + //printf(" stor is %f\n", stor.transform.translation.x); + EXPECT_TRUE(!std::isnan(stor.translation_.x())); + EXPECT_TRUE(!std::isnan(stor.translation_.y())); + EXPECT_TRUE(!std::isnan(stor.translation_.z())); + EXPECT_TRUE(!std::isnan(stor.rotation_.x())); + EXPECT_TRUE(!std::isnan(stor.rotation_.y())); + EXPECT_TRUE(!std::isnan(stor.rotation_.z())); + EXPECT_TRUE(!std::isnan(stor.rotation_.w())); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/geometry2/tf2_bullet/CHANGELOG.rst b/src/geometry2/tf2_bullet/CHANGELOG.rst new file mode 100644 index 0000000..2a47cf5 --- /dev/null +++ b/src/geometry2/tf2_bullet/CHANGELOG.rst @@ -0,0 +1,181 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_bullet +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ +* [windows][melodic] more portable fixes. (`#443 `_) +* Contributors: Sean Yen + +0.6.6 (2020-01-09) +------------------ +* Fix compile error missing ros/ros.h (`#400 `_) + * ros/ros.h -> ros/time.h + * tf2_bullet doesn't need ros.h + * tf2_eigen doesn't need ros/ros.h +* use find_package when pkg_check_modules doesn't work (`#364 `_) +* Contributors: James Xu, Shane Loretz + +0.6.5 (2018-11-16) +------------------ + +0.6.4 (2018-11-06) +------------------ + +0.6.3 (2018-07-09) +------------------ + +0.6.2 (2018-05-02) +------------------ + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ + +0.5.17 (2018-01-01) +------------------- + +0.5.16 (2017-07-14) +------------------- +* store gtest return value as int (`#229 `_) +* Contributors: dhood + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- +* Improve documentation +* Contributors: Jackie Kay + +0.5.13 (2016-03-04) +------------------- +* Don't export catkin includes + They only point to the temporary include in the build directory. +* Contributors: Jochen Sprickerhof + +0.5.12 (2015-08-05) +------------------- + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- + +0.5.9 (2015-03-25) +------------------ + +0.5.8 (2015-03-17) +------------------ +* remove useless Makefile files +* fix ODR violations +* Contributors: Vincent Rabaud + +0.5.7 (2014-12-23) +------------------ +* fixing install rules and adding backwards compatible include with #warning +* Contributors: Tully Foote + +0.5.6 (2014-09-18) +------------------ + +0.5.5 (2014-06-23) +------------------ + +0.5.4 (2014-05-07) +------------------ + +0.5.3 (2014-02-21) +------------------ + +0.5.2 (2014-02-20) +------------------ + +0.5.1 (2014-02-14) +------------------ + +0.5.0 (2014-02-14) +------------------ + +0.4.10 (2013-12-26) +------------------- + +0.4.9 (2013-11-06) +------------------ +* adding missing buildtool dependency on pkg-config + +0.4.8 (2013-11-06) +------------------ + +0.4.7 (2013-08-28) +------------------ + +0.4.6 (2013-08-28) +------------------ + +0.4.5 (2013-07-11) +------------------ + +0.4.4 (2013-07-09) +------------------ +* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. + +0.4.3 (2013-07-05) +------------------ + +0.4.2 (2013-07-05) +------------------ +* removing unused dependency on rostest + +0.4.1 (2013-07-05) +------------------ +* stripping tf2_ros dependency from tf2_bullet. Test was moved to test_tf2 + +0.4.0 (2013-06-27) +------------------ +* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 +* Cleaning up unnecessary dependency on roscpp +* converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace +* Cleaning up packaging of tf2 including: + removing unused nodehandle + cleaning up a few dependencies and linking + removing old backup of package.xml + making diff minimally different from tf version of library +* Restoring test packages and bullet packages. + reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented + reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ + +0.3.6 (2013-03-03) +------------------ + +0.3.5 (2013-02-15 14:46) +------------------------ + +0.3.4 (2013-02-15 13:14) +------------------------ + +0.3.3 (2013-02-15 11:30) +------------------------ + +0.3.2 (2013-02-15 00:42) +------------------------ + +0.3.1 (2013-02-14) +------------------ + +0.3.0 (2013-02-13) +------------------ +* fixing groovy-devel +* removing bullet and kdl related packages +* catkinizing geometry-experimental +* catkinizing tf2_bullet +* merge tf2_cpp and tf2_py into tf2_ros +* A working first version of transforming and converting between different types +* Fixing tests now that Buffer creates a NodeHandle +* add frame unit tests to kdl and bullet +* add first regression tests for kdl and bullet tf +* add btTransform transform +* add bullet transforms, and create tests for bullet and kdl diff --git a/src/geometry2/tf2_bullet/CMakeLists.txt b/src/geometry2/tf2_bullet/CMakeLists.txt new file mode 100644 index 0000000..15a8110 --- /dev/null +++ b/src/geometry2/tf2_bullet/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 2.8.3) +project(tf2_bullet) + +find_package(PkgConfig REQUIRED) + +set(bullet_FOUND 0) +pkg_check_modules(bullet bullet) +if(NOT bullet_FOUND) + # windows build bullet3 doesn't come with pkg-config by default and it only comes with CMake config files + # so pkg_check_modules will fail + find_package(bullet REQUIRED) + + # https://github.com/bulletphysics/bullet3/blob/master/BulletConfig.cmake.in + set(bullet_INCLUDE_DIRS "${BULLET_INCLUDE_DIRS}") +endif() + +find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2) + +include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) + +catkin_package(INCLUDE_DIRS include ${bullet_INCLUDE_DIRS}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + + +if(CATKIN_ENABLE_TESTING) + +catkin_add_gtest(test_bullet test/test_tf2_bullet.cpp) +target_link_libraries(test_bullet ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) + +endif() diff --git a/src/geometry2/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/src/geometry2/tf2_bullet/include/tf2_bullet/tf2_bullet.h new file mode 100644 index 0000000..63e88f5 --- /dev/null +++ b/src/geometry2/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -0,0 +1,117 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + +#ifndef TF2_BULLET_H +#define TF2_BULLET_H + +#include +#include +#include + + +namespace tf2 +{ +/** \brief Convert a timestamped transform to the equivalent Bullet data type. + * \param t The transform to convert, as a geometry_msgs TransformedStamped message. + * \return The transform message converted to a Bullet btTransform. + */ +inline +btTransform transformToBullet(const geometry_msgs::TransformStamped& t) + { + return btTransform(btQuaternion(t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + btVector3(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); + } + + +/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 type. + * This function is a specialization of the doTransform template defined in tf2/convert.h + * \param t_in The vector to transform, as a timestamped Bullet btVector3 data type. + * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) + { + t_out = tf2::Stamped(transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); + } + +/** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h + * \param in The timestamped Bullet btVector3 to convert. + * \return The vector converted to a PointStamped message. + */ +inline +geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::PointStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.point.x = in[0]; + msg.point.y = in[1]; + msg.point.z = in[2]; + return msg; +} + +/** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The PointStamped message to convert. + * \param out The point converted to a timestamped Bullet Vector3. + */ +inline +void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + out[0] = msg.point.x; + out[1] = msg.point.y; + out[2] = msg.point.z; +} + + +/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Transform data type. + * This function is a specialization of the doTransform template defined in tf2/convert.h + * \param t_in The frame to transform, as a timestamped Bullet btTransform. + * \param t_out The transformed frame, as a timestamped Bullet btTransform. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) + { + t_out = tf2::Stamped(transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id); + } + + +} // namespace + +#endif // TF2_BULLET_H diff --git a/src/geometry2/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h b/src/geometry2/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h new file mode 100644 index 0000000..2b257ee --- /dev/null +++ b/src/geometry2/tf2_bullet/include/tf2_bullet/tf2_bullet/tf2_bullet.h @@ -0,0 +1,3 @@ +#warning This header is at the wrong path you should include + +#include diff --git a/src/geometry2/tf2_bullet/mainpage.dox b/src/geometry2/tf2_bullet/mainpage.dox new file mode 100644 index 0000000..a2ec58c --- /dev/null +++ b/src/geometry2/tf2_bullet/mainpage.dox @@ -0,0 +1,19 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf2_bullet contains functions for converting between geometry_msgs and Bullet data types. + +This library is an implementation of the templated conversion interface specified in tf/convert.h. +It enables easy conversion from geometry_msgs Transform and Point types to the types specified +by the Bullet physics engine API (see http://bulletphysics.org). + +See the Conversions overview +wiki page for more information about datatype conversion in tf2. + +\section codeapi Code API + +This library consists of one header only, tf2_bullet/tf2_bullet.h, which consists mostly of +specializations of template functions defined in tf2/convert.h. + +*/ diff --git a/src/geometry2/tf2_bullet/package.xml b/src/geometry2/tf2_bullet/package.xml new file mode 100644 index 0000000..236db56 --- /dev/null +++ b/src/geometry2/tf2_bullet/package.xml @@ -0,0 +1,25 @@ + + tf2_bullet + 0.6.7 + + 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 + + + diff --git a/src/geometry2/tf2_bullet/test/test_tf2_bullet.cpp b/src/geometry2/tf2_bullet/test/test_tf2_bullet.cpp new file mode 100644 index 0000000..9ecda51 --- /dev/null +++ b/src/geometry2/tf2_bullet/test/test_tf2_bullet.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + + +#include +#include +#include + +static const double EPS = 1e-3; + + +TEST(TfBullet, ConvertVector) +{ + btVector3 v(1,2,3); + + btVector3 v1 = v; + tf2::convert(v1, v1); + + EXPECT_EQ(v, v1); + + btVector3 v2(3,4,5); + tf2::convert(v1, v2); + + EXPECT_EQ(v, v2); + EXPECT_EQ(v1, v2); +} + + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/src/geometry2/tf2_eigen/CHANGELOG.rst b/src/geometry2/tf2_eigen/CHANGELOG.rst new file mode 100644 index 0000000..338ca76 --- /dev/null +++ b/src/geometry2/tf2_eigen/CHANGELOG.rst @@ -0,0 +1,107 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_eigen +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ +* Revert "rework Eigen functions namespace hack" (`#436 `_) +* Contributors: Tully Foote + +0.6.6 (2020-01-09) +------------------ +* Fix compile error missing ros/ros.h (`#400 `_) + * ros/ros.h -> ros/time.h + * tf2_bullet doesn't need ros.h + * tf2_eigen doesn't need ros/ros.h +* rework Eigen functions namespace hack +* separate transform function declarations into transform_functions.h +* Contributors: James Xu, Shane Loretz, Tully Foote + +0.6.5 (2018-11-16) +------------------ + +0.6.4 (2018-11-06) +------------------ +* improve comments +* add Eigen::Isometry3d conversions +* normalize quaternions to be in half-space w >= 0 as in tf1 +* improve computation efficiency +* Contributors: Robert Haschke + +0.6.3 (2018-07-09) +------------------ + +0.6.2 (2018-05-02) +------------------ +* Adds toMsg & fromMsg for Eigen Vector3 (`#294 `_) + - Adds toMsg for geometry_msgs::Vector3& with dual argument syntax to + avoid an overload conflict with + geometry_msgs::Point& toMsg(contst Eigen::Vector3d& in) + - Adds corresponding fromMsg for Eigen Vector3d and + geometry_msgs::Vector3 + - Fixed typos in description of fromMsg for Twist and Eigen 6x1 Matrix +* Adds additional conversions for tf2, KDL, Eigen (`#292 `_) + - adds non-stamped Eigen to Transform function + - converts Eigen Matrix Vectors to and from geometry_msgs::Twist + - adds to/from message for geometry_msgs::Pose and KDL::Frame +* Contributors: Ian McMahon + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ + +0.5.17 (2018-01-01) +------------------- + +0.5.16 (2017-07-14) +------------------- +* fix return value to prevent warnings on windows (`#237 `_) +* fixing include directory order to support overlays (`#231 `_) +* tf2_eigen: added support for Quaternion and QuaternionStamped (`#230 `_) +* Remove an unused variable from the tf2_eigen test. (`#215 `_) +* Find eigen in a much nicer way. +* Switch tf2_eigen to use package.xml format 2. (`#216 `_) +* Contributors: Chris Lalancette, Mikael Arguedas, Tully Foote, cwecht + +0.5.15 (2017-01-24) +------------------- +* fixup `#186 `_: inline template specializations (`#200 `_) +* Contributors: Robert Haschke + +0.5.14 (2017-01-16) +------------------- +* Add tf2_eigen conversions for Pose and Point (not stamped) (`#186 `_) + * tf2_eigen: added conversions for Point msg type (not timestamped) to Eigen::Vector3d + * tf2_eigen: added conversions for Pose msg type (not timestamped) to Eigen::Affine3d + * tf2_eigen: new functions are inline now + * tf2_eigen test compiling again + * tf2_eigen: added tests for Affine3d and Vector3d conversion + * tf2_eigen: added redefinitions of non-stamped conversion function to make usage in tf2::convert() possible + * tf2_eigen: reduced redundancy by reusing non-stamped conversion-functions in their stamped counterparts + * tf2_eigen: added notes at doTransform-implementations which can not work with tf2_ros::BufferInterface::transform + * tf2_eigen: fixed typos +* Don't export local include dirs (`#180 `_) +* Improve documentation. +* Contributors: Jackie Kay, Jochen Sprickerhof, cwecht + +0.5.13 (2016-03-04) +------------------- +* Added missing inline +* Added unit test + - Testing conversion to msg forward/backward +* Added eigenTotransform function +* Contributors: Davide Tateo, boris-il-forte + +0.5.12 (2015-08-05) +------------------- + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- +* fixing CMakeLists.txt from `#97 `_ +* create tf2_eigen. +* Contributors: Tully Foote, koji diff --git a/src/geometry2/tf2_eigen/CMakeLists.txt b/src/geometry2/tf2_eigen/CMakeLists.txt new file mode 100644 index 0000000..a2b4c40 --- /dev/null +++ b/src/geometry2/tf2_eigen/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 2.8.3) +project(tf2_eigen) + +find_package(catkin REQUIRED COMPONENTS + cmake_modules + geometry_msgs + tf2 +) + +# Finding Eigen is somewhat complicated because of our need to support Ubuntu +# all the way back to saucy. First we look for the Eigen3 cmake module +# provided by the libeigen3-dev on newer Ubuntu. If that fails, then we +# fall-back to the version provided by cmake_modules, which is a stand-in. +find_package(Eigen3 QUIET) +if(NOT EIGEN3_FOUND) + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +endif() + +# Note that eigen 3.2 (on Ubuntu Wily) only provides EIGEN3_INCLUDE_DIR, +# not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former. +if(NOT EIGEN3_INCLUDE_DIRS) + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +endif() + +include_directories(include + ${EIGEN3_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS}) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS tf2 + DEPENDS EIGEN3) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + + +if(CATKIN_ENABLE_TESTING) + + catkin_add_gtest(tf2_eigen-test test/tf2_eigen-test.cpp) + target_link_libraries(tf2_eigen-test ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) + +endif() diff --git a/src/geometry2/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/src/geometry2/tf2_eigen/include/tf2_eigen/tf2_eigen.h new file mode 100644 index 0000000..00c789b --- /dev/null +++ b/src/geometry2/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -0,0 +1,585 @@ +/* + * Copyright (c) Koji Terada + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Koji Terada */ + +#ifndef TF2_EIGEN_H +#define TF2_EIGEN_H + +#include +#include +#include +#include +#include +#include + + +namespace tf2 +{ + +/** \brief Convert a timestamped transform to the equivalent Eigen data type. + * \param t The transform to convert, as a geometry_msgs Transform message. + * \return The transform message converted to an Eigen Isometry3d transform. + */ + inline + Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform& t) { + return Eigen::Isometry3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z) + * Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z)); +} + +/** \brief Convert a timestamped transform to the equivalent Eigen data type. + * \param t The transform to convert, as a geometry_msgs TransformedStamped message. + * \return The transform message converted to an Eigen Isometry3d transform. + */ +inline +Eigen::Isometry3d transformToEigen(const geometry_msgs::TransformStamped& t) { + return transformToEigen(t.transform); +} + +/** \brief Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. + * \param T The transform to convert, as an Eigen Affine3d transform. + * \return The transform converted to a TransformStamped message. + */ +inline +geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d& T) +{ + geometry_msgs::TransformStamped t; + t.transform.translation.x = T.translation().x(); + t.transform.translation.y = T.translation().y(); + t.transform.translation.z = T.translation().z(); + + Eigen::Quaterniond q(T.linear()); // assuming that upper 3x3 matrix is orthonormal + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + + return t; +} + +/** \brief Convert an Eigen Isometry3d transform to the equivalent geometry_msgs message type. + * \param T The transform to convert, as an Eigen Isometry3d transform. + * \return The transform converted to a TransformStamped message. + */ +inline +geometry_msgs::TransformStamped eigenToTransform(const Eigen::Isometry3d& T) +{ + geometry_msgs::TransformStamped t; + t.transform.translation.x = T.translation().x(); + t.transform.translation.y = T.translation().y(); + t.transform.translation.z = T.translation().z(); + + Eigen::Quaterniond q(T.rotation()); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + + return t; +} + +/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type. + * This function is a specialization of the doTransform template defined in tf2/convert.h, + * although it can not be used in tf2_ros::BufferInterface::transform because this + * functions rely on the existence of a time stamp and a frame id in the type which should + * get transformed. + * \param t_in The vector to transform, as a Eigen Vector3d data type. + * \param t_out The transformed vector, as a Eigen Vector3d data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geometry_msgs::TransformStamped& transform) +{ + t_out = Eigen::Vector3d(transformToEigen(transform) * t_in); +} + +/** \brief Convert a Eigen Vector3d type to a Point message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped Eigen Vector3d to convert. + * \return The vector converted to a Point message. + */ +inline +geometry_msgs::Point toMsg(const Eigen::Vector3d& in) +{ + geometry_msgs::Point msg; + msg.x = in.x(); + msg.y = in.y(); + msg.z = in.z(); + return msg; +} + +/** \brief Convert a Point message type to a Eigen-specific Vector3d type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The Point message to convert. + * \param out The point converted to a Eigen Vector3d. + */ +inline +void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) +{ + out.x() = msg.x; + out.y() = msg.y; + out.z() = msg.z; +} + +/** \brief Convert an Eigen Vector3d type to a Vector3 message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The Eigen Vector3d to convert. + * \return The vector converted to a Vector3 message. + */ +inline +geometry_msgs::Vector3& toMsg(const Eigen::Vector3d& in, geometry_msgs::Vector3& out) +{ + out.x = in.x(); + out.y = in.y(); + out.z = in.z(); + return out; +} + +/** \brief Convert a Vector3 message type to a Eigen-specific Vector3d type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The Vector3 message to convert. + * \param out The vector converted to a Eigen Vector3d. + */ +inline +void fromMsg(const geometry_msgs::Vector3& msg, Eigen::Vector3d& out) +{ + out.x() = msg.x; + out.y() = msg.y; + out.z() = msg.z; +} + +/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The vector to transform, as a timestamped Eigen Vector3d data type. + * \param t_out The transformed vector, as a timestamped Eigen Vector3d data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const tf2::Stamped& t_in, + tf2::Stamped& t_out, + const geometry_msgs::TransformStamped& transform) { + t_out = tf2::Stamped(transformToEigen(transform) * t_in, + transform.header.stamp, + transform.header.frame_id); +} + +/** \brief Convert a stamped Eigen Vector3d type to a PointStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped Eigen Vector3d to convert. + * \return The vector converted to a PointStamped message. + */ +inline +geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::PointStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.point = toMsg(static_cast(in)); + return msg; +} + +/** \brief Convert a PointStamped message type to a stamped Eigen-specific Vector3d type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The PointStamped message to convert. + * \param out The point converted to a timestamped Eigen Vector3d. + */ +inline +void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.point, static_cast(out)); +} + +/** \brief Apply a geometry_msgs Transform to an Eigen Affine3d transform. + * This function is a specialization of the doTransform template defined in tf2/convert.h, + * although it can not be used in tf2_ros::BufferInterface::transform because this + * function relies on the existence of a time stamp and a frame id in the type which should + * get transformed. + * \param t_in The frame to transform, as a Eigen Affine3d transform. + * \param t_out The transformed frame, as a Eigen Affine3d transform. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const Eigen::Affine3d& t_in, + Eigen::Affine3d& t_out, + const geometry_msgs::TransformStamped& transform) { + t_out = Eigen::Affine3d(transformToEigen(transform) * t_in); +} + +template <> +inline +void doTransform(const Eigen::Isometry3d& t_in, + Eigen::Isometry3d& t_out, + const geometry_msgs::TransformStamped& transform) { + t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in); +} + +/** \brief Convert a Eigen Quaterniond type to a Quaternion message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The Eigen Quaterniond to convert. + * \return The quaternion converted to a Quaterion message. + */ +inline +geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { + geometry_msgs::Quaternion msg; + msg.w = in.w(); + msg.x = in.x(); + msg.y = in.y(); + msg.z = in.z(); + return msg; +} + +/** \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The Quaternion message to convert. + * \param out The quaternion converted to a Eigen Quaterniond. + */ +inline +void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { + out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); +} + +/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type. + * This function is a specialization of the doTransform template defined in tf2/convert.h, + * although it can not be used in tf2_ros::BufferInterface::transform because this + * functions rely on the existence of a time stamp and a frame id in the type which should + * get transformed. + * \param t_in The vector to transform, as a Eigen Quaterniond data type. + * \param t_out The transformed vector, as a Eigen Quaterniond data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template<> +inline +void doTransform(const Eigen::Quaterniond& t_in, + Eigen::Quaterniond& t_out, + const geometry_msgs::TransformStamped& transform) { + Eigen::Quaterniond t; + fromMsg(transform.transform.rotation, t); + t_out = t.inverse() * t_in * t; +} + +/** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped Eigen Quaterniond to convert. + * \return The quaternion converted to a QuaternionStamped message. + */ +inline +geometry_msgs::QuaternionStamped toMsg(const Stamped& in) { + geometry_msgs::QuaternionStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.quaternion = toMsg(static_cast(in)); + return msg; +} + +/** \brief Convert a QuaternionStamped message type to a stamped Eigen-specific Quaterniond type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The QuaternionStamped message to convert. + * \param out The quaternion converted to a timestamped Eigen Quaterniond. + */ +inline +void fromMsg(const geometry_msgs::QuaternionStamped& msg, Stamped& out) { + out.frame_id_ = msg.header.frame_id; + out.stamp_ = msg.header.stamp; + fromMsg(msg.quaternion, static_cast(out)); +} + +/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The vector to transform, as a timestamped Eigen Quaterniond data type. + * \param t_out The transformed vector, as a timestamped Eigen Quaterniond data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const tf2::Stamped& t_in, + tf2::Stamped& t_out, + const geometry_msgs::TransformStamped& transform) { + t_out.frame_id_ = transform.header.frame_id; + t_out.stamp_ = transform.header.stamp; + doTransform(static_cast(t_in), static_cast(t_out), transform); +} + +/** \brief Convert a Eigen Affine3d transform type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The Eigen Affine3d to convert. + * \return The Eigen transform converted to a Pose message. + */ +inline +geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { + geometry_msgs::Pose msg; + msg.position.x = in.translation().x(); + msg.position.y = in.translation().y(); + msg.position.z = in.translation().z(); + Eigen::Quaterniond q(in.linear()); + msg.orientation.x = q.x(); + msg.orientation.y = q.y(); + msg.orientation.z = q.z(); + msg.orientation.w = q.w(); + if (msg.orientation.w < 0) { + msg.orientation.x *= -1; + msg.orientation.y *= -1; + msg.orientation.z *= -1; + msg.orientation.w *= -1; + } + return msg; +} + +/** \brief Convert a Eigen Isometry3d transform type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The Eigen Isometry3d to convert. + * \return The Eigen transform converted to a Pose message. + */ +inline +geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { + geometry_msgs::Pose msg; + msg.position.x = in.translation().x(); + msg.position.y = in.translation().y(); + msg.position.z = in.translation().z(); + Eigen::Quaterniond q(in.linear()); + msg.orientation.x = q.x(); + msg.orientation.y = q.y(); + msg.orientation.z = q.z(); + msg.orientation.w = q.w(); + if (msg.orientation.w < 0) { + msg.orientation.x *= -1; + msg.orientation.y *= -1; + msg.orientation.z *= -1; + msg.orientation.w *= -1; + } + return msg; +} + +/** \brief Convert a Pose message transform type to a Eigen Affine3d. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg The Pose message to convert. + * \param out The pose converted to a Eigen Affine3d. + */ +inline +void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { + out = Eigen::Affine3d( + Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) * + Eigen::Quaterniond(msg.orientation.w, + msg.orientation.x, + msg.orientation.y, + msg.orientation.z)); +} + +/** \brief Convert a Pose message transform type to a Eigen Isometry3d. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg The Pose message to convert. + * \param out The pose converted to a Eigen Isometry3d. + */ +inline +void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { + out = Eigen::Isometry3d( + Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) * + Eigen::Quaterniond(msg.orientation.w, + msg.orientation.x, + msg.orientation.y, + msg.orientation.z)); +} + +/** \brief Convert a Eigen 6x1 Matrix type to a Twist message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The 6x1 Eigen Matrix to convert. + * \return The Eigen Matrix converted to a Twist message. + */ +inline +geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { + geometry_msgs::Twist msg; + msg.linear.x = in[0]; + msg.linear.y = in[1]; + msg.linear.z = in[2]; + msg.angular.x = in[3]; + msg.angular.y = in[4]; + msg.angular.z = in[5]; + return msg; +} + +/** \brief Convert a Twist message transform type to a Eigen 6x1 Matrix. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg The Twist message to convert. + * \param out The twist converted to a Eigen 6x1 Matrix. + */ +inline +void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { + out[0] = msg.linear.x; + out[1] = msg.linear.y; + out[2] = msg.linear.z; + out[3] = msg.angular.x; + out[4] = msg.angular.y; + out[5] = msg.angular.z; +} + +/** \brief Apply a geometry_msgs TransformStamped to an Eigen Affine3d transform. + * This function is a specialization of the doTransform template defined in tf2/convert.h, + * although it can not be used in tf2_ros::BufferInterface::transform because this + * function relies on the existence of a time stamp and a frame id in the type which should + * get transformed. + * \param t_in The frame to transform, as a timestamped Eigen Affine3d transform. + * \param t_out The transformed frame, as a timestamped Eigen Affine3d transform. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const tf2::Stamped& t_in, + tf2::Stamped& t_out, + const geometry_msgs::TransformStamped& transform) { + t_out = tf2::Stamped(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} + +/** \brief Apply a geometry_msgs TransformStamped to an Eigen Isometry transform. + * This function is a specialization of the doTransform template defined in tf2/convert.h, + * although it can not be used in tf2_ros::BufferInterface::transform because this + * function relies on the existence of a time stamp and a frame id in the type which should + * get transformed. + * \param t_in The frame to transform, as a timestamped Eigen Isometry transform. + * \param t_out The transformed frame, as a timestamped Eigen Isometry transform. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const tf2::Stamped& t_in, + tf2::Stamped& t_out, + const geometry_msgs::TransformStamped& transform) { + t_out = tf2::Stamped(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id); +} + +/** \brief Convert a stamped Eigen Affine3d transform type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped Eigen Affine3d to convert. + * \return The Eigen transform converted to a PoseStamped message. + */ +inline +geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::PoseStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.pose = toMsg(static_cast(in)); + return msg; +} + +inline +geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::PoseStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.pose = toMsg(static_cast(in)); + return msg; +} + +/** \brief Convert a Pose message transform type to a stamped Eigen Affine3d. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg The PoseStamped message to convert. + * \param out The pose converted to a timestamped Eigen Affine3d. + */ +inline +void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.pose, static_cast(out)); +} + +inline +void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.pose, static_cast(out)); +} + +} // namespace + + +namespace Eigen { +// This is needed to make the usage of the following conversion functions usable in tf2::convert(). +// According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or +// in an associated namespace of one of its arguments. The stamped versions of this conversion +// functions work because they have tf2::Stamped as an argument which is the same namespace as +// which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is +// defined in tf2, so it take the following definitions in Eigen namespace to make them usable in +// tf2::convert(). + +inline +geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { + return tf2::toMsg(in); +} + +inline +geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Point toMsg(const Eigen::Vector3d& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { + tf2::fromMsg(msg, out); +} + +inline +void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { + tf2::fromMsg(msg, out); +} + +} // namespace + +#endif // TF2_EIGEN_H diff --git a/src/geometry2/tf2_eigen/mainpage.dox b/src/geometry2/tf2_eigen/mainpage.dox new file mode 100644 index 0000000..d129175 --- /dev/null +++ b/src/geometry2/tf2_eigen/mainpage.dox @@ -0,0 +1,19 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf2_eigen contains functions for converting between geometry_msgs and Eigen data types. + +This library is an implementation of the templated conversion interface specified in tf/convert.h. +It enables easy conversion from geometry_msgs Transform and Point types to the types specified +by the Eigen matrix algebra library (see http://eigen.tuxfamily.org). + +See the Conversions overview +wiki page for more information about datatype conversion in tf2. + +\section codeapi Code API + +This library consists of one header only, tf2_eigen/tf2_eigen.h, which consists mostly of +specializations of template functions defined in tf2/convert.h. + +*/ diff --git a/src/geometry2/tf2_eigen/package.xml b/src/geometry2/tf2_eigen/package.xml new file mode 100644 index 0000000..7751dfa --- /dev/null +++ b/src/geometry2/tf2_eigen/package.xml @@ -0,0 +1,20 @@ + + + tf2_eigen + 0.6.7 + tf2_eigen + Koji Terada + Koji Terada + BSD + + catkin + + geometry_msgs + tf2 + + cmake_modules + eigen + + eigen + + diff --git a/src/geometry2/tf2_eigen/test/tf2_eigen-test.cpp b/src/geometry2/tf2_eigen/test/tf2_eigen-test.cpp new file mode 100644 index 0000000..f175e6c --- /dev/null +++ b/src/geometry2/tf2_eigen/test/tf2_eigen-test.cpp @@ -0,0 +1,213 @@ +/* + * Copyright (c) Koji Terada + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + + +#include +#include +#include + +TEST(TfEigen, ConvertVector3dStamped) +{ + const tf2::Stamped v(Eigen::Vector3d(1,2,3), ros::Time(5), "test"); + + tf2::Stamped v1; + geometry_msgs::PointStamped p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v, v1); +} + +TEST(TfEigen, ConvertVector3d) +{ + const Eigen::Vector3d v(1,2,3); + + Eigen::Vector3d v1; + geometry_msgs::Point p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v, v1); +} + +TEST(TfEigen, ConvertQuaterniondStamped) +{ + const tf2::Stamped v(Eigen::Quaterniond(1,2,3,4), ros::Time(5), "test"); + + tf2::Stamped v1; + geometry_msgs::QuaternionStamped p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.frame_id_, v1.frame_id_); + EXPECT_EQ(v.stamp_, v1.stamp_); + EXPECT_EQ(v.w(), v1.w()); + EXPECT_EQ(v.x(), v1.x()); + EXPECT_EQ(v.y(), v1.y()); + EXPECT_EQ(v.z(), v1.z()); +} + +TEST(TfEigen, ConvertQuaterniond) +{ + const Eigen::Quaterniond v(1,2,3,4); + + Eigen::Quaterniond v1; + geometry_msgs::Quaternion p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.w(), v1.w()); + EXPECT_EQ(v.x(), v1.x()); + EXPECT_EQ(v.y(), v1.y()); + EXPECT_EQ(v.z(), v1.z()); +} + +TEST(TfEigen, TransformQuaterion) { + const tf2::Stamped in(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitX())), ros::Time(5), "test"); + const Eigen::Isometry3d iso(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY())); + const Eigen::Affine3d affine(iso); + const tf2::Stamped expected(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitZ())), ros::Time(10), "expected"); + + geometry_msgs::TransformStamped trafo = tf2::eigenToTransform(affine); + trafo.header.stamp = ros::Time(10); + trafo.header.frame_id = "expected"; + + tf2::Stamped out; + tf2::doTransform(in, out, trafo); + + EXPECT_TRUE(out.isApprox(expected)); + EXPECT_EQ(expected.stamp_, out.stamp_); + EXPECT_EQ(expected.frame_id_, out.frame_id_); + + // the same using Isometry + trafo = tf2::eigenToTransform(iso); + trafo.header.stamp = ros::Time(10); + trafo.header.frame_id = "expected"; + tf2::doTransform(in, out, trafo); + + EXPECT_TRUE(out.isApprox(expected)); + EXPECT_EQ(expected.stamp_, out.stamp_); + EXPECT_EQ(expected.frame_id_, out.frame_id_); +} + +TEST(TfEigen, ConvertAffine3dStamped) +{ + const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); + const tf2::Stamped v(v_nonstamped, ros::Time(42), "test_frame"); + + tf2::Stamped v1; + geometry_msgs::PoseStamped p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); + EXPECT_EQ(v.frame_id_, v1.frame_id_); + EXPECT_EQ(v.stamp_, v1.stamp_); +} + +TEST(TfEigen, ConvertIsometry3dStamped) +{ + const Eigen::Isometry3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); + const tf2::Stamped v(v_nonstamped, ros::Time(42), "test_frame"); + + tf2::Stamped v1; + geometry_msgs::PoseStamped p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); + EXPECT_EQ(v.frame_id_, v1.frame_id_); + EXPECT_EQ(v.stamp_, v1.stamp_); +} + +TEST(TfEigen, ConvertAffine3d) +{ + const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); + + Eigen::Affine3d v1; + geometry_msgs::Pose p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); +} + +TEST(TfEigen, ConvertIsometry3d) +{ + const Eigen::Isometry3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis(1, Eigen::Vector3d::UnitX())); + + Eigen::Isometry3d v1; + geometry_msgs::Pose p1; + tf2::convert(v, p1); + tf2::convert(p1, v1); + + EXPECT_EQ(v.translation(), v1.translation()); + EXPECT_EQ(v.rotation(), v1.rotation()); +} + +TEST(TfEigen, ConvertTransform) +{ + Eigen::Matrix4d tm; + + double alpha = M_PI/4.0; + double theta = M_PI/6.0; + double gamma = M_PI/12.0; + + tm << cos(theta)*cos(gamma),-cos(theta)*sin(gamma),sin(theta), 1, // + cos(alpha)*sin(gamma)+sin(alpha)*sin(theta)*cos(gamma),cos(alpha)*cos(gamma)-sin(alpha)*sin(theta)*sin(gamma),-sin(alpha)*cos(theta), 2, // + sin(alpha)*sin(gamma)-cos(alpha)*sin(theta)*cos(gamma),cos(alpha)*sin(theta)*sin(gamma)+sin(alpha)*cos(gamma),cos(alpha)*cos(theta), 3, // + 0, 0, 0, 1; + + Eigen::Affine3d T(tm); + + geometry_msgs::TransformStamped msg = tf2::eigenToTransform(T); + Eigen::Affine3d Tback = tf2::transformToEigen(msg); + + EXPECT_TRUE(T.isApprox(Tback)); + EXPECT_TRUE(tm.isApprox(Tback.matrix())); + + // same for Isometry + Eigen::Isometry3d I(tm); + + msg = tf2::eigenToTransform(T); + Eigen::Isometry3d Iback = tf2::transformToEigen(msg); + + EXPECT_TRUE(I.isApprox(Iback)); + EXPECT_TRUE(tm.isApprox(Iback.matrix())); +} + + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/src/geometry2/tf2_geometry_msgs/CHANGELOG.rst b/src/geometry2/tf2_geometry_msgs/CHANGELOG.rst new file mode 100644 index 0000000..e9bb12c --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/CHANGELOG.rst @@ -0,0 +1,223 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_geometry_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ + +0.6.6 (2020-01-09) +------------------ +* Make kdl headers available (`#419 `_) +* Fix python3 compatibility for noetic (`#416 `_) +* add from STL (`#366 `_) +* use ROS_DEPRECATED macro for portability (`#362 `_) +* Contributors: James Xu, Shane Loretz, Tully Foote + +0.6.5 (2018-11-16) +------------------ +* Fix python3 import error +* Contributors: Timon Engelke + +0.6.4 (2018-11-06) +------------------ + +0.6.3 (2018-07-09) +------------------ +* Changed access to Vector to prevent memory leak (`#305 `_) +* Added WrenchStamped transformation (`#302 `_) +* Contributors: Denis Štogl, Markus Grimm + +0.6.2 (2018-05-02) +------------------ + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ +* Boilerplate for Sphinx (`#284 `_) + Fixes `#264 `_ +* tf2_geometry_msgs added doTransform implementations for not stamped types (`#262 `_) + * tf2_geometry_msgs added doTransform implementations for not stamped Point, Quaterion, Pose and Vector3 message types +* New functionality to transform PoseWithCovarianceStamped messages. (`#282 `_) + * New functionality to transform PoseWithCovarianceStamped messages. +* Contributors: Blake Anderson, Tully Foote, cwecht + +0.5.17 (2018-01-01) +------------------- + +0.5.16 (2017-07-14) +------------------- +* remove explicit templating to standardize on overloading. But provide backwards compatibility with deprecation. +* adding unit tests for conversions +* Copy transform before altering it in do_transform_vector3 [issue 233] (`#235 `_) +* store gtest return value as int (`#229 `_) +* Document the lifetime of the returned reference for getFrameId and getTimestamp +* tf2_geometry_msgs: using tf2::Transform in doTransform-functions, marked gmTransformToKDL as deprecated +* Switch tf2_geometry_msgs to use package.xml format 2 (`#217 `_) +* tf2_geometry_msgs: added missing conversion functions +* Contributors: Christopher Wecht, Sebastian Wagner, Tully Foote, dhood, pAIgn10 + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- +* Add doxygen documentation for tf2_geometry_msgs +* Contributors: Jackie Kay + +0.5.13 (2016-03-04) +------------------- +* Add missing python_orocos_kdl dependency +* make example into unit test +* vector3 not affected by translation +* Contributors: Daniel Claes, chapulina + +0.5.12 (2015-08-05) +------------------- +* Merge pull request `#112 `_ from vrabaud/getYaw + Get yaw +* add toMsg and fromMsg for QuaternionStamped +* Contributors: Tully Foote, Vincent Rabaud + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- + +0.5.9 (2015-03-25) +------------------ + +0.5.8 (2015-03-17) +------------------ +* remove useless Makefile files +* tf2 optimizations +* add conversions of type between tf2 and geometry_msgs +* fix ODR violations +* Contributors: Vincent Rabaud + +0.5.7 (2014-12-23) +------------------ +* fixing transitive dependency for kdl. Fixes `#53 `_ +* Contributors: Tully Foote + +0.5.6 (2014-09-18) +------------------ + +0.5.5 (2014-06-23) +------------------ + +0.5.4 (2014-05-07) +------------------ + +0.5.3 (2014-02-21) +------------------ + +0.5.2 (2014-02-20) +------------------ + +0.5.1 (2014-02-14) +------------------ + +0.5.0 (2014-02-14) +------------------ + +0.4.10 (2013-12-26) +------------------- + +0.4.9 (2013-11-06) +------------------ + +0.4.8 (2013-11-06) +------------------ + +0.4.7 (2013-08-28) +------------------ + +0.4.6 (2013-08-28) +------------------ + +0.4.5 (2013-07-11) +------------------ + +0.4.4 (2013-07-09) +------------------ +* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. + +0.4.3 (2013-07-05) +------------------ + +0.4.2 (2013-07-05) +------------------ + +0.4.1 (2013-07-05) +------------------ + +0.4.0 (2013-06-27) +------------------ +* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 +* Cleaning up unnecessary dependency on roscpp +* converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace +* Cleaning up packaging of tf2 including: + removing unused nodehandle + cleaning up a few dependencies and linking + removing old backup of package.xml + making diff minimally different from tf version of library +* Restoring test packages and bullet packages. + reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented + reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ + +0.3.6 (2013-03-03) +------------------ + +0.3.5 (2013-02-15 14:46) +------------------------ +* 0.3.4 -> 0.3.5 + +0.3.4 (2013-02-15 13:14) +------------------------ +* 0.3.3 -> 0.3.4 + +0.3.3 (2013-02-15 11:30) +------------------------ +* 0.3.2 -> 0.3.3 + +0.3.2 (2013-02-15 00:42) +------------------------ +* 0.3.1 -> 0.3.2 + +0.3.1 (2013-02-14) +------------------ +* 0.3.0 -> 0.3.1 + +0.3.0 (2013-02-13) +------------------ +* switching to version 0.3.0 +* add setup.py +* added setup.py etc to tf2_geometry_msgs +* adding tf2 dependency to tf2_geometry_msgs +* adding tf2_geometry_msgs to groovy-devel (unit tests disabled) +* fixing groovy-devel +* removing bullet and kdl related packages +* disabling tf2_geometry_msgs due to missing kdl dependency +* catkinizing geometry-experimental +* catkinizing tf2_geometry_msgs +* add twist, wrench and pose conversion to kdl, fix message to message conversion by adding specific conversion functions +* merge tf2_cpp and tf2_py into tf2_ros +* Got transform with types working in python +* A working first version of transforming and converting between different types +* Moving from camelCase to undescores to be in line with python style guides +* Fixing tests now that Buffer creates a NodeHandle +* add posestamped +* import vector3stamped +* add support for Vector3Stamped and PoseStamped +* add support for PointStamped geometry_msgs +* add regression tests for geometry_msgs point, vector and pose +* Fixing missing export, compiling version of buffer_client test +* add bullet transforms, and create tests for bullet and kdl +* working transformations of messages +* add support for PoseStamped message +* test for pointstamped +* add PointStamped message transform methods +* transform for vector3stamped message diff --git a/src/geometry2/tf2_geometry_msgs/CMakeLists.txt b/src/geometry2/tf2_geometry_msgs/CMakeLists.txt new file mode 100644 index 0000000..4757437 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_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) + +catkin_add_gtest(test_tomsg_frommsg test/test_tomsg_frommsg.cpp) +target_include_directories(test_tomsg_frommsg PUBLIC ${orocos_kdl_INCLUDE_DIRS}) +target_link_libraries(test_tomsg_frommsg ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) + +find_package(catkin REQUIRED COMPONENTS geometry_msgs rostest tf2_ros tf2) + +add_executable(test_geometry_msgs EXCLUDE_FROM_ALL test/test_tf2_geometry_msgs.cpp) +target_include_directories(test_geometry_msgs PUBLIC ${orocos_kdl_INCLUDE_DIRS}) +target_link_libraries(test_geometry_msgs ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES}) +add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) +add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_python.launch) + + +if(TARGET tests) + add_dependencies(tests test_geometry_msgs) +endif() + + +endif() diff --git a/src/geometry2/tf2_geometry_msgs/conf.py b/src/geometry2/tf2_geometry_msgs/conf.py new file mode 100644 index 0000000..d358e5b --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/conf.py @@ -0,0 +1,290 @@ +# -*- coding: utf-8 -*- +# +# tf2_geometry_msgs documentation build configuration file, created by +# sphinx-quickstart on Tue Feb 13 15:34:25 2018. +# +# This file is execfile()d with the current directory set to its +# containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys +import os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.insert(0, os.path.abspath('.')) + +# -- General configuration ------------------------------------------------ + +# If your documentation needs a minimal Sphinx version, state it here. +#needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.doctest', + 'sphinx.ext.intersphinx', + 'sphinx.ext.todo', + 'sphinx.ext.pngmath', + 'sphinx.ext.viewcode', +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# source_suffix = ['.rst', '.md'] +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'tf2_geometry_msgs' +copyright = u'2018, Open Source Robotics Foundation, Inc.' +author = u'Tully Foote' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = u'0.1' +# The full version, including alpha/beta/rc tags. +release = u'0.1' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = ['_build'] + +# The reST default role (used for this markup: `text`) to use for all +# documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + +# If true, keep warnings as "system message" paragraphs in the built documents. +#keep_warnings = False + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. Major themes that come with +# Sphinx are currently 'default' and 'sphinxdoc'. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (relative to this directory) to use as a favicon of +# the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +#html_extra_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_domain_indices = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +#html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +#html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = None + +# Language to be used for generating the HTML full-text search index. +# Sphinx supports the following languages: +# 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' +# 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' +#html_search_language = 'en' + +# A dictionary with options for the search language support, empty by default. +# Now only 'ja' uses this config value +#html_search_options = {'type': 'default'} + +# The name of a javascript file (relative to the configuration directory) that +# implements a search results scorer. If empty, the default will be used. +#html_search_scorer = 'scorer.js' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'tf2_geometry_msgsdoc' + +# -- Options for LaTeX output --------------------------------------------- + +latex_elements = { +# The paper size ('letterpaper' or 'a4paper'). +#'papersize': 'letterpaper', + +# The font size ('10pt', '11pt' or '12pt'). +#'pointsize': '10pt', + +# Additional stuff for the LaTeX preamble. +#'preamble': '', + +# Latex figure (float) alignment +#'figure_align': 'htbp', +} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + (master_doc, 'tf2_geometry_msgs.tex', u'tf2\\_geometry\\_msgs Documentation', + u'Tully Foote', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# If true, show page references after internal links. +#latex_show_pagerefs = False + +# If true, show URL addresses after external links. +#latex_show_urls = False + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_domain_indices = True + + +# -- Options for manual page output --------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + (master_doc, 'tf2_geometry_msgs', u'tf2_geometry_msgs Documentation', + [author], 1) +] + +# If true, show URL addresses after external links. +#man_show_urls = False + + +# -- Options for Texinfo output ------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + (master_doc, 'tf2_geometry_msgs', u'tf2_geometry_msgs Documentation', + author, 'tf2_geometry_msgs', 'One line description of project.', + 'Miscellaneous'), +] + +# Documents to append as an appendix to all manuals. +#texinfo_appendices = [] + +# If false, no module index is generated. +#texinfo_domain_indices = True + +# How to display URL addresses: 'footnote', 'no', or 'inline'. +#texinfo_show_urls = 'footnote' + +# If true, do not generate a @detailmenu in the "Top" node's menu. +#texinfo_no_detailmenu = False + + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {'https://docs.python.org/': None} diff --git a/src/geometry2/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h b/src/geometry2/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h new file mode 100644 index 0000000..717b5eb --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h @@ -0,0 +1,1058 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + +#ifndef TF2_GEOMETRY_MSGS_H +#define TF2_GEOMETRY_MSGS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "ros/macros.h" + +namespace tf2 +{ + +/** \brief Convert a TransformStamped message to a KDL frame. + * \param t TransformStamped message to convert. + * \return The converted KDL Frame. + * \deprecated + */ +inline +ROS_DEPRECATED KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t); +inline +KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) + { + return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); + } + + +/*************/ +/** Vector3 **/ +/*************/ + +/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A tf2 Vector3 object. + * \return The Vector3 converted to a geometry_msgs message type. + */ +inline +geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) +{ + geometry_msgs::Vector3 out; + out.x = in.getX(); + out.y = in.getY(); + out.z = in.getZ(); + return out; +} + +/** \brief Convert a Vector3 message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param in A Vector3 message type. + * \param out The Vector3 converted to a tf2 type. + */ +inline +void fromMsg(const geometry_msgs::Vector3& in, tf2::Vector3& out) +{ + out = tf2::Vector3(in.x, in.y, in.z); +} + + +/********************/ +/** Vector3Stamped **/ +/********************/ + +/** \brief Extract a timestamp from the header of a Vector message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t VectorStamped message to extract the timestamp from. + * \return The timestamp of the message. The lifetime of the returned reference + * is bound to the lifetime of the argument. + */ +template <> +inline + const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;} + +/** \brief Extract a frame ID from the header of a Vector message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t VectorStamped message to extract the frame ID from. + * \return A string containing the frame ID of the message. The lifetime of the + * returned reference is bound to the lifetime of the argument. + */ +template <> +inline + const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;} + + +/** \brief Trivial "conversion" function for Vector3 message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A Vector3Stamped message. + * \return The input argument. + */ +inline +geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in) +{ + return in; +} + +/** \brief Trivial "conversion" function for Vector3 message type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A Vector3Stamped message. + * \param out The input argument. + */ +inline +void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out) +{ + out = msg; +} + +/** \brief Convert as stamped tf2 Vector3 type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in An instance of the tf2::Vector3 specialization of the tf2::Stamped template. + * \return The Vector3Stamped converted to a geometry_msgs Vector3Stamped message type. + */ +inline +geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::Vector3Stamped out; + out.header.stamp = in.stamp_; + out.header.frame_id = in.frame_id_; + out.vector.x = in.getX(); + out.vector.y = in.getY(); + out.vector.z = in.getZ(); + return out; +} + +/** \brief Convert a Vector3Stamped message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A Vector3Stamped message. + * \param out The Vector3Stamped converted to the equivalent tf2 type. + */ +inline +void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + out.setData(tf2::Vector3(msg.vector.x, msg.vector.y, msg.vector.z)); +} + + +/***********/ +/** Point **/ +/***********/ + +/** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A tf2 Vector3 object. + * \return The Vector3 converted to a geometry_msgs message type. + */ +inline +geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) +{ + out.x = in.getX(); + out.y = in.getY(); + out.z = in.getZ(); + return out; +} + +/** \brief Convert a Vector3 message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param in A Vector3 message type. + * \param out The Vector3 converted to a tf2 type. + */ +inline +void fromMsg(const geometry_msgs::Point& in, tf2::Vector3& out) +{ + out = tf2::Vector3(in.x, in.y, in.z); +} + + +/******************/ +/** PointStamped **/ +/******************/ + +/** \brief Extract a timestamp from the header of a Point message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t PointStamped message to extract the timestamp from. + * \return The timestamp of the message. The lifetime of the returned reference + * is bound to the lifetime of the argument. + */ +template <> +inline + const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;} + +/** \brief Extract a frame ID from the header of a Point message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t PointStamped message to extract the frame ID from. + * \return A string containing the frame ID of the message. The lifetime of the + * returned reference is bound to the lifetime of the argument. + */ +template <> +inline + const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;} + +/** \brief Trivial "conversion" function for Point message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A PointStamped message. + * \return The input argument. + */ +inline +geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in) +{ + return in; +} + +/** \brief Trivial "conversion" function for Point message type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A PointStamped message. + * \param out The input argument. + */ +inline +void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out) +{ + out = msg; +} + +/** \brief Convert as stamped tf2 Vector3 type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in An instance of the tf2::Vector3 specialization of the tf2::Stamped template. + * \return The Vector3Stamped converted to a geometry_msgs PointStamped message type. + */ +inline +geometry_msgs::PointStamped toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped & out) +{ + out.header.stamp = in.stamp_; + out.header.frame_id = in.frame_id_; + out.point.x = in.getX(); + out.point.y = in.getY(); + out.point.z = in.getZ(); + return out; +} + +/** \brief Convert a PointStamped message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A PointStamped message. + * \param out The PointStamped converted to the equivalent tf2 type. + */ +inline +void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + out.setData(tf2::Vector3(msg.point.x, msg.point.y, msg.point.z)); +} + + +/****************/ +/** Quaternion **/ +/****************/ + +/** \brief Convert a tf2 Quaternion type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A tf2 Quaternion object. + * \return The Quaternion converted to a geometry_msgs message type. + */ +inline +geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in) +{ + geometry_msgs::Quaternion out; + out.w = in.getW(); + out.x = in.getX(); + out.y = in.getY(); + out.z = in.getZ(); + return out; +} + +/** \brief Convert a Quaternion message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param in A Quaternion message type. + * \param out The Quaternion converted to a tf2 type. + */ +inline +void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out) +{ + // w at the end in the constructor + out = tf2::Quaternion(in.x, in.y, in.z, in.w); +} + + +/***********************/ +/** QuaternionStamped **/ +/***********************/ + +/** \brief Extract a timestamp from the header of a Quaternion message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t QuaternionStamped message to extract the timestamp from. + * \return The timestamp of the message. The lifetime of the returned reference + * is bound to the lifetime of the argument. + */ +template <> +inline +const ros::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;} + +/** \brief Extract a frame ID from the header of a Quaternion message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t QuaternionStamped message to extract the frame ID from. + * \return A string containing the frame ID of the message. The lifetime of the + * returned reference is bound to the lifetime of the argument. + */ +template <> +inline +const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;} + +/** \brief Trivial "conversion" function for Quaternion message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A QuaternionStamped message. + * \return The input argument. + */ +inline +geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in) +{ + return in; +} + +/** \brief Trivial "conversion" function for Quaternion message type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A QuaternionStamped message. + * \param out The input argument. + */ +inline +void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out) +{ + out = msg; +} + +/** \brief Convert as stamped tf2 Quaternion type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in An instance of the tf2::Quaternion specialization of the tf2::Stamped template. + * \return The QuaternionStamped converted to a geometry_msgs QuaternionStamped message type. + */ +inline +geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::QuaternionStamped out; + out.header.stamp = in.stamp_; + out.header.frame_id = in.frame_id_; + out.quaternion.w = in.getW(); + out.quaternion.x = in.getX(); + out.quaternion.y = in.getY(); + out.quaternion.z = in.getZ(); + return out; +} + +template <> +inline +ROS_DEPRECATED geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in); + + +//Backwards compatibility remove when forked for Lunar or newer +template <> +inline +geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) +{ + return toMsg(in); +} + +/** \brief Convert a QuaternionStamped message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param in A QuaternionStamped message type. + * \param out The QuaternionStamped converted to the equivalent tf2 type. + */ +inline +void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) +{ + out.stamp_ = in.header.stamp; + out.frame_id_ = in.header.frame_id; + tf2::Quaternion tmp; + fromMsg(in.quaternion, tmp); + out.setData(tmp); +} + +template<> +inline +ROS_DEPRECATED void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out); + +//Backwards compatibility remove when forked for Lunar or newer +template<> +inline +void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) +{ + fromMsg(in, out); +} + +/**********/ +/** Pose **/ +/**********/ + +/** \brief Convert a tf2 Transform type to an equivalent geometry_msgs Pose message. + * \param in A tf2 Transform object. + * \param out The Transform converted to a geometry_msgs Pose message type. + */ +inline +geometry_msgs::Pose& toMsg(const tf2::Transform& in, geometry_msgs::Pose& out) +{ + toMsg(in.getOrigin(), out.position); + out.orientation = toMsg(in.getRotation()); + return out; +} + +/** \brief Convert a geometry_msgs Pose message to an equivalent tf2 Transform type. + * \param in A Pose message. + * \param out The Pose converted to a tf2 Transform type. + */ +inline +void fromMsg(const geometry_msgs::Pose& in, tf2::Transform& out) +{ + out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z)); + // w at the end in the constructor + out.setRotation(tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w)); +} + + + +/*****************/ +/** PoseStamped **/ +/*****************/ + +/** \brief Extract a timestamp from the header of a Pose message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t PoseStamped message to extract the timestamp from. + * \return The timestamp of the message. + */ +template <> +inline + const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;} + +/** \brief Extract a frame ID from the header of a Pose message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t PoseStamped message to extract the frame ID from. + * \return A string containing the frame ID of the message. + */ +template <> +inline + const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;} + +/** \brief Trivial "conversion" function for Pose message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A PoseStamped message. + * \return The input argument. + */ +inline +geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in) +{ + return in; +} + +/** \brief Trivial "conversion" function for Pose message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg A PoseStamped message. + * \param out The input argument. + */ +inline +void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out) +{ + out = msg; +} + +/** \brief Convert as stamped tf2 Pose type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in An instance of the tf2::Pose specialization of the tf2::Stamped template. + * \return The PoseStamped converted to a geometry_msgs PoseStamped message type. + */ +inline +geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped & out) +{ + out.header.stamp = in.stamp_; + out.header.frame_id = in.frame_id_; + toMsg(in.getOrigin(), out.pose.position); + out.pose.orientation = toMsg(in.getRotation()); + return out; +} + +/** \brief Convert a PoseStamped message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A PoseStamped message. + * \param out The PoseStamped converted to the equivalent tf2 type. + */ +inline +void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + tf2::Transform tmp; + fromMsg(msg.pose, tmp); + out.setData(tmp); +} + +/*******************************/ +/** PoseWithCovarianceStamped **/ +/*******************************/ + +/** \brief Extract a timestamp from the header of a PoseWithCovarianceStamped message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t PoseWithCovarianceStamped message to extract the timestamp from. + * \return The timestamp of the message. + */ +template <> +inline + const ros::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.stamp;} + +/** \brief Extract a frame ID from the header of a PoseWithCovarianceStamped message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t PoseWithCovarianceStamped message to extract the frame ID from. + * \return A string containing the frame ID of the message. + */ +template <> +inline + const std::string& getFrameId(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.frame_id;} + +/** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A PoseWithCovarianceStamped message. + * \return The input argument. + */ +inline +geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCovarianceStamped& in) +{ + return in; +} + +/** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg A PoseWithCovarianceStamped message. + * \param out The input argument. + */ +inline +void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs::PoseWithCovarianceStamped& out) +{ + out = msg; +} + +/** \brief Convert as stamped tf2 PoseWithCovarianceStamped type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in An instance of the tf2::Pose specialization of the tf2::Stamped template. + * \return The PoseWithCovarianceStamped converted to a geometry_msgs PoseWithCovarianceStamped message type. + */ +inline +geometry_msgs::PoseWithCovarianceStamped toMsg(const tf2::Stamped& in, geometry_msgs::PoseWithCovarianceStamped & out) +{ + out.header.stamp = in.stamp_; + out.header.frame_id = in.frame_id_; + toMsg(in.getOrigin(), out.pose.pose.position); + out.pose.pose.orientation = toMsg(in.getRotation()); + return out; +} + +/** \brief Convert a PoseWithCovarianceStamped message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A PoseWithCovarianceStamped message. + * \param out The PoseWithCovarianceStamped converted to the equivalent tf2 type. + */ +inline +void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + tf2::Transform tmp; + fromMsg(msg.pose, tmp); + out.setData(tmp); +} + +/***************/ +/** Transform **/ +/***************/ + +/** \brief Convert a tf2 Transform type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A tf2 Transform object. + * \return The Transform converted to a geometry_msgs message type. + */ +inline +geometry_msgs::Transform toMsg(const tf2::Transform& in) +{ + geometry_msgs::Transform out; + out.translation = toMsg(in.getOrigin()); + out.rotation = toMsg(in.getRotation()); + return out; +} + +/** \brief Convert a Transform message to its equivalent tf2 representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A Transform message type. + * \param out The Transform converted to a tf2 type. + */ +inline +void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) +{ + tf2::Vector3 v; + fromMsg(in.translation, v); + out.setOrigin(v); + // w at the end in the constructor + tf2::Quaternion q; + fromMsg(in.rotation, q); + out.setRotation(q); +} + + +/**********************/ +/** TransformStamped **/ +/**********************/ + +/** \brief Extract a timestamp from the header of a Transform message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t TransformStamped message to extract the timestamp from. + * \return The timestamp of the message. + */ +template <> +inline +const ros::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;} + +/** \brief Extract a frame ID from the header of a Transform message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t TransformStamped message to extract the frame ID from. + * \return A string containing the frame ID of the message. + */ +template <> +inline +const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;} + +/** \brief Trivial "conversion" function for Transform message type. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in A TransformStamped message. + * \return The input argument. + */ +inline +geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in) +{ + return in; +} + +/** \brief Convert a TransformStamped message to its equivalent tf2 representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param msg A TransformStamped message type. + * \param out The TransformStamped converted to the equivalent tf2 type. + */ +inline +void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out) +{ + out = msg; +} + +/** \brief Convert as stamped tf2 Transform type to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in An instance of the tf2::Transform specialization of the tf2::Stamped template. + * \return The tf2::Stamped converted to a geometry_msgs TransformStamped message type. + */ +inline +geometry_msgs::TransformStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::TransformStamped out; + out.header.stamp = in.stamp_; + out.header.frame_id = in.frame_id_; + out.transform.translation = toMsg(in.getOrigin()); + out.transform.rotation = toMsg(in.getRotation()); + return out; +} + + +/** \brief Convert a TransformStamped message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg A TransformStamped message. + * \param out The TransformStamped converted to the equivalent tf2 type. + */ +inline +void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + tf2::Transform tmp; + fromMsg(msg.transform, tmp); + out.setData(tmp); +} + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Point type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The point to transform, as a Point3 message. + * \param t_out The transformed point, as a Point3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out, const geometry_msgs::TransformStamped& transform) + { + tf2::Transform t; + fromMsg(transform.transform, t); + tf2::Vector3 v_in; + fromMsg(t_in, v_in); + tf2::Vector3 v_out = t * v_in; + toMsg(v_out, t_out); + } + +/** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Point type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The point to transform, as a timestamped Point3 message. + * \param t_out The transformed point, as a timestamped Point3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform) + { + doTransform(t_in.point, t_out.point, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; + } + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The quaternion to transform, as a Quaternion3 message. + * \param t_out The transformed quaternion, as a Quaternion3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternion& t_out, const geometry_msgs::TransformStamped& transform) +{ + tf2::Quaternion t, q_in; + fromMsg(transform.transform.rotation, t); + fromMsg(t_in, q_in); + + tf2::Quaternion q_out = t * q_in; + t_out = toMsg(q_out); +} + +/** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Quaternion type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The quaternion to transform, as a timestamped Quaternion3 message. + * \param t_out The transformed quaternion, as a timestamped Quaternion3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const geometry_msgs::TransformStamped& transform) +{ + doTransform(t_in.quaternion, t_out.quaternion, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; +} + + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. +* This function is a specialization of the doTransform template defined in tf2/convert.h. +* \param t_in The pose to transform, as a Pose3 message. +* \param t_out The transformed pose, as a Pose3 message. +* \param transform The timestamped transform to apply, as a TransformStamped message. +*/ +template <> +inline +void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, const geometry_msgs::TransformStamped& transform) +{ + tf2::Vector3 v; + fromMsg(t_in.position, v); + tf2::Quaternion r; + fromMsg(t_in.orientation, r); + + tf2::Transform t; + fromMsg(transform.transform, t); + tf2::Transform v_out = t * tf2::Transform(r, v); + toMsg(v_out, t_out); +} + +/** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Pose type. +* This function is a specialization of the doTransform template defined in tf2/convert.h. +* \param t_in The pose to transform, as a timestamped Pose3 message. +* \param t_out The transformed pose, as a timestamped Pose3 message. +* \param transform The timestamped transform to apply, as a TransformStamped message. +*/ +template <> +inline +void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform) +{ + doTransform(t_in.pose, t_out.pose, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; +} + +/** \brief Transform the covariance matrix of a PoseWithCovarianceStamped message to a new frame. +* \param t_in The covariance matrix to transform. +* \param transform The timestamped transform to apply, as a TransformStamped message. +* \return The transformed covariance matrix. +*/ +inline +geometry_msgs::PoseWithCovariance::_covariance_type transformCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type& cov_in, const tf2::Transform& transform) +{ + /** + * To transform a covariance matrix: + * + * [R 0] COVARIANCE [R' 0 ] + * [0 R] [0 R'] + * + * Where: + * R is the rotation matrix (3x3). + * R' is the transpose of the rotation matrix. + * COVARIANCE is the 6x6 covariance matrix to be transformed. + */ + + // get rotation matrix transpose + const tf2::Matrix3x3 R_transpose = transform.getBasis().transpose(); + + // convert the covariance matrix into four 3x3 blocks + const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2], + cov_in[6], cov_in[7], cov_in[8], + cov_in[12], cov_in[13], cov_in[14]); + const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5], + cov_in[9], cov_in[10], cov_in[11], + cov_in[15], cov_in[16], cov_in[17]); + const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20], + cov_in[24], cov_in[25], cov_in[26], + cov_in[30], cov_in[31], cov_in[32]); + const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23], + cov_in[27], cov_in[28], cov_in[29], + cov_in[33], cov_in[34], cov_in[35]); + + // perform blockwise matrix multiplication + const tf2::Matrix3x3 result_11 = transform.getBasis()*cov_11*R_transpose; + const tf2::Matrix3x3 result_12 = transform.getBasis()*cov_12*R_transpose; + const tf2::Matrix3x3 result_21 = transform.getBasis()*cov_21*R_transpose; + const tf2::Matrix3x3 result_22 = transform.getBasis()*cov_22*R_transpose; + + // form the output + geometry_msgs::PoseWithCovariance::_covariance_type output; + output[0] = result_11[0][0]; + output[1] = result_11[0][1]; + output[2] = result_11[0][2]; + output[6] = result_11[1][0]; + output[7] = result_11[1][1]; + output[8] = result_11[1][2]; + output[12] = result_11[2][0]; + output[13] = result_11[2][1]; + output[14] = result_11[2][2]; + + output[3] = result_12[0][0]; + output[4] = result_12[0][1]; + output[5] = result_12[0][2]; + output[9] = result_12[1][0]; + output[10] = result_12[1][1]; + output[11] = result_12[1][2]; + output[15] = result_12[2][0]; + output[16] = result_12[2][1]; + output[17] = result_12[2][2]; + + output[18] = result_21[0][0]; + output[19] = result_21[0][1]; + output[20] = result_21[0][2]; + output[24] = result_21[1][0]; + output[25] = result_21[1][1]; + output[26] = result_21[1][2]; + output[30] = result_21[2][0]; + output[31] = result_21[2][1]; + output[32] = result_21[2][2]; + + output[21] = result_22[0][0]; + output[22] = result_22[0][1]; + output[23] = result_22[0][2]; + output[27] = result_22[1][0]; + output[28] = result_22[1][1]; + output[29] = result_22[1][2]; + output[33] = result_22[2][0]; + output[34] = result_22[2][1]; + output[35] = result_22[2][2]; + + return output; +} + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs PoseWithCovarianceStamped type. +* This function is a specialization of the doTransform template defined in tf2/convert.h. +* \param t_in The pose to transform, as a timestamped PoseWithCovarianceStamped message. +* \param t_out The transformed pose, as a timestamped PoseWithCovarianceStamped message. +* \param transform The timestamped transform to apply, as a TransformStamped message. +*/ +template <> +inline +void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_msgs::PoseWithCovarianceStamped& t_out, const geometry_msgs::TransformStamped& transform) +{ + tf2::Vector3 v; + fromMsg(t_in.pose.pose.position, v); + tf2::Quaternion r; + fromMsg(t_in.pose.pose.orientation, r); + + tf2::Transform t; + fromMsg(transform.transform, t); + tf2::Transform v_out = t * tf2::Transform(r, v); + toMsg(v_out, t_out.pose.pose); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; + + t_out.pose.covariance = transformCovariance(t_in.pose.covariance, t); +} + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Transform type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The frame to transform, as a timestamped Transform3 message. + * \param t_out The frame transform, as a timestamped Transform3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline +void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform) + { + tf2::Transform input; + fromMsg(t_in.transform, input); + + tf2::Transform t; + fromMsg(transform.transform, t); + tf2::Transform v_out = t * input; + + t_out.transform = toMsg(v_out); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; + } + +/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The vector to transform, as a Vector3 message. + * \param t_out The transformed vector, as a Vector3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const geometry_msgs::TransformStamped& transform) + { + tf2::Transform t; + fromMsg(transform.transform, t); + tf2::Vector3 v_out = t.getBasis() * tf2::Vector3(t_in.x, t_in.y, t_in.z); + t_out.x = v_out[0]; + t_out.y = v_out[1]; + t_out.z = v_out[2]; + } + +/** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Vector type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The vector to transform, as a timestamped Vector3 message. + * \param t_out The transformed vector, as a timestamped Vector3 message. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform) + { + doTransform(t_in.vector, t_out.vector, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; + } + + +/**********************/ +/*** WrenchStamped ****/ +/**********************/ +template <> +inline +const ros::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return t.header.stamp;} + + +template <> +inline +const std::string& getFrameId(const geometry_msgs::WrenchStamped& t) {return t.header.frame_id;} + + +inline +geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in) +{ + return in; +} + +inline +void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out) +{ + out = msg; +} + + +inline +geometry_msgs::WrenchStamped toMsg(const tf2::Stamped>& in, geometry_msgs::WrenchStamped & out) +{ + out.header.stamp = in.stamp_; + out.header.frame_id = in.frame_id_; + out.wrench.force = toMsg(in[0]); + out.wrench.torque = toMsg(in[1]); + return out; +} + + +inline +void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped>& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + tf2::Vector3 tmp; + fromMsg(msg.wrench.force, tmp); + tf2::Vector3 tmp1; + fromMsg(msg.wrench.torque, tmp1); + std::array tmp_array; + tmp_array[0] = tmp; + tmp_array[1] = tmp1; + out.setData(tmp_array); +} + +template<> +inline +void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out, const geometry_msgs::TransformStamped& transform) +{ + doTransform(t_in.force, t_out.force, transform); + doTransform(t_in.torque, t_out.torque, transform); +} + + +template<> +inline +void doTransform(const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out, const geometry_msgs::TransformStamped& transform) +{ + doTransform(t_in.wrench, t_out.wrench, transform); + t_out.header.stamp = transform.header.stamp; + t_out.header.frame_id = transform.header.frame_id; +} + +} // namespace + +#endif // TF2_GEOMETRY_MSGS_H diff --git a/src/geometry2/tf2_geometry_msgs/index.rst b/src/geometry2/tf2_geometry_msgs/index.rst new file mode 100644 index 0000000..b2cbcad --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/index.rst @@ -0,0 +1,17 @@ +Welcome to tf2_geometry_msgs's documentation! +============================================= + +Contents: + +.. toctree:: + :maxdepth: 2 + + + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` +* :ref:`search` + diff --git a/src/geometry2/tf2_geometry_msgs/mainpage.dox b/src/geometry2/tf2_geometry_msgs/mainpage.dox new file mode 100644 index 0000000..3641532 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/mainpage.dox @@ -0,0 +1,19 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf2_geometry_msgs contains functions for converting between various geometry_msgs data types. + +This library is an implementation of the templated conversion interface specified in tf/convert.h. +It offers conversion and transform convenience functions between various geometry_msgs data types, +such as Vector, Point, Pose, Transform, Quaternion, etc. + +See the Conversions overview +wiki page for more information about datatype conversion in tf2. + +\section codeapi Code API + +This library consists of one header only, tf2_geometry_msgs/tf2_geometry_msgs.h, which consists mostly of +specializations of template functions defined in tf2/convert.h. + +*/ diff --git a/src/geometry2/tf2_geometry_msgs/package.xml b/src/geometry2/tf2_geometry_msgs/package.xml new file mode 100644 index 0000000..43704ab --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/package.xml @@ -0,0 +1,27 @@ + + tf2_geometry_msgs + 0.6.7 + + tf2_geometry_msgs + + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/tf2_ros + + catkin + + geometry_msgs + orocos_kdl + tf2 + tf2_ros + + python_orocos_kdl + + python_orocos_kdl + + ros_environment + rostest + + diff --git a/src/geometry2/tf2_geometry_msgs/rosdoc.yaml b/src/geometry2/tf2_geometry_msgs/rosdoc.yaml new file mode 100644 index 0000000..a1d78b9 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/rosdoc.yaml @@ -0,0 +1,7 @@ + - builder: doxygen + name: C++ API + output_dir: c++ + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' + - builder: sphinx + name: Python API + output_dir: python diff --git a/src/geometry2/tf2_geometry_msgs/scripts/test.py b/src/geometry2/tf2_geometry_msgs/scripts/test.py new file mode 100755 index 0000000..25555da --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/scripts/test.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python + +import unittest +import rospy +import PyKDL +import tf2_ros +import tf2_geometry_msgs +from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped, WrenchStamped + +class GeometryMsgs(unittest.TestCase): + def test_transform(self): + b = tf2_ros.Buffer() + t = TransformStamped() + t.transform.translation.x = 1 + t.transform.rotation.x = 1 + t.header.stamp = rospy.Time(2.0) + t.header.frame_id = 'a' + t.child_frame_id = 'b' + b.set_transform(t, 'eitan_rocks') + out = b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0)) + self.assertEqual(out.transform.translation.x, 1) + self.assertEqual(out.transform.rotation.x, 1) + self.assertEqual(out.header.frame_id, 'a') + self.assertEqual(out.child_frame_id, 'b') + + v = PointStamped() + v.header.stamp = rospy.Time(2) + v.header.frame_id = 'a' + v.point.x = 1 + v.point.y = 2 + v.point.z = 3 + out = b.transform(v, 'b') + self.assertEqual(out.point.x, 0) + self.assertEqual(out.point.y, -2) + self.assertEqual(out.point.z, -3) + + v = PoseStamped() + v.header.stamp = rospy.Time(2) + v.header.frame_id = 'a' + v.pose.position.x = 1 + v.pose.position.y = 2 + v.pose.position.z = 3 + v.pose.orientation.x = 1 + out = b.transform(v, 'b') + self.assertEqual(out.pose.position.x, 0) + self.assertEqual(out.pose.position.y, -2) + self.assertEqual(out.pose.position.z, -3) + + # Translation shouldn't affect Vector3 + t = TransformStamped() + t.transform.translation.x = 1 + t.transform.translation.y = 2 + t.transform.translation.z = 3 + t.transform.rotation.w = 1 + v = Vector3Stamped() + v.vector.x = 1 + v.vector.y = 0 + v.vector.z = 0 + out = tf2_geometry_msgs.do_transform_vector3(v, t) + self.assertEqual(out.vector.x, 1) + self.assertEqual(out.vector.y, 0) + self.assertEqual(out.vector.z, 0) + + # Rotate Vector3 180 deg about y + t = TransformStamped() + t.transform.translation.x = 1 + t.transform.translation.y = 2 + t.transform.translation.z = 3 + t.transform.rotation.y = 1 + + v = Vector3Stamped() + v.vector.x = 1 + v.vector.y = 0 + v.vector.z = 0 + + out = tf2_geometry_msgs.do_transform_vector3(v, t) + self.assertEqual(out.vector.x, -1) + self.assertEqual(out.vector.y, 0) + self.assertEqual(out.vector.z, 0) + + v = WrenchStamped() + v.wrench.force.x = 1 + v.wrench.force.y = 0 + v.wrench.force.z = 0 + v.wrench.torque.x = 1 + v.wrench.torque.y = 0 + v.wrench.torque.z = 0 + + out = tf2_geometry_msgs.do_transform_wrench(v, t) + self.assertEqual(out.wrench.force.x, -1) + self.assertEqual(out.wrench.force.y, 0) + self.assertEqual(out.wrench.force.z, 0) + self.assertEqual(out.wrench.torque.x, -1) + self.assertEqual(out.wrench.torque.y, 0) + self.assertEqual(out.wrench.torque.z, 0) + +if __name__ == '__main__': + import rosunit + rospy.init_node('test_tf2_geometry_msgs_python') + rosunit.unitrun("test_tf2_geometry_msgs", "test_tf2_geometry_msgs_python", GeometryMsgs) diff --git a/src/geometry2/tf2_geometry_msgs/setup.py b/src/geometry2/tf2_geometry_msgs/setup.py new file mode 100644 index 0000000..debf796 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/setup.py @@ -0,0 +1,13 @@ +#!/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) + diff --git a/src/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py b/src/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py new file mode 100644 index 0000000..e072bac --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py @@ -0,0 +1 @@ +from .tf2_geometry_msgs import * diff --git a/src/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py b/src/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py new file mode 100644 index 0000000..f0edee2 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py @@ -0,0 +1,110 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# author: Wim Meeussen + +from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped, WrenchStamped +import PyKDL +import rospy +import tf2_ros +import copy + +def to_msg_msg(msg): + return msg + +tf2_ros.ConvertRegistration().add_to_msg(Vector3Stamped, to_msg_msg) +tf2_ros.ConvertRegistration().add_to_msg(PoseStamped, to_msg_msg) +tf2_ros.ConvertRegistration().add_to_msg(PointStamped, to_msg_msg) + +def from_msg_msg(msg): + return msg + +tf2_ros.ConvertRegistration().add_from_msg(Vector3Stamped, from_msg_msg) +tf2_ros.ConvertRegistration().add_from_msg(PoseStamped, from_msg_msg) +tf2_ros.ConvertRegistration().add_from_msg(PointStamped, from_msg_msg) + +def transform_to_kdl(t): + return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + PyKDL.Vector(t.transform.translation.x, + t.transform.translation.y, + t.transform.translation.z)) + + +# PointStamped +def do_transform_point(point, transform): + p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, point.point.y, point.point.z) + res = PointStamped() + res.point.x = p[0] + res.point.y = p[1] + res.point.z = p[2] + res.header = transform.header + return res +tf2_ros.TransformRegistration().add(PointStamped, do_transform_point) + + +# Vector3Stamped +def do_transform_vector3(vector3, transform): + transform = copy.deepcopy(transform) + transform.transform.translation.x = 0; + transform.transform.translation.y = 0; + transform.transform.translation.z = 0; + p = transform_to_kdl(transform) * PyKDL.Vector(vector3.vector.x, vector3.vector.y, vector3.vector.z) + res = Vector3Stamped() + res.vector.x = p[0] + res.vector.y = p[1] + res.vector.z = p[2] + res.header = transform.header + return res +tf2_ros.TransformRegistration().add(Vector3Stamped, do_transform_vector3) + +# PoseStamped +def do_transform_pose(pose, transform): + f = transform_to_kdl(transform) * PyKDL.Frame(PyKDL.Rotation.Quaternion(pose.pose.orientation.x, pose.pose.orientation.y, + pose.pose.orientation.z, pose.pose.orientation.w), + PyKDL.Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)) + res = PoseStamped() + res.pose.position.x = f[(0, 3)] + res.pose.position.y = f[(1, 3)] + res.pose.position.z = f[(2, 3)] + (res.pose.orientation.x, res.pose.orientation.y, res.pose.orientation.z, res.pose.orientation.w) = f.M.GetQuaternion() + res.header = transform.header + return res +tf2_ros.TransformRegistration().add(PoseStamped, do_transform_pose) + +# WrenchStamped +def do_transform_wrench(wrench, transform): + force = Vector3Stamped() + torque = Vector3Stamped() + force.vector = wrench.wrench.force + torque.vector = wrench.wrench.torque + res = WrenchStamped() + res.wrench.force = do_transform_vector3(force, transform).vector + res.wrench.torque = do_transform_vector3(torque, transform).vector + res.header = transform.header + return res +tf2_ros.TransformRegistration().add(WrenchStamped, do_transform_wrench) diff --git a/src/geometry2/tf2_geometry_msgs/test/test.launch b/src/geometry2/tf2_geometry_msgs/test/test.launch new file mode 100644 index 0000000..53bab78 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/test/test.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/src/geometry2/tf2_geometry_msgs/test/test_python.launch b/src/geometry2/tf2_geometry_msgs/test/test_python.launch new file mode 100644 index 0000000..b6bc793 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/test/test_python.launch @@ -0,0 +1,3 @@ + + + diff --git a/src/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/src/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp new file mode 100644 index 0000000..860db53 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -0,0 +1,380 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + + +#include +#include +#include +#include +#include +#include + +tf2_ros::Buffer* tf_buffer; +geometry_msgs::TransformStamped t; +static const double EPS = 1e-3; + + +TEST(TfGeometry, Frame) +{ + geometry_msgs::PoseStamped v1; + v1.pose.position.x = 1; + v1.pose.position.y = 2; + v1.pose.position.z = 3; + v1.pose.orientation.x = 1; + v1.header.stamp = ros::Time(2); + v1.header.frame_id = "A"; + + // simple api + geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); + EXPECT_NEAR(v_simple.pose.position.x, -9, EPS); + EXPECT_NEAR(v_simple.pose.position.y, 18, EPS); + EXPECT_NEAR(v_simple.pose.position.z, 27, EPS); + EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS); + + // advanced api + geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), + "A", ros::Duration(3.0)); + EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS); + EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS); + EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS); + EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS); +} + +TEST(TfGeometry, PoseWithCovarianceStamped) +{ + geometry_msgs::PoseWithCovarianceStamped v1; + v1.pose.pose.position.x = 1; + v1.pose.pose.position.y = 2; + v1.pose.pose.position.z = 3; + v1.pose.pose.orientation.x = 1; + v1.header.stamp = ros::Time(2); + v1.header.frame_id = "A"; + v1.pose.covariance[0] = 1; + v1.pose.covariance[7] = 1; + v1.pose.covariance[14] = 1; + v1.pose.covariance[21] = 1; + v1.pose.covariance[28] = 1; + v1.pose.covariance[35] = 1; + + // simple api + const geometry_msgs::PoseWithCovarianceStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); + EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS); + EXPECT_NEAR(v_simple.pose.pose.position.y, 18, EPS); + EXPECT_NEAR(v_simple.pose.pose.position.z, 27, EPS); + EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS); + + // no rotation in this transformation, so no change to covariance + EXPECT_NEAR(v_simple.pose.covariance[0], 1.0, EPS); + EXPECT_NEAR(v_simple.pose.covariance[7], 1.0, EPS); + EXPECT_NEAR(v_simple.pose.covariance[14], 1.0, EPS); + EXPECT_NEAR(v_simple.pose.covariance[21], 1.0, EPS); + EXPECT_NEAR(v_simple.pose.covariance[28], 1.0, EPS); + EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS); + + // advanced api + const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), + "A", ros::Duration(3.0)); + EXPECT_NEAR(v_advanced.pose.pose.position.x, -9, EPS); + EXPECT_NEAR(v_advanced.pose.pose.position.y, 18, EPS); + EXPECT_NEAR(v_advanced.pose.pose.position.z, 27, EPS); + EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS); + EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS); + + // no rotation in this transformation, so no change to covariance + EXPECT_NEAR(v_advanced.pose.covariance[0], 1.0, EPS); + EXPECT_NEAR(v_advanced.pose.covariance[7], 1.0, EPS); + EXPECT_NEAR(v_advanced.pose.covariance[14], 1.0, EPS); + EXPECT_NEAR(v_advanced.pose.covariance[21], 1.0, EPS); + EXPECT_NEAR(v_advanced.pose.covariance[28], 1.0, EPS); + EXPECT_NEAR(v_advanced.pose.covariance[35], 1.0, EPS); + + /** now add rotation to transform to test the effect on covariance **/ + + // rotate pi/2 radians about x-axis + geometry_msgs::TransformStamped t_rot; + t_rot.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(1,0,0), M_PI/2)); + t_rot.header.stamp = ros::Time(2.0); + t_rot.header.frame_id = "A"; + t_rot.child_frame_id = "rotated"; + tf_buffer->setTransform(t_rot, "rotation_test"); + + // need to put some covariance in the matrix + v1.pose.covariance[1] = 1; + v1.pose.covariance[6] = 1; + v1.pose.covariance[12] = 1; + + // perform rotation + const geometry_msgs::PoseWithCovarianceStamped v_rotated = tf_buffer->transform(v1, "rotated", ros::Duration(2.0)); + + // the covariance matrix should now be transformed + EXPECT_NEAR(v_rotated.pose.covariance[0], 1.0, EPS); + EXPECT_NEAR(v_rotated.pose.covariance[1], 0.0, EPS); + EXPECT_NEAR(v_rotated.pose.covariance[2],-1.0, EPS); + EXPECT_NEAR(v_rotated.pose.covariance[6], 1.0, EPS); + EXPECT_NEAR(v_rotated.pose.covariance[7], 1.0, EPS); + EXPECT_NEAR(v_rotated.pose.covariance[8], 0.0, EPS); + EXPECT_NEAR(v_rotated.pose.covariance[12],-1.0, EPS); + EXPECT_NEAR(v_rotated.pose.covariance[13], 0.0, EPS); + EXPECT_NEAR(v_rotated.pose.covariance[14], 1.0, EPS); + + // set buffer back to original transform + tf_buffer->setTransform(t, "test"); +} + +TEST(TfGeometry, Transform) +{ + geometry_msgs::TransformStamped v1; + v1.transform.translation.x = 1; + v1.transform.translation.y = 2; + v1.transform.translation.z = 3; + v1.transform.rotation.x = 1; + v1.header.stamp = ros::Time(2); + v1.header.frame_id = "A"; + + // simple api + geometry_msgs::TransformStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); + EXPECT_NEAR(v_simple.transform.translation.x, -9, EPS); + EXPECT_NEAR(v_simple.transform.translation.y, 18, EPS); + EXPECT_NEAR(v_simple.transform.translation.z, 27, EPS); + EXPECT_NEAR(v_simple.transform.rotation.x, 0.0, EPS); + EXPECT_NEAR(v_simple.transform.rotation.y, 0.0, EPS); + EXPECT_NEAR(v_simple.transform.rotation.z, 0.0, EPS); + EXPECT_NEAR(v_simple.transform.rotation.w, 1.0, EPS); + + + // advanced api + geometry_msgs::TransformStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), + "A", ros::Duration(3.0)); + EXPECT_NEAR(v_advanced.transform.translation.x, -9, EPS); + EXPECT_NEAR(v_advanced.transform.translation.y, 18, EPS); + EXPECT_NEAR(v_advanced.transform.translation.z, 27, EPS); + EXPECT_NEAR(v_advanced.transform.rotation.x, 0.0, EPS); + EXPECT_NEAR(v_advanced.transform.rotation.y, 0.0, EPS); + EXPECT_NEAR(v_advanced.transform.rotation.z, 0.0, EPS); + EXPECT_NEAR(v_advanced.transform.rotation.w, 1.0, EPS); +} + +TEST(TfGeometry, Vector) +{ + geometry_msgs::Vector3Stamped v1, res; + v1.vector.x = 1; + v1.vector.y = 2; + v1.vector.z = 3; + v1.header.stamp = ros::Time(2.0); + v1.header.frame_id = "A"; + + // simple api + geometry_msgs::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); + EXPECT_NEAR(v_simple.vector.x, 1, EPS); + EXPECT_NEAR(v_simple.vector.y, -2, EPS); + EXPECT_NEAR(v_simple.vector.z, -3, EPS); + + // advanced api + geometry_msgs::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), + "A", ros::Duration(3.0)); + EXPECT_NEAR(v_advanced.vector.x, 1, EPS); + EXPECT_NEAR(v_advanced.vector.y, -2, EPS); + EXPECT_NEAR(v_advanced.vector.z, -3, EPS); +} + + +TEST(TfGeometry, Point) +{ + geometry_msgs::PointStamped v1, res; + v1.point.x = 1; + v1.point.y = 2; + v1.point.z = 3; + v1.header.stamp = ros::Time(2.0); + v1.header.frame_id = "A"; + + // simple api + geometry_msgs::PointStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); + EXPECT_NEAR(v_simple.point.x, -9, EPS); + EXPECT_NEAR(v_simple.point.y, 18, EPS); + EXPECT_NEAR(v_simple.point.z, 27, EPS); + + // advanced api + geometry_msgs::PointStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), + "A", ros::Duration(3.0)); + EXPECT_NEAR(v_advanced.point.x, -9, EPS); + EXPECT_NEAR(v_advanced.point.y, 18, EPS); + EXPECT_NEAR(v_advanced.point.z, 27, EPS); +} + +TEST(TfGeometry, doTransformPoint) +{ + geometry_msgs::Point v1, res; + v1.x = 2; + v1.y = 1; + v1.z = 3; + + geometry_msgs::TransformStamped trafo; + trafo.transform.translation.x = -1; + trafo.transform.translation.y = 2; + trafo.transform.translation.z = -3; + trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + + tf2::doTransform(v1, res, trafo); + + EXPECT_NEAR(res.x, 0, EPS); + EXPECT_NEAR(res.y, 0, EPS); + EXPECT_NEAR(res.z, 0, EPS); +} + +TEST(TfGeometry, doTransformQuaterion) +{ + geometry_msgs::Quaternion v1, res; + v1.w = 1; + + geometry_msgs::TransformStamped trafo; + trafo.transform.translation.x = -1; + trafo.transform.translation.y = 2; + trafo.transform.translation.z = -3; + trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + + tf2::doTransform(v1, res, trafo); + + EXPECT_NEAR(res.x, trafo.transform.rotation.x, EPS); + EXPECT_NEAR(res.y, trafo.transform.rotation.y, EPS); + EXPECT_NEAR(res.z, trafo.transform.rotation.z, EPS); + EXPECT_NEAR(res.w, trafo.transform.rotation.w, EPS); +} + +TEST(TfGeometry, doTransformPose) +{ + geometry_msgs::Pose v1, res; + v1.position.x = 2; + v1.position.y = 1; + v1.position.z = 3; + v1.orientation.w = 1; + + geometry_msgs::TransformStamped trafo; + trafo.transform.translation.x = -1; + trafo.transform.translation.y = 2; + trafo.transform.translation.z = -3; + trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + + tf2::doTransform(v1, res, trafo); + + EXPECT_NEAR(res.position.x, 0, EPS); + EXPECT_NEAR(res.position.y, 0, EPS); + EXPECT_NEAR(res.position.z, 0, EPS); + + EXPECT_NEAR(res.orientation.x, trafo.transform.rotation.x, EPS); + EXPECT_NEAR(res.orientation.y, trafo.transform.rotation.y, EPS); + EXPECT_NEAR(res.orientation.z, trafo.transform.rotation.z, EPS); + EXPECT_NEAR(res.orientation.w, trafo.transform.rotation.w, EPS); +} + +TEST(TfGeometry, doTransformVector3) +{ + geometry_msgs::Vector3 v1, res; + v1.x = 2; + v1.y = 1; + v1.z = 3; + + geometry_msgs::TransformStamped trafo; + trafo.transform.translation.x = -1; + trafo.transform.translation.y = 2; + trafo.transform.translation.z = -3; + trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + + tf2::doTransform(v1, res, trafo); + + EXPECT_NEAR(res.x, 1, EPS); + EXPECT_NEAR(res.y, -2, EPS); + EXPECT_NEAR(res.z, 3, EPS); +} + +TEST(TfGeometry, doTransformWrench) +{ + geometry_msgs::Wrench v1, res; + v1.force.x = 2; + v1.force.y = 1; + v1.force.z = 3; + v1.torque.x = 2; + v1.torque.y = 1; + v1.torque.z = 3; + + geometry_msgs::TransformStamped trafo; + trafo.transform.translation.x = -1; + trafo.transform.translation.y = 2; + trafo.transform.translation.z = -3; + trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + + tf2::doTransform(v1, res, trafo); + EXPECT_NEAR(res.force.x, 1, EPS); + EXPECT_NEAR(res.force.y, -2, EPS); + EXPECT_NEAR(res.force.z, 3, EPS); + + EXPECT_NEAR(res.torque.x, 1, EPS); + EXPECT_NEAR(res.torque.y, -2, EPS); + EXPECT_NEAR(res.torque.z, 3, EPS); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test"); + ros::NodeHandle n; + + tf_buffer = new tf2_ros::Buffer(); + tf_buffer->setUsingDedicatedThread(true); + + // populate buffer + t.transform.translation.x = 10; + t.transform.translation.y = 20; + t.transform.translation.z = 30; + t.transform.rotation.x = 1; + t.header.stamp = ros::Time(2.0); + t.header.frame_id = "A"; + t.child_frame_id = "B"; + tf_buffer->setTransform(t, "test"); + + int ret = RUN_ALL_TESTS(); + delete tf_buffer; + return ret; +} + + + + + diff --git a/src/geometry2/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp b/src/geometry2/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp new file mode 100644 index 0000000..3139f23 --- /dev/null +++ b/src/geometry2/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp @@ -0,0 +1,405 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + + +#include +#include + +static const double EPS = 1e-6; + +tf2::Vector3 get_tf2_vector() +{ + return tf2::Vector3(1.0, 2.0, 3.0); +} + +geometry_msgs::Vector3& value_initialize(geometry_msgs::Vector3 &m1) +{ + m1.x = 1; + m1.y = 2; + m1.z = 3; + return m1; +} + +std_msgs::Header& value_initialize(std_msgs::Header & h) +{ + h.stamp = ros::Time(10); + h.frame_id = "foobar"; + return h; +} + +geometry_msgs::Vector3Stamped& value_initialize(geometry_msgs::Vector3Stamped &m1) +{ + value_initialize(m1.header); + value_initialize(m1.vector); + return m1; +} + +geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1) +{ + m1.x = 1; + m1.y = 2; + m1.z = 3; + return m1; +} + +geometry_msgs::PointStamped& value_initialize(geometry_msgs::PointStamped &m1) +{ + value_initialize(m1.header); + value_initialize(m1.point); + return m1; +} + +geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1) +{ + m1.x = 0; + m1.y = 0; + m1.z = 0.7071067811; + m1.w = 0.7071067811; + return m1; +} + +geometry_msgs::QuaternionStamped& value_initialize(geometry_msgs::QuaternionStamped &m1) +{ + value_initialize(m1.header); + value_initialize(m1.quaternion); + return m1; +} + +geometry_msgs::Pose & value_initialize(geometry_msgs::Pose & m1) +{ + value_initialize(m1.position); + value_initialize(m1.orientation); + return m1; +} + +geometry_msgs::PoseStamped& value_initialize(geometry_msgs::PoseStamped &m1) +{ + value_initialize(m1.header); + value_initialize(m1.pose); + return m1; +} + +geometry_msgs::Transform & value_initialize(geometry_msgs::Transform & m1) +{ + value_initialize(m1.translation); + value_initialize(m1.rotation); + return m1; +} + +geometry_msgs::TransformStamped& value_initialize(geometry_msgs::TransformStamped &m1) +{ + value_initialize(m1.header); + value_initialize(m1.transform); + return m1; +} + +void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2) +{ + EXPECT_NEAR(h1.stamp.toSec(), h2.stamp.toSec(), EPS); + EXPECT_STREQ(h1.frame_id.c_str(), h2.frame_id.c_str()); +} + +/* + * Vector3 + */ +void expect_near(const geometry_msgs::Vector3 & v1, const tf2::Vector3 & v2) +{ + EXPECT_NEAR(v1.x, v2.x(), EPS); + EXPECT_NEAR(v1.y, v2.y(), EPS); + EXPECT_NEAR(v1.z, v2.z(), EPS); +} + +void expect_near(const geometry_msgs::Vector3 & v1, const geometry_msgs::Vector3 & v2) +{ + EXPECT_NEAR(v1.x, v2.x, EPS); + EXPECT_NEAR(v1.y, v2.y, EPS); + EXPECT_NEAR(v1.z, v2.z, EPS); +} + +void expect_near(const tf2::Vector3 & v1, const tf2::Vector3 & v2) +{ + EXPECT_NEAR(v1.x(), v2.x(), EPS); + EXPECT_NEAR(v1.y(), v2.y(), EPS); + EXPECT_NEAR(v1.z(), v2.z(), EPS); +} + +void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::Vector3Stamped & p2) +{ + expect_near(p1.header, p2.header); + expect_near(p1.vector, p2.vector); +} + +/* + * Point + */ +void expect_near(const geometry_msgs::Point & p1, const tf2::Vector3 & v2) +{ + EXPECT_NEAR(p1.x, v2.x(), EPS); + EXPECT_NEAR(p1.y, v2.y(), EPS); + EXPECT_NEAR(p1.z, v2.z(), EPS); +} + +void expect_near(const geometry_msgs::Point & p1, const geometry_msgs::Point & v2) +{ + EXPECT_NEAR(p1.x, v2.x, EPS); + EXPECT_NEAR(p1.y, v2.y, EPS); + EXPECT_NEAR(p1.z, v2.z, EPS); +} + +void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::PointStamped & p2) +{ + expect_near(p1.header, p2.header); + expect_near(p1.point, p2.point); +} + + +/* + * Quaternion + */ +void expect_near(const geometry_msgs::Quaternion & q1, const tf2::Quaternion & v2) +{ + EXPECT_NEAR(q1.x, v2.x(), EPS); + EXPECT_NEAR(q1.y, v2.y(), EPS); + EXPECT_NEAR(q1.z, v2.z(), EPS); +} + +void expect_near(const geometry_msgs::Quaternion & q1, const geometry_msgs::Quaternion & v2) +{ + EXPECT_NEAR(q1.x, v2.x, EPS); + EXPECT_NEAR(q1.y, v2.y, EPS); + EXPECT_NEAR(q1.z, v2.z, EPS); +} + +void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msgs::QuaternionStamped & p2) +{ + expect_near(p1.header, p2.header); + expect_near(p1.quaternion, p2.quaternion); +} + +/* + * Pose + */ +void expect_near(const geometry_msgs::Pose & p, const tf2::Transform & t) +{ + expect_near(p.position, t.getOrigin()); + expect_near(p.orientation, t.getRotation()); +} + +void expect_near(const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2) +{ + expect_near(p1.position, p2.position); + expect_near(p1.orientation, p2.orientation); +} + +void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::PoseStamped & p2) +{ + expect_near(p1.header, p2.header); + expect_near(p1.pose, p2.pose); +} + +/* + * Transform + */ +void expect_near(const geometry_msgs::Transform & p, const tf2::Transform & t) +{ + expect_near(p.translation, t.getOrigin()); + expect_near(p.rotation, t.getRotation()); +} + +void expect_near(const geometry_msgs::Transform & p1, const geometry_msgs::Transform & p2) +{ + expect_near(p1.translation, p2.translation); + expect_near(p1.rotation, p2.rotation); +} + +void expect_near(const geometry_msgs::TransformStamped & p1, const geometry_msgs::TransformStamped & p2) +{ + expect_near(p1.header, p2.header); + expect_near(p1.transform, p2.transform); +} + +/* + * Stamped templated expect_near + */ + +template +void expect_near(const tf2::Stamped& s1, const tf2::Stamped& s2) +{ + expect_near((T)s1, (T)s2); +} + +/********************* + * Tests + *********************/ + +TEST(tf2_geometry_msgs, Vector3) +{ + geometry_msgs::Vector3 m1; + value_initialize(m1); + tf2::Vector3 v1; + fromMsg(m1, v1); + SCOPED_TRACE("m1 v1"); + expect_near(m1, v1); + geometry_msgs::Vector3 m2 = toMsg(v1); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, Point) +{ + geometry_msgs::Point m1; + value_initialize(m1); + tf2::Vector3 v1; + SCOPED_TRACE("m1 v1"); + fromMsg(m1, v1); + expect_near(m1, v1); + geometry_msgs::Point m2 = toMsg(v1, m2); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, Quaternion) +{ + geometry_msgs::Quaternion m1; + value_initialize(m1); + tf2::Quaternion q1; + SCOPED_TRACE("m1 q1"); + fromMsg(m1, q1); + expect_near(m1, q1); + geometry_msgs::Quaternion m2 = toMsg(q1); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, Pose) +{ + geometry_msgs::Pose m1; + value_initialize(m1); + tf2::Transform t1; + fromMsg(m1, t1); + SCOPED_TRACE("m1 t1"); + expect_near(m1, t1); + geometry_msgs::Pose m2 = toMsg(t1, m2); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, Transform) +{ + geometry_msgs::Transform m1; + value_initialize(m1); + tf2::Transform t1; + fromMsg(m1, t1); + SCOPED_TRACE("m1 t1"); + expect_near(m1, t1); + geometry_msgs::Transform m2 = toMsg(t1); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, Vector3Stamped) +{ + geometry_msgs::Vector3Stamped m1; + value_initialize(m1); + tf2::Stamped v1; + fromMsg(m1, v1); + SCOPED_TRACE("m1 v1"); + // expect_near(m1, v1); + geometry_msgs::Vector3Stamped m2; + m2 = toMsg(v1); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, PointStamped) +{ + geometry_msgs::PointStamped m1; + value_initialize(m1); + tf2::Stamped v1; + fromMsg(m1, v1); + SCOPED_TRACE("m1 v1"); + // expect_near(m1, v1); //TODO implement cross verification explicityly + geometry_msgs::PointStamped m2; + m2 = toMsg(v1, m2); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, QuaternionStamped) +{ + geometry_msgs::QuaternionStamped m1; + value_initialize(m1); + tf2::Stamped v1; + fromMsg(m1, v1); + SCOPED_TRACE("m1 v1"); + // expect_near(m1, v1); //TODO implement cross verification explicityly + geometry_msgs::QuaternionStamped m2; + m2 = tf2::toMsg(v1); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, PoseStamped) +{ + geometry_msgs::PoseStamped m1; + value_initialize(m1); + tf2::Stamped v1; + SCOPED_TRACE("m1 v1"); + fromMsg(m1, v1); + // expect_near(m1, v1); //TODO implement cross verification explicityly + geometry_msgs::PoseStamped m2; + m2 = tf2::toMsg(v1, m2); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + +TEST(tf2_geometry_msgs, TransformStamped) +{ + geometry_msgs::TransformStamped m1; + value_initialize(m1); + tf2::Stamped v1; + fromMsg(m1, v1); + SCOPED_TRACE("m1 v1"); + // expect_near(m1, v1); + geometry_msgs::TransformStamped m2; + m2 = tf2::toMsg(v1); + SCOPED_TRACE("m1 m2"); + expect_near(m1, m2); +} + + + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/src/geometry2/tf2_kdl/CHANGELOG.rst b/src/geometry2/tf2_kdl/CHANGELOG.rst new file mode 100644 index 0000000..a99b498 --- /dev/null +++ b/src/geometry2/tf2_kdl/CHANGELOG.rst @@ -0,0 +1,221 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_kdl +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ + +0.6.6 (2020-01-09) +------------------ +* Make kdl headers available (`#419 `_) +* Fix python3 compatibility for noetic (`#416 `_) +* Remove roslib.load_manifest `#404 `_ from otamachan/remove-load-manifest +* Python 3 compatibility: relative imports and print statement +* Contributors: Shane Loretz, Tamaki Nishino, Timon Engelke, Tully Foote + +0.6.5 (2018-11-16) +------------------ + +0.6.4 (2018-11-06) +------------------ + +0.6.3 (2018-07-09) +------------------ + +0.6.2 (2018-05-02) +------------------ +* Adds additional conversions for tf2, KDL, Eigen (`#292 `_) + - adds non-stamped Eigen to Transform function + - converts Eigen Matrix Vectors to and from geometry_msgs::Twist + - adds to/from message for geometry_msgs::Pose and KDL::Frame +* Contributors: Ian McMahon + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ + +0.5.17 (2018-01-01) +------------------- +* Merge pull request `#257 `_ from delftrobotics-forks/python3 + Make tf2_py python3 compatible again +* Use python3 print function. +* Contributors: Maarten de Vries, Tully Foote + +0.5.16 (2017-07-14) +------------------- +* store gtest return value as int (`#229 `_) +* Find eigen in a much nicer way. +* Switch tf2_kdl over to package.xml format 2. +* Contributors: Chris Lalancette, dhood + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- +* Add Python documentation for tf2_kdl +* Document kdl +* Contributors: Jackie Kay + +0.5.13 (2016-03-04) +------------------- +* converting python test script into unit test +* Don't export catkin includes +* Contributors: Jochen Sprickerhof, Tully Foote + +0.5.12 (2015-08-05) +------------------- +* Add kdl::Frame to TransformStamped conversion +* Contributors: Paul Bovbel + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- + +0.5.9 (2015-03-25) +------------------ + +0.5.8 (2015-03-17) +------------------ +* remove useless Makefile files +* fix ODR violations +* Contributors: Vincent Rabaud + +0.5.7 (2014-12-23) +------------------ +* fixing install rules and adding backwards compatible include with #warning +* Contributors: Tully Foote + +0.5.6 (2014-09-18) +------------------ + +0.5.5 (2014-06-23) +------------------ + +0.5.4 (2014-05-07) +------------------ + +0.5.3 (2014-02-21) +------------------ +* finding eigen from cmake_modules instead of from catkin +* Contributors: Tully Foote + +0.5.2 (2014-02-20) +------------------ +* add cmake_modules dependency for eigen find_package rules +* Contributors: Tully Foote + +0.5.1 (2014-02-14) +------------------ + +0.5.0 (2014-02-14) +------------------ + +0.4.10 (2013-12-26) +------------------- + +0.4.9 (2013-11-06) +------------------ + +0.4.8 (2013-11-06) +------------------ + +0.4.7 (2013-08-28) +------------------ + +0.4.6 (2013-08-28) +------------------ + +0.4.5 (2013-07-11) +------------------ + +0.4.4 (2013-07-09) +------------------ +* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. + +0.4.3 (2013-07-05) +------------------ + +0.4.2 (2013-07-05) +------------------ + +0.4.1 (2013-07-05) +------------------ + +0.4.0 (2013-06-27) +------------------ +* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 +* Cleaning up unnecessary dependency on roscpp +* converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace +* Cleaning up packaging of tf2 including: + removing unused nodehandle + cleaning up a few dependencies and linking + removing old backup of package.xml + making diff minimally different from tf version of library +* Restoring test packages and bullet packages. + reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented + reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ +* passing unit tests + +0.3.6 (2013-03-03) +------------------ +* fix compilation under Oneiric + +0.3.5 (2013-02-15 14:46) +------------------------ +* 0.3.4 -> 0.3.5 + +0.3.4 (2013-02-15 13:14) +------------------------ +* 0.3.3 -> 0.3.4 + +0.3.3 (2013-02-15 11:30) +------------------------ +* 0.3.2 -> 0.3.3 + +0.3.2 (2013-02-15 00:42) +------------------------ +* 0.3.1 -> 0.3.2 +* fixed missing include export & tf2_ros dependecy + +0.3.1 (2013-02-14) +------------------ +* fixing version number in tf2_kdl +* catkinized tf2_kdl + +0.3.0 (2013-02-13) +------------------ +* fixing groovy-devel +* removing bullet and kdl related packages +* catkinizing geometry-experimental +* catkinizing tf2_kdl +* fix for kdl rotaiton constrition +* add twist, wrench and pose conversion to kdl, fix message to message conversion by adding specific conversion functions +* merge tf2_cpp and tf2_py into tf2_ros +* Got transform with types working in python +* A working first version of transforming and converting between different types +* Moving from camelCase to undescores to be in line with python style guides +* kdl unittest passing +* whitespace test +* add support for PointStamped geometry_msgs +* Fixing script +* set transform for test +* add advanced api +* working to transform kdl objects with dummy buffer_core +* plugin for py kdl +* add regression tests for geometry_msgs point, vector and pose +* add frame unit tests to kdl and bullet +* add first regression tests for kdl and bullet tf +* add bullet transforms, and create tests for bullet and kdl +* transform for vector3stamped message +* move implementation into library +* add advanced api +* compiling again with new design +* renaming classes +* compiling now +* almost compiling version of template code +* add test to start compiling diff --git a/src/geometry2/tf2_kdl/CMakeLists.txt b/src/geometry2/tf2_kdl/CMakeLists.txt new file mode 100644 index 0000000..5bd4e50 --- /dev/null +++ b/src/geometry2/tf2_kdl/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_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) + +# Finding Eigen is somewhat complicated because of our need to support Ubuntu +# all the way back to saucy. First we look for the Eigen3 cmake module +# provided by the libeigen3-dev on newer Ubuntu. If that fails, then we +# fall-back to the version provided by cmake_modules, which is a stand-in. +find_package(Eigen3 QUIET) +if(NOT EIGEN3_FOUND) + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +endif() + +# Note that eigen 3.2 (on Ubuntu Wily) only provides EIGEN3_INCLUDE_DIR, +# not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former. +if(NOT EIGEN3_INCLUDE_DIRS) + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +endif() + + +catkin_package( + INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} + DEPENDS EIGEN3 orocos_kdl +) + + +catkin_python_setup() +link_directories(${orocos_kdl_LIBRARY_DIRS}) + +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${GTEST_INCLUDE_DIRS}) + + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +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_include_directories(test_kdl PUBLIC ${orocos_kdl_INCLUDE_DIRS}) + target_link_libraries(test_kdl ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES} ${GTEST_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) + + add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) + add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_python.launch) + + if(TARGET tests) + add_dependencies(tests test_kdl) + endif() + +endif() diff --git a/src/geometry2/tf2_kdl/conf.py b/src/geometry2/tf2_kdl/conf.py new file mode 100644 index 0000000..fb9bed5 --- /dev/null +++ b/src/geometry2/tf2_kdl/conf.py @@ -0,0 +1,206 @@ +# -*- coding: utf-8 -*- +# +# tf2 documentation build configuration file, created by +# sphinx-quickstart on Mon Jun 1 14:21:53 2009. +# +# This file is execfile()d with the current directory set to its containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import roslib +#roslib.load_manifest('tf2_kdl') +import sys, os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# sys.path.append(os.path.abspath('./src/tf2_kdl')) + +# -- General configuration ----------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be extensions +# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'tf2_kdl' +copyright = u'2016, Open Source Robotics Foundation' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '0.5' +# The full version, including alpha/beta/rc tags. +release = '0.5.13' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of documents that shouldn't be included in the build. +#unused_docs = [] + +# List of directories, relative to source directory, that shouldn't be searched +# for source files. +exclude_trees = ['_build'] + +exclude_patterns = ['_CHANGELOG.rst'] + +# The reST default role (used for this markup: `text`) to use for all documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. Major themes that come with +# Sphinx are currently 'default' and 'sphinxdoc'. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +#html_static_path = ['_static'] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_use_modindex = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = '' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'tfdoc' + + +# -- Options for LaTeX output -------------------------------------------------- + +# The paper size ('letter' or 'a4'). +#latex_paper_size = 'letter' + +# The font size ('10pt', '11pt' or '12pt'). +#latex_font_size = '10pt' + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, author, documentclass [howto/manual]). +latex_documents = [ + ('index', 'tf.tex', u'stereo\\_utils Documentation', + u'Tully Foote and Eitan Marder-Eppstein', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# Additional stuff for the LaTeX preamble. +#latex_preamble = '' + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_use_modindex = True + + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = { + 'http://docs.python.org/': None, + 'http://docs.opencv.org/3.0-last-rst/': None, + 'http://docs.scipy.org/doc/numpy' : None + } diff --git a/src/geometry2/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/src/geometry2/tf2_kdl/include/tf2_kdl/tf2_kdl.h new file mode 100644 index 0000000..e1f28bf --- /dev/null +++ b/src/geometry2/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -0,0 +1,309 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + +#ifndef TF2_KDL_H +#define TF2_KDL_H + +#include +#include +#include +#include +#include +#include +#include + + +namespace tf2 +{ +/** \brief Convert a timestamped transform to the equivalent KDL data type. + * \param t The transform to convert, as a geometry_msgs TransformedStamped message. + * \return The transform message converted to an KDL Frame. + */ +inline +KDL::Frame transformToKDL(const geometry_msgs::TransformStamped& t) + { + return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z)); + } + +/** \brief Convert an KDL Frame to the equivalent geometry_msgs message type. + * \param k The transform to convert, as an KDL Frame. + * \return The transform converted to a TransformStamped message. + */ +inline +geometry_msgs::TransformStamped kdlToTransform(const KDL::Frame& k) +{ + geometry_msgs::TransformStamped t; + t.transform.translation.x = k.p.x(); + t.transform.translation.y = k.p.y(); + t.transform.translation.z = k.p.z(); + k.M.GetQuaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w); + return t; +} + +// --------------------- +// Vector +// --------------------- +/** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Vector type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The vector to transform, as a timestamped KDL Vector data type. + * \param t_out The transformed vector, as a timestamped KDL Vector data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) + { + t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); + } + +/** \brief Convert a stamped KDL Vector type to a PointStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Vector to convert. + * \return The vector converted to a PointStamped message. + */ +inline +geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::PointStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.point.x = in[0]; + msg.point.y = in[1]; + msg.point.z = in[2]; + return msg; +} + +/** \brief Convert a PointStamped message type to a stamped KDL-specific Vector type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The PointStamped message to convert. + * \param out The point converted to a timestamped KDL Vector. + */ +inline +void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + out[0] = msg.point.x; + out[1] = msg.point.y; + out[2] = msg.point.z; +} + +// --------------------- +// Twist +// --------------------- +/** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Twist type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The twist to transform, as a timestamped KDL Twist data type. + * \param t_out The transformed Twist, as a timestamped KDL Frame data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) + { + t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); + } + +/** \brief Convert a stamped KDL Twist type to a TwistStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Twist to convert. + * \return The twist converted to a TwistStamped message. + */ +inline +geometry_msgs::TwistStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::TwistStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.twist.linear.x = in.vel[0]; + msg.twist.linear.y = in.vel[1]; + msg.twist.linear.z = in.vel[2]; + msg.twist.angular.x = in.rot[0]; + msg.twist.angular.y = in.rot[1]; + msg.twist.angular.z = in.rot[2]; + return msg; +} + +/** \brief Convert a TwistStamped message type to a stamped KDL-specific Twist type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The TwistStamped message to convert. + * \param out The twist converted to a timestamped KDL Twist. + */ +inline +void fromMsg(const geometry_msgs::TwistStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + out.vel[0] = msg.twist.linear.x; + out.vel[1] = msg.twist.linear.y; + out.vel[2] = msg.twist.linear.z; + out.rot[0] = msg.twist.angular.x; + out.rot[1] = msg.twist.angular.y; + out.rot[2] = msg.twist.angular.z; +} + + +// --------------------- +// Wrench +// --------------------- +/** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Wrench type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The wrench to transform, as a timestamped KDL Wrench data type. + * \param t_out The transformed Wrench, as a timestamped KDL Frame data type. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) + { + t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); + } + +/** \brief Convert a stamped KDL Wrench type to a WrenchStamped message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Wrench to convert. + * \return The wrench converted to a WrenchStamped message. + */ +inline +geometry_msgs::WrenchStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::WrenchStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.wrench.force.x = in.force[0]; + msg.wrench.force.y = in.force[1]; + msg.wrench.force.z = in.force[2]; + msg.wrench.torque.x = in.torque[0]; + msg.wrench.torque.y = in.torque[1]; + msg.wrench.torque.z = in.torque[2]; + return msg; +} + +/** \brief Convert a WrenchStamped message type to a stamped KDL-specific Wrench type. + * This function is a specialization of the fromMsg template defined in tf2/convert.h + * \param msg The WrenchStamped message to convert. + * \param out The wrench converted to a timestamped KDL Wrench. + */ +inline +void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + out.force[0] = msg.wrench.force.x; + out.force[1] = msg.wrench.force.y; + out.force[2] = msg.wrench.force.z; + out.torque[0] = msg.wrench.torque.x; + out.torque[1] = msg.wrench.torque.y; + out.torque[2] = msg.wrench.torque.z; +} + + + + +// --------------------- +// Frame +// --------------------- +/** \brief Apply a geometry_msgs TransformStamped to a KDL-specific Frame data type. + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The frame to transform, as a timestamped KDL Frame. + * \param t_out The transformed frame, as a timestamped KDL Frame. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ +template <> +inline + void doTransform(const tf2::Stamped& t_in, tf2::Stamped& t_out, const geometry_msgs::TransformStamped& transform) + { + t_out = tf2::Stamped(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id); + } + +/** \brief Convert a stamped KDL Frame type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Frame to convert. + * \return The frame converted to a Pose message. + */ +inline +geometry_msgs::Pose toMsg(const KDL::Frame& in) +{ + geometry_msgs::Pose msg; + msg.position.x = in.p[0]; + msg.position.y = in.p[1]; + msg.position.z = in.p[2]; + in.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); + return msg; +} + +/** \brief Convert a Pose message type to a KDL Frame. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg The Pose message to convert. + * \param out The pose converted to a KDL Frame. + */ +inline +void fromMsg(const geometry_msgs::Pose& msg, KDL::Frame& out) +{ + out.p[0] = msg.position.x; + out.p[1] = msg.position.y; + out.p[2] = msg.position.z; + out.M = KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w); +} + +/** \brief Convert a stamped KDL Frame type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/convert.h. + * \param in The timestamped KDL Frame to convert. + * \return The frame converted to a PoseStamped message. + */ +inline +geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) +{ + geometry_msgs::PoseStamped msg; + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + msg.pose = toMsg(static_cast(in)); + return msg; +} + +/** \brief Convert a Pose message transform type to a stamped KDL Frame. + * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * \param msg The PoseStamped message to convert. + * \param out The pose converted to a timestamped KDL Frame. + */ +inline +void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) +{ + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.pose, static_cast(out)); +} + +} // namespace + +#endif // TF2_KDL_H diff --git a/src/geometry2/tf2_kdl/include/tf2_kdl/tf2_kdl/tf2_kdl.h b/src/geometry2/tf2_kdl/include/tf2_kdl/tf2_kdl/tf2_kdl.h new file mode 100644 index 0000000..8fa45b2 --- /dev/null +++ b/src/geometry2/tf2_kdl/include/tf2_kdl/tf2_kdl/tf2_kdl.h @@ -0,0 +1,3 @@ +#warning This header is at the wrong path you should include + +#include diff --git a/src/geometry2/tf2_kdl/index.rst b/src/geometry2/tf2_kdl/index.rst new file mode 100644 index 0000000..73d0ff9 --- /dev/null +++ b/src/geometry2/tf2_kdl/index.rst @@ -0,0 +1,14 @@ +tf2_kdl documentation +===================== + +This is the Python API reference of the tf2_kdl package. + +.. automodule:: tf2_kdl.tf2_kdl + :members: + :undoc-members: + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` diff --git a/src/geometry2/tf2_kdl/mainpage.dox b/src/geometry2/tf2_kdl/mainpage.dox new file mode 100644 index 0000000..a3bbc8b --- /dev/null +++ b/src/geometry2/tf2_kdl/mainpage.dox @@ -0,0 +1,19 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf2_kdl contains functions for converting between geometry_msgs and KDL data types. + +This library is an implementation of the templated conversion interface specified in tf/convert.h. +It enables easy conversion from geometry_msgs Transform and Point types to the types specified +by the Orocos KDL (Kinematics and Dynamics Library) API (see http://www.orocos.org/kdl). + +See the Conversions overview +wiki page for more information about datatype conversion in tf2. + +\section codeapi Code API + +This library consists of one header only, tf2_kdl/tf2_kdl.h, which consists mostly of +specializations of template functions defined in tf2/convert.h. + +*/ diff --git a/src/geometry2/tf2_kdl/package.xml b/src/geometry2/tf2_kdl/package.xml new file mode 100644 index 0000000..44b2637 --- /dev/null +++ b/src/geometry2/tf2_kdl/package.xml @@ -0,0 +1,27 @@ + + tf2_kdl + 0.6.7 + + KDL binding for tf2 + + Tully Foote + Wim Meeussen + BSD + + http://ros.org/wiki/tf2 + + catkin + + cmake_modules + eigen + + eigen + + orocos_kdl + tf2 + tf2_ros + + ros_environment + rostest + + diff --git a/src/geometry2/tf2_kdl/rosdoc.yaml b/src/geometry2/tf2_kdl/rosdoc.yaml new file mode 100644 index 0000000..a1d78b9 --- /dev/null +++ b/src/geometry2/tf2_kdl/rosdoc.yaml @@ -0,0 +1,7 @@ + - builder: doxygen + name: C++ API + output_dir: c++ + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' + - builder: sphinx + name: Python API + output_dir: python diff --git a/src/geometry2/tf2_kdl/scripts/test.py b/src/geometry2/tf2_kdl/scripts/test.py new file mode 100755 index 0000000..e37579b --- /dev/null +++ b/src/geometry2/tf2_kdl/scripts/test.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import unittest +import rospy +import PyKDL +import tf2_ros +import tf2_kdl +from geometry_msgs.msg import TransformStamped +from copy import deepcopy + +class KDLConversions(unittest.TestCase): + def test_transform(self): + b = tf2_ros.Buffer() + t = TransformStamped() + t.transform.translation.x = 1 + t.transform.rotation.x = 1 + t.header.stamp = rospy.Time(2.0) + t.header.frame_id = 'a' + t.child_frame_id = 'b' + b.set_transform(t, 'eitan_rocks') + out = b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0)) + self.assertEqual(out.transform.translation.x, 1) + self.assertEqual(out.transform.rotation.x, 1) + self.assertEqual(out.header.frame_id, 'a') + self.assertEqual(out.child_frame_id, 'b') + + v = PyKDL.Vector(1,2,3) + out = b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b') + self.assertEqual(out.x(), 0) + self.assertEqual(out.y(), -2) + self.assertEqual(out.z(), -3) + + f = PyKDL.Frame(PyKDL.Rotation.RPY(1,2,3), PyKDL.Vector(1,2,3)) + out = b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b') + print(out) + self.assertEqual(out.p.x(), 0) + self.assertEqual(out.p.y(), -2) + self.assertEqual(out.p.z(), -3) + # TODO(tfoote) check values of rotation + + t = PyKDL.Twist(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6)) + out = b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b') + self.assertEqual(out.vel.x(), 1) + self.assertEqual(out.vel.y(), -8) + self.assertEqual(out.vel.z(), 2) + self.assertEqual(out.rot.x(), 4) + self.assertEqual(out.rot.y(), -5) + self.assertEqual(out.rot.z(), -6) + + w = PyKDL.Wrench(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6)) + out = b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b') + self.assertEqual(out.force.x(), 1) + self.assertEqual(out.force.y(), -2) + self.assertEqual(out.force.z(), -3) + self.assertEqual(out.torque.x(), 4) + self.assertEqual(out.torque.y(), -8) + self.assertEqual(out.torque.z(), -4) + + def test_convert(self): + v = PyKDL.Vector(1,2,3) + vs = tf2_ros.Stamped(v, rospy.Time(2), 'a') + vs2 = tf2_ros.convert(vs, PyKDL.Vector) + self.assertEqual(vs.x(), 1) + self.assertEqual(vs.y(), 2) + self.assertEqual(vs.z(), 3) + self.assertEqual(vs2.x(), 1) + self.assertEqual(vs2.y(), 2) + self.assertEqual(vs2.z(), 3) + + +if __name__ == '__main__': + import rosunit + rospy.init_node('test_tf2_kdl_python') + rosunit.unitrun("test_tf2_kdl", "test_tf2_kdl_python", KDLConversions) diff --git a/src/geometry2/tf2_kdl/setup.py b/src/geometry2/tf2_kdl/setup.py new file mode 100644 index 0000000..cdb0706 --- /dev/null +++ b/src/geometry2/tf2_kdl/setup.py @@ -0,0 +1,13 @@ +#!/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) diff --git a/src/geometry2/tf2_kdl/src/tf2_kdl/__init__.py b/src/geometry2/tf2_kdl/src/tf2_kdl/__init__.py new file mode 100644 index 0000000..97eef01 --- /dev/null +++ b/src/geometry2/tf2_kdl/src/tf2_kdl/__init__.py @@ -0,0 +1 @@ +from .tf2_kdl import * diff --git a/src/geometry2/tf2_kdl/src/tf2_kdl/tf2_kdl.py b/src/geometry2/tf2_kdl/src/tf2_kdl/tf2_kdl.py new file mode 100644 index 0000000..d01205a --- /dev/null +++ b/src/geometry2/tf2_kdl/src/tf2_kdl/tf2_kdl.py @@ -0,0 +1,153 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# author: Wim Meeussen + +import PyKDL +import rospy +import tf2_ros +from geometry_msgs.msg import PointStamped + +def transform_to_kdl(t): + """Convert a geometry_msgs Transform message to a PyKDL Frame. + + :param t: The Transform message to convert. + :type t: geometry_msgs.msg.TransformStamped + :return: The converted PyKDL frame. + :rtype: PyKDL.Frame + """ + + return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + PyKDL.Vector(t.transform.translation.x, + t.transform.translation.y, + t.transform.translation.z)) + + +def do_transform_vector(vector, transform): + """Apply a transform in the form of a geometry_msgs message to a PyKDL vector. + + :param vector: The PyKDL vector to transform. + :type vector: PyKDL.Vector + :param transform: The transform to apply. + :type transform: geometry_msgs.msg.TransformStamped + :return: The transformed vector. + :rtype: PyKDL.Vector + """ + res = transform_to_kdl(transform) * vector + res.header = transform.header + return res + +tf2_ros.TransformRegistration().add(PyKDL.Vector, do_transform_vector) + +def to_msg_vector(vector): + """Convert a PyKDL Vector to a geometry_msgs PointStamped message. + + :param vector: The vector to convert. + :type vector: PyKDL.Vector + :return: The converted vector/point. + :rtype: geometry_msgs.msg.PointStamped + """ + msg = PointStamped() + msg.header = vector.header + msg.point.x = vector[0] + msg.point.y = vector[1] + msg.point.z = vector[2] + return msg + +tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Vector, to_msg_vector) + +def from_msg_vector(msg): + """Convert a PointStamped message to a stamped PyKDL Vector. + + :param msg: The PointStamped message to convert. + :type msg: geometry_msgs.msg.PointStamped + :return: The timestamped converted PyKDL vector. + :rtype: PyKDL.Vector + """ + vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z) + return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id) + +tf2_ros.ConvertRegistration().add_from_msg(PyKDL.Vector, from_msg_vector) + +def convert_vector(vector): + """Convert a generic stamped triplet message to a stamped PyKDL Vector. + + :param vector: The message to convert. + :return: The timestamped converted PyKDL vector. + :rtype: PyKDL.Vector + """ + return tf2_ros.Stamped(PyKDL.Vector(vector), vector.header.stamp, vector.header.frame_id) + +tf2_ros.ConvertRegistration().add_convert((PyKDL.Vector, PyKDL.Vector), convert_vector) + +def do_transform_frame(frame, transform): + """Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. + + :param frame: The PyKDL frame to transform. + :type frame: PyKDL.Frame + :param transform: The transform to apply. + :type transform: geometry_msgs.msg.TransformStamped + :return: The transformed PyKDL frame. + :rtype: PyKDL.Frame + """ + res = transform_to_kdl(transform) * frame + res.header = transform.header + return res +tf2_ros.TransformRegistration().add(PyKDL.Frame, do_transform_frame) + +def do_transform_twist(twist, transform): + """Apply a transform in the form of a geometry_msgs message to a PyKDL Twist. + + :param twist: The PyKDL twist to transform. + :type twist: PyKDL.Twist + :param transform: The transform to apply. + :type transform: geometry_msgs.msg.TransformStamped + :return: The transformed PyKDL twist. + :rtype: PyKDL.Twist + """ + res = transform_to_kdl(transform) * twist + res.header = transform.header + return res +tf2_ros.TransformRegistration().add(PyKDL.Twist, do_transform_twist) + + +# Wrench +def do_transform_wrench(wrench, transform): + """Apply a transform in the form of a geometry_msgs message to a PyKDL Wrench. + + :param wrench: The PyKDL wrench to transform. + :type wrench: PyKDL.Wrench + :param transform: The transform to apply. + :type transform: geometry_msgs.msg.TransformStamped + :return: The transformed PyKDL wrench. + :rtype: PyKDL.Wrench + """ + res = transform_to_kdl(transform) * wrench + res.header = transform.header + return res +tf2_ros.TransformRegistration().add(PyKDL.Wrench, do_transform_wrench) diff --git a/src/geometry2/tf2_kdl/test/test.launch b/src/geometry2/tf2_kdl/test/test.launch new file mode 100644 index 0000000..bfad1de --- /dev/null +++ b/src/geometry2/tf2_kdl/test/test.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/src/geometry2/tf2_kdl/test/test_python.launch b/src/geometry2/tf2_kdl/test/test_python.launch new file mode 100644 index 0000000..f06b403 --- /dev/null +++ b/src/geometry2/tf2_kdl/test/test_python.launch @@ -0,0 +1,3 @@ + + + diff --git a/src/geometry2/tf2_kdl/test/test_tf2_kdl.cpp b/src/geometry2/tf2_kdl/test/test_tf2_kdl.cpp new file mode 100644 index 0000000..a0fdfd1 --- /dev/null +++ b/src/geometry2/tf2_kdl/test/test_tf2_kdl.cpp @@ -0,0 +1,131 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + + +#include +#include +#include +#include "tf2_ros/buffer.h" + + +tf2_ros::Buffer* tf_buffer; +static const double EPS = 1e-3; + +TEST(TfKDL, Frame) +{ + tf2::Stamped v1(KDL::Frame(KDL::Rotation::RPY(M_PI, 0, 0), KDL::Vector(1,2,3)), ros::Time(2.0), "A"); + + + // simple api + KDL::Frame v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); + EXPECT_NEAR(v_simple.p[0], -9, EPS); + EXPECT_NEAR(v_simple.p[1], 18, EPS); + EXPECT_NEAR(v_simple.p[2], 27, EPS); + double r, p, y; + v_simple.M.GetRPY(r, p, y); + EXPECT_NEAR(r, 0.0, EPS); + EXPECT_NEAR(p, 0.0, EPS); + EXPECT_NEAR(y, 0.0, EPS); + + + // advanced api + KDL::Frame v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), + "A", ros::Duration(3.0)); + EXPECT_NEAR(v_advanced.p[0], -9, EPS); + EXPECT_NEAR(v_advanced.p[1], 18, EPS); + EXPECT_NEAR(v_advanced.p[2], 27, EPS); + v_advanced.M.GetRPY(r, p, y); + EXPECT_NEAR(r, 0.0, EPS); + EXPECT_NEAR(p, 0.0, EPS); + EXPECT_NEAR(y, 0.0, EPS); + +} + + + +TEST(TfKDL, Vector) +{ + tf2::Stamped v1(KDL::Vector(1,2,3), ros::Time(2.0), "A"); + + + // simple api + KDL::Vector v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0)); + EXPECT_NEAR(v_simple[0], -9, EPS); + EXPECT_NEAR(v_simple[1], 18, EPS); + EXPECT_NEAR(v_simple[2], 27, EPS); + + // advanced api + KDL::Vector v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0), + "A", ros::Duration(3.0)); + EXPECT_NEAR(v_advanced[0], -9, EPS); + EXPECT_NEAR(v_advanced[1], 18, EPS); + EXPECT_NEAR(v_advanced[2], 27, EPS); +} + +TEST(TfKDL, ConvertVector) +{ + tf2::Stamped v(KDL::Vector(1,2,3), ros::Time(), "my_frame"); + + tf2::Stamped v1 = v; + tf2::convert(v1, v1); + + EXPECT_EQ(v, v1); + + tf2::Stamped v2(KDL::Vector(3,4,5), ros::Time(), "my_frame2"); + tf2::convert(v1, v2); + + EXPECT_EQ(v, v2); + EXPECT_EQ(v1, v2); +} + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test"); + ros::NodeHandle n; + + tf_buffer = new tf2_ros::Buffer(); + + // populate buffer + geometry_msgs::TransformStamped t; + t.transform.translation.x = 10; + t.transform.translation.y = 20; + t.transform.translation.z = 30; + t.transform.rotation.x = 1; + t.header.stamp = ros::Time(2.0); + t.header.frame_id = "A"; + t.child_frame_id = "B"; + tf_buffer->setTransform(t, "test"); + + int retval = RUN_ALL_TESTS(); + delete tf_buffer; + return retval; +} diff --git a/src/geometry2/tf2_msgs/CHANGELOG.rst b/src/geometry2/tf2_msgs/CHANGELOG.rst new file mode 100644 index 0000000..078e365 --- /dev/null +++ b/src/geometry2/tf2_msgs/CHANGELOG.rst @@ -0,0 +1,165 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ + +0.6.6 (2020-01-09) +------------------ + +0.6.5 (2018-11-16) +------------------ + +0.6.4 (2018-11-06) +------------------ + +0.6.3 (2018-07-09) +------------------ + +0.6.2 (2018-05-02) +------------------ + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ + +0.5.17 (2018-01-01) +------------------- + +0.5.16 (2017-07-14) +------------------- + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- + +0.5.13 (2016-03-04) +------------------- + +0.5.12 (2015-08-05) +------------------- + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- + +0.5.9 (2015-03-25) +------------------ + +0.5.8 (2015-03-17) +------------------ +* remove useless Makefile files +* Contributors: Vincent Rabaud + +0.5.7 (2014-12-23) +------------------ + +0.5.6 (2014-09-18) +------------------ + +0.5.5 (2014-06-23) +------------------ + +0.5.4 (2014-05-07) +------------------ + +0.5.3 (2014-02-21) +------------------ + +0.5.2 (2014-02-20) +------------------ + +0.5.1 (2014-02-14) +------------------ + +0.5.0 (2014-02-14) +------------------ + +0.4.10 (2013-12-26) +------------------- + +0.4.9 (2013-11-06) +------------------ + +0.4.8 (2013-11-06) +------------------ + +0.4.7 (2013-08-28) +------------------ + +0.4.6 (2013-08-28) +------------------ + +0.4.5 (2013-07-11) +------------------ + +0.4.4 (2013-07-09) +------------------ + +0.4.3 (2013-07-05) +------------------ + +0.4.2 (2013-07-05) +------------------ + +0.4.1 (2013-07-05) +------------------ + +0.4.0 (2013-06-27) +------------------ +* Restoring test packages and bullet packages. + reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented + reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ + +0.3.6 (2013-03-03) +------------------ + +0.3.5 (2013-02-15 14:46) +------------------------ +* 0.3.4 -> 0.3.5 + +0.3.4 (2013-02-15 13:14) +------------------------ +* 0.3.3 -> 0.3.4 + +0.3.3 (2013-02-15 11:30) +------------------------ +* 0.3.2 -> 0.3.3 + +0.3.2 (2013-02-15 00:42) +------------------------ +* 0.3.1 -> 0.3.2 + +0.3.1 (2013-02-14) +------------------ +* 0.3.0 -> 0.3.1 + +0.3.0 (2013-02-13) +------------------ +* switching to version 0.3.0 +* removing packages with missing deps +* adding include folder +* adding tf2_msgs/srv/FrameGraph.srv +* catkin fixes +* catkinizing geometry-experimental +* catkinizing tf2_msgs +* Adding ROS service interface to cpp Buffer +* fix tf messages dependency and name +* add python transform listener +* Compiling version of the buffer server +* Compiling version of the buffer client +* Adding a message that encapsulates errors that can be returned by tf +* A fully specified version of the LookupTransform.action +* Commiting so I can merge +* Adding action for LookupTransform +* Updating CMake to call genaction +* Moving tfMessage to TFMessage to adhere to naming conventions +* Copying tfMessage from tf to new tf2_msgs package +* Creating a package for new tf messages diff --git a/src/geometry2/tf2_msgs/CMakeLists.txt b/src/geometry2/tf2_msgs/CMakeLists.txt new file mode 100644 index 0000000..7c06a2a --- /dev/null +++ b/src/geometry2/tf2_msgs/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_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) + + + diff --git a/src/geometry2/tf2_msgs/action/LookupTransform.action b/src/geometry2/tf2_msgs/action/LookupTransform.action new file mode 100644 index 0000000..126e169 --- /dev/null +++ b/src/geometry2/tf2_msgs/action/LookupTransform.action @@ -0,0 +1,17 @@ +#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 +--- diff --git a/src/geometry2/tf2_msgs/include/foo b/src/geometry2/tf2_msgs/include/foo new file mode 100644 index 0000000..e69de29 diff --git a/src/geometry2/tf2_msgs/mainpage.dox b/src/geometry2/tf2_msgs/mainpage.dox new file mode 100644 index 0000000..19b1334 --- /dev/null +++ b/src/geometry2/tf2_msgs/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf2_msgs is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/src/geometry2/tf2_msgs/msg/TF2Error.msg b/src/geometry2/tf2_msgs/msg/TF2Error.msg new file mode 100644 index 0000000..e737b64 --- /dev/null +++ b/src/geometry2/tf2_msgs/msg/TF2Error.msg @@ -0,0 +1,10 @@ +uint8 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 diff --git a/src/geometry2/tf2_msgs/msg/TFMessage.msg b/src/geometry2/tf2_msgs/msg/TFMessage.msg new file mode 100644 index 0000000..fda1e4d --- /dev/null +++ b/src/geometry2/tf2_msgs/msg/TFMessage.msg @@ -0,0 +1 @@ +geometry_msgs/TransformStamped[] transforms diff --git a/src/geometry2/tf2_msgs/package.xml b/src/geometry2/tf2_msgs/package.xml new file mode 100644 index 0000000..ad5b51a --- /dev/null +++ b/src/geometry2/tf2_msgs/package.xml @@ -0,0 +1,26 @@ + + + tf2_msgs + 0.6.7 + + 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 + + + + diff --git a/src/geometry2/tf2_msgs/srv/FrameGraph.srv b/src/geometry2/tf2_msgs/srv/FrameGraph.srv new file mode 100755 index 0000000..568d196 --- /dev/null +++ b/src/geometry2/tf2_msgs/srv/FrameGraph.srv @@ -0,0 +1,2 @@ +--- +string frame_yaml diff --git a/src/geometry2/tf2_py/CHANGELOG.rst b/src/geometry2/tf2_py/CHANGELOG.rst new file mode 100644 index 0000000..be05691 --- /dev/null +++ b/src/geometry2/tf2_py/CHANGELOG.rst @@ -0,0 +1,152 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_py +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ +* [Windows][melodic-devel] Fix install locations (`#442 `_) +* Contributors: Sean Yen + +0.6.6 (2020-01-09) +------------------ +* use .pyd instead of .so on Windows and export symbols `#363 `_ from kejxu/fix_tf2_py_export +* limit MSVC-only change to MSVC scope (`#10 `_) +* Fix the pyd extension and export the init function. +* use windows counterpart for .so extension +* Contributors: James Xu, Sean Yen, Tully Foote + +0.6.5 (2018-11-16) +------------------ + +0.6.4 (2018-11-06) +------------------ +* fix translation vs rotation typo + Fixes `#324 `_ +* Add python3.7 compatibility. +* Contributors: Hans Gaiser, Tully Foote + +0.6.3 (2018-07-09) +------------------ + +0.6.2 (2018-05-02) +------------------ + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ + +0.5.17 (2018-01-01) +------------------- +* Merge pull request `#266 `_ from randoms/indigo-devel + fix METH_OLDARGS is no longer supported error in python3 +* Merge pull request `#260 `_ from randoms/indigo-devel + fix python3 import error +* Merge pull request `#257 `_ from delftrobotics-forks/python3 + Make tf2_py python3 compatible again +* Use string conversion from python_compat.h. +* Contributors: Maarten de Vries, Tully Foote, randoms + +0.5.16 (2017-07-14) +------------------- +* fix memory leak calling Py_DECREF for all created PyObject +* replaced dependencies on tf2_msgs_gencpp by exported dependencies +* Relax strict type checks at setTransform to only check for members (`#221 `_) +* expose deprecated methods in tf2_py API to support better backwards compatibility. Fixes `#206 `_ +* Contributors: Christopher Wecht, Sergio Ramos, Tully Foote, alex + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- +* Improve tf compatibility (`#192 `_) + getLatestCommonTime() is needed to implement the TF API. + See `ros/geometry#134 `_ +* Add missing type checks at Python/C++ tf2 transform interface `#159 `_ (`#197 `_) +* Make tf2_py compatible with python3. (`#173 `_) + * tf2_py: Use PyUnicode objects for text in python3. + * tf2_py: Make module initialization python3 compatible. + * tf2_py: Fix type definition for python3. + * tf2_py: Move and rename PyObject_BorrowAttrString. +* Contributors: Maarten de Vries, Timo Röhling, alex + +0.5.13 (2016-03-04) +------------------- + +0.5.12 (2015-08-05) +------------------- + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- + +0.5.9 (2015-03-25) +------------------ + +0.5.8 (2015-03-17) +------------------ + +0.5.7 (2014-12-23) +------------------ + +0.5.6 (2014-09-18) +------------------ + +0.5.5 (2014-06-23) +------------------ + +0.5.4 (2014-05-07) +------------------ + +0.5.3 (2014-02-21) +------------------ + +0.5.2 (2014-02-20) +------------------ + +0.5.1 (2014-02-14) +------------------ + +0.5.0 (2014-02-14) +------------------ + +0.4.10 (2013-12-26) +------------------- +* adding support for static transforms in python listener. Fixes `#46 `_ +* Contributors: Tully Foote + +0.4.9 (2013-11-06) +------------------ + +0.4.8 (2013-11-06) +------------------ + +0.4.7 (2013-08-28) +------------------ + +0.4.6 (2013-08-28) +------------------ + +0.4.5 (2013-07-11) +------------------ + +0.4.4 (2013-07-09) +------------------ +* tf2_py: Fixes warning, implicit conversion of NULL + +0.4.3 (2013-07-05) +------------------ + +0.4.2 (2013-07-05) +------------------ + +0.4.1 (2013-07-05) +------------------ + +0.4.0 (2013-06-27) +------------------ +* splitting rospy dependency into tf2_py so tf2 is pure c++ library. diff --git a/src/geometry2/tf2_py/CMakeLists.txt b/src/geometry2/tf2_py/CMakeLists.txt new file mode 100644 index 0000000..4b5d835 --- /dev/null +++ b/src/geometry2/tf2_py/CMakeLists.txt @@ -0,0 +1,162 @@ +cmake_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 ${catkin_EXPORTED_TARGETS}) + +if(WIN32) + # use .pyd extension on Windows + set_target_properties(tf2_py PROPERTIES OUTPUT_NAME "_tf2" SUFFIX ".pyd") +else() + set_target_properties(tf2_py PROPERTIES COMPILE_FLAGS "-g -Wno-missing-field-initializers") + set_target_properties(tf2_py PROPERTIES OUTPUT_NAME tf2 PREFIX "_" SUFFIX ".so") +endif() +set_target_properties(tf2_py PROPERTIES + ARCHIVE_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} + RUNTIME_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} +) +#!! rosbuild_add_compile_flags(tf2_py ${SSE_FLAGS}) #conditionally adds sse flags if available + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS tf2_py tf2_py_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +install(FILES $ + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_tf2_py.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/geometry2/tf2_py/package.xml b/src/geometry2/tf2_py/package.xml new file mode 100644 index 0000000..3413010 --- /dev/null +++ b/src/geometry2/tf2_py/package.xml @@ -0,0 +1,55 @@ + + + tf2_py + 0.6.7 + The tf2_py package + + + Tully Foote + + + + + + BSD + + + + + + http://ros.org/wiki/tf2_py + + + + + + + + + + + + + + + + + + + + catkin + tf2 + rospy + tf2 + rospy + + + + + + + + + + + \ No newline at end of file diff --git a/src/geometry2/tf2_py/setup.py b/src/geometry2/tf2_py/setup.py new file mode 100644 index 0000000..a21d885 --- /dev/null +++ b/src/geometry2/tf2_py/setup.py @@ -0,0 +1,12 @@ +#!/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) diff --git a/src/geometry2/tf2_py/src/python_compat.h b/src/geometry2/tf2_py/src/python_compat.h new file mode 100644 index 0000000..cb4c58c --- /dev/null +++ b/src/geometry2/tf2_py/src/python_compat.h @@ -0,0 +1,54 @@ +#ifndef TF2_PY_PYTHON_COMPAT_H +#define TF2_PY_PYTHON_COMPAT_H + +#include + +#include + +inline PyObject *stringToPython(const std::string &input) +{ +#if PY_MAJOR_VERSION >= 3 + return PyUnicode_FromStringAndSize(input.c_str(), input.size()); +#else + return PyString_FromStringAndSize(input.c_str(), input.size()); +#endif +} + +inline PyObject *stringToPython(const char *input) +{ +#if PY_MAJOR_VERSION >= 3 + return PyUnicode_FromString(input); +#else + return PyString_FromString(input); +#endif +} + +inline std::string stringFromPython(PyObject * input) +{ + Py_ssize_t size; +#if PY_MAJOR_VERSION >= 3 + const char * data; + data = PyUnicode_AsUTF8AndSize(input, &size); +#else + char * data; + PyString_AsStringAndSize(input, &data, &size); +#endif + return std::string(data, size); +} + +inline PyObject *pythonImport(const std::string & name) +{ + PyObject *py_name = stringToPython(name); + PyObject *module = PyImport_Import(py_name); + Py_XDECREF(py_name); + return module; +} + +inline PyObject *pythonBorrowAttrString(PyObject* o, const char *name) +{ + PyObject *r = PyObject_GetAttrString(o, name); + Py_XDECREF(r); + return r; +} + +#endif diff --git a/src/geometry2/tf2_py/src/tf2_py.cpp b/src/geometry2/tf2_py/src/tf2_py.cpp new file mode 100644 index 0000000..e6c2741 --- /dev/null +++ b/src/geometry2/tf2_py/src/tf2_py.cpp @@ -0,0 +1,644 @@ +#include + +#include +#include + +#include "python_compat.h" + +// Run x (a tf method, catching TF's exceptions and reraising them as Python exceptions) +// +#define WRAP(x) \ + do { \ + try \ + { \ + x; \ + } \ + catch (const tf2::ConnectivityException &e) \ + { \ + PyErr_SetString(tf2_connectivityexception, e.what()); \ + return NULL; \ + } \ + catch (const tf2::LookupException &e) \ + { \ + PyErr_SetString(tf2_lookupexception, e.what()); \ + return NULL; \ + } \ + catch (const tf2::ExtrapolationException &e) \ + { \ + PyErr_SetString(tf2_extrapolationexception, e.what()); \ + return NULL; \ + } \ + catch (const tf2::InvalidArgumentException &e) \ + { \ + PyErr_SetString(tf2_invalidargumentexception, e.what()); \ + return NULL; \ + } \ + catch (const tf2::TimeoutException &e) \ + { \ + PyErr_SetString(tf2_timeoutexception, e.what()); \ + return NULL; \ + } \ + catch (const tf2::TransformException &e) \ + { \ + PyErr_SetString(tf2_exception, e.what()); \ + return NULL; \ + } \ + } while (0) + +static PyObject *pModulerospy = NULL; +static PyObject *pModulegeometrymsgs = NULL; +static PyObject *tf2_exception = NULL; +static PyObject *tf2_connectivityexception = NULL, *tf2_lookupexception = NULL, *tf2_extrapolationexception = NULL, + *tf2_invalidargumentexception = NULL, *tf2_timeoutexception = NULL; + +struct buffer_core_t { + PyObject_HEAD + tf2::BufferCore *bc; +}; + + +static PyTypeObject buffer_core_Type = { +#if PY_MAJOR_VERSION < 3 + PyObject_HEAD_INIT(NULL) + 0, /*size*/ +# else + PyVarObject_HEAD_INIT(NULL, 0) +#endif + "_tf2.BufferCore", /*name*/ + sizeof(buffer_core_t), /*basicsize*/ +}; + +static PyObject *transform_converter(const geometry_msgs::TransformStamped* transform) +{ + PyObject *pclass, *pargs, *pinst = NULL; + pclass = PyObject_GetAttrString(pModulegeometrymsgs, "TransformStamped"); + if(pclass == NULL) + { + printf("Can't get geometry_msgs.msg.TransformedStamped\n"); + return NULL; + } + + pargs = Py_BuildValue("()"); + if(pargs == NULL) + { + printf("Can't build argument list\n"); + return NULL; + } + + pinst = PyEval_CallObject(pclass, pargs); + Py_DECREF(pclass); + Py_DECREF(pargs); + if(pinst == NULL) + { + printf("Can't create class\n"); + return NULL; + } + + //we need to convert the time to python + PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time"); + PyObject *args = Py_BuildValue("ii", transform->header.stamp.sec, transform->header.stamp.nsec); + PyObject *time_obj = PyObject_CallObject(rospy_time, args); + Py_DECREF(args); + Py_DECREF(rospy_time); + + PyObject* pheader = PyObject_GetAttrString(pinst, "header"); + PyObject_SetAttrString(pheader, "stamp", time_obj); + Py_DECREF(time_obj); + + PyObject *frame_id = stringToPython(transform->header.frame_id); + PyObject_SetAttrString(pheader, "frame_id", frame_id); + Py_DECREF(frame_id); + Py_DECREF(pheader); + + PyObject *ptransform = PyObject_GetAttrString(pinst, "transform"); + PyObject *ptranslation = PyObject_GetAttrString(ptransform, "translation"); + PyObject *protation = PyObject_GetAttrString(ptransform, "rotation"); + Py_DECREF(ptransform); + + PyObject *child_frame_id = stringToPython(transform->child_frame_id); + PyObject_SetAttrString(pinst, "child_frame_id", child_frame_id); + Py_DECREF(child_frame_id); + + PyObject *trans_x = PyFloat_FromDouble(transform->transform.translation.x); + PyObject *trans_y = PyFloat_FromDouble(transform->transform.translation.y); + PyObject *trans_z = PyFloat_FromDouble(transform->transform.translation.z); + PyObject_SetAttrString(ptranslation, "x", trans_x); + PyObject_SetAttrString(ptranslation, "y", trans_y); + PyObject_SetAttrString(ptranslation, "z", trans_z); + Py_DECREF(trans_x); + Py_DECREF(trans_y); + Py_DECREF(trans_z); + Py_DECREF(ptranslation); + + PyObject *rot_x = PyFloat_FromDouble(transform->transform.rotation.x); + PyObject *rot_y = PyFloat_FromDouble(transform->transform.rotation.y); + PyObject *rot_z = PyFloat_FromDouble(transform->transform.rotation.z); + PyObject *rot_w = PyFloat_FromDouble(transform->transform.rotation.w); + PyObject_SetAttrString(protation, "x", rot_x); + PyObject_SetAttrString(protation, "y", rot_y); + PyObject_SetAttrString(protation, "z", rot_z); + PyObject_SetAttrString(protation, "w", rot_w); + Py_DECREF(rot_x); + Py_DECREF(rot_y); + Py_DECREF(rot_z); + Py_DECREF(rot_w); + Py_DECREF(protation); + + return pinst; +} + +static int rostime_converter(PyObject *obj, ros::Time *rt) +{ + PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL); + if (tsr == NULL) { + PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration"); + return 0; + } else { + (*rt).fromSec(PyFloat_AsDouble(tsr)); + Py_DECREF(tsr); + return 1; + } +} + +static int rosduration_converter(PyObject *obj, ros::Duration *rt) +{ + PyObject *tsr = PyObject_CallMethod(obj, (char*)"to_sec", NULL); + if (tsr == NULL) { + PyErr_SetString(PyExc_TypeError, "time must have a to_sec method, e.g. rospy.Time or rospy.Duration"); + return 0; + } else { + (*rt).fromSec(PyFloat_AsDouble(tsr)); + Py_DECREF(tsr); + return 1; + } +} + +static int BufferCore_init(PyObject *self, PyObject *args, PyObject *kw) +{ + ros::Duration cache_time; + + cache_time.fromSec(tf2::BufferCore::DEFAULT_CACHE_TIME); + + if (!PyArg_ParseTuple(args, "|O&", rosduration_converter, &cache_time)) + return -1; + + ((buffer_core_t*)self)->bc = new tf2::BufferCore(cache_time); + + return 0; +} + +/* This may need to be implemented later if we decide to have it in the core +static PyObject *getTFPrefix(PyObject *self, PyObject *args) +{ + if (!PyArg_ParseTuple(args, "")) + return NULL; + tf::Transformer *t = ((transformer_t*)self)->t; + return stringToPython(t->getTFPrefix()); +} +*/ + +static PyObject *allFramesAsYAML(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + return stringToPython(bc->allFramesAsYAML()); +} + +static PyObject *allFramesAsString(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + return stringToPython(bc->allFramesAsString()); +} + +static PyObject *canTransformCore(PyObject *self, PyObject *args, PyObject *kw) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *target_frame, *source_frame; + ros::Time time; + static const char *keywords[] = { "target_frame", "source_frame", "time", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time)) + return NULL; + std::string error_msg; + bool can_transform = bc->canTransform(target_frame, source_frame, time, &error_msg); + //return PyBool_FromLong(t->canTransform(target_frame, source_frame, time)); + return Py_BuildValue("bs", can_transform, error_msg.c_str()); +} + +static PyObject *canTransformFullCore(PyObject *self, PyObject *args, PyObject *kw) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *target_frame, *source_frame, *fixed_frame; + ros::Time target_time, source_time; + static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords, + &target_frame, + rostime_converter, + &target_time, + &source_frame, + rostime_converter, + &source_time, + &fixed_frame)) + return NULL; + std::string error_msg; + bool can_transform = bc->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, &error_msg); + //return PyBool_FromLong(t->canTransform(target_frame, target_time, source_frame, source_time, fixed_frame)); + return Py_BuildValue("bs", can_transform, error_msg.c_str()); +} + +static PyObject *asListOfStrings(std::vector< std::string > los) +{ + PyObject *r = PyList_New(los.size()); + size_t i; + for (i = 0; i < los.size(); i++) { + PyList_SetItem(r, i, stringToPython(los[i])); + } + return r; +} + +static PyObject *_chain(PyObject *self, PyObject *args, PyObject *kw) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *target_frame, *source_frame, *fixed_frame; + ros::Time target_time, source_time; + std::vector< std::string > output; + static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords, + &target_frame, + rostime_converter, + &target_time, + &source_frame, + rostime_converter, + &source_time, + &fixed_frame)) + return NULL; + + WRAP(bc->_chainAsVector(target_frame, target_time, source_frame, source_time, fixed_frame, output)); + return asListOfStrings(output); +} + +static PyObject *getLatestCommonTime(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *target_frame, *source_frame; + tf2::CompactFrameID target_id, source_id; + ros::Time time; + std::string error_string; + + if (!PyArg_ParseTuple(args, "ss", &target_frame, &source_frame)) + return NULL; + WRAP(target_id = bc->_validateFrameId("get_latest_common_time", target_frame)); + WRAP(source_id = bc->_validateFrameId("get_latest_common_time", source_frame)); + int r = bc->_getLatestCommonTime(target_id, source_id, time, &error_string); + if (r == 0) { + PyObject *rospy_time = PyObject_GetAttrString(pModulerospy, "Time"); + PyObject *args = Py_BuildValue("ii", time.sec, time.nsec); + PyObject *ob = PyObject_CallObject(rospy_time, args); + Py_DECREF(args); + Py_DECREF(rospy_time); + return ob; + } else { + PyErr_SetString(tf2_exception, error_string.c_str()); + return NULL; + } +} + +static PyObject *lookupTransformCore(PyObject *self, PyObject *args, PyObject *kw) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *target_frame, *source_frame; + ros::Time time; + static const char *keywords[] = { "target_frame", "source_frame", "time", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&", (char**)keywords, &target_frame, &source_frame, rostime_converter, &time)) + return NULL; + geometry_msgs::TransformStamped transform; + WRAP(transform = bc->lookupTransform(target_frame, source_frame, time)); + geometry_msgs::Vector3 origin = transform.transform.translation; + geometry_msgs::Quaternion rotation = transform.transform.rotation; + //TODO: Create a converter that will actually return a python message + return Py_BuildValue("O&", transform_converter, &transform); + //return Py_BuildValue("(ddd)(dddd)", + // origin.x, origin.y, origin.z, + // rotation.x, rotation.y, rotation.z, rotation.w); +} + +static PyObject *lookupTransformFullCore(PyObject *self, PyObject *args, PyObject *kw) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *target_frame, *source_frame, *fixed_frame; + ros::Time target_time, source_time; + static const char *keywords[] = { "target_frame", "target_time", "source_frame", "source_time", "fixed_frame", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, kw, "sO&sO&s", (char**)keywords, + &target_frame, + rostime_converter, + &target_time, + &source_frame, + rostime_converter, + &source_time, + &fixed_frame)) + return NULL; + geometry_msgs::TransformStamped transform; + WRAP(transform = bc->lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame)); + geometry_msgs::Vector3 origin = transform.transform.translation; + geometry_msgs::Quaternion rotation = transform.transform.rotation; + //TODO: Create a converter that will actually return a python message + return Py_BuildValue("O&", transform_converter, &transform); +} +/* +static PyObject *lookupTwistCore(PyObject *self, PyObject *args, PyObject *kw) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *tracking_frame, *observation_frame; + ros::Time time; + ros::Duration averaging_interval; + static const char *keywords[] = { "tracking_frame", "observation_frame", "time", "averaging_interval", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&", (char**)keywords, &tracking_frame, &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval)) + return NULL; + geometry_msgs::Twist twist; + WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, time, averaging_interval)); + + return Py_BuildValue("(ddd)(ddd)", + twist.linear.x, twist.linear.y, twist.linear.z, + twist.angular.x, twist.angular.y, twist.angular.z); +} + +static PyObject *lookupTwistFullCore(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *tracking_frame, *observation_frame, *reference_frame, *reference_point_frame; + ros::Time time; + ros::Duration averaging_interval; + double px, py, pz; + + if (!PyArg_ParseTuple(args, "sss(ddd)sO&O&", + &tracking_frame, + &observation_frame, + &reference_frame, + &px, &py, &pz, + &reference_point_frame, + rostime_converter, &time, + rosduration_converter, &averaging_interval)) + return NULL; + geometry_msgs::Twist twist; + tf::Point pt(px, py, pz); + WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, reference_frame, pt, reference_point_frame, time, averaging_interval)); + + return Py_BuildValue("(ddd)(ddd)", + twist.linear.x, twist.linear.y, twist.linear.z, + twist.angular.x, twist.angular.y, twist.angular.z); +} +*/ +static inline int checkTranslationType(PyObject* o) +{ + PyTypeObject *translation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Vector3"); + int type_check = PyObject_TypeCheck(o, translation_type); + int attr_check = PyObject_HasAttrString(o, "x") && + PyObject_HasAttrString(o, "y") && + PyObject_HasAttrString(o, "z"); + if (!type_check) { + PyErr_WarnEx(PyExc_UserWarning, "translation should be of type Vector3", 1); + } + return attr_check; +} + +static inline int checkRotationType(PyObject* o) +{ + PyTypeObject *rotation_type = (PyTypeObject*) PyObject_GetAttrString(pModulegeometrymsgs, "Quaternion"); + int type_check = PyObject_TypeCheck(o, rotation_type); + int attr_check = PyObject_HasAttrString(o, "w") && + PyObject_HasAttrString(o, "x") && + PyObject_HasAttrString(o, "y") && + PyObject_HasAttrString(o, "z"); + if (!type_check) { + PyErr_WarnEx(PyExc_UserWarning, "rotation should be of type Quaternion", 1); + } + return attr_check; +} + +static PyObject *setTransform(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + PyObject *py_transform; + char *authority; + + if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority)) + return NULL; + + geometry_msgs::TransformStamped transform; + PyObject *header = pythonBorrowAttrString(py_transform, "header"); + transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id")); + transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id")); + if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) + return NULL; + + PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); + + PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); + if (!checkTranslationType(translation)) { + PyErr_SetString(PyExc_TypeError, "transform.translation must have members x, y, z"); + return NULL; + } + + transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x")); + transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y")); + transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z")); + + PyObject *rotation = pythonBorrowAttrString(mtransform, "rotation"); + if (!checkRotationType(rotation)) { + PyErr_SetString(PyExc_TypeError, "transform.rotation must have members w, x, y, z"); + return NULL; + } + + transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x")); + transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y")); + transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z")); + transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w")); + + bc->setTransform(transform, authority); + Py_RETURN_NONE; +} + +static PyObject *setTransformStatic(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + PyObject *py_transform; + char *authority; + + if (!PyArg_ParseTuple(args, "Os", &py_transform, &authority)) + return NULL; + + geometry_msgs::TransformStamped transform; + PyObject *header = pythonBorrowAttrString(py_transform, "header"); + transform.child_frame_id = stringFromPython(pythonBorrowAttrString(py_transform, "child_frame_id")); + transform.header.frame_id = stringFromPython(pythonBorrowAttrString(header, "frame_id")); + if (rostime_converter(pythonBorrowAttrString(header, "stamp"), &transform.header.stamp) != 1) + return NULL; + + PyObject *mtransform = pythonBorrowAttrString(py_transform, "transform"); + PyObject *translation = pythonBorrowAttrString(mtransform, "translation"); + if (!checkTranslationType(translation)) { + PyErr_SetString(PyExc_TypeError, "transform.translation must be of type Vector3"); + return NULL; + } + + transform.transform.translation.x = PyFloat_AsDouble(pythonBorrowAttrString(translation, "x")); + transform.transform.translation.y = PyFloat_AsDouble(pythonBorrowAttrString(translation, "y")); + transform.transform.translation.z = PyFloat_AsDouble(pythonBorrowAttrString(translation, "z")); + + PyObject *rotation = pythonBorrowAttrString(mtransform, "rotation"); + if (!checkRotationType(rotation)) { + PyErr_SetString(PyExc_TypeError, "transform.rotation must be of type Quaternion"); + return NULL; + } + + transform.transform.rotation.x = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "x")); + transform.transform.rotation.y = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "y")); + transform.transform.rotation.z = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "z")); + transform.transform.rotation.w = PyFloat_AsDouble(pythonBorrowAttrString(rotation, "w")); + + // only difference to above is is_static == True + bc->setTransform(transform, authority, true); + Py_RETURN_NONE; +} + +static PyObject *clear(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + bc->clear(); + Py_RETURN_NONE; +} + +static PyObject *_frameExists(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + char *frame_id_str; + if (!PyArg_ParseTuple(args, "s", &frame_id_str)) + return NULL; + return PyBool_FromLong(bc->_frameExists(frame_id_str)); +} + +static PyObject *_getFrameStrings(PyObject *self, PyObject *args) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + std::vector< std::string > ids; + bc->_getFrameStrings(ids); + return asListOfStrings(ids); +} + +static PyObject *_allFramesAsDot(PyObject *self, PyObject *args, PyObject *kw) +{ + tf2::BufferCore *bc = ((buffer_core_t*)self)->bc; + static const char *keywords[] = { "time", NULL }; + ros::Time time; + if (!PyArg_ParseTupleAndKeywords(args, kw, "|O&", (char**)keywords, rostime_converter, &time)) + return NULL; + return stringToPython(bc->_allFramesAsDot(time.toSec())); +} + + +static struct PyMethodDef buffer_core_methods[] = +{ + {"all_frames_as_yaml", allFramesAsYAML, METH_VARARGS}, + {"all_frames_as_string", allFramesAsString, METH_VARARGS}, + {"set_transform", setTransform, METH_VARARGS}, + {"set_transform_static", setTransformStatic, METH_VARARGS}, + {"can_transform_core", (PyCFunction)canTransformCore, METH_VARARGS | METH_KEYWORDS}, + {"can_transform_full_core", (PyCFunction)canTransformFullCore, METH_VARARGS | METH_KEYWORDS}, + {"_chain", (PyCFunction)_chain, METH_VARARGS | METH_KEYWORDS}, + {"clear", (PyCFunction)clear, METH_VARARGS | METH_KEYWORDS}, + {"_frameExists", (PyCFunction)_frameExists, METH_VARARGS}, + {"_getFrameStrings", (PyCFunction)_getFrameStrings, METH_VARARGS}, + {"_allFramesAsDot", (PyCFunction)_allFramesAsDot, METH_VARARGS | METH_KEYWORDS}, + {"get_latest_common_time", (PyCFunction)getLatestCommonTime, METH_VARARGS}, + {"lookup_transform_core", (PyCFunction)lookupTransformCore, METH_VARARGS | METH_KEYWORDS}, + {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | METH_KEYWORDS}, + //{"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS}, + //{"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS}, + //{"getTFPrefix", (PyCFunction)getTFPrefix, METH_VARARGS}, + {NULL, NULL} +}; + +static PyMethodDef module_methods[] = { + // {"Transformer", mkTransformer, METH_VARARGS}, + {0, 0, 0}, +}; + +bool staticInit() { +#if PYTHON_API_VERSION >= 1007 + tf2_exception = PyErr_NewException((char*)"tf2.TransformException", NULL, NULL); + tf2_connectivityexception = PyErr_NewException((char*)"tf2.ConnectivityException", tf2_exception, NULL); + tf2_lookupexception = PyErr_NewException((char*)"tf2.LookupException", tf2_exception, NULL); + tf2_extrapolationexception = PyErr_NewException((char*)"tf2.ExtrapolationException", tf2_exception, NULL); + tf2_invalidargumentexception = PyErr_NewException((char*)"tf2.InvalidArgumentException", tf2_exception, NULL); + tf2_timeoutexception = PyErr_NewException((char*)"tf2.TimeoutException", tf2_exception, NULL); +#else + tf2_exception = stringToPython("tf2.error"); + tf2_connectivityexception = stringToPython("tf2.ConnectivityException"); + tf2_lookupexception = stringToPython("tf2.LookupException"); + tf2_extrapolationexception = stringToPython("tf2.ExtrapolationException"); + tf2_invalidargumentexception = stringToPython("tf2.InvalidArgumentException"); + tf2_timeoutexception = stringToPython("tf2.TimeoutException"); +#endif + + pModulerospy = pythonImport("rospy"); + pModulegeometrymsgs = pythonImport("geometry_msgs.msg"); + + if(pModulegeometrymsgs == NULL) + { + printf("Cannot load geometry_msgs module"); + return false; + } + + buffer_core_Type.tp_alloc = PyType_GenericAlloc; + buffer_core_Type.tp_new = PyType_GenericNew; + buffer_core_Type.tp_init = BufferCore_init; + buffer_core_Type.tp_flags = Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE; + buffer_core_Type.tp_methods = buffer_core_methods; + if (PyType_Ready(&buffer_core_Type) != 0) + return false; + return true; +} + +PyObject *moduleInit(PyObject *m) { + PyModule_AddObject(m, "BufferCore", (PyObject *)&buffer_core_Type); + PyObject *d = PyModule_GetDict(m); + PyDict_SetItemString(d, "TransformException", tf2_exception); + PyDict_SetItemString(d, "ConnectivityException", tf2_connectivityexception); + PyDict_SetItemString(d, "LookupException", tf2_lookupexception); + PyDict_SetItemString(d, "ExtrapolationException", tf2_extrapolationexception); + PyDict_SetItemString(d, "InvalidArgumentException", tf2_invalidargumentexception); + PyDict_SetItemString(d, "TimeoutException", tf2_timeoutexception); + return m; +} + +#if PY_MAJOR_VERSION < 3 +extern "C" +{ + ROS_HELPER_EXPORT void init_tf2() + { + if (!staticInit()) + return; + moduleInit(Py_InitModule("_tf2", module_methods)); + } +} + +#else +struct PyModuleDef tf_module = { + PyModuleDef_HEAD_INIT, // base + "_tf2", // name + NULL, // docstring + -1, // state size (but we're using globals) + module_methods // methods +}; + +PyMODINIT_FUNC PyInit__tf2() +{ + if (!staticInit()) + return NULL; + return moduleInit(PyModule_Create(&tf_module)); +} +#endif diff --git a/src/geometry2/tf2_py/src/tf2_py/__init__.py b/src/geometry2/tf2_py/src/tf2_py/__init__.py new file mode 100644 index 0000000..5a38341 --- /dev/null +++ b/src/geometry2/tf2_py/src/tf2_py/__init__.py @@ -0,0 +1,38 @@ +#! /usr/bin/python +#*********************************************************** +#* Software License Agreement (BSD License) +#* +#* Copyright (c) 2009, Willow Garage, Inc. +#* All rights reserved. +#* +#* Redistribution and use in source and binary forms, with or without +#* modification, are permitted provided that the following conditions +#* are met: +#* +#* * Redistributions of source code must retain the above copyright +#* notice, this list of conditions and the following disclaimer. +#* * Redistributions in binary form must reproduce the above +#* copyright notice, this list of conditions and the following +#* disclaimer in the documentation and/or other materials provided +#* with the distribution. +#* * Neither the name of Willow Garage, Inc. nor the names of its +#* contributors may be used to endorse or promote products derived +#* from this software without specific prior written permission. +#* +#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +#* POSSIBILITY OF SUCH DAMAGE. +#* +#* Author: Eitan Marder-Eppstein +#*********************************************************** +from __future__ import absolute_import +from ._tf2 import * diff --git a/src/geometry2/tf2_ros/CHANGELOG.rst b/src/geometry2/tf2_ros/CHANGELOG.rst new file mode 100644 index 0000000..f2f3e4f --- /dev/null +++ b/src/geometry2/tf2_ros/CHANGELOG.rst @@ -0,0 +1,327 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ +* [windows][melodic] more portable fixes. (`#443 `_) +* [Windows][melodic-devel] Fix install locations (`#442 `_) +* Fixed warnings in message_filter.h (`#434 `_) + the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall +* Contributors: Sean Yen, moooeeeep + +0.6.6 (2020-01-09) +------------------ +* Remove roslib.load_manifest `#404 `_ +* Fix message filter `#402 `_ +* resolve virtual function call in destructor +* remove pending callbacks in clear() +* spelling fix: seperate -> separate `#372 `_ +* Fix dangling iterator references in buffer_server.cpp `#369 `_ +* Remove some useless code from buffer_server_main.cpp `#368 `_ +* Mark check_frequency as deprecated in docstring. +* Follow `#337 `_: use actionlib API in BufferClient::processGoal() +* Test for equality to None with 'is' instead of '==' `#355 `_ +* added parameter to advertise tf2-frames as a service, if needed +* Contributors: Daniel Ingram, Emre Sahin, JonasTietz, Lucas Walter, Michael Grupp, Robert Haschke, Tamaki Nishino, Tully Foote + +0.6.5 (2018-11-16) +------------------ +* Protect the time reset logic from a race condition. + Fixes `#341 `_ + This could incorrectly trigger a buffer clear if two concurrent callbacks were invoked. +* Contributors: Tully Foote + +0.6.4 (2018-11-06) +------------------ +* fix(buffer-client): Use actionlib api for obtaining result + Use the API provided by actionlib for waiting for result. This will improve the response time and prevent problems with custom solutions (see `#178 `_). This change makes constructor parameter check_frequency obsolute and deprecates it. +* Add check to buffer_client.py to make sure result is available + Related issue: `#178 `_ +* Add check to reset buffer when rostime goes backwards +* Fixed the value of expected_success_count\_ +* Added a tf2_ros message filter unittest with multiple target frames and non-zero time tolerance +* Contributors: Ewoud Pool, Jørgen Borgesen, Stephen Williams + +0.6.3 (2018-07-09) +------------------ + +0.6.2 (2018-05-02) +------------------ +* update buffer_server_name (`#296 `_) + * use nodename as namespace + * Update `#209 `_ to provide backwards compatibility. +* Contributors: Jihoon Lee, Tully Foote + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ +* tf2_ros::Buffer: canTransform can now deal with timeouts smaller than 10ms by using the hunderdth of the timeout for sleeping (`#286 `_) +* More spinning to make sure the message gets through for `#129 `_ `#283 `_ +* Contributors: Tully Foote, cwecht + +0.5.17 (2018-01-01) +------------------- +* Merge pull request `#260 `_ from randoms/indigo-devel + fix python3 import error +* Merge pull request `#257 `_ from delftrobotics-forks/python3 + Make tf2_py python3 compatible again +* Use python3 print function. +* Contributors: Maarten de Vries, Tully Foote, randoms + +0.5.16 (2017-07-14) +------------------- +* Merge pull request `#144 `_ from clearpathrobotics/dead_lock_fix + Solve a bug that causes a deadlock in MessageFilter +* Clear error string if it exists from the external entry points. + Fixes `#117 `_ +* Make buff_size and tcp_nodelay and subscriber queue size mutable. +* Remove generate_rand_vectors() from a number of tests. (`#227 `_) + * Remove generate_rand_vectors() from a number of tests. +* Log jump duration on backwards time jump detection. (`#234 `_) +* replaced dependencies on tf2_msgs_gencpp by exported dependencies +* Use new-style objects in python 2 +* Solve a bug that causes a deadlock in MessageFilter +* Contributors: Adel Fakih, Chris Lalancette, Christopher Wecht, Eric Wieser, Koji Terada, Stephan, Tully Foote, koji_terada + +0.5.15 (2017-01-24) +------------------- +* tf2_ros: add option to unregister TransformListener (`#201 `_) +* Contributors: Hans-Joachim Krauch + +0.5.14 (2017-01-16) +------------------- +* Drop roslib.load_manifest (`#191 `_) +* Adds ability to load TF from the ROS parameter server. +* Code linting & reorganization +* Fix indexing beyond end of array +* added a static transform broadcaster in python +* lots more documentation +* remove BufferCore doc, add BufferClient/BufferServer doc for C++, add Buffer/BufferInterface Python documentation +* Better overview for Python +* Contributors: Eric Wieser, Felix Duvallet, Jackie Kay, Mikael Arguedas, Mike Purvis + +0.5.13 (2016-03-04) +------------------- +* fix documentation warnings +* Adding tests to package +* Contributors: Laurent GEORGE, Vincent Rabaud + +0.5.12 (2015-08-05) +------------------- +* remove annoying gcc warning + This is because the roslog macro cannot have two arguments that are + formatting strings: we need to concatenate them first. +* break canTransform loop only for non-tiny negative time deltas + (At least) with Python 2 ros.Time.now() is not necessarily monotonic + and one can experience negative time deltas (usually well below 1s) + on real hardware under full load. This check was originally introduced + to allow for backjumps with rosbag replays, and only there it makes sense. + So we'll add a small duration threshold to ignore backjumps due to + non-monotonic clocks. +* Contributors: Vincent Rabaud, v4hn + +0.5.11 (2015-04-22) +------------------- +* do not short circuit waitForTransform timeout when running inside pytf. Fixes `#102 `_ + roscpp is not initialized inside pytf which means that ros::ok is not + valid. This was causing the timer to abort immediately. + This breaks support for pytf with respect to early breaking out of a loop re `#26 `_. + This is conceptually broken in pytf, and is fixed in tf2_ros python implementation. + If you want this behavior I recommend switching to the tf2 python bindings. +* inject timeout information into error string for canTransform with timeout +* Contributors: Tully Foote + +0.5.10 (2015-04-21) +------------------- +* switch to use a shared lock with upgrade instead of only a unique lock. For `#91 `_ +* Update message_filter.h +* filters: fix unsupported old messages with frame_id starting with '/' +* Enabled tf2 documentation +* make sure the messages get processed before testing the effects. Fixes `#88 `_ +* allowing to use message filters with PCL types +* Contributors: Brice Rebsamen, Jackie Kay, Tully Foote, Vincent Rabaud, jmtatsch + +0.5.9 (2015-03-25) +------------------ +* changed queue_size in Python transform boradcaster to match that in c++ +* Contributors: mrath + +0.5.8 (2015-03-17) +------------------ +* fix deadlock `#79 `_ +* break out of loop if ros is shutdown. Fixes `#26 `_ +* remove useless Makefile files +* Fix static broadcaster with rpy args +* Contributors: Paul Bovbel, Tully Foote, Vincent Rabaud + +0.5.7 (2014-12-23) +------------------ +* Added 6 param transform again + Yes, using Euler angles is a bad habit. But it is much more convenient if you just need a rotation by 90° somewhere to set it up in Euler angles. So I added the option to supply only the 3 angles. +* Remove tf2_py dependency for Android +* Contributors: Achim Königs, Gary Servin + +0.5.6 (2014-09-18) +------------------ +* support if canTransform(...): in python `#57 `_ +* Support clearing the cache when time jumps backwards `#68 `_ +* Contributors: Tully Foote + +0.5.5 (2014-06-23) +------------------ + +0.5.4 (2014-05-07) +------------------ +* surpressing autostart on the server objects to not incur warnings +* switch to boost signals2 following `ros/ros_comm#267 `_, blocking `ros/geometry#23 `_ +* fix compilation with gcc 4.9 +* make can_transform correctly wait +* explicitly set the publish queue size for rospy +* Contributors: Tully Foote, Vincent Rabaud, v4hn + +0.5.3 (2014-02-21) +------------------ + +0.5.2 (2014-02-20) +------------------ + +0.5.1 (2014-02-14) +------------------ +* adding const to MessageEvent data +* Contributors: Tully Foote + +0.5.0 (2014-02-14) +------------------ +* TF2 uses message events to get connection header information +* Contributors: Kevin Watts + +0.4.10 (2013-12-26) +------------------- +* adding support for static transforms in python listener. Fixes `#46 `_ +* Contributors: Tully Foote + +0.4.9 (2013-11-06) +------------------ + +0.4.8 (2013-11-06) +------------------ +* fixing pytf failing to sleep https://github.com/ros/geometry/issues/30 +* moving python documentation to tf2_ros from tf2 to follow the code +* Fixed static_transform_publisher duplicate check, added rostest. + +0.4.7 (2013-08-28) +------------------ +* fixing new conditional to cover the case that time has not progressed yet port forward of `ros/geometry#35 `_ in the python implementation +* fixing new conditional to cover the case that time has not progressed yet port forward of `ros/geometry#35 `_ + +0.4.6 (2013-08-28) +------------------ +* patching python implementation for `#24 `_ as well +* Stop waiting if time jumps backwards. fixes `#24 `_ +* patch to work around uninitiaized time. `#30 https://github.com/ros/geometry/issues/30`_ +* Removing unnecessary CATKIN_DEPENDS `#18 `_ + +0.4.5 (2013-07-11) +------------------ +* Revert "cherrypicking groovy patch for `#10 `_ into hydro" + This reverts commit 296d4916706d64f719b8c1592ab60d3686f994e1. + It was not starting up correctly. +* fixing usage string to show quaternions and using quaternions in the test app +* cherrypicking groovy patch for `#10 `_ into hydro + +0.4.4 (2013-07-09) +------------------ +* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change. +* reviving unrun unittest and adding CATKIN_ENABLE_TESTING guards + +0.4.3 (2013-07-05) +------------------ + +0.4.2 (2013-07-05) +------------------ + +0.4.1 (2013-07-05) +------------------ +* adding queue accessors lost in the new API +* exposing dedicated thread logic in BufferCore and checking in Buffer +* adding methods to enable backwards compatability for passing through to tf::Transformer + +0.4.0 (2013-06-27) +------------------ +* splitting rospy dependency into tf2_py so tf2 is pure c++ library. +* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2 +* Cleaning up unnecessary dependency on roscpp +* converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace +* fixing return by value for tranform method without preallocatoin +* Cleaning up packaging of tf2 including: + removing unused nodehandle + cleaning up a few dependencies and linking + removing old backup of package.xml + making diff minimally different from tf version of library +* Restoring test packages and bullet packages. + reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented + reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ +* Added link against catkin_LIBRARIES for tf2_ros lib, also CMakeLists.txt clean up + +0.3.6 (2013-03-03) +------------------ + +0.3.5 (2013-02-15 14:46) +------------------------ +* 0.3.4 -> 0.3.5 + +0.3.4 (2013-02-15 13:14) +------------------------ +* 0.3.3 -> 0.3.4 + +0.3.3 (2013-02-15 11:30) +------------------------ +* 0.3.2 -> 0.3.3 + +0.3.2 (2013-02-15 00:42) +------------------------ +* 0.3.1 -> 0.3.2 + +0.3.1 (2013-02-14) +------------------ +* 0.3.0 -> 0.3.1 + +0.3.0 (2013-02-13) +------------------ +* switching to version 0.3.0 +* Merge pull request `#2 `_ from KaijenHsiao/groovy-devel + added setup.py and catkin_python_setup() to tf2_ros +* added setup.py and catkin_python_setup() to tf2_ros +* fixing cmake target collisions +* fixing catkin message dependencies +* removing packages with missing deps +* catkin fixes +* catkinizing geometry-experimental +* catkinizing tf2_ros +* catching None result in buffer client before it becomes an AttributeError, raising tf2.TransformException instead +* oneiric linker fixes, bump version to 0.2.3 +* fix deprecated use of Header +* merged faust's changes 864 and 865 into non_optimized branch: BufferCore instead of Buffer in TransformListener, and added a constructor that takes a NodeHandle. +* add buffer server binary +* fix compilation on 32bit +* add missing file +* build buffer server +* TransformListener only needs a BufferCore +* Add TransformListener constructor that takes a NodeHandle so you can specify a callback queue to use +* Add option to use a callback queue in the message filter +* move the message filter to tf2_ros +* add missing std_msgs dependency +* missed 2 lines in last commit +* removing auto clearing from listener for it's unexpected from a library +* static transform tested and working +* subscriptions to tf_static unshelved +* static transform publisher executable running +* latching static transform publisher +* cleaning out old commented code +* Only query rospy.Time.now() when the timeout is greater than 0 +* debug comments removed +* move to tf2_ros completed. tests pass again +* merge tf2_cpp and tf2_py into tf2_ros diff --git a/src/geometry2/tf2_ros/CMakeLists.txt b/src/geometry2/tf2_ros/CMakeLists.txt new file mode 100644 index 0000000..201cb1c --- /dev/null +++ b/src/geometry2/tf2_ros/CMakeLists.txt @@ -0,0 +1,153 @@ +cmake_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} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +# buffer_server executable +add_executable(${PROJECT_NAME}_buffer_server src/buffer_server_main.cpp) +add_dependencies(${PROJECT_NAME}_buffer_server ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME}_buffer_server + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) +set_target_properties(${PROJECT_NAME}_buffer_server + PROPERTIES OUTPUT_NAME buffer_server +) + +# static_transform_publisher +add_executable(${PROJECT_NAME}_static_transform_publisher + src/static_transform_broadcaster_program.cpp +) +add_dependencies(${PROJECT_NAME}_static_transform_publisher ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME}_static_transform_publisher + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(${PROJECT_NAME}_static_transform_publisher + PROPERTIES OUTPUT_NAME static_transform_publisher +) + +# Install library +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Install executable +install(TARGETS + ${PROJECT_NAME}_buffer_server ${PROJECT_NAME}_static_transform_publisher + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + + +# Tests +if(CATKIN_ENABLE_TESTING) + +# new requirements for testing +find_package(catkin REQUIRED COMPONENTS + actionlib + actionlib_msgs + geometry_msgs + message_filters + roscpp + rosgraph + rospy + rostest + tf2 + tf2_msgs + ${TF2_PY} +) + + + +# tf2_ros_test_listener +add_executable(${PROJECT_NAME}_test_listener EXCLUDE_FROM_ALL test/listener_unittest.cpp) +add_dependencies(${PROJECT_NAME}_test_listener ${catkin_EXPORTED_TARGETS}) +add_executable(${PROJECT_NAME}_test_time_reset EXCLUDE_FROM_ALL test/time_reset_test.cpp) +add_dependencies(${PROJECT_NAME}_test_time_reset ${catkin_EXPORTED_TARGETS}) +add_executable(${PROJECT_NAME}_test_message_filter EXCLUDE_FROM_ALL test/message_filter_test.cpp) +add_dependencies(${PROJECT_NAME}_test_message_filter ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME}_test_listener + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${GTEST_LIBRARIES} +) + +target_link_libraries(${PROJECT_NAME}_test_time_reset + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${GTEST_LIBRARIES} +) + +target_link_libraries(${PROJECT_NAME}_test_message_filter + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${GTEST_LIBRARIES} +) + +add_dependencies(tests ${PROJECT_NAME}_test_listener) +add_dependencies(tests ${PROJECT_NAME}_test_time_reset) +add_dependencies(tests ${PROJECT_NAME}_test_message_filter) + +add_rostest(test/transform_listener_unittest.launch) +add_rostest(test/transform_listener_time_reset_test.launch) +add_rostest(test/message_filter_test.launch) + +endif() diff --git a/src/geometry2/tf2_ros/doc/conf.py b/src/geometry2/tf2_ros/doc/conf.py new file mode 100644 index 0000000..cee1a41 --- /dev/null +++ b/src/geometry2/tf2_ros/doc/conf.py @@ -0,0 +1,204 @@ +# -*- coding: utf-8 -*- +# +# tf documentation build configuration file, created by +# sphinx-quickstart on Mon Jun 1 14:21:53 2009. +# +# This file is execfile()d with the current directory set to its containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys, os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.append(os.path.abspath('.')) + +# -- General configuration ----------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be extensions +# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath'] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.rst' + +# The encoding of source files. +#source_encoding = 'utf-8' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'tf' +copyright = u'2009, Willow Garage, Inc.' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '0.1' +# The full version, including alpha/beta/rc tags. +release = '0.1.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of documents that shouldn't be included in the build. +#unused_docs = [] + +# List of directories, relative to source directory, that shouldn't be searched +# for source files. +exclude_trees = ['_build'] + +# The reST default role (used for this markup: `text`) to use for all documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. Major themes that come with +# Sphinx are currently 'default' and 'sphinxdoc'. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +#html_static_path = ['_static'] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_use_modindex = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = '' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'tfdoc' + + +# -- Options for LaTeX output -------------------------------------------------- + +# The paper size ('letter' or 'a4'). +#latex_paper_size = 'letter' + +# The font size ('10pt', '11pt' or '12pt'). +#latex_font_size = '10pt' + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, author, documentclass [howto/manual]). +latex_documents = [ + ('index', 'tf.tex', u'stereo\\_utils Documentation', + u'Tully Foote and Eitan Marder-Eppstein', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# Additional stuff for the LaTeX preamble. +#latex_preamble = '' + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_use_modindex = True + + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = { + 'http://docs.python.org/': None, + 'http://docs.opencv.org/3.0-last-rst/': None, + 'http://docs.scipy.org/doc/numpy' : None + } + +autoclass_content = "both" diff --git a/src/geometry2/tf2_ros/doc/index.rst b/src/geometry2/tf2_ros/doc/index.rst new file mode 100644 index 0000000..083feb1 --- /dev/null +++ b/src/geometry2/tf2_ros/doc/index.rst @@ -0,0 +1,48 @@ +tf2_ros Overview +================ + +This is the Python API reference for the tf2_ros package. + +To broadcast transforms using ROS: +- Call :meth:`rospy.init` to initialize a node. +- Construct a :class:`tf2_ros.TransformBroadcaster`. +- Pass a :class:`geometry_msgs.TransformStamped` message to :meth:`tf2_ros.TransformBroadcaster.sendTransform`. + + - Alternatively, pass a vector of :class:`geometry_msgs.TransformStamped` messages. + +To listen for transforms using ROS: +- Construct an instance of a class that implements :class:`tf2_ros.BufferInterface`. + + - :class:`tf2_ros.Buffer` is the standard implementation which offers a tf2_frames service that can respond to requests with a :class:`tf2_msgs.FrameGraph`. + - :class:`tf2_ros.BufferClient` uses an :class:`actionlib.SimpleActionClient` to wait for the requested transform to become available. + +- Pass the :class:`tf2_ros.Buffer` to the constructor of :class:`tf2_ros.TransformListener`. + - Optionally, pass a :class:`ros.NodeHandle` (otherwise TransformListener will connect to the node for the process). + - Optionally, specify if the TransformListener runs in its own thread or not. + +- Use :meth:`tf2_ros.BufferInterface.transform` to apply a transform on the tf server to an input frame. + - Or, check if a transform is available with :meth:`tf2_ros.BufferInterface.can_transform`. + - Then, call :meth:`tf2_ros.BufferInterface.lookup_transform` to get the transform between two frames. + +For more information, see the tf2 tutorials: http://wiki.ros.org/tf2/Tutorials + +Or, get an `overview`_ of data type conversion methods in geometry_experimental packages. + +See http://wiki.ros.org/tf2/Tutorials for more detailed usage. + +.. _overview: http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions + +Classes and Exceptions +====================== + +.. toctree:: + :maxdepth: 2 + + tf2_ros + + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` diff --git a/src/geometry2/tf2_ros/doc/mainpage.dox b/src/geometry2/tf2_ros/doc/mainpage.dox new file mode 100644 index 0000000..420a674 --- /dev/null +++ b/src/geometry2/tf2_ros/doc/mainpage.dox @@ -0,0 +1,41 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf2_ros is the C++ ROS wrapper around the tf2 transform library. + +\section codeapi Code API + +To broadcast transforms using ROS: +- Call ros::init() to initialize a node. +- Construct a tf2_ros::TransformBroadcaster. +- Pass a geometry_msgs::TransformStamped message to tf2_ros::TransformBroadcaster::sendTransform(). + - Alternatively, pass a vector of geometry_msgs::TransformStamped messages. +- Use tf2_ros::StaticTransformBroadcaster for "latching" behavior when transforms that are not expected to change. + +To listen for transforms using ROS: +- Construct an instance of a class that implements tf2_ros::BufferInterface. + - tf2_ros::Buffer is the standard implementation which offers a tf2_frames service that can respond to requests with a tf2_msgs::FrameGraph. + - tf2_ros::BufferClient uses an actionlib::SimpleActionClient to wait for the requested transform to become available. + - It should be used with a tf2_ros::BufferServer, which offers the corresponding actionlib::ActionSErver. +- Pass the tf2_ros::Buffer to the constructor of tf2_ros::TransformListener. + - Optionally, pass a ros::NodeHandle (otherwise TransformListener will connect to the node for the process). + - Optionally, specify if the TransformListener runs in its own thread or not. +- Use tf2_ros::BufferInterface::transform() to apply a transform on the tf server to an input frame. + - Or, check if a transform is available with tf2_ros::BufferInterface::canTransform(). + - Then, call tf2_ros::BufferInterface::lookupTransform() to get the transform between two frames. +- Construct a tf2_ros::MessageFilter with the TransformListener to apply a transformation to incoming frames. + - This is especially useful when streaming sensor data. + +List of exceptions thrown in this library: +- tf2::LookupException +- tf2::ConnectivityException +- tf2::ExtrapolationException +- tf2::InvalidArgumentException +- tf2::TimeoutException +- tf2::TransformException + +For more information, see the tf2 tutorials: http://wiki.ros.org/tf2/Tutorials + +Or, get an overview of data type conversion methods in geometry_experimental packages. +*/ diff --git a/src/geometry2/tf2_ros/doc/tf2_ros.rst b/src/geometry2/tf2_ros/doc/tf2_ros.rst new file mode 100644 index 0000000..10845ed --- /dev/null +++ b/src/geometry2/tf2_ros/doc/tf2_ros.rst @@ -0,0 +1,73 @@ +tf_ros2 Python API +================== + +Exceptions +---------- + +.. exception:: tf2.TransformException + + base class for tf exceptions. Because :exc:`tf2.TransformException` is the + base class for other exceptions, you can catch all tf exceptions + by writing:: + + try: + # do some tf2 work + except tf2.TransformException: + print "some tf2 exception happened" + + +.. exception:: tf2.ConnectivityException + + subclass of :exc:`TransformException`. + Raised when that the fixed_frame tree is not connected between the frames requested. + +.. exception:: tf2.LookupException + + subclass of :exc:`TransformException`. + Raised when a tf method has attempted to access a frame, but + the frame is not in the graph. + The most common reason for this is that the frame is not + being published, or a parent frame was not set correctly + causing the tree to be broken. + +.. exception:: tf2.ExtrapolationException + + subclass of :exc:`TransformException` + Raised when a tf method would have required extrapolation beyond current limits. + + +.. exception:: tf2.InvalidArgumentException + + subclass of :exc:`TransformException`. + Raised when the arguments to the method are called improperly formed. An example of why this might be raised is if an argument is nan. + +.. autoexception:: tf2_ros.buffer_interface.TypeException + +.. autoexception:: tf2_ros.buffer_interface.NotImplementedException + + +BufferInterface +--------------- +.. autoclass:: tf2_ros.buffer_interface.BufferInterface + :members: + +Buffer +------ +.. autoclass:: tf2_ros.buffer.Buffer + :members: + +BufferClient +------------ +.. autoclass:: tf2_ros.buffer_client.BufferClient + :members: + + +TransformBroadcaster +-------------------- +.. autoclass:: tf2_ros.transform_broadcaster.TransformBroadcaster + :members: + +TransformListener +----------------- +.. autoclass:: tf2_ros.transform_listener.TransformListener + :members: diff --git a/src/geometry2/tf2_ros/include/tf2_ros/buffer.h b/src/geometry2/tf2_ros/include/tf2_ros/buffer.h new file mode 100644 index 0000000..88ac1be --- /dev/null +++ b/src/geometry2/tf2_ros/include/tf2_ros/buffer.h @@ -0,0 +1,144 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + +#ifndef TF2_ROS_BUFFER_H +#define TF2_ROS_BUFFER_H + +#include +#include +#include +#include +#include + + +namespace tf2_ros +{ + + /** \brief Standard implementation of the tf2_ros::BufferInterface abstract data type. + * + * Inherits tf2_ros::BufferInterface and tf2::BufferCore. + * Stores known frames and offers a ROS service, "tf_frames", which responds to client requests + * with a response containing a tf2_msgs::FrameGraph representing the relationship of known frames. + */ + class Buffer: public BufferInterface, public tf2::BufferCore + { + public: + using tf2::BufferCore::lookupTransform; + using tf2::BufferCore::canTransform; + + /** + * @brief Constructor for a Buffer object + * @param cache_time How long to keep a history of transforms + * @param debug Whether to advertise the 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 target_time The time at which to transform + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + virtual bool + canTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& target_time, const ros::Duration timeout, std::string* errstr = NULL) const; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param target_time The time into which to transform + * \param source_frame The frame from which to transform + * \param source_time The time from which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + virtual bool + canTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr = NULL) const; + + + + + private: + bool getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res) ; + + + // conditionally error if dedicated_thread unset. + bool checkAndErrorDedicatedThreadPresent(std::string* errstr) const; + + ros::ServiceServer frames_server_; + + + }; // class + +static const std::string threading_error = "Do not call canTransform or lookupTransform with a timeout unless you are using another thread for populating data. Without a dedicated thread it will always timeout. If you have a separate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance."; + + +} // namespace + +#endif // TF2_ROS_BUFFER_H diff --git a/src/geometry2/tf2_ros/include/tf2_ros/buffer_client.h b/src/geometry2/tf2_ros/include/tf2_ros/buffer_client.h new file mode 100644 index 0000000..a5b0c08 --- /dev/null +++ b/src/geometry2/tf2_ros/include/tf2_ros/buffer_client.h @@ -0,0 +1,139 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ +#ifndef TF2_ROS_BUFFER_CLIENT_H_ +#define TF2_ROS_BUFFER_CLIENT_H_ + +#include +#include +#include + +namespace tf2_ros +{ + /** \brief Action client-based implementation of the tf2_ros::BufferInterface abstract data type. + * + * BufferClient uses actionlib to coordinate waiting for available transforms. + * + * You can use this class with a tf2_ros::BufferServer and tf2_ros::TransformListener in a separate process. + */ + class BufferClient : public BufferInterface + { + public: + typedef actionlib::SimpleActionClient LookupActionClient; + + /** \brief BufferClient constructor + * \param ns The namespace in which to look for a BufferServer + * \param check_frequency Deprecated, not used anymore + * \param timeout_padding The amount of time to allow passed the desired timeout on the client side for communication lag + */ + BufferClient(std::string ns, double check_frequency = 10.0, ros::Duration timeout_padding = ros::Duration(2.0)); + + /** \brief Get the transform between two frames by frame ID. + * \param target_frame The frame to which data should be transformed + * \param source_frame The frame where the data originated + * \param time The time at which the value of the transform is desired. (0 will get the latest) + * \param timeout How long to block before failing + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + virtual geometry_msgs::TransformStamped + lookupTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, const ros::Duration timeout = ros::Duration(0.0)) const; + + /** \brief Get the transform between two frames by frame ID assuming fixed frame. + * \param target_frame The frame to which data should be transformed + * \param target_time The time to which the data should be transformed. (0 will get the latest) + * \param source_frame The frame where the data originated + * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) + * \param fixed_frame The frame in which to assume the transform is constant in time. + * \param timeout How long to block before failing + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + virtual geometry_msgs::TransformStamped + lookupTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout = ros::Duration(0.0)) const; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param source_frame The frame from which to transform + * \param time The time at which to transform + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + virtual bool + canTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, const ros::Duration timeout = ros::Duration(0.0), std::string* errstr = NULL) const; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param target_time The time into which to transform + * \param source_frame The frame from which to transform + * \param source_time The time from which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + virtual bool + canTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout = ros::Duration(0.0), std::string* errstr = NULL) const; + + /** \brief Block until the action server is ready to respond to requests. + * \param timeout Time to wait for the server. + * \return True if the server is ready, false otherwise. + */ + bool waitForServer(const ros::Duration& timeout = ros::Duration(0)) + { + return client_.waitForServer(timeout); + } + + private: + geometry_msgs::TransformStamped processGoal(const tf2_msgs::LookupTransformGoal& goal) const; + geometry_msgs::TransformStamped processResult(const tf2_msgs::LookupTransformResult& result) const; + mutable LookupActionClient client_; + double check_frequency_; + ros::Duration timeout_padding_; + }; +}; +#endif diff --git a/src/geometry2/tf2_ros/include/tf2_ros/buffer_interface.h b/src/geometry2/tf2_ros/include/tf2_ros/buffer_interface.h new file mode 100644 index 0000000..9215008 --- /dev/null +++ b/src/geometry2/tf2_ros/include/tf2_ros/buffer_interface.h @@ -0,0 +1,267 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + +#ifndef TF2_ROS_BUFFER_INTERFACE_H +#define TF2_ROS_BUFFER_INTERFACE_H + +#include +#include +#include +#include +#include +#include + +namespace tf2_ros +{ + +/** \brief Abstract interface for wrapping tf2::BufferCore in a ROS-based API. + * Implementations include tf2_ros::Buffer and tf2_ros::BufferClient. + */ +class BufferInterface +{ +public: + + /** \brief Get the transform between two frames by frame ID. + * \param target_frame The frame to which data should be transformed + * \param source_frame The frame where the data originated + * \param time The time at which the value of the transform is desired. (0 will get the latest) + * \param timeout How long to block before failing + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + virtual geometry_msgs::TransformStamped + lookupTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, const ros::Duration timeout) const = 0; + + /** \brief Get the transform between two frames by frame ID assuming fixed frame. + * \param target_frame The frame to which data should be transformed + * \param target_time The time to which the data should be transformed. (0 will get the latest) + * \param source_frame The frame where the data originated + * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest) + * \param fixed_frame The frame in which to assume the transform is constant in time. + * \param timeout How long to block before failing + * \return The transform between the frames + * + * Possible exceptions tf2::LookupException, tf2::ConnectivityException, + * tf2::ExtrapolationException, tf2::InvalidArgumentException + */ + virtual geometry_msgs::TransformStamped + lookupTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout) const = 0; + + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param source_frame The frame from which to transform + * \param time The time at which to transform + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + virtual bool + canTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, const ros::Duration timeout, std::string* errstr = NULL) const = 0; + + /** \brief Test if a transform is possible + * \param target_frame The frame into which to transform + * \param target_time The time into which to transform + * \param source_frame The frame from which to transform + * \param source_time The time from which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time + * \param timeout How long to block before failing + * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL + * \return True if the transform is possible, false otherwise + */ + virtual bool + canTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr = NULL) const = 0; + + /** \brief Transform an input into the target frame. + * This function is templated and can take as input any valid mathematical object that tf knows + * how to apply a transform to, by way of the templated math conversions interface. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * \tparam T The type of the object to transform. + * \param in The object to transform + * \param out The transformed output, preallocated by the caller. + * \param target_frame The string identifer for the frame to transform into. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + */ + template + T& transform(const T& in, T& out, + const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const + { + // do the transform + tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout)); + return out; + } + + /** \brief Transform an input into the target frame. + * This function is templated and can take as input any valid mathematical object that tf knows + * how to apply a transform to, by way of the templated math conversions interface. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * \tparam T The type of the object to transform. + * \param in The object to transform. + * \param target_frame The string identifer for the frame to transform into. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + * \return The transformed output. + */ + template + T transform(const T& in, + const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const + { + T out; + return transform(in, out, target_frame, timeout); + } + + /** \brief Transform an input into the target frame and convert to a specified output type. + * It is templated on two types: the type of the input object and the type of the + * transformed output. + * For example, the template types could be Transform, Pose, Vector, or Quaternion messages + * type (as defined in geometry_msgs). + * The function will calculate the transformation and then convert the result into the + * specified output type. + * Compilation will fail if a known conversion does not exist bewteen the two template + * parameters. + * \tparam A The type of the object to transform. + * \tparam B The type of the transformed output. + * \param in The object to transform + * \param out The transformed output, converted to the specified type. + * \param target_frame The string identifer for the frame to transform into. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + * \return The transformed output, converted to the specified type. + */ + template + B& transform(const A& in, B& out, + const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const + { + A copy = transform(in, target_frame, timeout); + tf2::convert(copy, out); + return out; + } + + /** \brief Transform an input into the target frame (advanced). + * This function is templated and can take as input any valid mathematical object that tf knows + * how to apply a transform to, by way of the templated math conversions interface. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * This function follows the advanced API, which allows transforming between different time + * points, and specifying a fixed frame that does not varying in time. + * \tparam T The type of the object to transform. + * \param in The object to transform + * \param out The transformed output, preallocated by the caller. + * \param target_frame The string identifer for the frame to transform into. + * \param target_time The time into which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + */ + template + T& transform(const T& in, T& out, + const std::string& target_frame, const ros::Time& target_time, + const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const + { + // do the transform + tf2::doTransform(in, out, lookupTransform(target_frame, target_time, + tf2::getFrameId(in), tf2::getTimestamp(in), + fixed_frame, timeout)); + return out; + } + + + /** \brief Transform an input into the target frame (advanced). + * This function is templated and can take as input any valid mathematical object that tf knows + * how to apply a transform to, by way of the templated math conversions interface. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * This function follows the advanced API, which allows transforming between different time + * points, and specifying a fixed frame that does not varying in time. + * \tparam T The type of the object to transform. + * \param in The object to transform + * \param target_frame The string identifer for the frame to transform into. + * \param target_time The time into which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + * \return The transformed output. + */ + template + T transform(const T& in, + const std::string& target_frame, const ros::Time& target_time, + const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const + { + T out; + return transform(in, out, target_frame, target_time, fixed_frame, timeout); + } + + + /** \brief Transform an input into the target frame and convert to a specified output type (advanced). + * It is templated on two types: the type of the input object and the type of the + * transformed output. + * For example, the template type could be a Transform, Pose, Vector, or Quaternion message + * type (as defined in geometry_msgs). + * The function will calculate the transformation and then convert the result into the + * specified output type. + * Compilation will fail if a known conversion does not exist bewteen the two template + * parameters. + * This function follows the advanced API, which allows transforming between different time + * points, and specifying a fixed frame that does not varying in time. + * \tparam A The type of the object to transform. + * \tparam B The type of the transformed output. + * \param in The object to transform + * \param out The transformed output, converted to the specified output type. + * \param target_frame The string identifer for the frame to transform into. + * \param target_time The time into which to transform + * \param fixed_frame The frame in which to treat the transform as constant in time. + * \param timeout How long to wait for the target frame. Default value is zero (no blocking). + * \return The transformed output, converted to the specified output type. + */ + template + B& transform(const A& in, B& out, + const std::string& target_frame, const ros::Time& target_time, + const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const + { + // do the transform + A copy = transform(in, target_frame, target_time, fixed_frame, timeout); + tf2::convert(copy, out); + return out; + } + + + }; // class + + +} // namespace + +#endif // TF2_ROS_BUFFER_INTERFACE_H diff --git a/src/geometry2/tf2_ros/include/tf2_ros/buffer_server.h b/src/geometry2/tf2_ros/include/tf2_ros/buffer_server.h new file mode 100644 index 0000000..7481466 --- /dev/null +++ b/src/geometry2/tf2_ros/include/tf2_ros/buffer_server.h @@ -0,0 +1,92 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ +#ifndef TF2_ROS_BUFFER_SERVER_H_ +#define TF2_ROS_BUFFER_SERVER_H_ + +#include +#include +#include +#include + +namespace tf2_ros +{ + /** \brief Action server for the actionlib-based implementation of tf2_ros::BufferInterface. + * + * Use this class with a tf2_ros::TransformListener in the same process. + * You can use this class with a tf2_ros::BufferClient in a different process. + */ + class BufferServer + { + private: + typedef actionlib::ActionServer LookupTransformServer; + typedef LookupTransformServer::GoalHandle GoalHandle; + + struct GoalInfo + { + GoalHandle handle; + ros::Time end_time; + }; + + public: + /** \brief Constructor + * \param buffer The Buffer that this BufferServer will wrap. + * \param ns The namespace in which to look for action clients. + * \param auto_start Pass argument to the constructor of the ActionServer. + * \param check_period How often to check for changes to known transforms (via a timer event). + */ + BufferServer(const Buffer& buffer, const std::string& ns, + bool auto_start = true, ros::Duration check_period = ros::Duration(0.01)); + + /** \brief Start the action server. + */ + void start(); + + private: + void goalCB(GoalHandle gh); + void cancelCB(GoalHandle gh); + void checkTransforms(const ros::TimerEvent& e); + bool canTransform(GoalHandle gh); + geometry_msgs::TransformStamped lookupTransform(GoalHandle gh); + + const Buffer& buffer_; + LookupTransformServer server_; + std::list active_goals_; + boost::mutex mutex_; + ros::Timer check_timer_; + }; +} +#endif diff --git a/src/geometry2/tf2_ros/include/tf2_ros/message_filter.h b/src/geometry2/tf2_ros/include/tf2_ros/message_filter.h new file mode 100644 index 0000000..d741042 --- /dev/null +++ b/src/geometry2/tf2_ros/include/tf2_ros/message_filter.h @@ -0,0 +1,716 @@ +/* + * Copyright (c) 2010, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Josh Faust */ + +#ifndef TF2_ROS_MESSAGE_FILTER_H +#define TF2_ROS_MESSAGE_FILTER_H + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \ + ROS_DEBUG_NAMED("message_filter", std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), getTargetFramesString().c_str(), __VA_ARGS__) + +#define TF2_ROS_MESSAGEFILTER_WARN(fmt, ...) \ + ROS_WARN_NAMED("message_filter", std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), getTargetFramesString().c_str(), __VA_ARGS__) + +namespace tf2_ros +{ + +namespace filter_failure_reasons +{ +enum FilterFailureReason +{ + /// The message buffer overflowed, and this message was pushed off the back of the queue, but the reason it was unable to be transformed is unknown. + Unknown, + /// The timestamp on the message is more than the cache length earlier than the newest data in the transform cache + OutTheBack, + /// The frame_id on the message is empty + EmptyFrameID, +}; +} +typedef filter_failure_reasons::FilterFailureReason FilterFailureReason; + +class MessageFilterBase +{ +public: + typedef std::vector V_string; + + virtual ~MessageFilterBase(){} + virtual void clear() = 0; + virtual void setTargetFrame(const std::string& target_frame) = 0; + virtual void setTargetFrames(const V_string& target_frames) = 0; + virtual void setTolerance(const ros::Duration& tolerance) = 0; +}; + +/** + * \brief Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available + * + * The callbacks used in this class are of the same form as those used by roscpp's message callbacks. + * + * MessageFilter is templated on a message type. + * + * \section example_usage Example Usage + * + * If you want to hook a MessageFilter into a ROS topic: +\verbatim +message_filters::Subscriber sub(node_handle_, "topic", 10); +tf::MessageFilter tf_filter(sub, tf_listener_, "/map", 10); +tf_filter.registerCallback(&MyClass::myCallback, this); +\endverbatim + */ +template +class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter +{ +public: + typedef boost::shared_ptr MConstPtr; + typedef ros::MessageEvent MEvent; + typedef boost::function FailureCallback; + typedef boost::signals2::signal FailureSignal; + + // If you hit this assert your message does not have a header, or does not have the HasHeader trait defined for it + // Actually, we need to check that the message has a header, or that it + // has the FrameId and Stamp traits. However I don't know how to do that + // so simply commenting out for now. + //ROS_STATIC_ASSERT(ros::message_traits::HasHeader::value); + + /** + * \brief Constructor + * + * \param bc The tf2::BufferCore this filter should use + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param nh The NodeHandle whose callback queue we should add callbacks to + */ + MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh) + : bc_(bc) + , queue_size_(queue_size) + , callback_queue_(nh.getCallbackQueue()) + { + init(); + + setTargetFrame(target_frame); + } + + /** + * \brief Constructor + * + * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. + * \param bc The tf2::BufferCore this filter should use + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param nh The NodeHandle whose callback queue we should add callbacks to + */ + template + MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh) + : bc_(bc) + , queue_size_(queue_size) + , callback_queue_(nh.getCallbackQueue()) + { + init(); + + setTargetFrame(target_frame); + + connectInput(f); + } + + /** + * \brief Constructor + * + * \param bc The tf2::BufferCore this filter should use + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param cbqueue The callback queue to add callbacks to. If NULL, callbacks will happen from whatever thread either + * a) add() is called, which will generally be when the previous filter in the chain outputs a message, or + * b) tf2::BufferCore::setTransform() is called + */ + MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue) + : bc_(bc) + , queue_size_(queue_size) + , callback_queue_(cbqueue) + { + init(); + + setTargetFrame(target_frame); + } + + /** + * \brief Constructor + * + * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. + * \param bc The tf2::BufferCore this filter should use + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param cbqueue The callback queue to add callbacks to. If NULL, callbacks will happen from whatever thread either + * a) add() is called, which will generally be when the previous filter in the chain outputs a message, or + * b) tf2::BufferCore::setTransform() is called + */ + template + MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue) + : bc_(bc) + , queue_size_(queue_size) + , callback_queue_(cbqueue) + { + init(); + + setTargetFrame(target_frame); + + connectInput(f); + } + + /** + * \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first. + */ + template + void connectInput(F& f) + { + message_connection_.disconnect(); + message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this); + } + + /** + * \brief Destructor + */ + ~MessageFilter() + { + message_connection_.disconnect(); + + MessageFilter::clear(); + + TF2_ROS_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu", + (long long unsigned int)successful_transform_count_, + (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_, + (long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_); + + } + + /** + * \brief Set the frame you need to be able to transform to before getting a message callback + */ + void setTargetFrame(const std::string& target_frame) + { + V_string frames; + frames.push_back(target_frame); + setTargetFrames(frames); + } + + /** + * \brief Set the frames you need to be able to transform to before getting a message callback + */ + void setTargetFrames(const V_string& target_frames) + { + boost::mutex::scoped_lock frames_lock(target_frames_mutex_); + + target_frames_.resize(target_frames.size()); + std::transform(target_frames.begin(), target_frames.end(), target_frames_.begin(), this->stripSlash); + expected_success_count_ = target_frames_.size() * (time_tolerance_.isZero() ? 1 : 2); + + std::stringstream ss; + for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) + { + ss << *it << " "; + } + target_frames_string_ = ss.str(); + } + /** + * \brief Get the target frames as a string for debugging + */ + std::string getTargetFramesString() + { + boost::mutex::scoped_lock lock(target_frames_mutex_); + return target_frames_string_; + }; + + /** + * \brief Set the required tolerance for the notifier to return true + */ + void setTolerance(const ros::Duration& tolerance) + { + boost::mutex::scoped_lock lock(target_frames_mutex_); + time_tolerance_ = tolerance; + expected_success_count_ = target_frames_.size() * (time_tolerance_.isZero() ? 1 : 2); + } + + /** + * \brief Clear any messages currently in the queue + */ + void clear() + { + boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_); + + TF2_ROS_MESSAGEFILTER_DEBUG("%s", "Cleared"); + + bc_.removeTransformableCallback(callback_handle_); + callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, _1, _2, _3, _4, _5)); + + messages_.clear(); + message_count_ = 0; + + // remove pending callbacks in callback queue as well + if (callback_queue_) + callback_queue_->removeByID((uint64_t)this); + + warned_about_empty_frame_id_ = false; + } + + void add(const MEvent& evt) + { + if (target_frames_.empty()) + { + return; + } + + namespace mt = ros::message_traits; + const MConstPtr& message = evt.getMessage(); + std::string frame_id = stripSlash(mt::FrameId::value(*message)); + ros::Time stamp = mt::TimeStamp::value(*message); + + if (frame_id.empty()) + { + messageDropped(evt, filter_failure_reasons::EmptyFrameID); + return; + } + + // iterate through the target frames and add requests for each of them + MessageInfo info; + info.handles.reserve(expected_success_count_); + { + V_string target_frames_copy; + // Copy target_frames_ to avoid deadlock from #79 + { + boost::mutex::scoped_lock frames_lock(target_frames_mutex_); + target_frames_copy = target_frames_; + } + + V_string::iterator it = target_frames_copy.begin(); + V_string::iterator end = target_frames_copy.end(); + for (; it != end; ++it) + { + const std::string& target_frame = *it; + tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp); + if (handle == 0xffffffffffffffffULL) // never transformable + { + messageDropped(evt, filter_failure_reasons::OutTheBack); + return; + } + else if (handle == 0) + { + ++info.success_count; + } + else + { + info.handles.push_back(handle); + } + + if (!time_tolerance_.isZero()) + { + handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp + time_tolerance_); + if (handle == 0xffffffffffffffffULL) // never transformable + { + messageDropped(evt, filter_failure_reasons::OutTheBack); + return; + } + else if (handle == 0) + { + ++info.success_count; + } + else + { + info.handles.push_back(handle); + } + } + } + } + + + // We can transform already + if (info.success_count == expected_success_count_) + { + messageReady(evt); + } + else + { + boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_); + // If this message is about to push us past our queue size, erase the oldest message + if (queue_size_ != 0 && message_count_ + 1 > queue_size_) + { + ++dropped_message_count_; + const MessageInfo& front = messages_.front(); + TF2_ROS_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_, + (mt::FrameId::value(*front.event.getMessage())).c_str(), mt::TimeStamp::value(*front.event.getMessage()).toSec()); + + V_TransformableRequestHandle::const_iterator it = front.handles.begin(); + V_TransformableRequestHandle::const_iterator end = front.handles.end(); + + for (; it != end; ++it) + { + bc_.cancelTransformableRequest(*it); + } + + messageDropped(front.event, filter_failure_reasons::Unknown); + messages_.pop_front(); + --message_count_; + } + + // Add the message to our list + info.event = evt; + messages_.push_back(info); + ++message_count_; + } + + TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_); + + ++incoming_message_count_; + } + + /** + * \brief Manually add a message into this filter. + * \note If the message (or any other messages in the queue) are immediately transformable this will immediately call through to the output callback, possibly + * multiple times + */ + void add(const MConstPtr& message) + { + boost::shared_ptr > header(new std::map); + (*header)["callerid"] = "unknown"; + ros::WallTime n = ros::WallTime::now(); + ros::Time t(n.sec, n.nsec); + add(MEvent(message, header, t)); + } + + /** + * \brief Register a callback to be called when a message is about to be dropped + * \param callback The callback to call + */ + message_filters::Connection registerFailureCallback(const FailureCallback& callback) + { + boost::mutex::scoped_lock lock(failure_signal_mutex_); + return message_filters::Connection(boost::bind(&MessageFilter::disconnectFailure, this, _1), failure_signal_.connect(callback)); + } + + virtual void setQueueSize( uint32_t new_queue_size ) + { + queue_size_ = new_queue_size; + } + + virtual uint32_t getQueueSize() + { + return queue_size_; + } + + +private: + + void init() + { + message_count_ = 0; + successful_transform_count_ = 0; + failed_out_the_back_count_ = 0; + transform_message_count_ = 0; + incoming_message_count_ = 0; + dropped_message_count_ = 0; + time_tolerance_ = ros::Duration(0.0); + warned_about_empty_frame_id_ = false; + expected_success_count_ = 1; + + callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, _1, _2, _3, _4, _5)); + } + + void transformable(tf2::TransformableRequestHandle request_handle, const std::string& /* target_frame */, const std::string& /* source_frame */, + ros::Time /* time */, tf2::TransformableResult result) + { + namespace mt = ros::message_traits; + + boost::upgrade_lock< boost::shared_mutex > lock(messages_mutex_); + + // find the message this request is associated with + typename L_MessageInfo::iterator msg_it = messages_.begin(); + typename L_MessageInfo::iterator msg_end = messages_.end(); + for (; msg_it != msg_end; ++msg_it) + { + MessageInfo& info = *msg_it; + V_TransformableRequestHandle::const_iterator handle_it = std::find(info.handles.begin(), info.handles.end(), request_handle); + if (handle_it != info.handles.end()) + { + // found msg_it + ++info.success_count; + break; + } + } + + if (msg_it == msg_end) + { + return; + } + + const MessageInfo& info = *msg_it; + if (info.success_count < expected_success_count_) + { + return; + } + + bool can_transform = true; + const MConstPtr& message = info.event.getMessage(); + std::string frame_id = stripSlash(mt::FrameId::value(*message)); + ros::Time stamp = mt::TimeStamp::value(*message); + + if (result == tf2::TransformAvailable) + { + boost::mutex::scoped_lock frames_lock(target_frames_mutex_); + // make sure we can still perform all the necessary transforms + typename V_string::iterator it = target_frames_.begin(); + typename V_string::iterator end = target_frames_.end(); + for (; it != end; ++it) + { + const std::string& target = *it; + if (!bc_.canTransform(target, frame_id, stamp)) + { + can_transform = false; + break; + } + + if (!time_tolerance_.isZero()) + { + if (!bc_.canTransform(target, frame_id, stamp + time_tolerance_)) + { + can_transform = false; + break; + } + } + } + } + else + { + can_transform = false; + } + + // We will be mutating messages now, require unique lock + boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock); + if (can_transform) + { + TF2_ROS_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1); + + ++successful_transform_count_; + + messageReady(info.event); + + } + else + { + ++dropped_message_count_; + + TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1); + messageDropped(info.event, filter_failure_reasons::Unknown); + } + + messages_.erase(msg_it); + --message_count_; + } + + /** + * \brief Callback that happens when we receive a message on the message topic + */ + void incomingMessage(const ros::MessageEvent& evt) + { + add(evt); + } + + void checkFailures() + { + if (next_failure_warning_.isZero()) + { + next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(15); + } + + if (ros::WallTime::now() >= next_failure_warning_) + { + if (incoming_message_count_ - message_count_ == 0) + { + return; + } + + double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_); + if (dropped_pct > 0.95) + { + TF2_ROS_MESSAGEFILTER_WARN("Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct*100, ROSCONSOLE_DEFAULT_NAME); + next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(60); + + if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5) + { + TF2_ROS_MESSAGEFILTER_WARN(" The majority of dropped messages were due to messages growing older than the TF cache time. The last message's timestamp was: %f, and the last frame_id was: %s", last_out_the_back_stamp_.toSec(), last_out_the_back_frame_.c_str()); + } + } + } + } + + struct CBQueueCallback : public ros::CallbackInterface + { + CBQueueCallback(MessageFilter* filter, const MEvent& event, bool success, FilterFailureReason reason) + : event_(event) + , filter_(filter) + , reason_(reason) + , success_(success) + {} + + + virtual CallResult call() + { + if (success_) + { + filter_->signalMessage(event_); + } + else + { + filter_->signalFailure(event_, reason_); + } + + return Success; + } + + private: + MEvent event_; + MessageFilter* filter_; + FilterFailureReason reason_; + bool success_; + }; + + void messageDropped(const MEvent& evt, FilterFailureReason reason) + { + if (callback_queue_) + { + ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason)); + callback_queue_->addCallback(cb, (uint64_t)this); + } + else + { + signalFailure(evt, reason); + } + } + + void messageReady(const MEvent& evt) + { + if (callback_queue_) + { + ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, true, filter_failure_reasons::Unknown)); + callback_queue_->addCallback(cb, (uint64_t)this); + } + else + { + this->signalMessage(evt); + } + } + + void disconnectFailure(const message_filters::Connection& c) + { + boost::mutex::scoped_lock lock(failure_signal_mutex_); + c.getBoostConnection().disconnect(); + } + + void signalFailure(const MEvent& evt, FilterFailureReason reason) + { + boost::mutex::scoped_lock lock(failure_signal_mutex_); + failure_signal_(evt.getMessage(), reason); + } + + static + std::string stripSlash(const std::string& in) + { + if ( !in.empty() && (in[0] == '/')) + { + std::string out = in; + out.erase(0, 1); + return out; + } + return in; + } + + tf2::BufferCore& bc_; ///< The Transformer used to determine if transformation data is available + V_string target_frames_; ///< The frames we need to be able to transform to before a message is ready + std::string target_frames_string_; + boost::mutex target_frames_mutex_; ///< A mutex to protect access to the target_frames_ list and target_frames_string. + uint32_t queue_size_; ///< The maximum number of messages we queue up + tf2::TransformableCallbackHandle callback_handle_; + + typedef std::vector V_TransformableRequestHandle; + struct MessageInfo + { + MessageInfo() + : success_count(0) + {} + + MEvent event; + V_TransformableRequestHandle handles; + uint32_t success_count; + }; + typedef std::list L_MessageInfo; + L_MessageInfo messages_; + uint32_t message_count_; ///< The number of messages in the list. Used because \.size() may have linear cost + boost::shared_mutex messages_mutex_; ///< The mutex used for locking message list operations + uint32_t expected_success_count_; + + bool warned_about_empty_frame_id_; + + uint64_t successful_transform_count_; + uint64_t failed_out_the_back_count_; + uint64_t transform_message_count_; + uint64_t incoming_message_count_; + uint64_t dropped_message_count_; + + ros::Time last_out_the_back_stamp_; + std::string last_out_the_back_frame_; + + ros::WallTime next_failure_warning_; + + ros::Duration time_tolerance_; ///< Provide additional tolerance on time for messages which are stamped but can have associated duration + + message_filters::Connection message_connection_; + + FailureSignal failure_signal_; + boost::mutex failure_signal_mutex_; + + ros::CallbackQueueInterface* callback_queue_; +}; + +} // namespace tf2 + +#endif diff --git a/src/geometry2/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/src/geometry2/tf2_ros/include/tf2_ros/static_transform_broadcaster.h new file mode 100644 index 0000000..823062e --- /dev/null +++ b/src/geometry2/tf2_ros/include/tf2_ros/static_transform_broadcaster.h @@ -0,0 +1,73 @@ +/* + * 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 diff --git a/src/geometry2/tf2_ros/include/tf2_ros/transform_broadcaster.h b/src/geometry2/tf2_ros/include/tf2_ros/transform_broadcaster.h new file mode 100644 index 0000000..4f5f35c --- /dev/null +++ b/src/geometry2/tf2_ros/include/tf2_ros/transform_broadcaster.h @@ -0,0 +1,78 @@ +/* + * 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 diff --git a/src/geometry2/tf2_ros/include/tf2_ros/transform_listener.h b/src/geometry2/tf2_ros/include/tf2_ros/transform_listener.h new file mode 100644 index 0000000..d96864f --- /dev/null +++ b/src/geometry2/tf2_ros/include/tf2_ros/transform_listener.h @@ -0,0 +1,89 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Tully Foote */ + +#ifndef TF2_ROS_TRANSFORMLISTENER_H +#define TF2_ROS_TRANSFORMLISTENER_H + +#include "std_msgs/Empty.h" +#include "tf2_msgs/TFMessage.h" +#include "ros/ros.h" +#include "ros/callback_queue.h" + +#include "tf2_ros/buffer.h" + +#include "boost/thread.hpp" + +namespace tf2_ros{ + +/** \brief This class provides an easy way to request and receive coordinate frame transform information. + */ +class TransformListener +{ + +public: + /**@brief Constructor for transform listener */ + TransformListener(tf2::BufferCore& buffer, bool spin_thread = true); + 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 subscription + void subscription_callback(const ros::MessageEvent& msg_evt); + void static_subscription_callback(const ros::MessageEvent& msg_evt); + void subscription_callback_impl(const ros::MessageEvent& msg_evt, bool is_static); + + ros::CallbackQueue tf_message_callback_queue_; + boost::thread* dedicated_listener_thread_; + ros::NodeHandle node_; + ros::Subscriber message_subscriber_tf_; + ros::Subscriber message_subscriber_tf_static_; + tf2::BufferCore& buffer_; + bool using_dedicated_thread_; + ros::Time last_update_; + + void dedicatedListenerThread() + { + while (using_dedicated_thread_) + { + tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01)); + } + }; + +}; +} + +#endif //TF_TRANSFORMLISTENER_H diff --git a/src/geometry2/tf2_ros/package.xml b/src/geometry2/tf2_ros/package.xml new file mode 100644 index 0000000..8133349 --- /dev/null +++ b/src/geometry2/tf2_ros/package.xml @@ -0,0 +1,43 @@ + + tf2_ros + 0.6.7 + + This package contains the ROS bindings for the tf2 library, for both Python and C++. + + Eitan Marder-Eppstein + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/tf2_ros + + catkin + + actionlib + actionlib_msgs + geometry_msgs + message_filters + roscpp + rosgraph + rospy + std_msgs + tf2 + tf2_msgs + tf2_py + xmlrpcpp + + actionlib + actionlib_msgs + geometry_msgs + message_filters + roscpp + rosgraph + rospy + std_msgs + tf2 + tf2_msgs + tf2_py + xmlrpcpp + + rostest + diff --git a/src/geometry2/tf2_ros/rosdoc.yaml b/src/geometry2/tf2_ros/rosdoc.yaml new file mode 100644 index 0000000..0efc7fd --- /dev/null +++ b/src/geometry2/tf2_ros/rosdoc.yaml @@ -0,0 +1,8 @@ + - builder: doxygen + name: C++ API + output_dir: c++ + file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' + - builder: sphinx + name: Python API + output_dir: python + sphinx_root_dir: doc diff --git a/src/geometry2/tf2_ros/setup.py b/src/geometry2/tf2_ros/setup.py new file mode 100644 index 0000000..4473aa4 --- /dev/null +++ b/src/geometry2/tf2_ros/setup.py @@ -0,0 +1,13 @@ +#!/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) diff --git a/src/geometry2/tf2_ros/src/buffer.cpp b/src/geometry2/tf2_ros/src/buffer.cpp new file mode 100644 index 0000000..8030301 --- /dev/null +++ b/src/geometry2/tf2_ros/src/buffer.cpp @@ -0,0 +1,201 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Wim Meeussen */ + + +#include "tf2_ros/buffer.h" + +#include +#include + +namespace tf2_ros +{ + +static const double CAN_TRANSFORM_POLLING_SCALE = 0.01; + +Buffer::Buffer(ros::Duration cache_time, bool debug) : + BufferCore(cache_time) +{ + if(debug && !ros::service::exists("~tf2_frames", false)) + { + ros::NodeHandle n("~"); + frames_server_ = n.advertiseService("tf2_frames", &Buffer::getFrames, this); + } +} + +geometry_msgs::TransformStamped +Buffer::lookupTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, const ros::Duration timeout) const +{ + canTransform(target_frame, source_frame, time, timeout); + return lookupTransform(target_frame, source_frame, time); +} + + +geometry_msgs::TransformStamped +Buffer::lookupTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout) const +{ + canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); + return lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame); +} + +/** This is a workaround for the case that we're running inside of + rospy and ros::Time is not initialized inside the c++ instance. + This makes the system fall back to Wall time if not initialized. +*/ +ros::Time now_fallback_to_wall() +{ + try + { + return ros::Time::now(); + } + catch (ros::TimeNotInitializedException ex) + { + ros::WallTime wt = ros::WallTime::now(); + return ros::Time(wt.sec, wt.nsec); + } +} + +/** This is a workaround for the case that we're running inside of + rospy and ros::Time is not initialized inside the c++ instance. + This makes the system fall back to Wall time if not initialized. + https://github.com/ros/geometry/issues/30 +*/ +void sleep_fallback_to_wall(const ros::Duration& d) +{ + try + { + d.sleep(); + } + catch (ros::TimeNotInitializedException ex) + { + ros::WallDuration wd = ros::WallDuration(d.sec, d.nsec); + wd.sleep(); + } +} + +void conditionally_append_timeout_info(std::string * errstr, const ros::Time& start_time, + const ros::Duration& timeout) +{ + if (errstr) + { + std::stringstream ss; + ss << ". canTransform returned after "<< (now_fallback_to_wall() - start_time).toSec() \ + <<" timeout was " << timeout.toSec() << "."; + (*errstr) += ss.str(); + } +} + +bool +Buffer::canTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, const ros::Duration timeout, std::string* errstr) const +{ + // Clear the errstr before populating it if it's valid. + if (errstr) + { + errstr->clear(); + } + + if (!checkAndErrorDedicatedThreadPresent(errstr)) + return false; + + // poll for transform if timeout is set + ros::Time start_time = now_fallback_to_wall(); + const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE; + while (now_fallback_to_wall() < start_time + timeout && + !canTransform(target_frame, source_frame, time) && + (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop + (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf) + { + sleep_fallback_to_wall(sleep_duration); + } + bool retval = canTransform(target_frame, source_frame, time, errstr); + conditionally_append_timeout_info(errstr, start_time, timeout); + return retval; +} + + +bool +Buffer::canTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const +{ + // Clear the errstr before populating it if it's valid. + if (errstr) + { + errstr->clear(); + } + + if (!checkAndErrorDedicatedThreadPresent(errstr)) + return false; + + // poll for transform if timeout is set + ros::Time start_time = now_fallback_to_wall(); + const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE; + while (now_fallback_to_wall() < start_time + timeout && + !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) && + (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop + (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf) + { + sleep_fallback_to_wall(sleep_duration); + } + bool retval = canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, errstr); + conditionally_append_timeout_info(errstr, start_time, timeout); + return retval; +} + + +bool Buffer::getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res) +{ + res.frame_yaml = allFramesAsYAML(); + return true; +} + + + +bool Buffer::checkAndErrorDedicatedThreadPresent(std::string* error_str) const +{ + if (isUsingDedicatedThread()) + return true; + + + + if (error_str) + *error_str = tf2_ros::threading_error; + + ROS_ERROR("%s", tf2_ros::threading_error.c_str()); + return false; +} + + + +} diff --git a/src/geometry2/tf2_ros/src/buffer_client.cpp b/src/geometry2/tf2_ros/src/buffer_client.cpp new file mode 100644 index 0000000..98c56dd --- /dev/null +++ b/src/geometry2/tf2_ros/src/buffer_client.cpp @@ -0,0 +1,162 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ +#include + +namespace tf2_ros +{ + BufferClient::BufferClient(std::string ns, double check_frequency, ros::Duration timeout_padding): + client_(ns), + check_frequency_(check_frequency), + timeout_padding_(timeout_padding) + { + } + + geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, const ros::Duration timeout) const + { + //populate the goal message + tf2_msgs::LookupTransformGoal goal; + goal.target_frame = target_frame; + goal.source_frame = source_frame; + goal.source_time = time; + goal.timeout = timeout; + goal.advanced = false; + + return processGoal(goal); + } + + geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout) const + { + //populate the goal message + tf2_msgs::LookupTransformGoal goal; + goal.target_frame = target_frame; + goal.source_frame = source_frame; + goal.source_time = source_time; + goal.timeout = timeout; + goal.target_time = target_time; + goal.fixed_frame = fixed_frame; + goal.advanced = true; + + return processGoal(goal); + } + + geometry_msgs::TransformStamped BufferClient::processGoal(const tf2_msgs::LookupTransformGoal& goal) const + { + client_.sendGoal(goal); + + //this shouldn't happen, but could in rare cases where the server hangs + if(!client_.waitForResult(goal.timeout + timeout_padding_)) + { + //make sure to cancel the goal the server is pursuing + client_.cancelGoal(); + throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server."); + } + + if(client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) + throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server."); + + //process the result for errors and return it + return processResult(*client_.getResult()); + } + + geometry_msgs::TransformStamped BufferClient::processResult(const tf2_msgs::LookupTransformResult& result) const + { + //if there's no error, then we'll just return the transform + if(result.error.error != result.error.NO_ERROR){ + //otherwise, we'll have to throw the appropriate exception + if(result.error.error == result.error.LOOKUP_ERROR) + throw tf2::LookupException(result.error.error_string); + + if(result.error.error == result.error.CONNECTIVITY_ERROR) + throw tf2::ConnectivityException(result.error.error_string); + + if(result.error.error == result.error.EXTRAPOLATION_ERROR) + throw tf2::ExtrapolationException(result.error.error_string); + + if(result.error.error == result.error.INVALID_ARGUMENT_ERROR) + throw tf2::InvalidArgumentException(result.error.error_string); + + if(result.error.error == result.error.TIMEOUT_ERROR) + throw tf2::TimeoutException(result.error.error_string); + + throw tf2::TransformException(result.error.error_string); + } + + return result.transform; + } + + bool BufferClient::canTransform(const std::string& target_frame, const std::string& source_frame, + const ros::Time& time, const ros::Duration timeout, std::string* errstr) const + { + try + { + lookupTransform(target_frame, source_frame, time, timeout); + return true; + } + catch(tf2::TransformException& ex) + { + if(errstr) + { + errstr->clear(); + *errstr = ex.what(); + } + return false; + } + } + + bool BufferClient::canTransform(const std::string& target_frame, const ros::Time& target_time, + const std::string& source_frame, const ros::Time& source_time, + const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const + { + try + { + lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout); + return true; + } + catch(tf2::TransformException& ex) + { + if(errstr) + { + errstr->clear(); + *errstr = ex.what(); + } + return false; + } + } +}; diff --git a/src/geometry2/tf2_ros/src/buffer_server.cpp b/src/geometry2/tf2_ros/src/buffer_server.cpp new file mode 100644 index 0000000..ebfdd2f --- /dev/null +++ b/src/geometry2/tf2_ros/src/buffer_server.cpp @@ -0,0 +1,222 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Eitan Marder-Eppstein +*********************************************************************/ +#include + +namespace tf2_ros +{ + BufferServer::BufferServer(const Buffer& buffer, const std::string& ns, bool auto_start, ros::Duration check_period): + buffer_(buffer), + server_(ros::NodeHandle(), + ns, + boost::bind(&BufferServer::goalCB, this, _1), + boost::bind(&BufferServer::cancelCB, this, _1), + auto_start) + { + ros::NodeHandle n; + check_timer_ = n.createTimer(check_period, boost::bind(&BufferServer::checkTransforms, this, _1)); + } + + void BufferServer::checkTransforms(const ros::TimerEvent& e) + { + (void) e; //Unused + boost::mutex::scoped_lock l(mutex_); + for(std::list::iterator it = active_goals_.begin(); it != active_goals_.end();) + { + GoalInfo& info = *it; + + //we want to lookup a transform if the time on the goal + //has expired, or a transform is available + if(canTransform(info.handle) || info.end_time < ros::Time::now()) + { + tf2_msgs::LookupTransformResult result; + + //try to populate the result, catching exceptions if they occur + try + { + result.transform = lookupTransform(info.handle); + } + catch (tf2::ConnectivityException &ex) + { + result.error.error = result.error.CONNECTIVITY_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::LookupException &ex) + { + result.error.error = result.error.LOOKUP_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::ExtrapolationException &ex) + { + result.error.error = result.error.EXTRAPOLATION_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::InvalidArgumentException &ex) + { + result.error.error = result.error.INVALID_ARGUMENT_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::TimeoutException &ex) + { + result.error.error = result.error.TIMEOUT_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::TransformException &ex) + { + result.error.error = result.error.TRANSFORM_ERROR; + result.error.error_string = ex.what(); + } + + //make sure to pass the result to the client + //even failed transforms are considered a success + //since the request was successfully processed + info.handle.setSucceeded(result); + it = active_goals_.erase(it); + } + else + ++it; + } + } + + void BufferServer::cancelCB(GoalHandle gh) + { + boost::mutex::scoped_lock l(mutex_); + //we need to find the goal in the list and remove it... also setting it as canceled + //if its not in the list, we won't do anything since it will have already been set + //as completed + for(std::list::iterator it = active_goals_.begin(); it != active_goals_.end();) + { + GoalInfo& info = *it; + if(info.handle == gh) + { + info.handle.setCanceled(); + it = active_goals_.erase(it); + return; + } + else + ++it; + } + } + + void BufferServer::goalCB(GoalHandle gh) + { + //we'll accept all goals we get + gh.setAccepted(); + + //if the transform isn't immediately available, we'll push it onto our list to check + //along with the time that the goal will end + GoalInfo goal_info; + goal_info.handle = gh; + goal_info.end_time = ros::Time::now() + gh.getGoal()->timeout; + + //we can do a quick check here to see if the transform is valid + //we'll also do this if the end time has been reached + if(canTransform(gh) || goal_info.end_time <= ros::Time::now()) + { + tf2_msgs::LookupTransformResult result; + try + { + result.transform = lookupTransform(gh); + } + catch (tf2::ConnectivityException &ex) + { + result.error.error = result.error.CONNECTIVITY_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::LookupException &ex) + { + result.error.error = result.error.LOOKUP_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::ExtrapolationException &ex) + { + result.error.error = result.error.EXTRAPOLATION_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::InvalidArgumentException &ex) + { + result.error.error = result.error.INVALID_ARGUMENT_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::TimeoutException &ex) + { + result.error.error = result.error.TIMEOUT_ERROR; + result.error.error_string = ex.what(); + } + catch (tf2::TransformException &ex) + { + result.error.error = result.error.TRANSFORM_ERROR; + result.error.error_string = ex.what(); + } + + gh.setSucceeded(result); + return; + } + + boost::mutex::scoped_lock l(mutex_); + active_goals_.push_back(goal_info); + } + + bool BufferServer::canTransform(GoalHandle gh) + { + const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal(); + + //check whether we need to used the advanced or simple api + if(!goal->advanced) + return buffer_.canTransform(goal->target_frame, goal->source_frame, goal->source_time); + + return buffer_.canTransform(goal->target_frame, goal->target_time, + goal->source_frame, goal->source_time, goal->fixed_frame); + } + + geometry_msgs::TransformStamped BufferServer::lookupTransform(GoalHandle gh) + { + const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal(); + + //check whether we need to used the advanced or simple api + if(!goal->advanced) + return buffer_.lookupTransform(goal->target_frame, goal->source_frame, goal->source_time); + + return buffer_.lookupTransform(goal->target_frame, goal->target_time, + goal->source_frame, goal->source_time, goal->fixed_frame); + } + + void BufferServer::start() + { + server_.start(); + } + +}; diff --git a/src/geometry2/tf2_ros/src/buffer_server_main.cpp b/src/geometry2/tf2_ros/src/buffer_server_main.cpp new file mode 100644 index 0000000..85a79c2 --- /dev/null +++ b/src/geometry2/tf2_ros/src/buffer_server_main.cpp @@ -0,0 +1,71 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* Author: Wim Meeussen +*********************************************************************/ +#include +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "tf_buffer"); + ros::NodeHandle nh; + + double buffer_size; + nh.param("buffer_size", buffer_size, 120.0); + + bool publish_frame_service; + nh.param("publish_frame_service", publish_frame_service, false); + + // Legacy behavior re: #209 + bool use_node_namespace; + nh.param("use_node_namespace", use_node_namespace, false); + std::string node_name; + if (use_node_namespace) + { + node_name = ros::this_node::getName(); + } + else + { + node_name = "tf2_buffer_server"; + } + + tf2_ros::Buffer buffer_core(ros::Duration(buffer_size), publish_frame_service); + tf2_ros::TransformListener listener(buffer_core); + tf2_ros::BufferServer buffer_server(buffer_core, node_name , false); + buffer_server.start(); + + ros::spin(); +} diff --git a/src/geometry2/tf2_ros/src/static_transform_broadcaster.cpp b/src/geometry2/tf2_ros/src/static_transform_broadcaster.cpp new file mode 100644 index 0000000..df17ed9 --- /dev/null +++ b/src/geometry2/tf2_ros/src/static_transform_broadcaster.cpp @@ -0,0 +1,77 @@ +/* + * 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_); +} + + +} + + diff --git a/src/geometry2/tf2_ros/src/static_transform_broadcaster_program.cpp b/src/geometry2/tf2_ros/src/static_transform_broadcaster_program.cpp new file mode 100644 index 0000000..3485d1d --- /dev/null +++ b/src/geometry2/tf2_ros/src/static_transform_broadcaster_program.cpp @@ -0,0 +1,145 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include "tf2_ros/static_transform_broadcaster.h" + + +bool validateXmlRpcTf(XmlRpc::XmlRpcValue tf_data) { + // Validate a TF stored in XML RPC format: ensures the appropriate fields + // exist. Note this does not check data types. + return tf_data.hasMember("child_frame_id") && + tf_data.hasMember("header") && + tf_data["header"].hasMember("frame_id") && + tf_data.hasMember("transform") && + tf_data["transform"].hasMember("translation") && + tf_data["transform"]["translation"].hasMember("x") && + tf_data["transform"]["translation"].hasMember("y") && + tf_data["transform"]["translation"].hasMember("z") && + tf_data["transform"].hasMember("rotation") && + tf_data["transform"]["rotation"].hasMember("x") && + tf_data["transform"]["rotation"].hasMember("y") && + tf_data["transform"]["rotation"].hasMember("z") && + tf_data["transform"]["rotation"].hasMember("w"); +}; + +int main(int argc, char ** argv) +{ + //Initialize ROS + ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName); + tf2_ros::StaticTransformBroadcaster broadcaster; + geometry_msgs::TransformStamped msg; + + if(argc == 10) + { + msg.transform.translation.x = atof(argv[1]); + msg.transform.translation.y = atof(argv[2]); + msg.transform.translation.z = atof(argv[3]); + msg.transform.rotation.x = atof(argv[4]); + msg.transform.rotation.y = atof(argv[5]); + msg.transform.rotation.z = atof(argv[6]); + msg.transform.rotation.w = atof(argv[7]); + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = argv[8]; + msg.child_frame_id = argv[9]; + } + else if (argc == 9) + { + msg.transform.translation.x = atof(argv[1]); + msg.transform.translation.y = atof(argv[2]); + msg.transform.translation.z = atof(argv[3]); + + tf2::Quaternion quat; + quat.setRPY(atof(argv[6]), atof(argv[5]), atof(argv[4])); + msg.transform.rotation.x = quat.x(); + msg.transform.rotation.y = quat.y(); + msg.transform.rotation.z = quat.z(); + msg.transform.rotation.w = quat.w(); + + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = argv[7]; + msg.child_frame_id = argv[8]; + } + else if (argc == 2) { + const std::string param_name = argv[1]; + ROS_INFO_STREAM("Looking for TF in parameter: " << param_name); + XmlRpc::XmlRpcValue tf_data; + + if (!ros::param::has(param_name) || !ros::param::get(param_name, tf_data)) { + ROS_FATAL_STREAM("Could not read TF from parameter server: " << param_name); + return -1; + } + + // Check that all required members are present & of the right type. + if (!validateXmlRpcTf(tf_data)) { + ROS_FATAL_STREAM("Could not validate XmlRpcC for TF data: " << tf_data); + return -1; + } + + msg.transform.translation.x = (double) tf_data["transform"]["translation"]["x"]; + msg.transform.translation.y = (double) tf_data["transform"]["translation"]["y"]; + msg.transform.translation.z = (double) tf_data["transform"]["translation"]["z"]; + msg.transform.rotation.x = (double) tf_data["transform"]["rotation"]["x"]; + msg.transform.rotation.y = (double) tf_data["transform"]["rotation"]["y"]; + msg.transform.rotation.z = (double) tf_data["transform"]["rotation"]["z"]; + msg.transform.rotation.w = (double) tf_data["transform"]["rotation"]["w"]; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = (std::string) tf_data["header"]["frame_id"]; + msg.child_frame_id = (std::string) tf_data["child_frame_id"]; + } + else + { + printf("A command line utility for manually sending a transform.\n"); + //printf("It will periodicaly republish the given transform. \n"); + printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \n"); + printf("OR \n"); + printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \n"); + printf("OR \n"); + printf("Usage: static_transform_publisher /param_name \n"); + printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n"); + printf("of the child_frame_id. \n"); + ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments"); + return -1; + } + + // Checks: frames should not be the same. + if (msg.header.frame_id == msg.child_frame_id) + { + ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", + msg.header.frame_id.c_str(), msg.child_frame_id.c_str()); + return 1; + } + + broadcaster.sendTransform(msg); + ROS_INFO("Spinning until killed publishing %s to %s", + msg.header.frame_id.c_str(), msg.child_frame_id.c_str()); + ros::spin(); + return 0; +}; diff --git a/src/geometry2/tf2_ros/src/tf2_ros/__init__.py b/src/geometry2/tf2_ros/src/tf2_ros/__init__.py new file mode 100644 index 0000000..af5642c --- /dev/null +++ b/src/geometry2/tf2_ros/src/tf2_ros/__init__.py @@ -0,0 +1,44 @@ +#! /usr/bin/python +#*********************************************************** +#* Software License Agreement (BSD License) +#* +#* Copyright (c) 2009, Willow Garage, Inc. +#* All rights reserved. +#* +#* Redistribution and use in source and binary forms, with or without +#* modification, are permitted provided that the following conditions +#* are met: +#* +#* * Redistributions of source code must retain the above copyright +#* notice, this list of conditions and the following disclaimer. +#* * Redistributions in binary form must reproduce the above +#* copyright notice, this list of conditions and the following +#* disclaimer in the documentation and/or other materials provided +#* with the distribution. +#* * Neither the name of Willow Garage, Inc. nor the names of its +#* contributors may be used to endorse or promote products derived +#* from this software without specific prior written permission. +#* +#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +#* POSSIBILITY OF SUCH DAMAGE. +#* +#* Author: Eitan Marder-Eppstein +#*********************************************************** +from __future__ import absolute_import +from tf2_py import * +from .buffer_interface import * +from .buffer import * +from .buffer_client import * +from .transform_listener import * +from .transform_broadcaster import * +from .static_transform_broadcaster import * diff --git a/src/geometry2/tf2_ros/src/tf2_ros/buffer.py b/src/geometry2/tf2_ros/src/tf2_ros/buffer.py new file mode 100644 index 0000000..3d14ac8 --- /dev/null +++ b/src/geometry2/tf2_ros/src/tf2_ros/buffer.py @@ -0,0 +1,158 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# author: Wim Meeussen + +import rospy +import tf2_py as tf2 +import tf2_ros +from tf2_msgs.srv import FrameGraph, FrameGraphResponse +import rosgraph.masterapi + +class Buffer(tf2.BufferCore, tf2_ros.BufferInterface): + """ + Standard implementation of the :class:`tf2_ros.BufferInterface` abstract data type. + + Inherits from :class:`tf2_ros.buffer_interface.BufferInterface` and :class:`tf2.BufferCore`. + + Stores known frames and offers a ROS service, "tf_frames", which responds to client requests + with a response containing a :class:`tf2_msgs.FrameGraph` representing the relationship of + known frames. + """ + + def __init__(self, cache_time = None, debug = True): + """ + .. function:: __init__(cache_time = None, debug = True) + + Constructor. + + :param cache_time: (Optional) How long to retain past information in BufferCore. + :param debug: (Optional) If true, check if another tf2_frames service has been advertised. + """ + if cache_time != None: + tf2.BufferCore.__init__(self, cache_time) + else: + tf2.BufferCore.__init__(self) + tf2_ros.BufferInterface.__init__(self) + + if debug: + #Check to see if the service has already been advertised in this node + try: + m = rosgraph.masterapi.Master(rospy.get_name()) + m.lookupService('~tf2_frames') + except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure): + self.frame_server = rospy.Service('~tf2_frames', FrameGraph, self.__get_frames) + + def __get_frames(self, req): + return FrameGraphResponse(self.all_frames_as_yaml()) + + def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + """ + Get the transform from the source frame to the target frame. + + :param target_frame: Name of the frame to transform into. + :param source_frame: Name of the input frame. + :param time: The time at which to get the transform. (0 will get the latest) + :param timeout: (Optional) Time to wait for the target frame to become available. + :return: The transform between the frames. + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + + self.can_transform(target_frame, source_frame, time, timeout) + return self.lookup_transform_core(target_frame, source_frame, time) + + def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + """ + Get the transform from the source frame to the target frame using the advanced API. + + :param target_frame: Name of the frame to transform into. + :param target_time: The time to transform to. (0 will get the latest) + :param source_frame: Name of the input frame. + :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param fixed_frame: Name of the frame to consider constant in time. + :param timeout: (Optional) Time to wait for the target frame to become available. + :return: The transform between the frames. + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + self.can_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout) + return self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame) + + + def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0), return_debug_tuple=False): + """ + Check if a transform from the source frame to the target frame is possible. + + :param target_frame: Name of the frame to transform into. + :param source_frame: Name of the input frame. + :param time: The time at which to get the transform. (0 will get the latest) + :param timeout: (Optional) Time to wait for the target frame to become available. + :param return_debug_type: (Optional) If true, return a tuple representing debug information. + :return: True if the transform is possible, false otherwise. + :rtype: bool + """ + if timeout != rospy.Duration(0.0): + start_time = rospy.Time.now() + r= rospy.Rate(20) + while (rospy.Time.now() < start_time + timeout and + not self.can_transform_core(target_frame, source_frame, time)[0] and + (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them + r.sleep() + core_result = self.can_transform_core(target_frame, source_frame, time) + if return_debug_tuple: + return core_result + return core_result[0] + + def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0), + + return_debug_tuple=False): + """ + Check if a transform from the source frame to the target frame is possible (advanced API). + + Must be implemented by a subclass of BufferInterface. + + :param target_frame: Name of the frame to transform into. + :param target_time: The time to transform to. (0 will get the latest) + :param source_frame: Name of the input frame. + :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param fixed_frame: Name of the frame to consider constant in time. + :param timeout: (Optional) Time to wait for the target frame to become available. + :param return_debug_type: (Optional) If true, return a tuple representing debug information. + :return: True if the transform is possible, false otherwise. + :rtype: bool + """ + if timeout != rospy.Duration(0.0): + start_time = rospy.Time.now() + r= rospy.Rate(20) + while (rospy.Time.now() < start_time + timeout and + not self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0] and + (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them + r.sleep() + core_result = self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame) + if return_debug_tuple: + return core_result + return core_result[0] + diff --git a/src/geometry2/tf2_ros/src/tf2_ros/buffer_client.py b/src/geometry2/tf2_ros/src/tf2_ros/buffer_client.py new file mode 100644 index 0000000..ae4d9fe --- /dev/null +++ b/src/geometry2/tf2_ros/src/tf2_ros/buffer_client.py @@ -0,0 +1,196 @@ +#! /usr/bin/python +#*********************************************************** +#* Software License Agreement (BSD License) +#* +#* Copyright (c) 2009, Willow Garage, Inc. +#* All rights reserved. +#* +#* Redistribution and use in source and binary forms, with or without +#* modification, are permitted provided that the following conditions +#* are met: +#* +#* * Redistributions of source code must retain the above copyright +#* notice, this list of conditions and the following disclaimer. +#* * Redistributions in binary form must reproduce the above +#* copyright notice, this list of conditions and the following +#* disclaimer in the documentation and/or other materials provided +#* with the distribution. +#* * Neither the name of Willow Garage, Inc. nor the names of its +#* contributors may be used to endorse or promote products derived +#* from this software without specific prior written permission. +#* +#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +#* POSSIBILITY OF SUCH DAMAGE. +#* +#* Author: Eitan Marder-Eppstein +#*********************************************************** +import rospy +import actionlib +import tf2_py as tf2 +import tf2_ros + +from tf2_msgs.msg import LookupTransformAction, LookupTransformGoal +from actionlib_msgs.msg import GoalStatus + +class BufferClient(tf2_ros.BufferInterface): + """ + Action client-based implementation of BufferInterface. + """ + def __init__(self, ns, check_frequency = None, timeout_padding = rospy.Duration.from_sec(2.0)): + """ + .. function:: __init__(ns, check_frequency = None, timeout_padding = rospy.Duration.from_sec(2.0)) + + Constructor. + + :param ns: The namespace in which to look for a BufferServer. + :param check_frequency: How frequently to check for updates to known transforms. + :param timeout_padding: A constant timeout to add to blocking calls. + """ + tf2_ros.BufferInterface.__init__(self) + self.client = actionlib.SimpleActionClient(ns, LookupTransformAction) + self.timeout_padding = timeout_padding + + if check_frequency is not None: + rospy.logwarn('Argument check_frequency is deprecated and should not be used.') + + def wait_for_server(self, timeout = rospy.Duration()): + """ + Block until the action server is ready to respond to requests. + + :param timeout: Time to wait for the server. + :return: True if the server is ready, false otherwise. + :rtype: bool + """ + return self.client.wait_for_server(timeout) + + # lookup, simple api + def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + """ + Get the transform from the source frame to the target frame. + + :param target_frame: Name of the frame to transform into. + :param source_frame: Name of the input frame. + :param time: The time at which to get the transform. (0 will get the latest) + :param timeout: (Optional) Time to wait for the target frame to become available. + :return: The transform between the frames. + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + goal = LookupTransformGoal() + goal.target_frame = target_frame; + goal.source_frame = source_frame; + goal.source_time = time; + goal.timeout = timeout; + goal.advanced = False; + + return self.__process_goal(goal) + + # lookup, advanced api + def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + """ + Get the transform from the source frame to the target frame using the advanced API. + + :param target_frame: Name of the frame to transform into. + :param target_time: The time to transform to. (0 will get the latest) + :param source_frame: Name of the input frame. + :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param fixed_frame: Name of the frame to consider constant in time. + :param timeout: (Optional) Time to wait for the target frame to become available. + :return: The transform between the frames. + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + goal = LookupTransformGoal() + goal.target_frame = target_frame; + goal.source_frame = source_frame; + goal.source_time = source_time; + goal.timeout = timeout; + goal.target_time = target_time; + goal.fixed_frame = fixed_frame; + goal.advanced = True; + + return self.__process_goal(goal) + + # can, simple api + def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + """ + Check if a transform from the source frame to the target frame is possible. + + :param target_frame: Name of the frame to transform into. + :param source_frame: Name of the input frame. + :param time: The time at which to get the transform. (0 will get the latest) + :param timeout: (Optional) Time to wait for the target frame to become available. + :param return_debug_type: (Optional) If true, return a tuple representing debug information. + :return: True if the transform is possible, false otherwise. + :rtype: bool + """ + try: + self.lookup_transform(target_frame, source_frame, time, timeout) + return True + except tf2.TransformException: + return False + + + # can, advanced api + def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + """ + Check if a transform from the source frame to the target frame is possible (advanced API). + + Must be implemented by a subclass of BufferInterface. + + :param target_frame: Name of the frame to transform into. + :param target_time: The time to transform to. (0 will get the latest) + :param source_frame: Name of the input frame. + :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param fixed_frame: Name of the frame to consider constant in time. + :param timeout: (Optional) Time to wait for the target frame to become available. + :param return_debug_type: (Optional) If true, return a tuple representing debug information. + :return: True if the transform is possible, false otherwise. + :rtype: bool + """ + try: + self.lookup_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout) + return True + except tf2.TransformException: + return False + + def __process_goal(self, goal): + self.client.send_goal(goal) + + if not self.client.wait_for_result(goal.timeout + self.timeout_padding): + #This shouldn't happen, but could in rare cases where the server hangs + raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server") + + if self.client.get_state() != GoalStatus.SUCCEEDED: + raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.") + + return self.__process_result(self.client.get_result()) + + def __process_result(self, result): + if not result: + raise tf2.TransformException("The BufferServer returned None for result! Something is likely wrong with the server.") + if not result.error: + raise tf2.TransformException("The BufferServer returned None for result.error! Something is likely wrong with the server.") + if result.error.error != result.error.NO_ERROR: + if result.error.error == result.error.LOOKUP_ERROR: + raise tf2.LookupException(result.error.error_string) + if result.error.error == result.error.CONNECTIVITY_ERROR: + raise tf2.ConnectivityException(result.error.error_string) + if result.error.error == result.error.EXTRAPOLATION_ERROR: + raise tf2.ExtrapolationException(result.error.error_string) + if result.error.error == result.error.INVALID_ARGUMENT_ERROR: + raise tf2.InvalidArgumentException(result.error.error_string) + if result.error.error == result.error.TIMEOUT_ERROR: + raise tf2.TimeoutException(result.error.error_string) + + raise tf2.TransformException(result.error.error_string) + + return result.transform diff --git a/src/geometry2/tf2_ros/src/tf2_ros/buffer_interface.py b/src/geometry2/tf2_ros/src/tf2_ros/buffer_interface.py new file mode 100644 index 0000000..2cb753e --- /dev/null +++ b/src/geometry2/tf2_ros/src/tf2_ros/buffer_interface.py @@ -0,0 +1,254 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# author: Wim Meeussen + +from __future__ import print_function + +import rospy +import tf2_py as tf2 +import tf2_ros +from copy import deepcopy +from std_msgs.msg import Header + +class BufferInterface: + """ + Abstract interface for wrapping the Python bindings for the tf2 library in + a ROS-based convenience API. + Implementations include :class:tf2_ros.buffer.Buffer and + :class:tf2_ros.buffer_client.BufferClient. + """ + def __init__(self): + self.registration = tf2_ros.TransformRegistration() + + # transform, simple api + def transform(self, object_stamped, target_frame, timeout=rospy.Duration(0.0), new_type = None): + """ + Transform an input into the target frame. + + The input must be a known transformable type (by way of the tf2 data type conversion interface). + + If new_type is not None, the type specified must have a valid conversion from the input type, + else the function will raise an exception. + + :param object_stamped: The timestamped object the transform. + :param target_frame: Name of the frame to transform the input into. + :param timeout: (Optional) Time to wait for the target frame to become available. + :param new_type: (Optional) Type to convert the object to. + :return: The transformed, timestamped output, possibly converted to a new type. + """ + do_transform = self.registration.get(type(object_stamped)) + res = do_transform(object_stamped, self.lookup_transform(target_frame, object_stamped.header.frame_id, + object_stamped.header.stamp, timeout)) + if not new_type: + return res + + return convert(res, new_type) + + # transform, advanced api + def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=rospy.Duration(0.0), new_type = None): + """ + Transform an input into the target frame (advanced API). + + The input must be a known transformable type (by way of the tf2 data type conversion interface). + + If new_type is not None, the type specified must have a valid conversion from the input type, + else the function will raise an exception. + + This function follows the advanced API, which allows tranforming between different time points, + as well as specifying a frame to be considered fixed in time. + + :param object_stamped: The timestamped object the transform. + :param target_frame: Name of the frame to transform the input into. + :param target_time: Time to transform the input into. + :param fixed_frame: Name of the frame to consider constant in time. + :param timeout: (Optional) Time to wait for the target frame to become available. + :param new_type: (Optional) Type to convert the object to. + :return: The transformed, timestamped output, possibly converted to a new type. + """ + do_transform = self.registration.get(type(object_stamped)) + res = do_transform(object_stamped, self.lookup_transform_full(target_frame, target_time, + object_stamped.header.frame_id, object_stamped.header.stamp, + fixed_frame, timeout)) + if not new_type: + return res + + return convert(res, new_type) + + def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + """ + Get the transform from the source frame to the target frame. + + Must be implemented by a subclass of BufferInterface. + + :param target_frame: Name of the frame to transform into. + :param source_frame: Name of the input frame. + :param time: The time at which to get the transform. (0 will get the latest) + :param timeout: (Optional) Time to wait for the target frame to become available. + :return: The transform between the frames. + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + raise NotImplementedException() + + def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + """ + Get the transform from the source frame to the target frame using the advanced API. + + Must be implemented by a subclass of BufferInterface. + + :param target_frame: Name of the frame to transform into. + :param target_time: The time to transform to. (0 will get the latest) + :param source_frame: Name of the input frame. + :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param fixed_frame: Name of the frame to consider constant in time. + :param timeout: (Optional) Time to wait for the target frame to become available. + :return: The transform between the frames. + :rtype: :class:`geometry_msgs.msg.TransformStamped` + """ + raise NotImplementedException() + + # can, simple api + def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): + """ + Check if a transform from the source frame to the target frame is possible. + + Must be implemented by a subclass of BufferInterface. + + :param target_frame: Name of the frame to transform into. + :param source_frame: Name of the input frame. + :param time: The time at which to get the transform. (0 will get the latest) + :param timeout: (Optional) Time to wait for the target frame to become available. + :return: True if the transform is possible, false otherwise. + :rtype: bool + """ + raise NotImplementedException() + + # can, advanced api + def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)): + """ + Check if a transform from the source frame to the target frame is possible (advanced API). + + Must be implemented by a subclass of BufferInterface. + + :param target_frame: Name of the frame to transform into. + :param target_time: The time to transform to. (0 will get the latest) + :param source_frame: Name of the input frame. + :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param fixed_frame: Name of the frame to consider constant in time. + :param timeout: (Optional) Time to wait for the target frame to become available. + :return: True if the transform is possible, false otherwise. + :rtype: bool + """ + raise NotImplementedException() + + +def Stamped(obj, stamp, frame_id): + obj.header = Header(frame_id=frame_id, stamp=stamp) + return obj + + + +class TypeException(Exception): + """ + Raised when an unexpected type is received while registering a transform + in :class:`tf2_ros.buffer_interface.BufferInterface`. + """ + def __init__(self, errstr): + self.errstr = errstr + +class NotImplementedException(Exception): + """ + Raised when can_transform or lookup_transform is not implemented in a + subclass of :class:`tf2_ros.buffer_interface.BufferInterface`. + """ + def __init__(self): + self.errstr = 'CanTransform or LookupTransform not implemented' + + +class TransformRegistration(): + __type_map = {} + + def print_me(self): + print(TransformRegistration.__type_map) + + def add(self, key, callback): + TransformRegistration.__type_map[key] = callback + + def get(self, key): + if not key in TransformRegistration.__type_map: + raise TypeException('Type %s if not loaded or supported'% str(key)) + else: + return TransformRegistration.__type_map[key] + +class ConvertRegistration(): + __to_msg_map = {} + __from_msg_map = {} + __convert_map = {} + + def add_from_msg(self, key, callback): + ConvertRegistration.__from_msg_map[key] = callback + + def add_to_msg(self, key, callback): + ConvertRegistration.__to_msg_map[key] = callback + + def add_convert(self, key, callback): + ConvertRegistration.__convert_map[key] = callback + + def get_from_msg(self, key): + if not key in ConvertRegistration.__from_msg_map: + raise TypeException('Type %s if not loaded or supported'% str(key)) + else: + return ConvertRegistration.__from_msg_map[key] + + def get_to_msg(self, key): + if not key in ConvertRegistration.__to_msg_map: + raise TypeException('Type %s if not loaded or supported'%str(key)) + else: + return ConvertRegistration.__to_msg_map[key] + + def get_convert(self, key): + if not key in ConvertRegistration.__convert_map: + raise TypeException("Type %s if not loaded or supported" % str(key)) + else: + return ConvertRegistration.__convert_map[key] + +def convert(a, b_type): + c = ConvertRegistration() + #check if an efficient conversion function between the types exists + try: + f = c.get_convert((type(a), b_type)) + print("efficient copy") + return f(a) + except TypeException: + if type(a) == b_type: + print("deep copy") + return deepcopy(a) + + f_to = c.get_to_msg(type(a)) + f_from = c.get_from_msg(b_type) + print("message copy") + return f_from(f_to(a)) diff --git a/src/geometry2/tf2_ros/src/tf2_ros/static_transform_broadcaster.py b/src/geometry2/tf2_ros/src/tf2_ros/static_transform_broadcaster.py new file mode 100644 index 0000000..064a687 --- /dev/null +++ b/src/geometry2/tf2_ros/src/tf2_ros/static_transform_broadcaster.py @@ -0,0 +1,51 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import rospy +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import TransformStamped + + +class StaticTransformBroadcaster(object): + """ + :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic. + """ + + def __init__(self): + self.pub_tf = rospy.Publisher("/tf_static", TFMessage, queue_size=100, latch=True) + + def sendTransform(self, transform): + if not isinstance(transform, list): + transform = [transform] + self.pub_tf.publish(TFMessage(transform)) + + diff --git a/src/geometry2/tf2_ros/src/tf2_ros/transform_broadcaster.py b/src/geometry2/tf2_ros/src/tf2_ros/transform_broadcaster.py new file mode 100644 index 0000000..e86e835 --- /dev/null +++ b/src/geometry2/tf2_ros/src/tf2_ros/transform_broadcaster.py @@ -0,0 +1,56 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import rospy +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import TransformStamped + + +class TransformBroadcaster: + """ + :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. + """ + + def __init__(self): + self.pub_tf = rospy.Publisher("/tf", TFMessage, queue_size=100) + + def sendTransform(self, transform): + """ + Send a transform, or a list of transforms, to the Buffer associated with this TransformBroadcaster. + + :param transform: A transform or list of transforms to send. + """ + if not isinstance(transform, list): + transform = [transform] + self.pub_tf.publish(TFMessage(transform)) + + diff --git a/src/geometry2/tf2_ros/src/tf2_ros/transform_listener.py b/src/geometry2/tf2_ros/src/tf2_ros/transform_listener.py new file mode 100644 index 0000000..6a53c1a --- /dev/null +++ b/src/geometry2/tf2_ros/src/tf2_ros/transform_listener.py @@ -0,0 +1,89 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# author: Wim Meeussen + +import threading + +import rospy +import tf2_ros +from tf2_msgs.msg import TFMessage + +class TransformListener(): + """ + :class:`TransformListener` is a convenient way to listen for coordinate frame transformation info. + This class takes an object that instantiates the :class:`BufferInterface` interface, to which + it propagates changes to the tf frame graph. + """ + def __init__(self, buffer, queue_size=None, buff_size=65536, tcp_nodelay=False): + """ + .. function:: __init__(buffer) + + Constructor. + + :param buffer: The buffer to propagate changes to when tf info updates. + :param queue_size (int) - maximum number of messages to receive at a time. This will generally be 1 or None (infinite, default). buff_size should be increased if this parameter is set as incoming data still needs to sit in the incoming buffer before being discarded. Setting queue_size buff_size to a non-default value affects all subscribers to this topic in this process. + :param buff_size (int) - incoming message buffer size in bytes. If queue_size is set, this should be set to a number greater than the queue_size times the average message size. Setting buff_size to a non-default value affects all subscribers to this topic in this process. + :param tcp_nodelay (bool) - if True, request TCP_NODELAY from publisher. Use of this option is not generally recommended in most cases as it is better to rely on timestamps in message data. Setting tcp_nodelay to True enables TCP_NODELAY for all subscribers in the same python process. + """ + self.buffer = buffer + self.last_update = rospy.Time.now() + self.last_update_lock = threading.Lock() + self.tf_sub = rospy.Subscriber("/tf", TFMessage, self.callback, queue_size=queue_size, buff_size=buff_size, tcp_nodelay=tcp_nodelay) + self.tf_static_sub = rospy.Subscriber("/tf_static", TFMessage, self.static_callback, queue_size=queue_size, buff_size=buff_size, tcp_nodelay=tcp_nodelay) + + def __del__(self): + self.unregister() + + def unregister(self): + """ + Unregisters all tf subscribers. + """ + self.tf_sub.unregister() + self.tf_static_sub.unregister() + + def check_for_reset(self): + # Lock to prevent different threads racing on this test and update. + # https://github.com/ros/geometry2/issues/341 + with self.last_update_lock: + now = rospy.Time.now() + if now < self.last_update: + rospy.logwarn("Detected jump back in time of %fs. Clearing TF buffer." % (self.last_update - now).to_sec()) + self.buffer.clear() + self.last_update = now + + def callback(self, data): + self.check_for_reset() + who = data._connection_header.get('callerid', "default_authority") + for transform in data.transforms: + self.buffer.set_transform(transform, who) + + def static_callback(self, data): + self.check_for_reset() + who = data._connection_header.get('callerid', "default_authority") + for transform in data.transforms: + self.buffer.set_transform_static(transform, who) diff --git a/src/geometry2/tf2_ros/src/transform_broadcaster.cpp b/src/geometry2/tf2_ros/src/transform_broadcaster.cpp new file mode 100644 index 0000000..94cce47 --- /dev/null +++ b/src/geometry2/tf2_ros/src/transform_broadcaster.cpp @@ -0,0 +1,66 @@ +/* + * 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); +} + + +} + + diff --git a/src/geometry2/tf2_ros/src/transform_listener.cpp b/src/geometry2/tf2_ros/src/transform_listener.cpp new file mode 100644 index 0000000..f130be6 --- /dev/null +++ b/src/geometry2/tf2_ros/src/transform_listener.cpp @@ -0,0 +1,134 @@ +/* + * 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_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing TF buffer."); + buffer_.clear(); + } + last_update_ = now; + + + + const tf2_msgs::TFMessage& msg_in = *(msg_evt.getConstMessage()); + std::string authority = msg_evt.getPublisherName(); // lookup the authority + for (unsigned int i = 0; i < msg_in.transforms.size(); i++) + { + try + { + buffer_.setTransform(msg_in.transforms[i], authority, is_static); + } + + catch (tf2::TransformException& ex) + { + ///\todo Use error reporting + std::string temp = ex.what(); + ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str()); + } + } +}; + + + + + diff --git a/src/geometry2/tf2_ros/test/listener_unittest.cpp b/src/geometry2/tf2_ros/test/listener_unittest.cpp new file mode 100644 index 0000000..4b9594f --- /dev/null +++ b/src/geometry2/tf2_ros/test/listener_unittest.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +using namespace tf2; + +TEST(tf2_ros_transform, transform_listener) +{ + tf2_ros::Buffer buffer; + tf2_ros::TransformListener tfl(buffer); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "transform_listener_unittest"); + return RUN_ALL_TESTS(); +} diff --git a/src/geometry2/tf2_ros/test/message_filter_test.cpp b/src/geometry2/tf2_ros/test/message_filter_test.cpp new file mode 100644 index 0000000..b613d24 --- /dev/null +++ b/src/geometry2/tf2_ros/test/message_filter_test.cpp @@ -0,0 +1,125 @@ +/* + * Copyright (c) 2014, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +void spin_for_a_second() +{ + ros::spinOnce(); + for (uint8_t i = 0; i < 10; ++i) + { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + ros::spinOnce(); + } +} + +bool filter_callback_fired = false; +void filter_callback(const geometry_msgs::PointStamped& msg) +{ + filter_callback_fired = true; +} + +TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) +{ + ros::NodeHandle nh; + message_filters::Subscriber sub; + sub.subscribe(nh, "point", 10); + + tf2_ros::Buffer buffer; + tf2_ros::TransformListener tfl(buffer); + tf2_ros::MessageFilter filter(buffer, "map", 10, nh); + filter.connectInput(sub); + filter.registerCallback(&filter_callback); + // Register multiple target frames + std::vector frames; + frames.push_back("odom"); + frames.push_back("map"); + filter.setTargetFrames(frames); + // Set a non-zero time tolerance + filter.setTolerance(ros::Duration(1, 0)); + + // Publish static transforms so the frame transformations will always be valid + tf2_ros::StaticTransformBroadcaster tfb; + geometry_msgs::TransformStamped map_to_odom; + map_to_odom.header.stamp = ros::Time(0, 0); + map_to_odom.header.frame_id = "map"; + map_to_odom.child_frame_id = "odom"; + map_to_odom.transform.translation.x = 0.0; + map_to_odom.transform.translation.y = 0.0; + map_to_odom.transform.translation.z = 0.0; + map_to_odom.transform.rotation.x = 0.0; + map_to_odom.transform.rotation.y = 0.0; + map_to_odom.transform.rotation.z = 0.0; + map_to_odom.transform.rotation.w = 1.0; + tfb.sendTransform(map_to_odom); + + geometry_msgs::TransformStamped odom_to_base; + odom_to_base.header.stamp = ros::Time(0, 0); + odom_to_base.header.frame_id = "odom"; + odom_to_base.child_frame_id = "base"; + odom_to_base.transform.translation.x = 0.0; + odom_to_base.transform.translation.y = 0.0; + odom_to_base.transform.translation.z = 0.0; + odom_to_base.transform.rotation.x = 0.0; + odom_to_base.transform.rotation.y = 0.0; + odom_to_base.transform.rotation.z = 0.0; + odom_to_base.transform.rotation.w = 1.0; + tfb.sendTransform(odom_to_base); + + // Publish a Point message in the "base" frame + ros::Publisher pub = nh.advertise("point", 10); + geometry_msgs::PointStamped point; + point.header.stamp = ros::Time::now(); + point.header.frame_id = "base"; + pub.publish(point); + + // make sure it arrives + spin_for_a_second(); + + // The filter callback should have been fired because all required transforms are available + ASSERT_TRUE(filter_callback_fired); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "tf2_ros_message_filter"); + return RUN_ALL_TESTS(); +} diff --git a/src/geometry2/tf2_ros/test/message_filter_test.launch b/src/geometry2/tf2_ros/test/message_filter_test.launch new file mode 100644 index 0000000..b32f8dc --- /dev/null +++ b/src/geometry2/tf2_ros/test/message_filter_test.launch @@ -0,0 +1,4 @@ + + + + diff --git a/src/geometry2/tf2_ros/test/time_reset_test.cpp b/src/geometry2/tf2_ros/test/time_reset_test.cpp new file mode 100644 index 0000000..8142fd9 --- /dev/null +++ b/src/geometry2/tf2_ros/test/time_reset_test.cpp @@ -0,0 +1,111 @@ +/* + * Copyright (c) 2014, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include + +using namespace tf2; + +void spin_for_a_second() +{ + ros::spinOnce(); + for (uint8_t i = 0; i < 10; ++i) + { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + ros::spinOnce(); + } +} + +TEST(tf2_ros_transform_listener, time_backwards) +{ + + tf2_ros::Buffer buffer; + tf2_ros::TransformListener tfl(buffer); + tf2_ros::TransformBroadcaster tfb; + + ros::NodeHandle nh = ros::NodeHandle(); + + ros::Publisher clock = nh.advertise("/clock", 5); + + rosgraph_msgs::Clock c; + c.clock = ros::Time(100); + clock.publish(c); + + // basic test + ASSERT_FALSE(buffer.canTransform("foo", "bar", ros::Time(101, 0))); + + // set the transform + geometry_msgs::TransformStamped msg; + msg.header.stamp = ros::Time(100, 0); + msg.header.frame_id = "foo"; + msg.child_frame_id = "bar"; + msg.transform.rotation.x = 1.0; + tfb.sendTransform(msg); + msg.header.stamp = ros::Time(102, 0); + tfb.sendTransform(msg); + + + // make sure it arrives + spin_for_a_second(); + + // verify it's been set + ASSERT_TRUE(buffer.canTransform("foo", "bar", ros::Time(101, 0))); + + c.clock = ros::Time(90); + clock.publish(c); + + // make sure it arrives + spin_for_a_second(); + + //Send another message to trigger clock test on an unrelated frame + msg.header.stamp = ros::Time(110, 0); + msg.header.frame_id = "foo2"; + msg.child_frame_id = "bar2"; + tfb.sendTransform(msg); + + // make sure it arrives + spin_for_a_second(); + + //verify the data's been cleared + ASSERT_FALSE(buffer.canTransform("foo", "bar", ros::Time(101, 0))); + +} + + + + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "transform_listener_backwards_reset"); + return RUN_ALL_TESTS(); +} diff --git a/src/geometry2/tf2_ros/test/transform_listener_time_reset_test.launch b/src/geometry2/tf2_ros/test/transform_listener_time_reset_test.launch new file mode 100644 index 0000000..ae31a90 --- /dev/null +++ b/src/geometry2/tf2_ros/test/transform_listener_time_reset_test.launch @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/src/geometry2/tf2_ros/test/transform_listener_unittest.launch b/src/geometry2/tf2_ros/test/transform_listener_unittest.launch new file mode 100644 index 0000000..42a8149 --- /dev/null +++ b/src/geometry2/tf2_ros/test/transform_listener_unittest.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/src/geometry2/tf2_sensor_msgs/CHANGELOG.rst b/src/geometry2/tf2_sensor_msgs/CHANGELOG.rst new file mode 100644 index 0000000..7de91fb --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/CHANGELOG.rst @@ -0,0 +1,88 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_sensor_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ + +0.6.6 (2020-01-09) +------------------ +* Affine->Isometry `#378 `_ +* Python 3 compatibility: relative imports and print statement +* Contributors: Martin Pecka, Timon Engelke, Tully Foote + +0.6.5 (2018-11-16) +------------------ + +0.6.4 (2018-11-06) +------------------ + +0.6.3 (2018-07-09) +------------------ + +0.6.2 (2018-05-02) +------------------ + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ + +0.5.17 (2018-01-01) +------------------- +* Merge pull request `#257 `_ from delftrobotics-forks/python3 + Make tf2_py python3 compatible again +* Use python3 print function. +* Contributors: Maarten de Vries, Tully Foote + +0.5.16 (2017-07-14) +------------------- +* Fix do_transform_cloud for multi-channelled pointcloud2. (`#241 `_) +* store gtest return value as int (`#229 `_) +* Document the lifetime of the returned reference for getFrameId and getTimestamp +* Find eigen in a much nicer way. +* Switch tf2_sensor_msgs over to package format 2. +* Contributors: Atsushi Watanabe, Chris Lalancette, dhood + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- + +0.5.13 (2016-03-04) +------------------- +* add missing Python runtime dependency +* fix wrong comment +* Adding tests to package +* Fixing do_transform_cloud for python + The previous code was not used at all (it was a mistake in the __init_\_.py so + the do_transform_cloud was not available to the python users). + The python code need some little correction (e.g there is no method named + read_cloud but it's read_points for instance, and as we are in python we can't + use the same trick as in c++ when we got an immutable) +* Contributors: Laurent GEORGE, Vincent Rabaud + +0.5.12 (2015-08-05) +------------------- + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- + +0.5.9 (2015-03-25) +------------------ + +0.5.8 (2015-03-17) +------------------ +* ODR violation fixes and more conversions +* Fix keeping original pointcloud header in transformed pointcloud +* Contributors: Paul Bovbel, Tully Foote, Vincent Rabaud + +0.5.7 (2014-12-23) +------------------ +* add support for transforming sensor_msgs::PointCloud2 +* Contributors: Vincent Rabaud diff --git a/src/geometry2/tf2_sensor_msgs/CMakeLists.txt b/src/geometry2/tf2_sensor_msgs/CMakeLists.txt new file mode 100644 index 0000000..d9d2fb7 --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_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) + +# Finding Eigen is somewhat complicated because of our need to support Ubuntu +# all the way back to saucy. First we look for the Eigen3 cmake module +# provided by the libeigen3-dev on newer Ubuntu. If that fails, then we +# fall-back to the version provided by cmake_modules, which is a stand-in. +find_package(Eigen3 QUIET) +if(NOT EIGEN3_FOUND) + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +endif() + +# Note that eigen 3.2 (on Ubuntu Wily) only provides EIGEN3_INCLUDE_DIR, +# not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former. +if(NOT EIGEN3_INCLUDE_DIRS) + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +endif() + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS sensor_msgs tf2_ros tf2 + DEPENDS EIGEN3 +) + + +include_directories(include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +catkin_python_setup() + +if(CATKIN_ENABLE_TESTING) + catkin_add_nosetests(test/test_tf2_sensor_msgs.py) + + find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs rostest tf2_ros tf2) + + add_executable(test_tf2_sensor_msgs_cpp EXCLUDE_FROM_ALL test/test_tf2_sensor_msgs.cpp) + target_link_libraries(test_tf2_sensor_msgs_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) + + + if(TARGET tests) + add_dependencies(tests test_tf2_sensor_msgs_cpp) + endif() + add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch) + +endif() diff --git a/src/geometry2/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h b/src/geometry2/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h new file mode 100644 index 0000000..9e16e0d --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h @@ -0,0 +1,107 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef TF2_SENSOR_MSGS_H +#define TF2_SENSOR_MSGS_H + +#include +#include +#include +#include +#include + +namespace tf2 +{ + +/********************/ +/** PointCloud2 **/ +/********************/ + +/** \brief Extract a timestamp from the header of a PointCloud2 message. + * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * \param t PointCloud2 message to extract the timestamp from. + * \return The timestamp of the message. The lifetime of the returned reference + * is bound to the lifetime of the argument. + */ +template <> +inline +const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;} + +/** \brief Extract a frame ID from the header of a PointCloud2 message. + * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * \param t PointCloud2 message to extract the frame ID from. + * \return A string containing the frame ID of the message. The lifetime of the + * returned reference is bound to the lifetime of the argument. + */ +template <> +inline +const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;} + +// this method needs to be implemented by client library developers +template <> +inline +void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in) +{ + p_out = p_in; + p_out.header = t_in.header; + Eigen::Transform t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y, + t_in.transform.translation.z) * Eigen::Quaternion( + t_in.transform.rotation.w, t_in.transform.rotation.x, + t_in.transform.rotation.y, t_in.transform.rotation.z); + + sensor_msgs::PointCloud2ConstIterator x_in(p_in, "x"); + sensor_msgs::PointCloud2ConstIterator y_in(p_in, "y"); + sensor_msgs::PointCloud2ConstIterator z_in(p_in, "z"); + + sensor_msgs::PointCloud2Iterator x_out(p_out, "x"); + sensor_msgs::PointCloud2Iterator y_out(p_out, "y"); + sensor_msgs::PointCloud2Iterator z_out(p_out, "z"); + + Eigen::Vector3f point; + for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) { + point = t * Eigen::Vector3f(*x_in, *y_in, *z_in); + *x_out = point.x(); + *y_out = point.y(); + *z_out = point.z(); + } +} +inline +sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in) +{ + return in; +} +inline +void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out) +{ + out = msg; +} + +} // namespace + +#endif // TF2_SENSOR_MSGS_H diff --git a/src/geometry2/tf2_sensor_msgs/package.xml b/src/geometry2/tf2_sensor_msgs/package.xml new file mode 100644 index 0000000..c0f8686 --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/package.xml @@ -0,0 +1,31 @@ + + tf2_sensor_msgs + 0.6.7 + + Small lib to transform sensor_msgs with tf. Most notably, PointCloud2 + + Vincent Rabaud + Vincent Rabaud + BSD + + http://www.ros.org/wiki/tf2_ros + + catkin + + cmake_modules + eigen + + sensor_msgs + tf2 + tf2_ros + + python_orocos_kdl + rospy + + eigen + + rostest + geometry_msgs + + + diff --git a/src/geometry2/tf2_sensor_msgs/setup.py b/src/geometry2/tf2_sensor_msgs/setup.py new file mode 100644 index 0000000..7ec9abe --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/setup.py @@ -0,0 +1,13 @@ +#!/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) + diff --git a/src/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py b/src/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py new file mode 100644 index 0000000..187bd6b --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py @@ -0,0 +1 @@ +from .tf2_sensor_msgs import * diff --git a/src/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py b/src/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py new file mode 100644 index 0000000..74cacd3 --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py @@ -0,0 +1,60 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.point_cloud2 import read_points, create_cloud +import PyKDL +import rospy +import tf2_ros + +def to_msg_msg(msg): + return msg + +tf2_ros.ConvertRegistration().add_to_msg(PointCloud2, to_msg_msg) + +def from_msg_msg(msg): + return msg + +tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg) + +def transform_to_kdl(t): + return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + PyKDL.Vector(t.transform.translation.x, + t.transform.translation.y, + t.transform.translation.z)) + +# PointStamped +def do_transform_cloud(cloud, transform): + t_kdl = transform_to_kdl(transform) + points_out = [] + for p_in in read_points(cloud): + p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2]) + points_out.append((p_out[0], p_out[1], p_out[2]) + p_in[3:]) + res = create_cloud(transform.header, cloud.fields, points_out) + return res +tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud) diff --git a/src/geometry2/tf2_sensor_msgs/test/test.launch b/src/geometry2/tf2_sensor_msgs/test/test.launch new file mode 100644 index 0000000..a948a06 --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/test/test.launch @@ -0,0 +1,3 @@ + + + diff --git a/src/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp b/src/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp new file mode 100644 index 0000000..8607501 --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp @@ -0,0 +1,104 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include +#include +#include +#include +#include + +tf2_ros::Buffer* tf_buffer; +static const double EPS = 1e-3; + + +TEST(Tf2Sensor, PointCloud2) +{ + sensor_msgs::PointCloud2 cloud; + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + modifier.resize(1); + + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + + *iter_x = 1; + *iter_y = 2; + *iter_z = 3; + + cloud.header.stamp = ros::Time(2); + cloud.header.frame_id = "A"; + + // simple api + sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0)); + sensor_msgs::PointCloud2Iterator iter_x_after(cloud_simple, "x"); + sensor_msgs::PointCloud2Iterator iter_y_after(cloud_simple, "y"); + sensor_msgs::PointCloud2Iterator iter_z_after(cloud_simple, "z"); + EXPECT_NEAR(*iter_x_after, -9, EPS); + EXPECT_NEAR(*iter_y_after, 18, EPS); + EXPECT_NEAR(*iter_z_after, 27, EPS); + + // advanced api + sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0), + "A", ros::Duration(3.0)); + sensor_msgs::PointCloud2Iterator iter_x_advanced(cloud_advanced, "x"); + sensor_msgs::PointCloud2Iterator iter_y_advanced(cloud_advanced, "y"); + sensor_msgs::PointCloud2Iterator iter_z_advanced(cloud_advanced, "z"); + EXPECT_NEAR(*iter_x_advanced, -9, EPS); + EXPECT_NEAR(*iter_y_advanced, 18, EPS); + EXPECT_NEAR(*iter_z_advanced, 27, EPS); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test"); + ros::NodeHandle n; + + tf_buffer = new tf2_ros::Buffer(); + + // populate buffer + geometry_msgs::TransformStamped t; + t.transform.translation.x = 10; + t.transform.translation.y = 20; + t.transform.translation.z = 30; + t.transform.rotation.x = 1; + t.transform.rotation.y = 0; + t.transform.rotation.z = 0; + t.transform.rotation.w = 0; + t.header.stamp = ros::Time(2.0); + t.header.frame_id = "A"; + t.child_frame_id = "B"; + tf_buffer->setTransform(t, "test"); + + int ret = RUN_ALL_TESTS(); + delete tf_buffer; + return ret; +} diff --git a/src/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py b/src/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py new file mode 100644 index 0000000..b797b7a --- /dev/null +++ b/src/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import unittest +import struct +import tf2_sensor_msgs +from sensor_msgs import point_cloud2 +from sensor_msgs.msg import PointField +from tf2_ros import TransformStamped +import copy + +## A sample python unit test +class PointCloudConversions(unittest.TestCase): + def setUp(self): + self.point_cloud_in = point_cloud2.PointCloud2() + self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1)] + + self.point_cloud_in.point_step = 4 * 3 + self.point_cloud_in.height = 1 + # we add two points (with x, y, z to the cloud) + self.point_cloud_in.width = 2 + self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width + + points = [1, 2, 0, 10, 20, 30] + self.point_cloud_in.data = struct.pack('%sf' % len(points), *points) + + + self.transform_translate_xyz_300 = TransformStamped() + self.transform_translate_xyz_300.transform.translation.x = 300 + self.transform_translate_xyz_300.transform.translation.y = 300 + self.transform_translate_xyz_300.transform.translation.z = 300 + self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w + + assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)]) + + def test_simple_transform(self): + old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now + point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300) + + k = 300 + expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)] + new_points = list(point_cloud2.read_points(point_cloud_transformed)) + print("new_points are %s" % new_points) + assert(expected_coordinates == new_points) + assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud + + +## A simple unit test for tf2_sensor_msgs.do_transform_cloud (multi channel version) +class PointCloudConversionsMultichannel(unittest.TestCase): + TRANSFORM_OFFSET_DISTANCE = 300 + + def setUp(self): + self.point_cloud_in = point_cloud2.PointCloud2() + self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1), + PointField('index', 12, PointField.INT32, 1)] + + self.point_cloud_in.point_step = 4 * 4 + self.point_cloud_in.height = 1 + # we add two points (with x, y, z to the cloud) + self.point_cloud_in.width = 2 + self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width + + self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)] + for point in self.points: + self.point_cloud_in.data += struct.pack('3fi', *point) + + self.transform_translate_xyz_300 = TransformStamped() + self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE + self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE + self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE + self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w + + assert(list(point_cloud2.read_points(self.point_cloud_in)) == self.points) + + def test_simple_transform_multichannel(self): + old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now + point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300) + + expected_coordinates = [] + for point in self.points: + expected_coordinates += [( + point[0] + self.TRANSFORM_OFFSET_DISTANCE, + point[1] + self.TRANSFORM_OFFSET_DISTANCE, + point[2] + self.TRANSFORM_OFFSET_DISTANCE, + point[3] # index channel must be kept same + )] + + new_points = list(point_cloud2.read_points(point_cloud_transformed)) + print("new_points are %s" % new_points) + assert(expected_coordinates == new_points) + assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud + + +if __name__ == '__main__': + import rosunit + rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions) + rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMultichannel) + diff --git a/src/geometry2/tf2_tools/CHANGELOG.rst b/src/geometry2/tf2_tools/CHANGELOG.rst new file mode 100644 index 0000000..e1e503a --- /dev/null +++ b/src/geometry2/tf2_tools/CHANGELOG.rst @@ -0,0 +1,191 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_tools +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.7 (2020-03-09) +------------------ + +0.6.6 (2020-01-09) +------------------ +* Allow to choose output precision in echo `#377 `_ +* use yaml.safe_load instead of deprecated yaml.load `#373 `_ +* Python 3 compatibility: relative imports and print statement +* Contributors: Mikael Arguedas, Timon Engelke, Tully Foote, Victor Lamoine + +0.6.5 (2018-11-16) +------------------ + +0.6.4 (2018-11-06) +------------------ + +0.6.3 (2018-07-09) +------------------ + +0.6.2 (2018-05-02) +------------------ +* Tf2 tools echo (`#289 `_) + * tf2_tools echo is working but not yet printing the rotation `#287 `_ + * install echo.py + * Added quaternion output but importing from tf1 for euler_from_quaternion seems wrong (`#222 `_) so not doing that yet. Also made count exit after n counts even if exceptions occurred, also printing time of lookup for exceptions `#287 `_ + * Fixed time query option, also changing message text to be more clear `#287 `_ + * Added bsd license, code from transform3d transformations.py `#287 `_ + * Get rid of tabs + * docstring for each function +* Contributors: Lucas Walter + +0.6.1 (2018-03-21) +------------------ + +0.6.0 (2018-03-21) +------------------ + +0.5.17 (2018-01-01) +------------------- +* Merge pull request `#268 `_ from smnogar/indigo-devel + Fixed for cases of non-standard python install +* Contributors: Steve Nogar, Tully Foote + +0.5.16 (2017-07-14) +------------------- + +0.5.15 (2017-01-24) +------------------- + +0.5.14 (2017-01-16) +------------------- +* Remove old load_manifest from view_frames (`#182 `_) +* Contributors: Jochen Sprickerhof + +0.5.13 (2016-03-04) +------------------- +* casted el to string in view_frames +* Contributors: g_gemignani + +0.5.12 (2015-08-05) +------------------- + +0.5.11 (2015-04-22) +------------------- + +0.5.10 (2015-04-21) +------------------- + +0.5.9 (2015-03-25) +------------------ + +0.5.8 (2015-03-17) +------------------ +* remove useless Makefile files +* Contributors: Vincent Rabaud + +0.5.7 (2014-12-23) +------------------ + +0.5.6 (2014-09-18) +------------------ + +0.5.5 (2014-06-23) +------------------ + +0.5.4 (2014-05-07) +------------------ + +0.5.3 (2014-02-21) +------------------ + +0.5.2 (2014-02-20) +------------------ + +0.5.1 (2014-02-14) +------------------ + +0.5.0 (2014-02-14) +------------------ + +0.4.10 (2013-12-26) +------------------- + +0.4.9 (2013-11-06) +------------------ + +0.4.8 (2013-11-06) +------------------ +* updating install rule for view_frames.py fixes `#44 `_ + +0.4.7 (2013-08-28) +------------------ + +0.4.6 (2013-08-28) +------------------ + +0.4.5 (2013-07-11) +------------------ + +0.4.4 (2013-07-09) +------------------ + +0.4.3 (2013-07-05) +------------------ + +0.4.2 (2013-07-05) +------------------ + +0.4.1 (2013-07-05) +------------------ + +0.4.0 (2013-06-27) +------------------ +* splitting rospy dependency into tf2_py so tf2 is pure c++ library. +* Restoring test packages and bullet packages. + reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented + reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 `_ + +0.3.6 (2013-03-03) +------------------ + +0.3.5 (2013-02-15 14:46) +------------------------ +* 0.3.4 -> 0.3.5 + +0.3.4 (2013-02-15 13:14) +------------------------ +* 0.3.3 -> 0.3.4 + +0.3.3 (2013-02-15 11:30) +------------------------ +* 0.3.2 -> 0.3.3 + +0.3.2 (2013-02-15 00:42) +------------------------ +* 0.3.1 -> 0.3.2 + +0.3.1 (2013-02-14) +------------------ +* 0.3.0 -> 0.3.1 + +0.3.0 (2013-02-13) +------------------ +* switching to version 0.3.0 +* removing packages with missing deps +* catkinizing geometry-experimental +* catkinizing tf2_tools +* strip out rx dependencies +* Some fixes to make things work with rxbag +* Threading ns list +* merge tf2_cpp and tf2_py into tf2_ros +* Now catching exceptions correctly with echo +* Working version of tf echo +* Making sure to clear details when switching frames +* Changing file format to tf +* First cut at loading, saving, and exporting support +* tf frame viewer is now an rxbag plugin +* Can now connect to any node in the system that has a tf2 buffer +* Now populates namespaces as well +* Now populates a frame list on the fly +* Got the GUI set up for a bunch of features, now just have to implement the backend of them +* Persistent service call to speed things up. Also, coloring on click +* Adding a first version of frame_viewer +* Adding xdot as a dep in prep for frame_viewer +* working view frames +* call new service +* new version of view_frames in new tf2_tools package diff --git a/src/geometry2/tf2_tools/CMakeLists.txt b/src/geometry2/tf2_tools/CMakeLists.txt new file mode 100644 index 0000000..3f4b5e0 --- /dev/null +++ b/src/geometry2/tf2_tools/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_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 scripts/echo.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/src/geometry2/tf2_tools/mainpage.dox b/src/geometry2/tf2_tools/mainpage.dox new file mode 100644 index 0000000..4ea7a70 --- /dev/null +++ b/src/geometry2/tf2_tools/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf2_tools is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/src/geometry2/tf2_tools/package.xml b/src/geometry2/tf2_tools/package.xml new file mode 100644 index 0000000..783a443 --- /dev/null +++ b/src/geometry2/tf2_tools/package.xml @@ -0,0 +1,24 @@ + + tf2_tools + 0.6.7 + + 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 + + + diff --git a/src/geometry2/tf2_tools/scripts/echo.py b/src/geometry2/tf2_tools/scripts/echo.py new file mode 100755 index 0000000..9bae2d5 --- /dev/null +++ b/src/geometry2/tf2_tools/scripts/echo.py @@ -0,0 +1,249 @@ +#!/usr/bin/env python + +# tf2 echo code Copyright (c) 2018, Lucas Walter +# transformations.py code Copyright (c) 2006-2017, Christoph Gohlke +# transformations.py code Copyright (c) 2006-2017, The Regents of the University of California +# Produced at the Laboratory for Fluorescence Dynamics +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the copyright holders nor the names of any +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from __future__ import print_function + +import argparse +import math +import numpy +import rospy +import sys +import tf2_py as tf2 +import tf2_ros + +from geometry_msgs.msg import TransformStamped +# https://github.com/ros/geometry2/issues/222 +# from tf import transformations + +""" +The following euler conversion functions are from https://github.com/matthew-brett/transforms3d +which adapted it from transformations.py, it is needed here until transforms3d is available +as a dependency. + +They are for internal use only. +""" + +# epsilon for testing whether a number is close to zero +_EPS = numpy.finfo(float).eps * 4.0 + +# axis sequences for Euler angles +_NEXT_AXIS = [1, 2, 0, 1] + +# TODO(lucasw) if sxyz works then eliminate the other possibilities +# map axes strings to/from tuples of inner axis, parity, repetition, frame +_AXES2TUPLE = { + 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), + 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), + 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), + 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), + 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), + 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), + 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), + 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} + +def _euler_from_matrix(matrix, axes='sxyz'): + """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" + try: + firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] + except (AttributeError, KeyError): + _TUPLE2AXES[axes] # validation + firstaxis, parity, repetition, frame = axes + + i = firstaxis + j = _NEXT_AXIS[i+parity] + k = _NEXT_AXIS[i-parity+1] + + M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] + if repetition: + sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) + if sy > _EPS: + ax = math.atan2( M[i, j], M[i, k]) + ay = math.atan2( sy, M[i, i]) + az = math.atan2( M[j, i], -M[k, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2( sy, M[i, i]) + az = 0.0 + else: + cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) + if cy > _EPS: + ax = math.atan2( M[k, j], M[k, k]) + ay = math.atan2(-M[k, i], cy) + az = math.atan2( M[j, i], M[i, i]) + else: + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2(-M[k, i], cy) + az = 0.0 + + if parity: + ax, ay, az = -ax, -ay, -az + if frame: + ax, az = az, ax + return ax, ay, az + +def _quaternion_matrix(quaternion): + """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" + q = numpy.array(quaternion, dtype=numpy.float64, copy=True) + n = numpy.dot(q, q) + if n < _EPS: + return numpy.identity(4) + q *= math.sqrt(2.0 / n) + q = numpy.outer(q, q) + return numpy.array([ + [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], + [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], + [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], + [ 0.0, 0.0, 0.0, 1.0]]) + +def _euler_from_quaternion(quaternion, axes='sxyz'): + """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" + return _euler_from_matrix(_quaternion_matrix(quaternion), axes) + +def _euler_from_quaternion_msg(quaternion): + # the above code is from transform3 which changed convention from old transformations.py + # from xyzw to wxyz + # return transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w]) + return _euler_from_quaternion([quaternion.w, + quaternion.x, + quaternion.y, + quaternion.z]) + +class Echo(): + def __init__(self, args): + self.tf_buffer = tf2_ros.Buffer(cache_time=args.cache_time) + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + self.args = args + + self.count = 0 + self.timer = rospy.Timer(rospy.Duration(1.0 / self.args.rate), self.lookup) + + def lookup(self, event): + self.count += 1 + if self.args.limit: + if self.count > self.args.limit: + # TODO(lucasw) is there a better method to stop the spin()? + rospy.signal_shutdown("tf echo finished") + return + + cur_time = rospy.Time.now() + # If the transform is from tf_static the ts.header.stamp will be 0.0 + # when offset == 0 or lookup_time is rospy.Time() + if self.args.time: + lookup_time = rospy.Time(self.args.time) + elif self.args.offset: + # If the transform is static this will always work + lookup_time = cur_time + rospy.Duration(self.args.offset) + else: + # Get the most recent transform + lookup_time = rospy.Time() + + try: + ts = self.tf_buffer.lookup_transform(self.args.source_frame, + self.args.target_frame, + lookup_time) + except tf2.LookupException as ex: + msg = "At time {}, (current time {}) ".format(lookup_time.to_sec(), cur_time.to_sec()) + rospy.logerr(msg + str(ex)) + return + except tf2.ExtrapolationException as ex: + msg = "(current time {}) ".format(cur_time.to_sec()) + rospy.logerr(msg + str(ex)) + return + + # The old tf1 static_transform_publisher (which published into /tf, not /tf_static) + # publishes transforms 0.5 seconds into future so the cur_time and header stamp + # will be identical. + msg = "At time {}, (current time {})".format(ts.header.stamp.to_sec(), cur_time.to_sec()) + xyz = ts.transform.translation + msg += "\n- Translation: [{:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(xyz.x, xyz.y, xyz.z, p=self.args.precision) + quat = ts.transform.rotation + msg += "- Rotation: in Quaternion [{:.{p}f}, {:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(quat.x, quat.y, quat.z, quat.w, p=self.args.precision) + # TODO(lucasw) need to get quaternion to euler from somewhere, but not tf1 + # or a dependency that isn't in Ubuntu or ros repos + euler = _euler_from_quaternion_msg(quat) + msg += " in RPY (radian) " + msg += "[{:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(euler[0], euler[1], euler[2], p=self.args.precision) + msg += " in RPY (degree) " + msg += "[{:.{p}f}, {:.{p}f}, {:.{p}f}]".format(math.degrees(euler[0]), + math.degrees(euler[1]), + math.degrees(euler[2]), p=self.args.precision) + print(msg) + +def positive_float(x): + x = float(x) + if x <= 0.0: + raise argparse.ArgumentTypeError("{} must be > 0.0".format(x)) + return x + +def positive_int(x): + x = int(x) + if x <= 0: + raise argparse.ArgumentTypeError("{} must be > 0".format(x)) + return x + +if __name__ == '__main__': + rospy.init_node("echo") + + other_args = rospy.myargv(argv=sys.argv) + precision=3 + try: + precision = rospy.get_param('~precision') + rospy.loginfo("Precision default value was overriden, new value: %d", precision) + except KeyError: + pass + + parser = argparse.ArgumentParser() + parser.add_argument("source_frame") # parent + parser.add_argument("target_frame") # child + parser.add_argument("-r", "--rate", + help="update rate, must be > 0.0", + default=1.0, + type=positive_float) + parser.add_argument("-c", "--cache_time", + help="length of tf buffer cache in seconds", + type=positive_float) + parser.add_argument("-o", "--offset", + help="offset the lookup from current time, ignored if using -t", + type=float) + parser.add_argument("-t", "--time", + help="fixed time to do the lookup", + type=float) + parser.add_argument("-l", "--limit", + help="lookup fixed number of times", + type=positive_int) + parser.add_argument("-p", "--precision", + help="output precision", + default=precision, + type=positive_int) + args = parser.parse_args(other_args[1:]) # Remove first arg + echo = Echo(args) + rospy.spin() diff --git a/src/geometry2/tf2_tools/scripts/view_frames.py b/src/geometry2/tf2_tools/scripts/view_frames.py new file mode 100755 index 0000000..e135e72 --- /dev/null +++ b/src/geometry2/tf2_tools/scripts/view_frames.py @@ -0,0 +1,81 @@ +#!/usr/bin/env 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 rospy +import tf2_py as tf2 +import yaml +import subprocess +from tf2_msgs.srv import FrameGraph +import tf2_ros + +def main(): + rospy.init_node('view_frames') + + # listen to tf for 5 seconds + rospy.loginfo('Listening to tf data during 5 seconds...') + rospy.sleep(0.00001) + buffer = tf2_ros.Buffer() + listener = tf2_ros.TransformListener(buffer) + rospy.sleep(5.0) + + rospy.loginfo('Generating graph in frames.pdf file...') + rospy.wait_for_service('~tf2_frames') + srv = rospy.ServiceProxy('~tf2_frames', FrameGraph) + data = yaml.safe_load(srv().frame_yaml) + with open('frames.gv', 'w') as f: + f.write(generate_dot(data)) + subprocess.Popen('dot -Tpdf frames.gv -o frames.pdf'.split(' ')).communicate() + +def generate_dot(data): + if len(data) == 0: + return 'digraph G { "No tf data received" }' + + dot = 'digraph G {\n' + for el in data: + map = data[el] + dot += '"'+map['parent']+'" -> "'+str(el)+'"' + dot += '[label=" ' + dot += 'Broadcaster: '+map['broadcaster']+'\\n' + dot += 'Average rate: '+str(map['rate'])+'\\n' + dot += 'Buffer length: '+str(map['buffer_length'])+'\\n' + dot += 'Most recent transform: '+str(map['most_recent_transform'])+'\\n' + dot += 'Oldest transform: '+str(map['oldest_transform'])+'\\n' + dot += '"];\n' + if not map['parent'] in data: + root = map['parent'] + dot += 'edge [style=invis];\n' + dot += ' subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";\n' + dot += '"Recorded at time: '+str(rospy.Time.now().to_sec())+'"[ shape=plaintext ] ;\n' + dot += '}->"'+root+'";\n}' + return dot + + +if __name__ == '__main__': + main()