git commit -m "first commit"

2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
--- a/navigations/geometry2/tf2_geometry_msgs/CHANGELOG.rst
+++ b/navigations/geometry2/tf2_geometry_msgs/CHANGELOG.rst
@@ -0,0 +1,248 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package tf2_geometry_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.7.6 (2022-10-11)
+------------------
+* remove method with misleading doc (`#534 <https://github.com/ros/geometry2/issues/534>`_)
+* Contributors: Wellington Castro
+
+0.7.5 (2020-09-01)
+------------------
+
+0.7.4 (2020-09-01)
+------------------
+
+0.7.3 (2020-08-25)
+------------------
+* Use list instead of set to make build reproducible (`#473 <https://github.com/ros/geometry2/issues/473>`_)
+* Contributors: Jochen Sprickerhof
+
+0.7.2 (2020-06-08)
+------------------
+
+0.7.1 (2020-05-13)
+------------------
+* import setup from setuptools instead of distutils-core (`#449 <https://github.com/ros/geometry2/issues/449>`_)
+* Contributors: Alejandro Hernández Cordero
+
+0.7.0 (2020-03-09)
+------------------
+* Replace kdl packages with rosdep keys (`#447 <https://github.com/ros/geometry2/issues/447>`_)
+* Bump CMake version to avoid CMP0048 warning (`#445 <https://github.com/ros/geometry2/issues/445>`_)
+* 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>`_)
+  * use ROS_DEPRECATED for better portability
+  * change ROS_DEPRECATED position (`#5 <https://github.com/ros/geometry2/issues/5>`_)
+* Contributors: James Xu, Shane Loretz, Tully Foote
+
+0.6.5 (2018-11-16)
+------------------
+* Fix python3 import error
+* Contributors: Timon Engelke
+
+0.6.4 (2018-11-06)
+------------------
+
+0.6.3 (2018-07-09)
+------------------
+* Changed access to Vector to prevent memory leak (`#305 <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
--- a/navigations/geometry2/tf2_geometry_msgs/CMakeLists.txt
+++ b/navigations/geometry2/tf2_geometry_msgs/CMakeLists.txt
@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(tf2_geometry_msgs)
+
+find_package(orocos_kdl)
+find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2_ros tf2)
+find_package(Boost COMPONENTS thread REQUIRED)
+
+# Issue #53
+find_library(KDL_LIBRARY REQUIRED NAMES orocos-kdl HINTS ${orocos_kdl_LIBRARY_DIRS})
+
+catkin_package(
+   LIBRARIES ${KDL_LIBRARY}
+   INCLUDE_DIRS include
+   DEPENDS orocos_kdl
+   CATKIN_DEPENDS  geometry_msgs tf2_ros tf2)
+
+
+include_directories(include
+                    ${catkin_INCLUDE_DIRS}
+)
+
+link_directories(${orocos_kdl_LIBRARY_DIRS})
+
+
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+catkin_python_setup()
+
+if(CATKIN_ENABLE_TESTING)
+
+catkin_add_gtest(test_tomsg_frommsg test/test_tomsg_frommsg.cpp)
+target_include_directories(test_tomsg_frommsg PUBLIC ${orocos_kdl_INCLUDE_DIRS})
+target_link_libraries(test_tomsg_frommsg ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES})
+
+find_package(catkin REQUIRED COMPONENTS geometry_msgs rostest tf2_ros tf2)
+
+add_executable(test_geometry_msgs EXCLUDE_FROM_ALL test/test_tf2_geometry_msgs.cpp)
+target_include_directories(test_geometry_msgs PUBLIC ${orocos_kdl_INCLUDE_DIRS})
+target_link_libraries(test_geometry_msgs ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES})
+add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch)
+add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_python.launch)
+
+
+if(TARGET tests)
+  add_dependencies(tests test_geometry_msgs)
+endif()
+
+
+endif()
--- a/navigations/geometry2/tf2_geometry_msgs/conf.py
+++ b/navigations/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}
--- a/navigations/geometry2/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h
+++ b/navigations/geometry2/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h
--- a/navigations/geometry2/tf2_geometry_msgs/index.rst
+++ b/navigations/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`
+
--- a/navigations/geometry2/tf2_geometry_msgs/mainpage.dox
+++ b/navigations/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.
+
+*/
--- a/navigations/geometry2/tf2_geometry_msgs/package.xml
+++ b/navigations/geometry2/tf2_geometry_msgs/package.xml
@@ -0,0 +1,27 @@
+<package format="2">
+  <name>tf2_geometry_msgs</name>
+  <version>0.7.6</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>liborocos-kdl-dev</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <build_depend>python3-pykdl</build_depend>
+ 
+  <exec_depend>python3-pykdl</exec_depend>
+
+  <test_depend>ros_environment</test_depend>
+  <test_depend>rostest</test_depend>
+</package>
+
--- a/navigations/geometry2/tf2_geometry_msgs/rosdoc.yaml
+++ b/navigations/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
--- a/navigations/geometry2/tf2_geometry_msgs/scripts/test.py
+++ b/navigations/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)
--- a/navigations/geometry2/tf2_geometry_msgs/setup.py
+++ b/navigations/geometry2/tf2_geometry_msgs/setup.py
@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from setuptools import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['tf2_geometry_msgs'],
+    package_dir={'': 'src'},
+    requires=['rospy','geometry_msgs','tf2_ros','orocos_kdl']
+)
+
+setup(**d)
+
--- a/navigations/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/init.py
+++ b/navigations/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/init.py
@@ -0,0 +1 @@
+from .tf2_geometry_msgs import *
--- a/navigations/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py
+++ b/navigations/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)
--- a/navigations/geometry2/tf2_geometry_msgs/test/test.launch
+++ b/navigations/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>
--- a/navigations/geometry2/tf2_geometry_msgs/test/test_python.launch
+++ b/navigations/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>
--- a/navigations/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
+++ b/navigations/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;
+}
+
+
+
+
+
--- a/navigations/geometry2/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp
+++ b/navigations/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;
+}