modified: src/maintain/scripts/maintain.py

modified:   src/yolov5_ros/src/detect.py

.gitignore

@@ -1,6 +1,8 @@
 .catkin_tools
 .vscode
+.vscode/
 .vscode/*
 /build
 /devel
 /logs
+*.pyc

.vscode/c_cpp_properties.json

@@ -1,20 +0,0 @@
-{
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "${default}",
-        "limitSymbolsToIncludedHeaders": false
-      },
-      "includePath": [
-        "/opt/ros/melodic/include/**",
-        "/usr/include/**"
-      ],
-      "name": "ROS",
-      "intelliSenseMode": "gcc-x64",
-      "compilerPath": "/usr/bin/gcc",
-      "cStandard": "gnu11",
-      "cppStandard": "c++14"
-    }
-  ],
-  "version": 4
-}

.vscode/settings.json

@@ -1,8 +0,0 @@
-{
-    "python.autoComplete.extraPaths": [
-        "/opt/ros/melodic/lib/python2.7/dist-packages"
-    ],
-    "python.analysis.extraPaths": [
-        "/opt/ros/melodic/lib/python2.7/dist-packages"
-    ]
-}

README.md

@@ -0,0 +1 @@
+git clone -b melodic-devel https://github.com/ros/geometry2

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

src/geometry2/geometry2/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(geometry2)
+find_package(catkin REQUIRED)
+catkin_metapackage()

src/geometry2/geometry2/package.xml

@@ -0,0 +1,29 @@
+<package>
+  <name>geometry2</name>
+  <version>0.6.7</version>
+  <description>
+    A metapackage to bring in the default packages second generation Transform Library in ros, tf2.
+  </description>
+  <author>Tully Foote</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/geometry2</url>
+    
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_bullet</run_depend>
+  <run_depend>tf2_eigen</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
+  <run_depend>tf2_kdl</run_depend>
+  <run_depend>tf2_msgs</run_depend>
+  <run_depend>tf2_py</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_sensor_msgs</run_depend>
+  <run_depend>tf2_tools</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>

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 <https://github.com/ros/geometry2/issues/443>`_)
+  * more portable fixes.
+* Contributors: Sean Yen
+
+0.6.6 (2020-01-09)
+------------------
+* Update shebang and add launch prefixes for python3 support (`#421 <https://github.com/ros/geometry2/issues/421>`_)
+* Always call catkin_package() (`#418 <https://github.com/ros/geometry2/issues/418>`_)
+* Remove roslib.load_manifest `#404 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/301>`_)
+* update cmake order (`#298 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/227>`_)
+  * Remove a slew of trailing whitespace.
+  Signed-off-by: Chris Lalancette <clalancette@osrfoundation.org>
+  * 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 <clalancette@osrfoundation.org>
+* store gtest return value as int (`#229 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry_experimental/issues/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

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()

src/geometry2/test_tf2/mainpage.dox

@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b test_tf2 is ... 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/

src/geometry2/test_tf2/package.xml

@@ -0,0 +1,45 @@
+<package>
+  <name>test_tf2</name>
+  <version>0.6.7</version>
+  <description>
+    tf2 unit tests
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/geometry_experimental</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>rosconsole</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rostest</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_bullet</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>tf2_kdl</build_depend>
+  <build_depend>tf2_msgs</build_depend>
+  <build_depend>tf2_eigen</build_depend>
+
+  <run_depend>rosconsole</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rostest</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_bullet</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
+  <run_depend>tf2_kdl</run_depend>
+  <run_depend>tf2_msgs</run_depend>
+  <run_depend>tf2_eigen</run_depend>
+
+  <test_depend>rosunit</test_depend>
+  <test_depend>rosbash</test_depend>
+
+</package>
+
+

src/geometry2/test_tf2/test/buffer_client_tester.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="tf2_ros" type="static_transform_publisher" name="test_static_pub" args="5 6 7 0 0 0 1 a b" />
+  <node name="test_buffer_server" pkg="test_tf2" type="test_buffer_server" output="screen" />
+  <test test-name="test_buffer_client_test" pkg="test_tf2" type="test_buffer_client" args="--text"/>
+</launch>

src/geometry2/test_tf2/test/static_publisher.launch

@@ -0,0 +1,34 @@
+<launch>
+  <node pkg="tf2_ros" type="static_transform_publisher" name="test_static_pub_ab" args="1 0 0 0 0 0 1 a b" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="test_static_pub_bc" args="0 1 0 0 0 0 1 b c" />
+
+  <!-- Start the static tf publisher with a valid tf.  There is a test
+       (tf_from_param_server_valid) that ensures the TF is published. -->
+  <rosparam param="test_tf2/tf_valid"
+            file="$(find test_tf2)/test/test_tf_valid.yaml" />
+  <node pkg="tf2_ros" type="static_transform_publisher"
+        name="test_static_pub_param_valid"
+        args="test_tf2/tf_valid" />
+
+  <!-- Start the static tf publisher with an *invalid* tf.
+       The main purpose of this test is to ensure the node dies gracefully. -->
+  <rosparam param="test_tf2/tf_invalid"
+            file="$(find test_tf2)/test/test_tf_invalid.yaml" />
+  <node pkg="tf2_ros" type="static_transform_publisher"
+        name="test_static_pub_param_invalid"
+        args="test_tf2/tf_invalid" />
+
+  <!-- Start the static tf publisher with a non-existent parameter.
+       The main purpose of this test is to ensure the node dies gracefully. -->
+  <node pkg="tf2_ros" type="static_transform_publisher"
+        name="test_static_pub_param_null"
+        args="test_tf2/tf_null" />
+
+  <test test-name="test_static_publisher" pkg="test_tf2"
+        type="test_static_publisher" args="--text" />
+
+  <test test-name="test_static_publisher_py"
+        pkg="test_tf2"
+        type="test_static_publisher.py"
+        launch-prefix="python$(env ROS_PYTHON_VERSION)"/>
+</launch>

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 <gtest/gtest.h>
+#include <tf2_ros/buffer_client.h>
+#include <ros/ros.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_kdl/tf2_kdl.h>
+#include <tf2_bullet/tf2_bullet.h>
+
+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<KDL::Vector> k1(KDL::Vector(0, 0, 0), ros::Time(), "a");
+
+  try
+  {
+    tf2::Stamped<btVector3> 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();
+}
+

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)

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 <tf2_ros/buffer_server.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <ros/ros.h>
+
+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();
+}
+

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 <gtest/gtest.h>
+#include <tf2/convert.h>
+#include <tf2_kdl/tf2_kdl.h>
+#include <tf2_bullet/tf2_bullet.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include <ros/time.h>
+
+TEST(tf2Convert, kdlToBullet)
+{
+  double epsilon = 1e-9;
+
+  tf2::Stamped<btVector3> b(btVector3(1,2,3), ros::Time(), "my_frame");
+
+  tf2::Stamped<btVector3> b1 = b;
+  tf2::Stamped<KDL::Vector> k1;
+  tf2::convert(b1, k1);
+
+  tf2::Stamped<btVector3> 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<btVector3> b1(btVector3(1,2,3), ros::Time(), "my_frame"), b2, b3, b4;
+  geometry_msgs::PointStamped r1, r2, r3;
+  tf2::Stamped<KDL::Vector> 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();
+}
+

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)

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 <tf2_ros/message_filter.h>
+#include <tf2/buffer_core.h>
+#include <geometry_msgs/PointStamped.h>
+#include <boost/bind.hpp>
+#include <boost/scoped_ptr.hpp>
+
+#include "ros/ros.h"
+#include "ros/callback_queue.h"
+
+#include <gtest/gtest.h>
+
+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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> filter(bc, "", 1, 0);
+  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
+
+  std::vector<std::string> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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<geometry_msgs::PointStamped> 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;
+}

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 <gtest/gtest.h>
+#include <tf2/buffer_core.h>
+#include "tf2/exceptions.h"
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <ros/ros.h>
+#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();
+}

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)

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 <tf2_bullet/tf2_bullet.h>
+#include <tf2_ros/buffer.h>
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+#include <tf2/convert.h>
+
+tf2_ros::Buffer* tf_buffer;
+static const double EPS = 1e-3;
+
+TEST(TfBullet, Transform)
+{
+  tf2::Stamped<btTransform> 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<btVector3>  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;
+}

src/geometry2/test_tf2/test/test_tf2_bullet.launch

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="test_tf2_bullet" pkg="test_tf2" type="test_tf2_bullet" time-limit="120" />
+</launch>

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

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

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 <gtest/gtest.h>
+#include <tf2/utils.h>
+#include <tf2_kdl/tf2_kdl.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <ros/time.h>
+
+double epsilon = 1e-9;
+
+template<typename T>
+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::Quaternion>
+    tf2::Stamped<tf2::Quaternion> 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<geometry_msgs::Transform>();
+
+  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();
+}
+

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 <https://github.com/ros/geometry2/issues/443>`_)
+* [Windows][melodic-devel] Fix install locations (`#442 <https://github.com/ros/geometry2/issues/442>`_)
+* Revert "rework Eigen functions namespace hack" (`#436 <https://github.com/ros/geometry2/issues/436>`_)
+* Contributors: Sean Yen, Tully Foote
+
+0.6.6 (2020-01-09)
+------------------
+* Fix compile error missing ros/ros.h (`#400 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/307>`_)
+* Change comment style for unused doxygen (`#297 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/267>`_ from at-wat/speedup-timecache-for-large-buffer
+  Speed-up TimeCache search for large cache time.
+* Merge pull request `#265 <https://github.com/ros/geometry2/issues/265>`_ from vsherrod/interpolation_fix
+  Corrected time output on interpolation function.
+* Add time_interval option to tf2 speed-test.
+* Merge pull request `#269 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/198>`_
+* Remove generate_rand_vectors() from a number of tests. (`#227 <https://github.com/ros/geometry2/issues/227>`_)
+* fixing include directory order to support overlays (`#231 <https://github.com/ros/geometry2/issues/231>`_)
+* replaced dependencies on tf2_msgs_gencpp by exported dependencies
+* Document the lifetime of the returned reference for getFrameId getTimestamp
+* relax normalization tolerance. `#196 <https://github.com/ros/geometry2/issues/196>`_ was too strict for some use cases. (`#220 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/194>`_ check for quaternion normalization before inserting into storage (`#196 <https://github.com/ros/geometry2/issues/196>`_)
+  * check for quaternion normalization before inserting into storage
+  * Add test to check for transform failure on invalid quaternion input
+* updating getAngleShortestPath() (`#187 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry_experimental/issues/92>`_
+* Clean up range checking. Re: `#92 <https://github.com/ros/geometry_experimental/issues/92>`_
+* Fixed chainToVector
+* release lock before possibly invoking user callbacks. Fixes `#91 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/ros_comm/issues/267>`_, blocking `ros/geometry#23 <https://github.com/ros/geometry/issues/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 <https://github.com/ros/geometry_experimental/issues/38>`_
+* tf2: add missing console bridge include directories (fix `#48 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/19>`_
+* fix pointer initialization.  Fixes `#19 <https://github.com/ros/geometry_experimental/issues/19>`_
+* fixes `#18 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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

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()

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 <ros/macros.h>
+
+namespace tf2
+{
+
+
+#define Matrix3x3Data	Matrix3x3DoubleData 
+
+
+/**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3.
+* Make sure to only include a pure orthogonal matrix without scaling. */
+class Matrix3x3 {
+
+	///Data storage for the matrix, each vector is a row of the matrix
+	Vector3 m_el[3];
+
+public:
+	/** @brief No initializaion constructor */
+	Matrix3x3 () {}
+
+	//		explicit Matrix3x3(const tf2Scalar *m) { setFromOpenGLSubMatrix(m); }
+
+	/**@brief Constructor from Quaternion */
+	explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
+	/*
+	template <typename tf2Scalar>
+	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
+

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 <class T>
+TF2SIMD_FORCE_INLINE const T& tf2Min(const T& a, const T& b) 
+{
+  return a < b ? a : b ;
+}
+
+template <class T>
+TF2SIMD_FORCE_INLINE const T& tf2Max(const T& a, const T& b) 
+{
+  return  a > b ? a : b;
+}
+
+template <class T>
+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 <class T>
+TF2SIMD_FORCE_INLINE void tf2SetMin(T& a, const T& b) 
+{
+    if (b < a) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+TF2SIMD_FORCE_INLINE void tf2SetMax(T& a, const T& b) 
+{
+    if (a < b) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+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

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 <altivec.h>
+#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

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 <ros/macros.h>
+
+namespace tf2
+{
+
+/**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */
+class Quaternion : public QuadWord {
+public:
+  /**@brief No initialization constructor */
+	Quaternion() {}
+
+	//		template <typename tf2Scalar>
+	//		explicit Quaternion(const tf2Scalar *v) : Tuple4<tf2Scalar>(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
+
+
+
+

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 <math.h>
+#include <stdlib.h>//size_t for MSVC 6.0
+#include <cstdlib>
+#include <cfloat>
+#include <float.h>
+
+#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 <ppcintrinsics.h>
+ 			#define TF2_HAVE_NATIVE_FSEL
+ 			#define tf2Fsel(a,b,c) __fsel((a),(b),(c))
+		#else
+
+
+		#endif//_XBOX
+
+		#endif //__MINGW32__
+
+		#include <assert.h>
+#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 <assert.h>
+		#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 <assert.h>
+		#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 <assert.h>
+		#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 (x<tf2Scalar(-1))	x=tf2Scalar(-1); if (x>tf2Scalar(1))	x=tf2Scalar(1); return acos(x); }
+TF2SIMD_FORCE_INLINE tf2Scalar tf2Asin(tf2Scalar x) { if (x<tf2Scalar(-1))	x=tf2Scalar(-1); if (x>tf2Scalar(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<int>((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<typename T> 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<unsigned short>(((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

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

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] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
+	}
+
+  /**@brief Return the axis with the largest value 
+   * Note return values are 0,1,2 for x, y, or z */
+	TF2SIMD_FORCE_INLINE int maxAxis() const 
+	{
+		return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
+	}
+
+	TF2SIMD_FORCE_INLINE int furthestAxis() const
+	{
+		return absolute().minAxis();
+	}
+
+	TF2SIMD_FORCE_INLINE int closestAxis() const 
+	{
+		return absolute().maxAxis();
+	}
+
+	TF2SIMD_FORCE_INLINE void setInterpolate3(const Vector3& v0, const Vector3& v1, tf2Scalar rt)
+	{
+		tf2Scalar s = tf2Scalar(1.0) - rt;
+		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+		m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
+		//don't do the unused w component
+		//		m_co[3] = s * v0[3] + rt * v1[3];
+	}
+
+  /**@brief Return the linear interpolation between this and another vector 
+   * @param v The other vector 
+   * @param t The ration of this to v (t = 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

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 <boost/signals2.hpp>
+
+#include <string>
+
+#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 <boost/unordered_map.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/function.hpp>
+#include <boost/shared_ptr.hpp>
+
+namespace tf2
+{
+
+typedef std::pair<ros::Time, CompactFrameID> P_TimeAndFrameID;
+typedef uint32_t TransformableCallbackHandle;
+typedef uint64_t TransformableRequestHandle;
+
+class TimeCacheInterface;
+typedef boost::shared_ptr<TimeCacheInterface> 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<void(TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
+                               ros::Time time, TransformableResult result)> 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<void(void)> 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<std::string>& 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<std::string>& 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<TimeCacheInterfacePtr> 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<std::string, CompactFrameID> M_StringToCompactFrameID;
+  M_StringToCompactFrameID frameIDs_;
+  /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */
+  std::vector<std::string> frameIDs_reverse;
+  /** \brief A map to lookup the most recent authority for a given frame */
+  std::map<CompactFrameID, std::string> frame_authority_;
+
+
+  /// How long to cache transform history
+  ros::Duration cache_time_;
+
+  typedef boost::unordered_map<TransformableCallbackHandle, TransformableCallback> 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<TransformableRequest> 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<void(void)> 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<typename F>
+  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<typename F>
+  int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *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<CompactFrameID> *frame_chain) const;
+  const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const
+  {
+    return buffer.lookupFrameString(frame_id_num);
+  }
+};
+}
+
+#endif //TF2_CORE_H

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 <tf2/transform_datatypes.h>
+#include <tf2/exceptions.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2/impl/convert.h>
+
+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 <class T>
+  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 <class T>
+  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 <class T>
+  const std::string& getFrameId(const T& t);
+
+
+
+/* An implementation for Stamped<P> datatypes */
+template <class P>
+  const ros::Time& getTimestamp(const tf2::Stamped<P>& t)
+  {
+    return t.stamp_;
+  }
+
+/* An implementation for Stamped<P> datatypes */
+template <class P>
+  const std::string& getFrameId(const tf2::Stamped<P>& 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<typename A, typename B>
+  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<typename A, typename B>
+  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 <class A, class B>
+  void convert(const A& a, B& b)
+  {
+    //printf("In double type convert\n");
+    impl::Converter<ros::message_traits::IsMessage<A>::value, ros::message_traits::IsMessage<B>::value>::convert(a, b);
+  }
+
+template <class A>
+  void convert(const A& a1, A& a2)
+  {
+    //printf("In single type convert\n");
+    if(&a1 != &a2)
+      a2 = a1;
+  }
+
+
+}
+
+#endif //TF2_CONVERT_H

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 <stdexcept>
+
+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

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 <bool IS_MESSAGE_A, bool IS_MESSAGE_B>
+class Converter {
+public:
+  template<typename A, typename B>
+  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 <typename A, typename B>
+//inline void Converter<true, true>::convert(const A& a, B& b);
+
+template <>
+template <typename A, typename B>
+inline void Converter<true, false>::convert(const A& a, B& b)
+{
+#ifdef _MSC_VER
+  tf2::fromMsg(a, b);
+#else
+  fromMsg(a, b);
+#endif
+}
+
+template <>
+template <typename A, typename B>
+inline void Converter<false, true>::convert(const A& a, B& b)
+{
+#ifdef _MSC_VER
+  b = tf2::toMsg(a);
+#else
+  b = toMsg(a);
+#endif
+}
+
+template <>
+template <typename A, typename B>
+inline void Converter<false, false>::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

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 <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+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<typename T>
+  tf2::Quaternion toQuaternion(const tf2::Stamped<T>& 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<typename T>
+  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

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 <deque>
+
+#include <sstream>
+
+#include <ros/message_forward.h>
+#include <ros/time.h>
+
+#include <boost/shared_ptr.hpp>
+
+namespace geometry_msgs
+{
+ROS_DECLARE_MESSAGE(TransformStamped);
+}
+
+namespace tf2
+{
+
+typedef std::pair<ros::Time, CompactFrameID> 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<TimeCacheInterface> 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<TransformStorage> 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

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 <string>
+#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 <typename T>
+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<T>& s):
+    T (s),
+    stamp_(s.stamp_),
+    frame_id_(s.frame_id_) {}
+  
+  /** Set the data element */
+  void setData(const T& input){*static_cast<T*>(this) = input;};
+};
+
+/** \brief Comparison Operator for Stamped datatypes */
+template <typename T> 
+bool operator==(const Stamped<T> &a, const Stamped<T> &b) {
+  return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast<const T&>(a) == static_cast<const T&>(b);
+}
+
+
+}
+#endif //TF2_TRANSFORM_DATATYPES_H

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 <tf2/LinearMath/Vector3.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <ros/message_forward.h>
+#include <ros/time.h>
+#include <ros/types.h>
+
+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
+

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 <tf2/LinearMath/Transform.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/impl/utils.h>
+
+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 <class A>
+  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 <class A>
+  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 <class A>
+  A getTransformIdentity()
+  {
+    tf2::Transform t;
+    t.setIdentity();
+    A a;
+    convert(t, a);
+    return a;
+  }
+
+}
+
+#endif //TF2_UTILS_H

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`

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 <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">ROS Wiki</A>.
+
+*/

src/geometry2/tf2/package.xml

@@ -0,0 +1,34 @@
+<package>
+  <name>tf2</name>
+  <version>0.6.7</version>
+  <description>
+    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.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/tf2</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>rostime</build_depend>
+  <build_depend>tf2_msgs</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>rostime</run_depend>
+  <run_depend>tf2_msgs</run_depend>
+
+</package>
+
+

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 <tf2/LinearMath/Vector3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <assert.h>
+
+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<TransformStorage>());
+
+  //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
+}

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();
+};
+

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 <gtest/gtest.h>
+#include <tf2/time_cache.h>
+#include "tf2/LinearMath/Quaternion.h"
+#include <stdexcept>
+
+#include <geometry_msgs/TransformStamped.h>
+
+#include <cmath>
+
+std::vector<double> 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<double> 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 <runs ; i++ )
+  {
+    values[i] = 10.0 * get_rand();
+    std::stringstream ss;
+    ss << values[i];
+    stor.header.frame_id = ss.str();
+    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));
+    std::stringstream ss;
+    ss << values[i];
+    EXPECT_EQ(stor.header.frame_id, ss.str());
+  }
+  
+}
+#endif
+
+TEST(TimeCache, ZeroAtFront)
+{
+  uint64_t 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);
+  }
+
+  stor.frame_id_ = runs;
+  stor.stamp_ = ros::Time().fromNSec(runs);
+  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));
+  }
+
+  cache.getData(ros::Time(), stor);
+  EXPECT_EQ(stor.frame_id_, runs);
+  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
+
+  stor.frame_id_ = runs;
+  stor.stamp_ = ros::Time().fromNSec(runs+1);
+  cache.insertData(stor);
+
+
+  //Make sure we get a different value now that a new values is added at the front
+  cache.getData(ros::Time(), stor);
+  EXPECT_EQ(stor.frame_id_, runs);
+  EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
+  
+}
+
+TEST(TimeCache, CartesianInterpolation)
+{
+  uint64_t runs = 100;
+  double epsilon = 2e-6;
+  seed_rand();
+  
+  tf2::TimeCache  cache;
+  std::vector<double> xvalues(2);
+  std::vector<double> yvalues(2);
+  std::vector<double> 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<double> xvalues(2);
+  std::vector<double> yvalues(2);
+  std::vector<double> 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<double> yawvalues(2);
+  std::vector<double> pitchvalues(2);
+  std::vector<double> rollvalues(2);
+  uint64_t offset = 200;
+
+  std::vector<tf2::Quaternion> 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();
+}
+

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 <gtest/gtest.h>
+#include <tf2/buffer_core.h>
+#include <ros/time.h>
+#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<std::string> 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<tf2::CompactFrameID> 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<tf2::CompactFrameID> 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();
+}

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 <tf2/buffer_core.h>
+
+#include <ros/time.h>
+#include <console_bridge/console.h>
+
+#include <boost/lexical_cast.hpp>
+
+int main(int argc, char** argv)
+{
+  uint32_t num_levels = 10;
+  if (argc > 1)
+  {
+    num_levels = boost::lexical_cast<uint32_t>(argv[1]);
+  }
+  double time_interval = 1.0;
+  if (argc > 2)
+  {
+    time_interval = boost::lexical_cast<double>(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<std::string>(num_levels - 1);
+  std::string v_frame1 = boost::lexical_cast<std::string>(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
+}

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 <gtest/gtest.h>
+#include <tf2/time_cache.h>
+#include <stdexcept>
+
+#include <geometry_msgs/TransformStamped.h>
+
+#include <cmath>
+
+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();
+}

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 <https://github.com/ros/geometry2/issues/443>`_)
+* Contributors: Sean Yen
+
+0.6.6 (2020-01-09)
+------------------
+* Fix compile error missing ros/ros.h (`#400 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry_experimental/issues/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

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()

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 <tf2/convert.h>
+#include <LinearMath/btTransform.h>
+#include <geometry_msgs/PointStamped.h>
+
+
+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<btVector3>& t_in, tf2::Stamped<btVector3>& t_out, const geometry_msgs::TransformStamped& transform)
+  {
+    t_out = tf2::Stamped<btVector3>(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<btVector3>& 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<btVector3>& 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<btTransform>& t_in, tf2::Stamped<btTransform>& t_out, const geometry_msgs::TransformStamped& transform)
+  {
+    t_out = tf2::Stamped<btTransform>(transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id);
+  }
+
+
+} // namespace
+
+#endif // TF2_BULLET_H

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 <tf2_bullet/tf2_bullet.h>
+
+#include <tf2_bullet/tf2_bullet.h>

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 <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">Conversions overview</A>
+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.
+
+*/

src/geometry2/tf2_bullet/package.xml

@@ -0,0 +1,25 @@
+<package>
+  <name>tf2_bullet</name>
+  <version>0.6.7</version>
+  <description>
+    tf2_bullet
+  </description>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/tf2_bullet</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+  <buildtool_depend>pkg-config</buildtool_depend>
+
+  <build_depend>tf2</build_depend>
+  <build_depend>bullet</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <run_depend>tf2</run_depend>
+  <run_depend>bullet</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+</package>
+

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 <tf2_bullet/tf2_bullet.h>
+#include <gtest/gtest.h>
+#include <tf2/convert.h>
+
+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;
+}

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 <https://github.com/ros/geometry2/issues/436>`_)
+* Contributors: Tully Foote
+
+0.6.6 (2020-01-09)
+------------------
+* Fix compile error missing ros/ros.h (`#400 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/237>`_)
+* fixing include directory order to support overlays (`#231 <https://github.com/ros/geometry2/issues/231>`_)
+* tf2_eigen: added support for Quaternion and QuaternionStamped (`#230 <https://github.com/ros/geometry2/issues/230>`_)
+* Remove an unused variable from the tf2_eigen test. (`#215 <https://github.com/ros/geometry2/issues/215>`_)
+* Find eigen in a much nicer way.
+* Switch tf2_eigen to use package.xml format 2. (`#216 <https://github.com/ros/geometry2/issues/216>`_)
+* Contributors: Chris Lalancette, Mikael Arguedas, Tully Foote, cwecht
+
+0.5.15 (2017-01-24)
+-------------------
+* fixup `#186 <https://github.com/ros/geometry2/issues/186>`_: inline template specializations (`#200 <https://github.com/ros/geometry2/issues/200>`_)
+* Contributors: Robert Haschke
+
+0.5.14 (2017-01-16)
+-------------------
+* Add tf2_eigen conversions for Pose and Point (not stamped) (`#186 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry_experimental/issues/97>`_
+* create tf2_eigen.
+* Contributors: Tully Foote, koji

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()

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 <tf2/convert.h>
+#include <Eigen/Geometry>
+#include <geometry_msgs/QuaternionStamped.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+
+
+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<Eigen::Vector3d>& t_in,
+		 tf2::Stamped<Eigen::Vector3d>& t_out,
+		 const geometry_msgs::TransformStamped& transform) {
+  t_out = tf2::Stamped<Eigen::Vector3d>(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<Eigen::Vector3d>& in)
+{
+  geometry_msgs::PointStamped msg;
+  msg.header.stamp = in.stamp_;
+  msg.header.frame_id = in.frame_id_;
+  msg.point = toMsg(static_cast<const Eigen::Vector3d&>(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<Eigen::Vector3d>& out) {
+  out.stamp_ = msg.header.stamp;
+  out.frame_id_ = msg.header.frame_id;
+  fromMsg(msg.point, static_cast<Eigen::Vector3d&>(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<Eigen::Quaterniond>& in) {
+  geometry_msgs::QuaternionStamped msg;
+  msg.header.stamp = in.stamp_;
+  msg.header.frame_id = in.frame_id_;
+  msg.quaternion = toMsg(static_cast<const Eigen::Quaterniond&>(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<Eigen::Quaterniond>& out) {
+  out.frame_id_ = msg.header.frame_id;
+  out.stamp_ = msg.header.stamp;
+  fromMsg(msg.quaternion, static_cast<Eigen::Quaterniond&>(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<Eigen::Quaterniond>& t_in,
+     tf2::Stamped<Eigen::Quaterniond>& t_out,
+     const geometry_msgs::TransformStamped& transform) {
+  t_out.frame_id_ = transform.header.frame_id;
+  t_out.stamp_ = transform.header.stamp;
+  doTransform(static_cast<const Eigen::Quaterniond&>(t_in), static_cast<Eigen::Quaterniond&>(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<double,6,1>& 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<double,6,1>& 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<Eigen::Affine3d>& t_in,
+		 tf2::Stamped<Eigen::Affine3d>& t_out,
+		 const geometry_msgs::TransformStamped& transform) {
+  t_out = tf2::Stamped<Eigen::Affine3d>(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<Eigen::Isometry3d>& t_in,
+		 tf2::Stamped<Eigen::Isometry3d>& t_out,
+		 const geometry_msgs::TransformStamped& transform) {
+  t_out = tf2::Stamped<Eigen::Isometry3d>(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<Eigen::Affine3d>& in)
+{
+  geometry_msgs::PoseStamped msg;
+  msg.header.stamp = in.stamp_;
+  msg.header.frame_id = in.frame_id_;
+  msg.pose = toMsg(static_cast<const Eigen::Affine3d&>(in));
+  return msg;
+}
+
+inline
+geometry_msgs::PoseStamped toMsg(const tf2::Stamped<Eigen::Isometry3d>& in)
+{
+  geometry_msgs::PoseStamped msg;
+  msg.header.stamp = in.stamp_;
+  msg.header.frame_id = in.frame_id_;
+  msg.pose = toMsg(static_cast<const Eigen::Isometry3d&>(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<Eigen::Affine3d>& out)
+{
+  out.stamp_ = msg.header.stamp;
+  out.frame_id_ = msg.header.frame_id;
+  fromMsg(msg.pose, static_cast<Eigen::Affine3d&>(out));
+}
+
+inline
+void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<Eigen::Isometry3d>& out)
+{
+  out.stamp_ = msg.header.stamp;
+  out.frame_id_ = msg.header.frame_id;
+  fromMsg(msg.pose, static_cast<Eigen::Isometry3d&>(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<double,6,1>& in) {
+  return tf2::toMsg(in);
+}
+
+inline
+void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix<double,6,1>& out) {
+  tf2::fromMsg(msg, out);
+}
+
+} // namespace
+
+#endif // TF2_EIGEN_H

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 <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">Conversions overview</A>
+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.
+
+*/

src/geometry2/tf2_eigen/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>tf2_eigen</name>
+  <version>0.6.7</version>
+  <description>tf2_eigen</description>
+  <author>Koji Terada</author>
+  <maintainer email="terakoji@gmail.com">Koji Terada</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>eigen</build_depend>
+
+  <build_export_depend>eigen</build_export_depend>
+
+</package>

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 <tf2_eigen/tf2_eigen.h>
+#include <gtest/gtest.h>
+#include <tf2/convert.h>
+
+TEST(TfEigen, ConvertVector3dStamped)
+{
+  const tf2::Stamped<Eigen::Vector3d> v(Eigen::Vector3d(1,2,3), ros::Time(5), "test");
+
+  tf2::Stamped<Eigen::Vector3d> 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<Eigen::Quaterniond> v(Eigen::Quaterniond(1,2,3,4), ros::Time(5), "test");
+
+  tf2::Stamped<Eigen::Quaterniond> 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<Eigen::Quaterniond> 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<Eigen::Quaterniond> 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<Eigen::Quaterniond> 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<double>(1, Eigen::Vector3d::UnitX()));
+  const tf2::Stamped<Eigen::Affine3d> v(v_nonstamped, ros::Time(42), "test_frame");
+
+  tf2::Stamped<Eigen::Affine3d> 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<double>(1, Eigen::Vector3d::UnitX()));
+  const tf2::Stamped<Eigen::Isometry3d> v(v_nonstamped, ros::Time(42), "test_frame");
+
+  tf2::Stamped<Eigen::Isometry3d> 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<double>(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<double>(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();
+}

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 <https://github.com/ros/geometry2/issues/419>`_)
+* Fix python3 compatibility for noetic (`#416 <https://github.com/ros/geometry2/issues/416>`_)
+* add <array> from STL (`#366 <https://github.com/ros/geometry2/issues/366>`_)
+* use ROS_DEPRECATED macro for portability (`#362 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/305>`_)
+* Added WrenchStamped transformation (`#302 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/284>`_)
+  Fixes `#264 <https://github.com/ros/geometry2/issues/264>`_
+* tf2_geometry_msgs added doTransform implementations for not stamped types (`#262 <https://github.com/ros/geometry2/issues/262>`_)
+  * tf2_geometry_msgs added doTransform implementations for not stamped Point, Quaterion, Pose and Vector3 message types
+* New functionality to transform PoseWithCovarianceStamped messages. (`#282 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/235>`_)
+* store gtest return value as int (`#229 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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 <https://github.com/ros/geometry_experimental/issues/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

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()

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
+# "<project> v<release> 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 <link> 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}

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`
+

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 <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">Conversions overview</A>
+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.
+
+*/

src/geometry2/tf2_geometry_msgs/package.xml

@@ -0,0 +1,27 @@
+<package format="2">
+  <name>tf2_geometry_msgs</name>
+  <version>0.6.7</version>
+  <description>
+    tf2_geometry_msgs
+  </description>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/tf2_ros</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>orocos_kdl</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <build_depend>python_orocos_kdl</build_depend>
+ 
+  <exec_depend>python_orocos_kdl</exec_depend>
+
+  <test_depend>ros_environment</test_depend>
+  <test_depend>rostest</test_depend>
+</package>
+

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

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)

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)
+

src/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py

@@ -0,0 +1 @@
+from .tf2_geometry_msgs import *

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)

src/geometry2/tf2_geometry_msgs/test/test.launch

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="test_tf_geometry_msgs" pkg="tf2_geometry_msgs" type="test_geometry_msgs" time-limit="120" />
+</launch>

src/geometry2/tf2_geometry_msgs/test/test_python.launch

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="test_tf_geometry_msgs_python" pkg="tf2_geometry_msgs" type="test.py" time-limit="120"  launch-prefix="python$(env ROS_PYTHON_VERSION)"/>
+</launch>

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 <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+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;
+}
+
+
+
+
+

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 <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <gtest/gtest.h>
+
+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 <typename T>
+void expect_near(const tf2::Stamped<T>& s1, const tf2::Stamped<T>& 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<tf2::Vector3> 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<tf2::Vector3> 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<tf2::Quaternion> 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<tf2::Transform> 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<tf2::Transform> 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;
+}

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 <https://github.com/ros/geometry2/issues/419>`_)
+* Fix python3 compatibility for noetic (`#416 <https://github.com/ros/geometry2/issues/416>`_)
+* Remove roslib.load_manifest `#404 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry2/issues/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 <https://github.com/ros/geometry_experimental/issues/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

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()

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
+# "<project> v<release> 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 <link> 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
+    }

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 <tf2/convert.h>
+#include <kdl/frames.hpp>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Pose.h>
+
+
+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<KDL::Vector>& t_in, tf2::Stamped<KDL::Vector>& t_out, const geometry_msgs::TransformStamped& transform)
+  {
+    t_out = tf2::Stamped<KDL::Vector>(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<KDL::Vector>& 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<KDL::Vector>& 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<KDL::Twist>& t_in, tf2::Stamped<KDL::Twist>& t_out, const geometry_msgs::TransformStamped& transform)
+  {
+    t_out = tf2::Stamped<KDL::Twist>(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<KDL::Twist>& 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<KDL::Twist>& 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<KDL::Wrench>& t_in, tf2::Stamped<KDL::Wrench>& t_out, const geometry_msgs::TransformStamped& transform)
+  {
+    t_out = tf2::Stamped<KDL::Wrench>(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<KDL::Wrench>& 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<KDL::Wrench>& 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<KDL::Frame>& t_in, tf2::Stamped<KDL::Frame>& t_out, const geometry_msgs::TransformStamped& transform)
+  {
+    t_out = tf2::Stamped<KDL::Frame>(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<KDL::Frame>& in)
+{
+  geometry_msgs::PoseStamped msg;
+  msg.header.stamp = in.stamp_;
+  msg.header.frame_id = in.frame_id_;
+  msg.pose = toMsg(static_cast<const KDL::Frame&>(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<KDL::Frame>& out)
+{
+  out.stamp_ = msg.header.stamp;
+  out.frame_id_ = msg.header.frame_id;
+  fromMsg(msg.pose, static_cast<KDL::Frame&>(out));
+}
+
+} // namespace
+
+#endif // TF2_KDL_H

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 <tf2_kdl/tf2_kdl.h>
+
+#include <tf2_kdl/tf2_kdl.h>

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`

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 <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">Conversions overview</A>
+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.
+
+*/

src/geometry2/tf2_kdl/package.xml

@@ -0,0 +1,27 @@
+<package format="2">
+  <name>tf2_kdl</name>
+  <version>0.6.7</version>
+  <description>
+    KDL binding for tf2
+  </description>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <author>Wim Meeussen</author>
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/tf2</url>
+
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>cmake_modules</build_depend> <!-- needed for eigen -->
+  <build_depend>eigen</build_depend> <!-- needed by kdl internally -->
+
+  <build_export_depend>eigen</build_export_depend> <!-- needed by kdl internally -->
+
+  <depend>orocos_kdl</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ros_environment</test_depend>
+  <test_depend>rostest</test_depend>
+
+</package>

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

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)

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)

src/geometry2/tf2_kdl/src/tf2_kdl/__init__.py

@@ -0,0 +1 @@
+from .tf2_kdl import *

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)

src/geometry2/tf2_kdl/test/test.launch

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="test_tf_kdl" pkg="tf2_kdl" type="test_kdl" time-limit="120" />
+</launch>

src/geometry2/tf2_kdl/test/test_python.launch

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="test_tf2_kdl_python" pkg="tf2_kdl" type="test.py" time-limit="120" launch-prefix="python$(env ROS_PYTHON_VERSION)"/>
+</launch>