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_ros/CHANGELOG.rst
+++ b/navigations/geometry2/tf2_ros/CHANGELOG.rst
@@ -0,0 +1,371 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package tf2_ros
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.7.6 (2022-10-11)
+------------------
+* tf2_ros polling interval proportional to timeout (`#492 <https://github.com/ros/geometry2/issues/492>`_)
+  * polling interval proportional to timeout
+  * CAN_TRANSFORM_POLLING_SCALE as global
+  * add DEFAULT_CAN_TRANSFORM_POLLING_SCALE
+* Removed print statements from buffer interface (`#530 <https://github.com/ros/geometry2/issues/530>`_)
+* Switch to new boost/bind/bind.hpp (`#528 <https://github.com/ros/geometry2/issues/528>`_)
+* Updating the documentation to reflect current constructor for a MessageFilter (`#527 <https://github.com/ros/geometry2/issues/527>`_)
+* (tf2_ros) Docs working on python 3 (`#521 <https://github.com/ros/geometry2/issues/521>`_)
+* Mitigate flakey test in tf2_ros (`#490 <https://github.com/ros/geometry2/issues/490>`_)
+* Contributors: Atsushi Watanabe, Janno Lunenburg, Jochen Sprickerhof, Matthijs van der Burgh, Shih-Wei Guo, Tassos Natsakis
+
+0.7.5 (2020-09-01)
+------------------
+
+0.7.4 (2020-09-01)
+------------------
+
+0.7.3 (2020-08-25)
+------------------
+* Use correct frame service name in docstrings. (`#476 <https://github.com/ros/geometry2/issues/476>`_)
+  Replaces the deprecated names
+  {tf_frames, view_frames} -> tf2_frames
+* Cherry-picking various commits from Melodic (`#471 <https://github.com/ros/geometry2/issues/471>`_)
+  * Revert "rework Eigen functions namespace hack" (`#436 <https://github.com/ros/geometry2/issues/436>`_)
+  * Fixed warnings in message_filter.h (`#434 <https://github.com/ros/geometry2/issues/434>`_)
+  the variables are not used in function body and caused -Wunused-parameter to trigger with -Wall
+  * Fix ambiguous call for tf2::convert on MSVC (`#444 <https://github.com/ros/geometry2/issues/444>`_)
+  * rework ambiguous call on MSVC.
+* Contributors: Michael Grupp, Robert Haschke
+
+0.7.2 (2020-06-08)
+------------------
+
+0.7.1 (2020-05-13)
+------------------
+* StatisTransformBroadcaster: simplify/modernize code
+* [noetic] cherry-pick Windows fixes from melodic-devel (`#450 <https://github.com/ros/geometry2/issues/450>`_)
+  * [Windows][melodic-devel] Fix install locations (`#442 <https://github.com/ros/geometry2/issues/442>`_)
+  * fixed install locations of tf2
+  * [windows][melodic] more portable fixes. (`#443 <https://github.com/ros/geometry2/issues/443>`_)
+  * more portable fixes.
+* import setup from setuptools instead of distutils-core (`#449 <https://github.com/ros/geometry2/issues/449>`_)
+* Contributors: Alejandro Hernández Cordero, Robert Haschke, Sean Yen
+
+0.7.0 (2020-03-09)
+------------------
+* Bump CMake version to avoid CMP0048 warning (`#445 <https://github.com/ros/geometry2/issues/445>`_)
+* Add arguments to TransformListener constructors that accept TransportHints for the tf topic subscriber (`#438 <https://github.com/ros/geometry2/issues/438>`_)
+* Merge pull request `#404 <https://github.com/ros/geometry2/issues/404>`_ from otamachan/remove-load-manifest
+  Remove roslib.load_manifest
+* Merge pull request `#402 <https://github.com/ros/geometry2/issues/402>`_ from rhaschke/fix-message-filter
+  Fix message filter
+* resolve virtual function call in destructor
+* remove pending callbacks in clear()
+* Merge pull request `#372 <https://github.com/ros/geometry2/issues/372>`_ from lucasw/patch-1
+  spelling fix: seperate -> separate
+* Merge pull request `#369 <https://github.com/ros/geometry2/issues/369>`_ from magazino/fix-dangling-reference
+* Fix dangling iterator references in buffer_server.cpp
+* Remove some useless code from buffer_server_main.cpp (`#368 <https://github.com/ros/geometry2/issues/368>`_)
+* Mark check_frequency as deprecated in docstring.
+* Follow `#337 <https://github.com/ros/geometry2/issues/337>`_: use actionlib API in BufferClient::processGoal()
+* Test for equality to None with 'is' instead of '==' (`#355 <https://github.com/ros/geometry2/issues/355>`_)
+* added parameter to advertise tf2-frames as a service, if needed
+* Contributors: Daniel Ingram, Emre Sahin, JonasTietz, Lucas Walter, Michael Grupp, Robert Haschke, Shane Loretz, Tamaki Nishino, Tully Foote, toliver
+
+0.6.5 (2018-11-16)
+------------------
+* Protect the time reset logic from a race condition.
+  Fixes `#341 <https://github.com/ros/geometry2/issues/341>`_
+  This could incorrectly trigger a buffer clear if two concurrent callbacks were invoked.
+* Contributors: Tully Foote
+
+0.6.4 (2018-11-06)
+------------------
+* fix(buffer-client): Use actionlib api for obtaining result
+  Use the API provided by actionlib for waiting for result. This will improve the response time and prevent problems with custom solutions (see `#178 <https://github.com/ros/geometry2/issues/178>`_). This change makes constructor parameter check_frequency obsolute and deprecates it.
+* Add check to buffer_client.py to make sure result is available
+  Related issue: `#178 <https://github.com/ros/geometry2/issues/178>`_
+* Add check to reset buffer when rostime goes backwards
+* Fixed the value of expected_success_count\_
+* Added a tf2_ros message filter unittest with multiple target frames and non-zero time tolerance
+* Contributors: Ewoud Pool, Jørgen Borgesen, Stephen Williams
+
+0.6.3 (2018-07-09)
+------------------
+
+0.6.2 (2018-05-02)
+------------------
+* update buffer_server_name (`#296 <https://github.com/ros/geometry2/issues/296>`_)
+  * use nodename as namespace
+  * Update `#209 <https://github.com/ros/geometry2/issues/209>`_ to provide backwards compatibility.
+* Contributors: Jihoon Lee, Tully Foote
+
+0.6.1 (2018-03-21)
+------------------
+
+0.6.0 (2018-03-21)
+------------------
+* tf2_ros::Buffer: canTransform can now deal with timeouts smaller than 10ms by using the hunderdth of the timeout for sleeping (`#286 <https://github.com/ros/geometry2/issues/286>`_)
+* More spinning to make sure the message gets through for `#129 <https://github.com/ros/geometry2/issues/129>`_ `#283 <https://github.com/ros/geometry2/issues/283>`_
+* Contributors: Tully Foote, cwecht
+
+0.5.17 (2018-01-01)
+-------------------
+* Merge pull request `#260 <https://github.com/ros/geometry2/issues/260>`_ from randoms/indigo-devel
+  fix python3 import error
+* 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, randoms
+
+0.5.16 (2017-07-14)
+-------------------
+* 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
+* Clear error string if it exists from the external entry points.
+  Fixes `#117 <https://github.com/ros/geometry2/issues/117>`_
+* Make buff_size and tcp_nodelay and subscriber queue size mutable.
+* Remove generate_rand_vectors() from a number of tests. (`#227 <https://github.com/ros/geometry2/issues/227>`_)
+  * Remove generate_rand_vectors() from a number of tests.
+* Log jump duration on backwards time jump detection. (`#234 <https://github.com/ros/geometry2/issues/234>`_)
+* replaced dependencies on tf2_msgs_gencpp by exported dependencies
+* Use new-style objects in python 2
+* Solve a bug that causes a deadlock in MessageFilter
+* Contributors: Adel Fakih, Chris Lalancette, Christopher Wecht, Eric Wieser, Koji Terada, Stephan, Tully Foote, koji_terada
+
+0.5.15 (2017-01-24)
+-------------------
+* tf2_ros: add option to unregister TransformListener (`#201 <https://github.com/ros/geometry2/issues/201>`_)
+* Contributors: Hans-Joachim Krauch
+
+0.5.14 (2017-01-16)
+-------------------
+* Drop roslib.load_manifest (`#191 <https://github.com/ros/geometry2/issues/191>`_)
+* Adds ability to load TF from the ROS parameter server.
+* Code linting & reorganization
+* Fix indexing beyond end of array
+* added a static transform broadcaster in python
+* lots more documentation
+* remove BufferCore doc, add BufferClient/BufferServer doc for C++, add Buffer/BufferInterface Python documentation
+* Better overview for Python
+* Contributors: Eric Wieser, Felix Duvallet, Jackie Kay, Mikael Arguedas, Mike Purvis
+
+0.5.13 (2016-03-04)
+-------------------
+* fix documentation warnings
+* Adding tests to package
+* Contributors: Laurent GEORGE, Vincent Rabaud
+
+0.5.12 (2015-08-05)
+-------------------
+* remove annoying gcc warning
+  This is because the roslog macro cannot have two arguments that are
+  formatting strings: we need to concatenate them first.
+* break canTransform loop only for non-tiny negative time deltas
+  (At least) with Python 2 ros.Time.now() is not necessarily monotonic
+  and one can experience negative time deltas (usually well below 1s)
+  on real hardware under full load. This check was originally introduced
+  to allow for backjumps with rosbag replays, and only there it makes sense.
+  So we'll add a small duration threshold to ignore backjumps due to
+  non-monotonic clocks.
+* Contributors: Vincent Rabaud, v4hn
+
+0.5.11 (2015-04-22)
+-------------------
+* do not short circuit waitForTransform timeout when running inside pytf. Fixes `#102 <https://github.com/ros/geometry_experimental/issues/102>`_
+  roscpp is not initialized inside pytf which means that ros::ok is not
+  valid. This was causing the timer to abort immediately.
+  This breaks support for pytf with respect to early breaking out of a loop re `#26 <https://github.com/ros/geometry_experimental/issues/26>`_.
+  This is conceptually broken in pytf, and is fixed in tf2_ros python implementation.
+  If you want this behavior I recommend switching to the tf2 python bindings.
+* inject timeout information into error string for canTransform with timeout
+* Contributors: Tully Foote
+
+0.5.10 (2015-04-21)
+-------------------
+* switch to use a shared lock with upgrade instead of only a unique lock. For `#91 <https://github.com/ros/geometry_experimental/issues/91>`_
+* Update message_filter.h
+* filters: fix unsupported old messages with frame_id starting with '/'
+* Enabled tf2 documentation
+* make sure the messages get processed before testing the effects. Fixes `#88 <https://github.com/ros/geometry_experimental/issues/88>`_
+* allowing to use message filters with PCL types
+* Contributors: Brice Rebsamen, Jackie Kay, Tully Foote, Vincent Rabaud, jmtatsch
+
+0.5.9 (2015-03-25)
+------------------
+* changed queue_size in Python transform boradcaster to match that in c++
+* Contributors: mrath
+
+0.5.8 (2015-03-17)
+------------------
+* fix deadlock `#79 <https://github.com/ros/geometry_experimental/issues/79>`_
+* break out of loop if ros is shutdown. Fixes `#26 <https://github.com/ros/geometry_experimental/issues/26>`_
+* remove useless Makefile files
+* Fix static broadcaster with rpy args
+* Contributors: Paul Bovbel, Tully Foote, Vincent Rabaud
+
+0.5.7 (2014-12-23)
+------------------
+* Added 6 param transform again
+  Yes, using Euler angles is a bad habit. But it is much more convenient if you just need a rotation by 90° somewhere to set it up in Euler angles. So I added the option to supply only the 3 angles.
+* Remove tf2_py dependency for Android
+* Contributors: Achim Königs, Gary Servin
+
+0.5.6 (2014-09-18)
+------------------
+* support if canTransform(...): in python `#57 <https://github.com/ros/geometry_experimental/issues/57>`_
+* Support clearing the cache when time jumps backwards `#68 <https://github.com/ros/geometry_experimental/issues/68>`_
+* Contributors: Tully Foote
+
+0.5.5 (2014-06-23)
+------------------
+
+0.5.4 (2014-05-07)
+------------------
+* surpressing autostart on the server objects to not incur warnings
+* switch to boost signals2 following `ros/ros_comm#267 <https://github.com/ros/ros_comm/issues/267>`_, blocking `ros/geometry#23 <https://github.com/ros/geometry/issues/23>`_
+* fix compilation with gcc 4.9
+* make can_transform correctly wait
+* explicitly set the publish queue size for rospy
+* Contributors: Tully Foote, Vincent Rabaud, v4hn
+
+0.5.3 (2014-02-21)
+------------------
+
+0.5.2 (2014-02-20)
+------------------
+
+0.5.1 (2014-02-14)
+------------------
+* adding const to MessageEvent data
+* Contributors: Tully Foote
+
+0.5.0 (2014-02-14)
+------------------
+* TF2 uses message events to get connection header information
+* Contributors: Kevin Watts
+
+0.4.10 (2013-12-26)
+-------------------
+* adding support for static transforms in python listener. Fixes `#46 <https://github.com/ros/geometry_experimental/issues/46>`_
+* Contributors: Tully Foote
+
+0.4.9 (2013-11-06)
+------------------
+
+0.4.8 (2013-11-06)
+------------------
+* fixing pytf failing to sleep https://github.com/ros/geometry/issues/30
+* moving python documentation to tf2_ros from tf2 to follow the code
+* Fixed static_transform_publisher duplicate check, added rostest.
+
+0.4.7 (2013-08-28)
+------------------
+* fixing new conditional to cover the case that time has not progressed yet port forward of `ros/geometry#35 <https://github.com/ros/geometry/issues/35>`_ in the python implementation
+* fixing new conditional to cover the case that time has not progressed yet port forward of `ros/geometry#35 <https://github.com/ros/geometry/issues/35>`_
+
+0.4.6 (2013-08-28)
+------------------
+* patching python implementation for `#24 <https://github.com/ros/geometry_experimental/issues/24>`_ as well
+* Stop waiting if time jumps backwards.  fixes `#24 <https://github.com/ros/geometry_experimental/issues/24>`_
+* patch to work around uninitiaized time. `#30 https://github.com/ros/geometry/issues/30`_
+* Removing unnecessary CATKIN_DEPENDS  `#18 <https://github.com/ros/geometry_experimental/issues/18>`_
+
+0.4.5 (2013-07-11)
+------------------
+* Revert "cherrypicking groovy patch for `#10 <https://github.com/ros/geometry_experimental/issues/10>`_ into hydro"
+  This reverts commit 296d4916706d64f719b8c1592ab60d3686f994e1.
+  It was not starting up correctly.
+* fixing usage string to show quaternions and using quaternions in the test app
+* cherrypicking groovy patch for `#10 <https://github.com/ros/geometry_experimental/issues/10>`_ into hydro
+
+0.4.4 (2013-07-09)
+------------------
+* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change.
+* reviving unrun unittest and adding CATKIN_ENABLE_TESTING guards
+
+0.4.3 (2013-07-05)
+------------------
+
+0.4.2 (2013-07-05)
+------------------
+
+0.4.1 (2013-07-05)
+------------------
+* adding queue accessors lost in the new API
+* exposing dedicated thread logic in BufferCore and checking in Buffer
+* adding methods to enable backwards compatability for passing through to tf::Transformer
+
+0.4.0 (2013-06-27)
+------------------
+* splitting rospy dependency into tf2_py so tf2 is pure c++ library.
+* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2
+* Cleaning up unnecessary dependency on roscpp
+* converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace
+* fixing return by value for tranform method without preallocatoin
+* Cleaning up packaging of tf2 including:
+  removing unused nodehandle
+  cleaning up a few dependencies and linking
+  removing old backup of package.xml
+  making diff minimally different from tf version of library
+* Restoring test packages and bullet packages.
+  reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented
+  reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 <https://github.com/ros/geometry_experimental/issues/7>`_
+* Added link against catkin_LIBRARIES for tf2_ros lib, also CMakeLists.txt clean up
+
+0.3.6 (2013-03-03)
+------------------
+
+0.3.5 (2013-02-15 14:46)
+------------------------
+* 0.3.4 -> 0.3.5
+
+0.3.4 (2013-02-15 13:14)
+------------------------
+* 0.3.3 -> 0.3.4
+
+0.3.3 (2013-02-15 11:30)
+------------------------
+* 0.3.2 -> 0.3.3
+
+0.3.2 (2013-02-15 00:42)
+------------------------
+* 0.3.1 -> 0.3.2
+
+0.3.1 (2013-02-14)
+------------------
+* 0.3.0 -> 0.3.1
+
+0.3.0 (2013-02-13)
+------------------
+* switching to version 0.3.0
+* Merge pull request `#2 <https://github.com/ros/geometry_experimental/issues/2>`_ from KaijenHsiao/groovy-devel
+  added setup.py and catkin_python_setup() to tf2_ros
+* added setup.py and catkin_python_setup() to tf2_ros
+* fixing cmake target collisions
+* fixing catkin message dependencies
+* removing packages with missing deps
+* catkin fixes
+* catkinizing geometry-experimental
+* catkinizing tf2_ros
+* catching None result in buffer client before it becomes an AttributeError, raising tf2.TransformException instead
+* oneiric linker fixes, bump version to 0.2.3
+* fix deprecated use of Header
+* merged faust's changes 864 and 865 into non_optimized branch: BufferCore instead of Buffer in TransformListener, and added a constructor that takes a NodeHandle.
+* add buffer server binary
+* fix compilation on 32bit
+* add missing file
+* build buffer server
+* TransformListener only needs a BufferCore
+* Add TransformListener constructor that takes a NodeHandle so you can specify a callback queue to use
+* Add option to use a callback queue in the message filter
+* move the message filter to tf2_ros
+* add missing std_msgs dependency
+* missed 2 lines in last commit
+* removing auto clearing from listener for it's unexpected from a library
+* static transform tested and working
+* subscriptions to tf_static unshelved
+* static transform publisher executable running
+* latching static transform publisher
+* cleaning out old commented code
+* Only query rospy.Time.now() when the timeout is greater than 0
+* debug comments removed
+* move to tf2_ros completed. tests pass again
+* merge tf2_cpp and tf2_py into tf2_ros
--- a/navigations/geometry2/tf2_ros/CMakeLists.txt
+++ b/navigations/geometry2/tf2_ros/CMakeLists.txt
@@ -0,0 +1,153 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(tf2_ros)
+
+if(NOT ANDROID)
+set(TF2_PY tf2_py)
+endif()
+
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  geometry_msgs
+  message_filters
+  roscpp
+  rosgraph
+  rospy
+  tf2
+  tf2_msgs
+  ${TF2_PY}
+)
+find_package(Boost REQUIRED COMPONENTS thread)
+
+catkin_python_setup()
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS
+  actionlib
+  actionlib_msgs
+  geometry_msgs
+  message_filters
+  roscpp
+  rosgraph
+  tf2
+  tf2_msgs
+  ${TF2_PY}
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+# tf2_ros library
+add_library(${PROJECT_NAME}
+  src/buffer.cpp
+  src/transform_listener.cpp
+  src/buffer_client.cpp
+  src/buffer_server.cpp
+  src/transform_broadcaster.cpp
+  src/static_transform_broadcaster.cpp
+)
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# buffer_server executable
+add_executable(${PROJECT_NAME}_buffer_server src/buffer_server_main.cpp)
+add_dependencies(${PROJECT_NAME}_buffer_server ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_buffer_server
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+set_target_properties(${PROJECT_NAME}_buffer_server
+  PROPERTIES OUTPUT_NAME buffer_server
+)
+
+# static_transform_publisher
+add_executable(${PROJECT_NAME}_static_transform_publisher
+  src/static_transform_broadcaster_program.cpp
+)
+add_dependencies(${PROJECT_NAME}_static_transform_publisher ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_static_transform_publisher
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+set_target_properties(${PROJECT_NAME}_static_transform_publisher
+  PROPERTIES OUTPUT_NAME static_transform_publisher
+)
+
+# Install library
+install(TARGETS
+  ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+# Install executable
+install(TARGETS
+  ${PROJECT_NAME}_buffer_server ${PROJECT_NAME}_static_transform_publisher
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+
+# Tests
+if(CATKIN_ENABLE_TESTING)
+
+# new requirements for testing
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  geometry_msgs
+  message_filters
+  roscpp
+  rosgraph
+  rospy
+  rostest
+  tf2
+  tf2_msgs
+  ${TF2_PY}
+)
+
+
+
+# tf2_ros_test_listener
+add_executable(${PROJECT_NAME}_test_listener EXCLUDE_FROM_ALL test/listener_unittest.cpp)
+add_dependencies(${PROJECT_NAME}_test_listener ${catkin_EXPORTED_TARGETS})
+add_executable(${PROJECT_NAME}_test_time_reset EXCLUDE_FROM_ALL test/time_reset_test.cpp)
+add_dependencies(${PROJECT_NAME}_test_time_reset ${catkin_EXPORTED_TARGETS})
+add_executable(${PROJECT_NAME}_test_message_filter EXCLUDE_FROM_ALL test/message_filter_test.cpp)
+add_dependencies(${PROJECT_NAME}_test_message_filter ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME}_test_listener
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${GTEST_LIBRARIES}
+)
+
+target_link_libraries(${PROJECT_NAME}_test_time_reset
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${GTEST_LIBRARIES}
+)
+
+target_link_libraries(${PROJECT_NAME}_test_message_filter
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${GTEST_LIBRARIES}
+)
+
+add_dependencies(tests ${PROJECT_NAME}_test_listener)
+add_dependencies(tests ${PROJECT_NAME}_test_time_reset)
+add_dependencies(tests ${PROJECT_NAME}_test_message_filter)
+
+add_rostest(test/transform_listener_unittest.launch)
+add_rostest(test/transform_listener_time_reset_test.launch)
+add_rostest(test/message_filter_test.launch)
+
+endif()
--- a/navigations/geometry2/tf2_ros/doc/conf.py
+++ b/navigations/geometry2/tf2_ros/doc/conf.py
@@ -0,0 +1,204 @@
+# -*- coding: utf-8 -*-
+#
+# tf documentation build configuration file, created by
+# sphinx-quickstart on Mon Jun  1 14:21:53 2009.
+#
+# This file is execfile()d with the current directory set to its containing dir.
+#
+# Note that not all possible configuration values are present in this
+# autogenerated file.
+#
+# All configuration values have a default; values that are commented out
+# serve to show the default.
+
+import sys, os
+
+# If extensions (or modules to document with autodoc) are in another directory,
+# add these directories to sys.path here. If the directory is relative to the
+# documentation root, use os.path.abspath to make it absolute, like shown here.
+#sys.path.append(os.path.abspath('.'))
+
+# -- General configuration -----------------------------------------------------
+
+# Add any Sphinx extension module names here, as strings. They can be extensions
+# coming with Sphinx (named 'sphinx.ext.*') or your custom ones.
+extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.imgmath']
+
+# Add any paths that contain templates here, relative to this directory.
+templates_path = ['_templates']
+
+# The suffix of source filenames.
+source_suffix = '.rst'
+
+# The encoding of source files.
+#source_encoding = 'utf-8'
+
+# The master toctree document.
+master_doc = 'index'
+
+# General information about the project.
+project = u'tf'
+copyright = u'2009, Willow Garage, Inc.'
+
+# The version info for the project you're documenting, acts as replacement for
+# |version| and |release|, also used in various other places throughout the
+# built documents.
+#
+# The short X.Y version.
+version = '0.1'
+# The full version, including alpha/beta/rc tags.
+release = '0.1.0'
+
+# The language for content autogenerated by Sphinx. Refer to documentation
+# for a list of supported languages.
+#language = None
+
+# There are two options for replacing |today|: either, you set today to some
+# non-false value, then it is used:
+#today = ''
+# Else, today_fmt is used as the format for a strftime call.
+#today_fmt = '%B %d, %Y'
+
+# List of documents that shouldn't be included in the build.
+#unused_docs = []
+
+# List of directories, relative to source directory, that shouldn't be searched
+# for source files.
+exclude_trees = ['_build']
+
+# The reST default role (used for this markup: `text`) to use for all documents.
+#default_role = None
+
+# If true, '()' will be appended to :func: etc. cross-reference text.
+#add_function_parentheses = True
+
+# If true, the current module name will be prepended to all description
+# unit titles (such as .. function::).
+#add_module_names = True
+
+# If true, sectionauthor and moduleauthor directives will be shown in the
+# output. They are ignored by default.
+#show_authors = False
+
+# The name of the Pygments (syntax highlighting) style to use.
+pygments_style = 'sphinx'
+
+# A list of ignored prefixes for module index sorting.
+#modindex_common_prefix = []
+
+
+# -- Options for HTML output ---------------------------------------------------
+
+# The theme to use for HTML and HTML Help pages.  Major themes that come with
+# Sphinx are currently 'default' and 'sphinxdoc'.
+html_theme = 'default'
+
+# Theme options are theme-specific and customize the look and feel of a theme
+# further.  For a list of options available for each theme, see the
+# documentation.
+#html_theme_options = {}
+
+# Add any paths that contain custom themes here, relative to this directory.
+#html_theme_path = []
+
+# The name for this set of Sphinx documents.  If None, it defaults to
+# "<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
+    }
+
+autoclass_content = "both"
--- a/navigations/geometry2/tf2_ros/doc/index.rst
+++ b/navigations/geometry2/tf2_ros/doc/index.rst
@@ -0,0 +1,48 @@
+tf2_ros Overview
+================
+
+This is the Python API reference for the tf2_ros package.
+
+To broadcast transforms using ROS:
+- Call :meth:`rospy.init` to initialize a node.
+- Construct a :class:`tf2_ros.TransformBroadcaster`.
+- Pass a :class:`geometry_msgs.TransformStamped` message to :meth:`tf2_ros.TransformBroadcaster.sendTransform`.
+
+    - Alternatively, pass a vector of :class:`geometry_msgs.TransformStamped` messages.
+
+To listen for transforms using ROS:
+- Construct an instance of a class that implements :class:`tf2_ros.BufferInterface`.
+
+    - :class:`tf2_ros.Buffer` is the standard implementation which offers a tf2_frames service that can respond to requests with a :class:`tf2_msgs.FrameGraph`.
+    - :class:`tf2_ros.BufferClient` uses an :class:`actionlib.SimpleActionClient` to wait for the requested transform to become available.
+
+- Pass the :class:`tf2_ros.Buffer` to the constructor of :class:`tf2_ros.TransformListener`.
+    - Optionally, pass a :class:`ros.NodeHandle` (otherwise TransformListener will connect to the node for the process).
+    - Optionally, specify if the TransformListener runs in its own thread or not.
+
+- Use :meth:`tf2_ros.BufferInterface.transform` to apply a transform on the tf server to an input frame.
+    - Or, check if a transform is available with :meth:`tf2_ros.BufferInterface.can_transform`.
+    - Then, call :meth:`tf2_ros.BufferInterface.lookup_transform` to get the transform between two frames.
+
+For more information, see the tf2 tutorials: http://wiki.ros.org/tf2/Tutorials
+
+Or, get an `overview`_ of data type conversion methods in geometry_experimental packages.
+
+See http://wiki.ros.org/tf2/Tutorials for more detailed usage.
+
+.. _overview: http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions
+
+Classes and Exceptions
+======================
+
+.. toctree::
+    :maxdepth: 2
+
+    tf2_ros
+
+
+Indices and tables
+==================
+
+* :ref:`genindex`
+* :ref:`search`
--- a/navigations/geometry2/tf2_ros/doc/mainpage.dox
+++ b/navigations/geometry2/tf2_ros/doc/mainpage.dox
@@ -0,0 +1,41 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b tf2_ros is the C++ ROS wrapper around the tf2 transform library.
+
+\section codeapi Code API
+
+To broadcast transforms using ROS:
+- Call ros::init() to initialize a node.
+- Construct a tf2_ros::TransformBroadcaster.
+- Pass a geometry_msgs::TransformStamped message to tf2_ros::TransformBroadcaster::sendTransform().
+  - Alternatively, pass a vector of geometry_msgs::TransformStamped messages.
+- Use tf2_ros::StaticTransformBroadcaster for "latching" behavior when transforms that are not expected to change.
+
+To listen for transforms using ROS:
+- Construct an instance of a class that implements tf2_ros::BufferInterface.
+  - tf2_ros::Buffer is the standard implementation which offers a tf2_frames service that can respond to requests with a tf2_msgs::FrameGraph.
+  - tf2_ros::BufferClient uses an actionlib::SimpleActionClient to wait for the requested transform to become available.
+    - It should be used with a tf2_ros::BufferServer, which offers the corresponding actionlib::ActionSErver.
+- Pass the tf2_ros::Buffer to the constructor of tf2_ros::TransformListener.
+  - Optionally, pass a ros::NodeHandle (otherwise TransformListener will connect to the node for the process).
+  - Optionally, specify if the TransformListener runs in its own thread or not.
+- Use tf2_ros::BufferInterface::transform() to apply a transform on the tf server to an input frame.
+  - Or, check if a transform is available with tf2_ros::BufferInterface::canTransform().
+  - Then, call tf2_ros::BufferInterface::lookupTransform() to get the transform between two frames.
+- Construct a tf2_ros::MessageFilter with the TransformListener to apply a transformation to incoming frames.
+  - This is especially useful when streaming sensor data.
+
+List of exceptions thrown in this library:
+- tf2::LookupException
+- tf2::ConnectivityException
+- tf2::ExtrapolationException
+- tf2::InvalidArgumentException
+- tf2::TimeoutException
+- tf2::TransformException
+
+For more information, see the tf2 tutorials: http://wiki.ros.org/tf2/Tutorials
+
+Or, get an <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">overview</A> of data type conversion methods in geometry_experimental packages.
+*/
--- a/navigations/geometry2/tf2_ros/doc/tf2_ros.rst
+++ b/navigations/geometry2/tf2_ros/doc/tf2_ros.rst
@@ -0,0 +1,73 @@
+tf_ros2 Python API
+==================
+
+Exceptions
+----------
+
+.. exception:: tf2.TransformException
+
+    base class for tf exceptions.  Because :exc:`tf2.TransformException` is the
+    base class for other exceptions, you can catch all tf exceptions
+    by writing::
+
+        try:
+            # do some tf2 work
+        except tf2.TransformException:
+            print "some tf2 exception happened"
+        
+
+.. exception:: tf2.ConnectivityException
+
+   subclass of :exc:`TransformException`.
+   Raised when that the fixed_frame tree is not connected between the frames requested.
+
+.. exception:: tf2.LookupException
+
+   subclass of :exc:`TransformException`.
+   Raised when a tf method has attempted to access a frame, but
+   the frame is not in the graph.
+   The most common reason for this is that the frame is not
+   being published, or a parent frame was not set correctly 
+   causing the tree to be broken.  
+
+.. exception:: tf2.ExtrapolationException
+
+   subclass of :exc:`TransformException`
+   Raised when a tf method would have required extrapolation beyond current limits.
+
+
+.. exception:: tf2.InvalidArgumentException
+
+   subclass of :exc:`TransformException`.
+   Raised when the arguments to the method are called improperly formed.  An example of why this might be raised is if an argument is nan. 
+
+.. autoexception:: tf2_ros.buffer_interface.TypeException
+
+.. autoexception:: tf2_ros.buffer_interface.NotImplementedException
+
+
+BufferInterface
+---------------
+.. autoclass:: tf2_ros.buffer_interface.BufferInterface
+    :members:
+
+Buffer
+------
+.. autoclass:: tf2_ros.buffer.Buffer
+    :members:
+
+BufferClient
+------------
+.. autoclass:: tf2_ros.buffer_client.BufferClient
+    :members:
+
+
+TransformBroadcaster
+--------------------
+.. autoclass:: tf2_ros.transform_broadcaster.TransformBroadcaster
+    :members:
+
+TransformListener
+-----------------
+.. autoclass:: tf2_ros.transform_listener.TransformListener
+    :members:
--- a/navigations/geometry2/tf2_ros/include/tf2_ros/buffer.h
+++ b/navigations/geometry2/tf2_ros/include/tf2_ros/buffer.h
@@ -0,0 +1,144 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Wim Meeussen */
+
+#ifndef TF2_ROS_BUFFER_H
+#define TF2_ROS_BUFFER_H
+
+#include <tf2_ros/buffer_interface.h>
+#include <tf2/buffer_core.h>
+#include <tf2_msgs/FrameGraph.h>
+#include <ros/ros.h>
+#include <tf2/convert.h>
+
+
+namespace tf2_ros
+{
+
+  /** \brief Standard implementation of the tf2_ros::BufferInterface abstract data type.
+   *
+   * Inherits tf2_ros::BufferInterface and tf2::BufferCore.
+   * Stores known frames and optionally offers a ROS service, "tf2_frames", which responds to client requests
+   * with a response containing a tf2_msgs::FrameGraph representing the relationship of known frames.
+   */
+  class Buffer: public BufferInterface, public tf2::BufferCore
+  {
+  public:
+    using tf2::BufferCore::lookupTransform;
+    using tf2::BufferCore::canTransform;
+
+    /**
+     * @brief  Constructor for a Buffer object
+     * @param cache_time How long to keep a history of transforms
+     * @param debug Whether to advertise the tf2_frames service that exposes debugging information from the buffer
+     * @return 
+     */
+    Buffer(ros::Duration cache_time = ros::Duration(BufferCore::DEFAULT_CACHE_TIME), bool debug = false);
+
+    /** \brief Get the transform between two frames by frame ID.
+     * \param target_frame The frame to which data should be transformed
+     * \param source_frame The frame where the data originated
+     * \param time The time at which the value of the transform is desired. (0 will get the latest)
+     * \param timeout How long to block before failing
+     * \return The transform between the frames
+     *
+     * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
+     * tf2::ExtrapolationException, tf2::InvalidArgumentException
+     */
+    virtual geometry_msgs::TransformStamped 
+    lookupTransform(const std::string& target_frame, const std::string& source_frame,
+                    const ros::Time& time, const ros::Duration timeout) const;
+
+    /** \brief Get the transform between two frames by frame ID assuming fixed frame.
+     * \param target_frame The frame to which data should be transformed
+     * \param target_time The time to which the data should be transformed. (0 will get the latest)
+     * \param source_frame The frame where the data originated
+     * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
+     * \param fixed_frame The frame in which to assume the transform is constant in time. 
+     * \param timeout How long to block before failing
+     * \return The transform between the frames
+     *
+     * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
+     * tf2::ExtrapolationException, tf2::InvalidArgumentException
+     */
+    virtual geometry_msgs::TransformStamped 
+    lookupTransform(const std::string& target_frame, const ros::Time& target_time,
+                    const std::string& source_frame, const ros::Time& source_time,
+                    const std::string& fixed_frame, const ros::Duration timeout) const;
+
+
+    /** \brief Test if a transform is possible
+     * \param target_frame The frame into which to transform
+     * \param source_frame The frame from which to transform
+     * \param target_time The time at which to transform
+     * \param timeout How long to block before failing
+     * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
+     * \return True if the transform is possible, false otherwise 
+     */
+    virtual bool
+    canTransform(const std::string& target_frame, const std::string& source_frame, 
+                 const ros::Time& target_time, const ros::Duration timeout, std::string* errstr = NULL) const;
+    
+    /** \brief Test if a transform is possible
+     * \param target_frame The frame into which to transform
+     * \param target_time The time into which to transform
+     * \param source_frame The frame from which to transform
+     * \param source_time The time from which to transform
+     * \param fixed_frame The frame in which to treat the transform as constant in time
+     * \param timeout How long to block before failing
+     * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
+     * \return True if the transform is possible, false otherwise 
+     */
+    virtual bool
+      canTransform(const std::string& target_frame, const ros::Time& target_time,
+                   const std::string& source_frame, const ros::Time& source_time,
+                   const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr = NULL) const;
+
+
+    
+    
+  private:
+    bool getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res) ;
+
+
+    // conditionally error if dedicated_thread unset.
+    bool checkAndErrorDedicatedThreadPresent(std::string* errstr) const;
+
+    ros::ServiceServer frames_server_;
+
+
+  }; // class 
+
+static const std::string threading_error = "Do not call canTransform or lookupTransform with a timeout unless you are using another thread for populating data. Without a dedicated thread it will always timeout.  If you have a separate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance.";
+
+  
+} // namespace
+
+#endif // TF2_ROS_BUFFER_H
--- a/navigations/geometry2/tf2_ros/include/tf2_ros/buffer_client.h
+++ b/navigations/geometry2/tf2_ros/include/tf2_ros/buffer_client.h
@@ -0,0 +1,139 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef TF2_ROS_BUFFER_CLIENT_H_
+#define TF2_ROS_BUFFER_CLIENT_H_
+
+#include <tf2_ros/buffer_interface.h>
+#include <actionlib/client/simple_action_client.h>
+#include <tf2_msgs/LookupTransformAction.h>
+
+namespace tf2_ros
+{
+  /** \brief Action client-based implementation of the tf2_ros::BufferInterface abstract data type.
+   *
+   * BufferClient uses actionlib to coordinate waiting for available transforms.
+   *
+   * You can use this class with a tf2_ros::BufferServer and tf2_ros::TransformListener in a separate process.
+   */
+  class BufferClient : public BufferInterface
+  {
+    public:
+      typedef actionlib::SimpleActionClient<tf2_msgs::LookupTransformAction> LookupActionClient;
+
+      /** \brief BufferClient constructor
+       * \param ns The namespace in which to look for a BufferServer
+       * \param check_frequency Deprecated, not used anymore
+       * \param timeout_padding The amount of time to allow passed the desired timeout on the client side for communication lag
+       */
+      BufferClient(std::string ns, double check_frequency = 10.0, ros::Duration timeout_padding = ros::Duration(2.0));
+
+      /** \brief Get the transform between two frames by frame ID.
+       * \param target_frame The frame to which data should be transformed
+       * \param source_frame The frame where the data originated
+       * \param time The time at which the value of the transform is desired. (0 will get the latest)
+       * \param timeout How long to block before failing
+       * \return The transform between the frames
+       *
+       * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
+       * tf2::ExtrapolationException, tf2::InvalidArgumentException
+       */
+      virtual geometry_msgs::TransformStamped
+        lookupTransform(const std::string& target_frame, const std::string& source_frame,
+            const ros::Time& time, const ros::Duration timeout = ros::Duration(0.0)) const;
+
+      /** \brief Get the transform between two frames by frame ID assuming fixed frame.
+       * \param target_frame The frame to which data should be transformed
+       * \param target_time The time to which the data should be transformed. (0 will get the latest)
+       * \param source_frame The frame where the data originated
+       * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
+       * \param fixed_frame The frame in which to assume the transform is constant in time. 
+       * \param timeout How long to block before failing
+       * \return The transform between the frames
+       *
+       * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
+       * tf2::ExtrapolationException, tf2::InvalidArgumentException
+       */
+      virtual geometry_msgs::TransformStamped 
+        lookupTransform(const std::string& target_frame, const ros::Time& target_time,
+            const std::string& source_frame, const ros::Time& source_time,
+            const std::string& fixed_frame, const ros::Duration timeout = ros::Duration(0.0)) const;
+
+      /** \brief Test if a transform is possible
+       * \param target_frame The frame into which to transform
+       * \param source_frame The frame from which to transform
+       * \param time The time at which to transform
+       * \param timeout How long to block before failing
+       * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
+       * \return True if the transform is possible, false otherwise 
+       */
+      virtual bool
+        canTransform(const std::string& target_frame, const std::string& source_frame, 
+            const ros::Time& time, const ros::Duration timeout = ros::Duration(0.0), std::string* errstr = NULL) const;
+
+      /** \brief Test if a transform is possible
+       * \param target_frame The frame into which to transform
+       * \param target_time The time into which to transform
+       * \param source_frame The frame from which to transform
+       * \param source_time The time from which to transform
+       * \param fixed_frame The frame in which to treat the transform as constant in time
+       * \param timeout How long to block before failing
+       * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
+       * \return True if the transform is possible, false otherwise 
+       */
+      virtual bool
+        canTransform(const std::string& target_frame, const ros::Time& target_time,
+            const std::string& source_frame, const ros::Time& source_time,
+            const std::string& fixed_frame, const ros::Duration timeout = ros::Duration(0.0), std::string* errstr = NULL) const;
+
+      /** \brief Block until the action server is ready to respond to requests.
+       * \param timeout Time to wait for the server.
+       * \return True if the server is ready, false otherwise.
+       */
+      bool waitForServer(const ros::Duration& timeout = ros::Duration(0))
+      {
+        return client_.waitForServer(timeout);
+      }
+
+    private:
+      geometry_msgs::TransformStamped processGoal(const tf2_msgs::LookupTransformGoal& goal) const;
+      geometry_msgs::TransformStamped processResult(const tf2_msgs::LookupTransformResult& result) const;
+      mutable LookupActionClient client_;
+      double check_frequency_;
+      ros::Duration timeout_padding_;
+  };
+};
+#endif
--- a/navigations/geometry2/tf2_ros/include/tf2_ros/buffer_interface.h
+++ b/navigations/geometry2/tf2_ros/include/tf2_ros/buffer_interface.h
@@ -0,0 +1,267 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Wim Meeussen */
+
+#ifndef TF2_ROS_BUFFER_INTERFACE_H
+#define TF2_ROS_BUFFER_INTERFACE_H
+
+#include <tf2/buffer_core.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2/exceptions.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <sstream>
+#include <tf2/convert.h>
+
+namespace tf2_ros
+{
+
+/** \brief Abstract interface for wrapping tf2::BufferCore in a ROS-based API.
+ * Implementations include tf2_ros::Buffer and tf2_ros::BufferClient.
+ */
+class BufferInterface
+{
+public:
+
+  /** \brief Get the transform between two frames by frame ID.
+   * \param target_frame The frame to which data should be transformed
+   * \param source_frame The frame where the data originated
+   * \param time The time at which the value of the transform is desired. (0 will get the latest)
+   * \param timeout How long to block before failing
+   * \return The transform between the frames
+   *
+   * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
+   * tf2::ExtrapolationException, tf2::InvalidArgumentException
+   */
+  virtual geometry_msgs::TransformStamped
+    lookupTransform(const std::string& target_frame, const std::string& source_frame, 
+		    const ros::Time& time, const ros::Duration timeout) const = 0;
+
+  /** \brief Get the transform between two frames by frame ID assuming fixed frame.
+   * \param target_frame The frame to which data should be transformed
+   * \param target_time The time to which the data should be transformed. (0 will get the latest)
+   * \param source_frame The frame where the data originated
+   * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
+   * \param fixed_frame The frame in which to assume the transform is constant in time. 
+   * \param timeout How long to block before failing
+   * \return The transform between the frames
+   *
+   * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
+   * tf2::ExtrapolationException, tf2::InvalidArgumentException
+   */
+  virtual geometry_msgs::TransformStamped 
+    lookupTransform(const std::string& target_frame, const ros::Time& target_time,
+		    const std::string& source_frame, const ros::Time& source_time,
+		    const std::string& fixed_frame, const ros::Duration timeout) const = 0;
+
+
+  /** \brief Test if a transform is possible
+   * \param target_frame The frame into which to transform
+   * \param source_frame The frame from which to transform
+   * \param time The time at which to transform
+   * \param timeout How long to block before failing
+   * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
+   * \return True if the transform is possible, false otherwise 
+   */
+  virtual bool
+    canTransform(const std::string& target_frame, const std::string& source_frame, 
+		 const ros::Time& time, const ros::Duration timeout, std::string* errstr = NULL) const = 0;
+
+  /** \brief Test if a transform is possible
+   * \param target_frame The frame into which to transform
+   * \param target_time The time into which to transform
+   * \param source_frame The frame from which to transform
+   * \param source_time The time from which to transform
+   * \param fixed_frame The frame in which to treat the transform as constant in time
+   * \param timeout How long to block before failing
+   * \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
+   * \return True if the transform is possible, false otherwise 
+   */
+  virtual bool
+    canTransform(const std::string& target_frame, const ros::Time& target_time,
+		 const std::string& source_frame, const ros::Time& source_time,
+		 const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr = NULL) const = 0;
+
+  /** \brief Transform an input into the target frame.
+   * This function is templated and can take as input any valid mathematical object that tf knows
+   * how to apply a transform to, by way of the templated math conversions interface.
+   * For example, the template type could be a Transform, Pose, Vector, or Quaternion message
+   * type (as defined in geometry_msgs).
+   * \tparam T The type of the object to transform.
+   * \param in The object to transform
+   * \param out The transformed output, preallocated by the caller.
+   * \param target_frame The string identifer for the frame to transform into.
+   * \param timeout How long to wait for the target frame. Default value is zero (no blocking).
+   */
+  template <class T>
+    T& transform(const T& in, T& out, 
+		 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
+  {
+    // do the transform
+    tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
+    return out;
+  }
+
+  /** \brief Transform an input into the target frame.
+   * This function is templated and can take as input any valid mathematical object that tf knows
+   * how to apply a transform to, by way of the templated math conversions interface.
+   * For example, the template type could be a Transform, Pose, Vector, or Quaternion message
+   * type (as defined in geometry_msgs).
+   * \tparam T The type of the object to transform.
+   * \param in The object to transform.
+   * \param target_frame The string identifer for the frame to transform into.
+   * \param timeout How long to wait for the target frame. Default value is zero (no blocking).
+   * \return The transformed output.
+   */
+  template <class T>
+    T transform(const T& in,
+		const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
+  {
+    T out;
+    return transform(in, out, target_frame, timeout);
+  }
+
+  /** \brief Transform an input into the target frame and convert to a specified output type.
+   * It is templated on two types: the type of the input object and the type of the
+   * transformed output.
+   * For example, the template types could be Transform, Pose, Vector, or Quaternion messages
+   * type (as defined in geometry_msgs).
+   * The function will calculate the transformation and then convert the result into the
+   * specified output type.
+   * Compilation will fail if a known conversion does not exist bewteen the two template
+   * parameters.
+   * \tparam A The type of the object to transform.
+   * \tparam B The type of the transformed output.
+   * \param in The object to transform
+   * \param out The transformed output, converted to the specified type.
+   * \param target_frame The string identifer for the frame to transform into.
+   * \param timeout How long to wait for the target frame. Default value is zero (no blocking).
+   * \return The transformed output, converted to the specified type.
+   */
+  template <class A, class B>
+    B& transform(const A& in, B& out,
+        const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
+  {
+    A copy = transform(in, target_frame, timeout);
+    tf2::convert(copy, out);
+    return out;
+  }
+
+  /** \brief Transform an input into the target frame (advanced).
+   * This function is templated and can take as input any valid mathematical object that tf knows
+   * how to apply a transform to, by way of the templated math conversions interface.
+   * For example, the template type could be a Transform, Pose, Vector, or Quaternion message
+   * type (as defined in geometry_msgs).
+   * This function follows the advanced API, which allows transforming between different time
+   * points, and specifying a fixed frame that does not varying in time.
+   * \tparam T The type of the object to transform.
+   * \param in The object to transform
+   * \param out The transformed output, preallocated by the caller.
+   * \param target_frame The string identifer for the frame to transform into.
+   * \param target_time The time into which to transform
+   * \param fixed_frame The frame in which to treat the transform as constant in time.
+   * \param timeout How long to wait for the target frame. Default value is zero (no blocking).
+   */
+  template <class T>
+    T& transform(const T& in, T& out, 
+		 const std::string& target_frame, const ros::Time& target_time,
+		 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
+  {
+    // do the transform
+    tf2::doTransform(in, out, lookupTransform(target_frame, target_time, 
+                                              tf2::getFrameId(in), tf2::getTimestamp(in), 
+                                              fixed_frame, timeout));
+    return out;
+  }
+
+
+  /** \brief Transform an input into the target frame (advanced).
+   * This function is templated and can take as input any valid mathematical object that tf knows
+   * how to apply a transform to, by way of the templated math conversions interface.
+   * For example, the template type could be a Transform, Pose, Vector, or Quaternion message
+   * type (as defined in geometry_msgs).
+   * This function follows the advanced API, which allows transforming between different time
+   * points, and specifying a fixed frame that does not varying in time.
+   * \tparam T The type of the object to transform.
+   * \param in The object to transform
+   * \param target_frame The string identifer for the frame to transform into.
+   * \param target_time The time into which to transform
+   * \param fixed_frame The frame in which to treat the transform as constant in time.
+   * \param timeout How long to wait for the target frame. Default value is zero (no blocking).
+   * \return The transformed output.
+   */
+  template <class T>
+    T transform(const T& in, 
+		 const std::string& target_frame, const ros::Time& target_time,
+		 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
+  {
+    T out;
+    return transform(in, out, target_frame, target_time, fixed_frame, timeout);
+  }
+
+
+  /** \brief Transform an input into the target frame and convert to a specified output type (advanced).
+   * It is templated on two types: the type of the input object and the type of the
+   * transformed output.
+   * For example, the template type could be a Transform, Pose, Vector, or Quaternion message
+   * type (as defined in geometry_msgs).
+   * The function will calculate the transformation and then convert the result into the
+   * specified output type.
+   * Compilation will fail if a known conversion does not exist bewteen the two template
+   * parameters.
+   * This function follows the advanced API, which allows transforming between different time
+   * points, and specifying a fixed frame that does not varying in time.
+   * \tparam A The type of the object to transform.
+   * \tparam B The type of the transformed output.
+   * \param in The object to transform
+   * \param out The transformed output, converted to the specified output type.
+   * \param target_frame The string identifer for the frame to transform into.
+   * \param target_time The time into which to transform
+   * \param fixed_frame The frame in which to treat the transform as constant in time.
+   * \param timeout How long to wait for the target frame. Default value is zero (no blocking).
+   * \return The transformed output, converted to the specified output type.
+   */
+  template <class A, class B>
+    B& transform(const A& in, B& out, 
+		 const std::string& target_frame, const ros::Time& target_time,
+		 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
+  {
+    // do the transform
+    A copy = transform(in, target_frame, target_time, fixed_frame, timeout);
+    tf2::convert(copy, out);
+    return out;
+  }
+
+
+ }; // class
+
+
+} // namespace
+
+#endif // TF2_ROS_BUFFER_INTERFACE_H
--- a/navigations/geometry2/tf2_ros/include/tf2_ros/buffer_server.h
+++ b/navigations/geometry2/tf2_ros/include/tf2_ros/buffer_server.h
@@ -0,0 +1,92 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef TF2_ROS_BUFFER_SERVER_H_
+#define TF2_ROS_BUFFER_SERVER_H_
+
+#include <actionlib/server/action_server.h>
+#include <tf2_msgs/LookupTransformAction.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2_ros/buffer.h>
+
+namespace tf2_ros
+{
+  /** \brief Action server for the actionlib-based implementation of tf2_ros::BufferInterface.
+   * 
+   * Use this class with a tf2_ros::TransformListener in the same process.
+   * You can use this class with a tf2_ros::BufferClient in a different process.
+   */
+  class BufferServer
+  {
+    private:
+      typedef actionlib::ActionServer<tf2_msgs::LookupTransformAction> LookupTransformServer;
+      typedef LookupTransformServer::GoalHandle GoalHandle;
+
+      struct GoalInfo
+      {
+        GoalHandle handle;
+        ros::Time end_time;
+      };
+
+    public:
+      /** \brief Constructor
+       * \param buffer The Buffer that this BufferServer will wrap.
+       * \param ns The namespace in which to look for action clients.
+       * \param auto_start Pass argument to the constructor of the ActionServer.
+       * \param check_period How often to check for changes to known transforms (via a timer event).
+       */
+      BufferServer(const Buffer& buffer, const std::string& ns,
+          bool auto_start = true, ros::Duration check_period = ros::Duration(0.01));
+
+      /** \brief Start the action server.
+       */
+      void start();
+
+    private:
+      void goalCB(GoalHandle gh);
+      void cancelCB(GoalHandle gh);
+      void checkTransforms(const ros::TimerEvent& e);
+      bool canTransform(GoalHandle gh);
+      geometry_msgs::TransformStamped lookupTransform(GoalHandle gh);
+
+      const Buffer& buffer_;
+      LookupTransformServer server_;
+      std::list<GoalInfo> active_goals_;
+      boost::mutex mutex_;
+      ros::Timer check_timer_;
+  };
+}
+#endif
--- a/navigations/geometry2/tf2_ros/include/tf2_ros/message_filter.h
+++ b/navigations/geometry2/tf2_ros/include/tf2_ros/message_filter.h
@@ -0,0 +1,716 @@
+/*
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Josh Faust */
+
+#ifndef TF2_ROS_MESSAGE_FILTER_H
+#define TF2_ROS_MESSAGE_FILTER_H
+
+#include <tf2/buffer_core.h>
+
+#include <string>
+#include <list>
+#include <vector>
+#include <boost/function.hpp>
+#include <boost/bind/bind.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread.hpp>
+
+#include <message_filters/connection.h>
+#include <message_filters/simple_filter.h>
+
+#include <ros/node_handle.h>
+#include <ros/callback_queue_interface.h>
+#include <ros/init.h>
+
+#define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \
+  ROS_DEBUG_NAMED("message_filter", std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), getTargetFramesString().c_str(), __VA_ARGS__)
+
+#define TF2_ROS_MESSAGEFILTER_WARN(fmt, ...) \
+  ROS_WARN_NAMED("message_filter", std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), getTargetFramesString().c_str(), __VA_ARGS__)
+
+namespace tf2_ros
+{
+
+namespace filter_failure_reasons
+{
+enum FilterFailureReason
+{
+  /// The message buffer overflowed, and this message was pushed off the back of the queue, but the reason it was unable to be transformed is unknown.
+  Unknown,
+  /// The timestamp on the message is more than the cache length earlier than the newest data in the transform cache
+  OutTheBack,
+  /// The frame_id on the message is empty
+  EmptyFrameID,
+};
+}
+typedef filter_failure_reasons::FilterFailureReason FilterFailureReason;
+
+class MessageFilterBase
+{
+public:
+  typedef std::vector<std::string> V_string;
+
+  virtual ~MessageFilterBase(){}
+  virtual void clear() = 0;
+  virtual void setTargetFrame(const std::string& target_frame) = 0;
+  virtual void setTargetFrames(const V_string& target_frames) = 0;
+  virtual void setTolerance(const ros::Duration& tolerance) = 0;
+};
+
+/**
+ * \brief Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available
+ *
+ * The callbacks used in this class are of the same form as those used by roscpp's message callbacks.
+ *
+ * MessageFilter is templated on a message type.
+ *
+ * \section example_usage Example Usage
+ *
+ * If you want to hook a MessageFilter into a ROS topic:
+\verbatim
+message_filters::Subscriber<MessageType> sub(node_handle_, "topic", 10);
+tf2_ros::MessageFilter<MessageType> tf_filter(sub, tf_buffer_, "/map", 10, 0);
+tf_filter.registerCallback(&MyClass::myCallback, this);
+\endverbatim
+ */
+template<class M>
+class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter<M>
+{
+public:
+  typedef boost::shared_ptr<M const> MConstPtr;
+  typedef ros::MessageEvent<M const> MEvent;
+  typedef boost::function<void(const MConstPtr&, FilterFailureReason)> FailureCallback;
+  typedef boost::signals2::signal<void(const MConstPtr&, FilterFailureReason)> FailureSignal;
+
+  // If you hit this assert your message does not have a header, or does not have the HasHeader trait defined for it
+  // Actually, we need to check that the message has a header, or that it
+  // has the FrameId and Stamp traits. However I don't know how to do that
+  // so simply commenting out for now.
+  //ROS_STATIC_ASSERT(ros::message_traits::HasHeader<M>::value);
+
+  /**
+   * \brief Constructor
+   *
+   * \param bc The tf2::BufferCore this filter should use
+   * \param target_frame The frame this filter should attempt to transform to.  To use multiple frames, pass an empty string here and use the setTargetFrames() function.
+   * \param queue_size The number of messages to queue up before throwing away old ones.  0 means infinite (dangerous).
+   * \param nh The NodeHandle whose callback queue we should add callbacks to
+   */
+  MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh)
+  : bc_(bc)
+  , queue_size_(queue_size)
+  , callback_queue_(nh.getCallbackQueue())
+  {
+    init();
+
+    setTargetFrame(target_frame);
+  }
+
+  /**
+   * \brief Constructor
+   *
+   * \param f The filter to connect this filter's input to.  Often will be a message_filters::Subscriber.
+   * \param bc The tf2::BufferCore this filter should use
+   * \param target_frame The frame this filter should attempt to transform to.  To use multiple frames, pass an empty string here and use the setTargetFrames() function.
+   * \param queue_size The number of messages to queue up before throwing away old ones.  0 means infinite (dangerous).
+   * \param nh The NodeHandle whose callback queue we should add callbacks to
+   */
+  template<class F>
+  MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh)
+  : bc_(bc)
+  , queue_size_(queue_size)
+  , callback_queue_(nh.getCallbackQueue())
+  {
+    init();
+
+    setTargetFrame(target_frame);
+
+    connectInput(f);
+  }
+
+  /**
+   * \brief Constructor
+   *
+   * \param bc The tf2::BufferCore this filter should use
+   * \param target_frame The frame this filter should attempt to transform to.  To use multiple frames, pass an empty string here and use the setTargetFrames() function.
+   * \param queue_size The number of messages to queue up before throwing away old ones.  0 means infinite (dangerous).
+   * \param cbqueue The callback queue to add callbacks to.  If NULL, callbacks will happen from whatever thread either
+   *    a) add() is called, which will generally be when the previous filter in the chain outputs a message, or
+   *    b) tf2::BufferCore::setTransform() is called
+   */
+  MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue)
+  : bc_(bc)
+  , queue_size_(queue_size)
+  , callback_queue_(cbqueue)
+  {
+    init();
+
+    setTargetFrame(target_frame);
+  }
+
+  /**
+   * \brief Constructor
+   *
+   * \param f The filter to connect this filter's input to.  Often will be a message_filters::Subscriber.
+   * \param bc The tf2::BufferCore this filter should use
+   * \param target_frame The frame this filter should attempt to transform to.  To use multiple frames, pass an empty string here and use the setTargetFrames() function.
+   * \param queue_size The number of messages to queue up before throwing away old ones.  0 means infinite (dangerous).
+   * \param cbqueue The callback queue to add callbacks to.  If NULL, callbacks will happen from whatever thread either
+   *    a) add() is called, which will generally be when the previous filter in the chain outputs a message, or
+   *    b) tf2::BufferCore::setTransform() is called
+   */
+  template<class F>
+  MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue)
+  : bc_(bc)
+  , queue_size_(queue_size)
+  , callback_queue_(cbqueue)
+  {
+    init();
+
+    setTargetFrame(target_frame);
+
+    connectInput(f);
+  }
+
+  /**
+   * \brief Connect this filter's input to another filter's output.  If this filter is already connected, disconnects first.
+   */
+  template<class F>
+  void connectInput(F& f)
+  {
+    message_connection_.disconnect();
+    message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this);
+  }
+
+  /**
+   * \brief Destructor
+   */
+  ~MessageFilter()
+  {
+    message_connection_.disconnect();
+
+    MessageFilter::clear();
+
+    TF2_ROS_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
+                           (long long unsigned int)successful_transform_count_,
+                           (long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_, 
+                           (long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_);
+
+  }
+
+  /**
+   * \brief Set the frame you need to be able to transform to before getting a message callback
+   */
+  void setTargetFrame(const std::string& target_frame)
+  {
+    V_string frames;
+    frames.push_back(target_frame);
+    setTargetFrames(frames);
+  }
+
+  /**
+   * \brief Set the frames you need to be able to transform to before getting a message callback
+   */
+  void setTargetFrames(const V_string& target_frames)
+  {
+    boost::mutex::scoped_lock frames_lock(target_frames_mutex_);
+
+    target_frames_.resize(target_frames.size());
+    std::transform(target_frames.begin(), target_frames.end(), target_frames_.begin(), this->stripSlash);
+    expected_success_count_ = target_frames_.size() * (time_tolerance_.isZero() ? 1 : 2);
+
+    std::stringstream ss;
+    for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it)
+    {
+      ss << *it << " ";
+    }
+    target_frames_string_ = ss.str();
+  }
+  /**
+   * \brief Get the target frames as a string for debugging
+   */
+  std::string getTargetFramesString()
+  {
+    boost::mutex::scoped_lock lock(target_frames_mutex_);
+    return target_frames_string_;
+  };
+
+  /**
+   * \brief Set the required tolerance for the notifier to return true
+   */
+  void setTolerance(const ros::Duration& tolerance)
+  {
+    boost::mutex::scoped_lock lock(target_frames_mutex_);
+    time_tolerance_ = tolerance;
+    expected_success_count_ = target_frames_.size() * (time_tolerance_.isZero() ? 1 : 2);
+  }
+
+  /**
+   * \brief Clear any messages currently in the queue
+   */
+  void clear()
+  {
+    boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
+
+    TF2_ROS_MESSAGEFILTER_DEBUG("%s", "Cleared");
+
+    bc_.removeTransformableCallback(callback_handle_);
+    callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
+
+    messages_.clear();
+    message_count_ = 0;
+
+    // remove pending callbacks in callback queue as well
+    if (callback_queue_)
+      callback_queue_->removeByID((uint64_t)this);
+
+    warned_about_empty_frame_id_ = false;
+  }
+
+  void add(const MEvent& evt)
+  {
+    if (target_frames_.empty())
+    {
+      return;
+    }
+
+    namespace mt = ros::message_traits;
+    const MConstPtr& message = evt.getMessage();
+    std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
+    ros::Time stamp = mt::TimeStamp<M>::value(*message);
+
+    if (frame_id.empty())
+    {
+      messageDropped(evt, filter_failure_reasons::EmptyFrameID);
+      return;
+    }
+
+    // iterate through the target frames and add requests for each of them
+    MessageInfo info;
+    info.handles.reserve(expected_success_count_);
+    {
+      V_string target_frames_copy;
+      // Copy target_frames_ to avoid deadlock from #79
+      {
+        boost::mutex::scoped_lock frames_lock(target_frames_mutex_);
+        target_frames_copy = target_frames_;
+      }
+
+      V_string::iterator it = target_frames_copy.begin();
+      V_string::iterator end = target_frames_copy.end();
+      for (; it != end; ++it)
+      {
+        const std::string& target_frame = *it;
+        tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp);
+        if (handle == 0xffffffffffffffffULL) // never transformable
+        {
+          messageDropped(evt, filter_failure_reasons::OutTheBack);
+          return;
+        }
+        else if (handle == 0)
+        {
+          ++info.success_count;
+        }
+        else
+        {
+          info.handles.push_back(handle);
+        }
+
+        if (!time_tolerance_.isZero())
+        {
+          handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp + time_tolerance_);
+          if (handle == 0xffffffffffffffffULL) // never transformable
+          {
+            messageDropped(evt, filter_failure_reasons::OutTheBack);
+            return;
+          }
+          else if (handle == 0)
+          {
+            ++info.success_count;
+          }
+          else
+          {
+            info.handles.push_back(handle);
+          }
+        }
+      }
+    }
+
+
+    // We can transform already
+    if (info.success_count == expected_success_count_)
+    {
+      messageReady(evt);
+    }
+    else
+    {
+      boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
+      // If this message is about to push us past our queue size, erase the oldest message
+      if (queue_size_ != 0 && message_count_ + 1 > queue_size_)
+      {
+        ++dropped_message_count_;
+        const MessageInfo& front = messages_.front();
+        TF2_ROS_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_,
+                                (mt::FrameId<M>::value(*front.event.getMessage())).c_str(), mt::TimeStamp<M>::value(*front.event.getMessage()).toSec());
+
+        V_TransformableRequestHandle::const_iterator it = front.handles.begin();
+        V_TransformableRequestHandle::const_iterator end = front.handles.end();
+
+        for (; it != end; ++it)
+        {
+          bc_.cancelTransformableRequest(*it);
+        }
+
+        messageDropped(front.event, filter_failure_reasons::Unknown);
+        messages_.pop_front();
+         --message_count_;
+      }
+
+      // Add the message to our list
+      info.event = evt;
+      messages_.push_back(info);
+      ++message_count_;
+    }
+
+    TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_);
+
+    ++incoming_message_count_;
+  }
+
+  /**
+   * \brief Manually add a message into this filter.
+   * \note If the message (or any other messages in the queue) are immediately transformable this will immediately call through to the output callback, possibly
+   * multiple times
+   */
+  void add(const MConstPtr& message)
+  {
+    boost::shared_ptr<std::map<std::string, std::string> > header(new std::map<std::string, std::string>);
+    (*header)["callerid"] = "unknown";
+    ros::WallTime n = ros::WallTime::now();
+    ros::Time t(n.sec, n.nsec);
+    add(MEvent(message, header, t));
+  }
+
+  /**
+   * \brief Register a callback to be called when a message is about to be dropped
+   * \param callback The callback to call
+   */
+  message_filters::Connection registerFailureCallback(const FailureCallback& callback)
+  {
+    boost::mutex::scoped_lock lock(failure_signal_mutex_);
+    return message_filters::Connection(boost::bind(&MessageFilter::disconnectFailure, this, boost::placeholders::_1), failure_signal_.connect(callback));
+  }
+
+  virtual void setQueueSize( uint32_t new_queue_size )
+  {
+    queue_size_ = new_queue_size;
+  }
+
+  virtual uint32_t getQueueSize()
+  {
+    return queue_size_;
+  }
+
+
+private:
+
+  void init()
+  {
+    message_count_ = 0;
+    successful_transform_count_ = 0;
+    failed_out_the_back_count_ = 0;
+    transform_message_count_ = 0;
+    incoming_message_count_ = 0;
+    dropped_message_count_ = 0;
+    time_tolerance_ = ros::Duration(0.0);
+    warned_about_empty_frame_id_ = false;
+    expected_success_count_ = 1;
+
+    callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
+  }
+
+  void transformable(tf2::TransformableRequestHandle request_handle, const std::string& /* target_frame */, const std::string& /* source_frame */,
+                     ros::Time /* time */, tf2::TransformableResult result)
+  {
+    namespace mt = ros::message_traits;
+
+    boost::upgrade_lock< boost::shared_mutex > lock(messages_mutex_);
+
+    // find the message this request is associated with
+    typename L_MessageInfo::iterator msg_it = messages_.begin();
+    typename L_MessageInfo::iterator msg_end = messages_.end();
+    for (; msg_it != msg_end; ++msg_it)
+    {
+      MessageInfo& info = *msg_it;
+      V_TransformableRequestHandle::const_iterator handle_it = std::find(info.handles.begin(), info.handles.end(), request_handle);
+      if (handle_it != info.handles.end())
+      {
+        // found msg_it
+        ++info.success_count;
+        break;
+      }
+    }
+
+    if (msg_it == msg_end)
+    {
+      return;
+    }
+
+    const MessageInfo& info = *msg_it;
+    if (info.success_count < expected_success_count_)
+    {
+      return;
+    }
+
+    bool can_transform = true;
+    const MConstPtr& message = info.event.getMessage();
+    std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
+    ros::Time stamp = mt::TimeStamp<M>::value(*message);
+
+    if (result == tf2::TransformAvailable)
+    {
+      boost::mutex::scoped_lock frames_lock(target_frames_mutex_);
+      // make sure we can still perform all the necessary transforms
+      typename V_string::iterator it = target_frames_.begin();
+      typename V_string::iterator end = target_frames_.end();
+      for (; it != end; ++it)
+      {
+        const std::string& target = *it;
+        if (!bc_.canTransform(target, frame_id, stamp))
+        {
+          can_transform = false;
+          break;
+        }
+
+        if (!time_tolerance_.isZero())
+        {
+          if (!bc_.canTransform(target, frame_id, stamp + time_tolerance_))
+          {
+            can_transform = false;
+            break;
+          }
+        }
+      }
+    }
+    else
+    {
+      can_transform = false;
+    }
+
+    // We will be mutating messages now, require unique lock
+    boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);
+    if (can_transform)
+    {
+      TF2_ROS_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1);
+
+      ++successful_transform_count_;
+
+      messageReady(info.event);
+
+    }
+    else
+    {
+      ++dropped_message_count_;
+
+      TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1);
+      messageDropped(info.event, filter_failure_reasons::Unknown);
+    }
+
+    messages_.erase(msg_it);
+    --message_count_;
+  }
+
+  /**
+   * \brief Callback that happens when we receive a message on the message topic
+   */
+  void incomingMessage(const ros::MessageEvent<M const>& evt)
+  {
+    add(evt);
+  }
+
+  void checkFailures()
+  {
+    if (next_failure_warning_.isZero())
+    {
+      next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(15);
+    }
+
+    if (ros::WallTime::now() >= next_failure_warning_)
+    {
+      if (incoming_message_count_ - message_count_ == 0)
+      {
+        return;
+      }
+
+      double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_);
+      if (dropped_pct > 0.95)
+      {
+        TF2_ROS_MESSAGEFILTER_WARN("Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct*100, ROSCONSOLE_DEFAULT_NAME);
+        next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(60);
+
+        if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5)
+        {
+          TF2_ROS_MESSAGEFILTER_WARN("  The majority of dropped messages were due to messages growing older than the TF cache time.  The last message's timestamp was: %f, and the last frame_id was: %s", last_out_the_back_stamp_.toSec(), last_out_the_back_frame_.c_str());
+        }
+      }
+    }
+  }
+
+  struct CBQueueCallback : public ros::CallbackInterface
+  {
+    CBQueueCallback(MessageFilter* filter, const MEvent& event, bool success, FilterFailureReason reason)
+    : event_(event)
+    , filter_(filter)
+    , reason_(reason)
+    , success_(success)
+    {}
+
+
+    virtual CallResult call()
+    {
+      if (success_)
+      {
+        filter_->signalMessage(event_);
+      }
+      else
+      {
+        filter_->signalFailure(event_, reason_);
+      }
+
+      return Success;
+    }
+
+  private:
+    MEvent event_;
+    MessageFilter* filter_;
+    FilterFailureReason reason_;
+    bool success_;
+  };
+
+  void messageDropped(const MEvent& evt, FilterFailureReason reason)
+  {
+    if (callback_queue_)
+    {
+      ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason));
+      callback_queue_->addCallback(cb, (uint64_t)this);
+    }
+    else
+    {
+      signalFailure(evt, reason);
+    }
+  }
+
+  void messageReady(const MEvent& evt)
+  {
+    if (callback_queue_)
+    {
+      ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, true, filter_failure_reasons::Unknown));
+      callback_queue_->addCallback(cb, (uint64_t)this);
+    }
+    else
+    {
+      this->signalMessage(evt);
+    }
+  }
+
+  void disconnectFailure(const message_filters::Connection& c)
+  {
+    boost::mutex::scoped_lock lock(failure_signal_mutex_);
+    c.getBoostConnection().disconnect();
+  }
+
+  void signalFailure(const MEvent& evt, FilterFailureReason reason)
+  {
+    boost::mutex::scoped_lock lock(failure_signal_mutex_);
+    failure_signal_(evt.getMessage(), reason);
+  }
+
+  static
+  std::string stripSlash(const std::string& in)
+  {
+    if ( !in.empty() && (in[0] == '/'))
+    {
+      std::string out = in;
+      out.erase(0, 1);
+      return out;
+    }
+    return in;
+  }
+
+  tf2::BufferCore& bc_; ///< The Transformer used to determine if transformation data is available
+  V_string target_frames_; ///< The frames we need to be able to transform to before a message is ready
+  std::string target_frames_string_;
+  boost::mutex target_frames_mutex_; ///< A mutex to protect access to the target_frames_ list and target_frames_string.
+  uint32_t queue_size_; ///< The maximum number of messages we queue up
+  tf2::TransformableCallbackHandle callback_handle_;
+
+  typedef std::vector<tf2::TransformableRequestHandle> V_TransformableRequestHandle;
+  struct MessageInfo
+  {
+    MessageInfo()
+    : success_count(0)
+    {}
+
+    MEvent event;
+    V_TransformableRequestHandle handles;
+    uint32_t success_count;
+  };
+  typedef std::list<MessageInfo> L_MessageInfo;
+  L_MessageInfo messages_;
+  uint32_t message_count_; ///< The number of messages in the list.  Used because \<container\>.size() may have linear cost
+  boost::shared_mutex messages_mutex_; ///< The mutex used for locking message list operations
+  uint32_t expected_success_count_;
+
+  bool warned_about_empty_frame_id_;
+
+  uint64_t successful_transform_count_;
+  uint64_t failed_out_the_back_count_;
+  uint64_t transform_message_count_;
+  uint64_t incoming_message_count_;
+  uint64_t dropped_message_count_;
+
+  ros::Time last_out_the_back_stamp_;
+  std::string last_out_the_back_frame_;
+
+  ros::WallTime next_failure_warning_;
+
+  ros::Duration time_tolerance_; ///< Provide additional tolerance on time for messages which are stamped but can have associated duration
+
+  message_filters::Connection message_connection_;
+
+  FailureSignal failure_signal_;
+  boost::mutex failure_signal_mutex_;
+
+  ros::CallbackQueueInterface* callback_queue_;
+};
+
+} // namespace tf2
+
+#endif
--- a/navigations/geometry2/tf2_ros/include/tf2_ros/static_transform_broadcaster.h
+++ b/navigations/geometry2/tf2_ros/include/tf2_ros/static_transform_broadcaster.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/** \author Tully Foote */
+
+#ifndef TF2_ROS_STATICTRANSFORMBROADCASTER_H
+#define TF2_ROS_STATICTRANSFORMBROADCASTER_H
+
+
+
+#include "ros/ros.h"
+#include "geometry_msgs/TransformStamped.h"
+#include "tf2_msgs/TFMessage.h"
+
+namespace tf2_ros
+{
+
+
+/** \brief This class provides an easy way to publish coordinate frame transform information.  
+ * It will handle all the messaging and stuffing of messages.  And the function prototypes lay out all the 
+ * necessary data needed for each message.  */
+
+class StaticTransformBroadcaster{
+public:
+  /** \brief Constructor (needs a ros::Node reference) */
+  StaticTransformBroadcaster();
+
+  /** \brief Send a TransformStamped message
+   * The stamped data structure includes frame_id, and time, and parent_id already.  */
+  void sendTransform(const geometry_msgs::TransformStamped & transform) {
+    sendTransform(std::vector<geometry_msgs::TransformStamped>({transform}));
+  }
+
+  /** \brief Send a vector of TransformStamped messages
+   * The stamped data structure includes frame_id, and time, and parent_id already.  */
+  void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
+
+private:
+  /// Internal reference to ros::Node
+  ros::NodeHandle node_;
+  ros::Publisher publisher_;
+  tf2_msgs::TFMessage net_message_;
+
+};
+
+}
+
+#endif //TF_STATICTRANSFORMBROADCASTER_H
--- a/navigations/geometry2/tf2_ros/include/tf2_ros/transform_broadcaster.h
+++ b/navigations/geometry2/tf2_ros/include/tf2_ros/transform_broadcaster.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/** \author Tully Foote */
+
+#ifndef TF2_ROS_TRANSFORMBROADCASTER_H
+#define TF2_ROS_TRANSFORMBROADCASTER_H
+
+
+
+#include "ros/ros.h"
+#include "geometry_msgs/TransformStamped.h"
+namespace tf2_ros
+{
+
+
+/** \brief This class provides an easy way to publish coordinate frame transform information.  
+ * It will handle all the messaging and stuffing of messages.  And the function prototypes lay out all the 
+ * necessary data needed for each message.  */
+
+class TransformBroadcaster{
+public:
+  /** \brief Constructor (needs a ros::Node reference) */
+  TransformBroadcaster();
+
+  /** \brief Send a StampedTransform 
+   * The stamped data structure includes frame_id, and time, and parent_id already.  */
+  //  void sendTransform(const StampedTransform & transform);
+
+  /** \brief Send a vector of StampedTransforms 
+   * The stamped data structure includes frame_id, and time, and parent_id already.  */
+  //void sendTransform(const std::vector<StampedTransform> & transforms);
+
+  /** \brief Send a TransformStamped message
+   * The stamped data structure includes frame_id, and time, and parent_id already.  */
+  void sendTransform(const geometry_msgs::TransformStamped & transform);
+
+  /** \brief Send a vector of TransformStamped messages
+   * The stamped data structure includes frame_id, and time, and parent_id already.  */
+  void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
+
+private:
+  /// Internal reference to ros::Node
+  ros::NodeHandle node_;
+  ros::Publisher publisher_;
+
+};
+
+}
+
+#endif //TF_TRANSFORMBROADCASTER_H
--- a/navigations/geometry2/tf2_ros/include/tf2_ros/transform_listener.h
+++ b/navigations/geometry2/tf2_ros/include/tf2_ros/transform_listener.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#ifndef TF2_ROS_TRANSFORMLISTENER_H
+#define TF2_ROS_TRANSFORMLISTENER_H
+
+#include "std_msgs/Empty.h"
+#include "tf2_msgs/TFMessage.h"
+#include "ros/ros.h"
+#include "ros/callback_queue.h"
+
+#include "tf2_ros/buffer.h"
+
+#include "boost/thread.hpp"
+
+namespace tf2_ros{
+
+/** \brief This class provides an easy way to request and receive coordinate frame transform information.
+ */
+class TransformListener
+{
+
+public:
+  /**@brief Constructor for transform listener */
+  TransformListener(tf2::BufferCore& buffer, bool spin_thread = true,
+      ros::TransportHints transport_hints = ros::TransportHints());
+  TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread = true,
+      ros::TransportHints transport_hints = ros::TransportHints());
+
+  ~TransformListener();
+
+private:
+
+  /// Initialize this transform listener, subscribing, advertising services, etc.
+  void init();
+  void initWithThread();
+
+  /// Callback function for ros message subscription
+  void subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt);
+  void static_subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt);
+  void subscription_callback_impl(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt, bool is_static);
+
+  ros::CallbackQueue tf_message_callback_queue_;
+  boost::thread* dedicated_listener_thread_;
+  ros::NodeHandle node_;
+  ros::Subscriber message_subscriber_tf_;
+  ros::Subscriber message_subscriber_tf_static_;
+  tf2::BufferCore& buffer_;
+  bool using_dedicated_thread_;
+  ros::TransportHints transport_hints_;
+  ros::Time last_update_;
+ 
+  void dedicatedListenerThread()
+  {
+    while (using_dedicated_thread_)
+    {
+      tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01));
+    }
+  };
+
+};
+}
+
+#endif //TF_TRANSFORMLISTENER_H
--- a/navigations/geometry2/tf2_ros/package.xml
+++ b/navigations/geometry2/tf2_ros/package.xml
@@ -0,0 +1,43 @@
+<package>
+  <name>tf2_ros</name>
+  <version>0.7.6</version>
+  <description>
+    This package contains the ROS bindings for the tf2 library, for both Python and C++.
+  </description>
+  <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_ros</url>
+
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend version_gte="1.11.1">message_filters</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rosgraph</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_msgs</build_depend>
+  <build_depend>tf2_py</build_depend>
+  <build_depend>xmlrpcpp</build_depend>
+
+  <run_depend>actionlib</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rosgraph</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_msgs</run_depend>
+  <run_depend>tf2_py</run_depend>
+  <run_depend>xmlrpcpp</run_depend>
+
+  <test_depend>rostest</test_depend>
+</package>
--- a/navigations/geometry2/tf2_ros/rosdoc.yaml
+++ b/navigations/geometry2/tf2_ros/rosdoc.yaml
@@ -0,0 +1,8 @@
+ - builder: doxygen
+   name: C++ API
+   output_dir: c++
+   file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
+ - builder: sphinx
+   name: Python API
+   output_dir: python
+   sphinx_root_dir: doc
--- a/navigations/geometry2/tf2_ros/setup.py
+++ b/navigations/geometry2/tf2_ros/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_ros'],
+    package_dir={'': 'src'},
+    requires=['rospy', 'actionlib', 'actionlib_msgs', 'tf2_msgs',
+              'tf2_py', 'geometry_msgs']
+)
+
+setup(**d)
--- a/navigations/geometry2/tf2_ros/src/buffer.cpp
+++ b/navigations/geometry2/tf2_ros/src/buffer.cpp
@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Wim Meeussen */
+
+
+#include "tf2_ros/buffer.h"
+
+#include <ros/assert.h>
+#include <sstream>
+
+namespace tf2_ros
+{
+
+static const double CAN_TRANSFORM_POLLING_SCALE = 0.01;
+
+Buffer::Buffer(ros::Duration cache_time, bool debug) :
+  BufferCore(cache_time)
+{
+  if(debug && !ros::service::exists("~tf2_frames", false))
+  {
+    ros::NodeHandle n("~");
+    frames_server_ = n.advertiseService("tf2_frames", &Buffer::getFrames, this);
+  }
+}
+
+geometry_msgs::TransformStamped 
+Buffer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
+                        const ros::Time& time, const ros::Duration timeout) const
+{
+  canTransform(target_frame, source_frame, time, timeout);
+  return lookupTransform(target_frame, source_frame, time);
+}
+
+
+geometry_msgs::TransformStamped 
+Buffer::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
+                        const std::string& source_frame, const ros::Time& source_time,
+                        const std::string& fixed_frame, const ros::Duration timeout) const
+{
+  canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
+  return lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame);
+}
+
+/** This is a workaround for the case that we're running inside of
+    rospy and ros::Time is not initialized inside the c++ instance. 
+    This makes the system fall back to Wall time if not initialized.  
+*/
+ros::Time now_fallback_to_wall()
+{
+  try
+  {
+    return ros::Time::now();
+  }
+  catch (ros::TimeNotInitializedException ex)
+  {
+    ros::WallTime wt = ros::WallTime::now(); 
+    return ros::Time(wt.sec, wt.nsec); 
+  }
+}
+
+/** This is a workaround for the case that we're running inside of
+    rospy and ros::Time is not initialized inside the c++ instance. 
+    This makes the system fall back to Wall time if not initialized.  
+    https://github.com/ros/geometry/issues/30
+*/
+void sleep_fallback_to_wall(const ros::Duration& d)
+{
+  try
+  {
+      d.sleep();
+  }
+  catch (ros::TimeNotInitializedException ex)
+  {
+    ros::WallDuration wd = ros::WallDuration(d.sec, d.nsec); 
+    wd.sleep();
+  }
+}
+
+void conditionally_append_timeout_info(std::string * errstr, const ros::Time& start_time,
+                                       const ros::Duration& timeout)
+{
+  if (errstr)
+  {
+    std::stringstream ss;
+    ss << ". canTransform returned after "<< (now_fallback_to_wall() - start_time).toSec() \
+       <<" timeout was " << timeout.toSec() << ".";
+    (*errstr) += ss.str();
+  }
+}
+
+bool
+Buffer::canTransform(const std::string& target_frame, const std::string& source_frame, 
+                     const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
+{
+  // Clear the errstr before populating it if it's valid.
+  if (errstr)
+    {
+      errstr->clear();
+    }
+
+  if (!checkAndErrorDedicatedThreadPresent(errstr))
+    return false;
+
+  // poll for transform if timeout is set
+  ros::Time start_time = now_fallback_to_wall();
+  const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE;
+  while (now_fallback_to_wall() < start_time + timeout && 
+         !canTransform(target_frame, source_frame, time) &&
+         (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) &&  //don't wait when we detect a bag loop
+         (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
+    {
+      sleep_fallback_to_wall(sleep_duration);
+    }
+  bool retval = canTransform(target_frame, source_frame, time, errstr);
+  conditionally_append_timeout_info(errstr, start_time, timeout);
+  return retval;
+}
+
+    
+bool
+Buffer::canTransform(const std::string& target_frame, const ros::Time& target_time,
+                     const std::string& source_frame, const ros::Time& source_time,
+                     const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
+{
+  // Clear the errstr before populating it if it's valid.
+  if (errstr)
+    {
+      errstr->clear();
+    }
+
+  if (!checkAndErrorDedicatedThreadPresent(errstr))
+    return false;
+
+  // poll for transform if timeout is set
+  ros::Time start_time = now_fallback_to_wall();
+  const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE;
+  while (now_fallback_to_wall() < start_time + timeout && 
+         !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) &&
+         (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) &&  //don't wait when we detect a bag loop
+         (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
+         {  
+           sleep_fallback_to_wall(sleep_duration);
+         }
+  bool retval = canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, errstr);
+  conditionally_append_timeout_info(errstr, start_time, timeout);
+  return retval; 
+}
+
+
+bool Buffer::getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res) 
+{
+  res.frame_yaml = allFramesAsYAML();
+  return true;
+}
+
+
+
+bool Buffer::checkAndErrorDedicatedThreadPresent(std::string* error_str) const
+{
+  if (isUsingDedicatedThread())
+    return true;
+  
+
+
+  if (error_str)
+    *error_str = tf2_ros::threading_error;
+
+  ROS_ERROR("%s", tf2_ros::threading_error.c_str());
+  return false;
+}
+
+
+
+}
--- a/navigations/geometry2/tf2_ros/src/buffer_client.cpp
+++ b/navigations/geometry2/tf2_ros/src/buffer_client.cpp
@@ -0,0 +1,162 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <tf2_ros/buffer_client.h>
+
+namespace tf2_ros
+{
+  BufferClient::BufferClient(std::string ns, double check_frequency, ros::Duration timeout_padding): 
+    client_(ns), 
+    check_frequency_(check_frequency),
+    timeout_padding_(timeout_padding)
+  {
+  }
+
+  geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const std::string& source_frame,
+      const ros::Time& time, const ros::Duration timeout) const
+  {
+    //populate the goal message
+    tf2_msgs::LookupTransformGoal goal;
+    goal.target_frame = target_frame;
+    goal.source_frame = source_frame;
+    goal.source_time = time;
+    goal.timeout = timeout;
+    goal.advanced = false;
+
+    return processGoal(goal);
+  }
+
+  geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
+      const std::string& source_frame, const ros::Time& source_time,
+      const std::string& fixed_frame, const ros::Duration timeout) const
+  {
+    //populate the goal message
+    tf2_msgs::LookupTransformGoal goal;
+    goal.target_frame = target_frame;
+    goal.source_frame = source_frame;
+    goal.source_time = source_time;
+    goal.timeout = timeout;
+    goal.target_time = target_time;
+    goal.fixed_frame = fixed_frame;
+    goal.advanced = true;
+
+    return processGoal(goal);
+  }
+
+  geometry_msgs::TransformStamped BufferClient::processGoal(const tf2_msgs::LookupTransformGoal& goal) const
+  {
+    client_.sendGoal(goal);
+
+    //this shouldn't happen, but could in rare cases where the server hangs
+    if(!client_.waitForResult(goal.timeout + timeout_padding_))
+    {
+      //make sure to cancel the goal the server is pursuing
+      client_.cancelGoal();
+      throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.");
+    }
+
+    if(client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
+      throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.");
+
+    //process the result for errors and return it
+    return processResult(*client_.getResult());
+  }
+
+  geometry_msgs::TransformStamped BufferClient::processResult(const tf2_msgs::LookupTransformResult& result) const
+  {
+    //if there's no error, then we'll just return the transform
+    if(result.error.error != result.error.NO_ERROR){
+      //otherwise, we'll have to throw the appropriate exception
+      if(result.error.error == result.error.LOOKUP_ERROR)
+        throw tf2::LookupException(result.error.error_string);
+
+      if(result.error.error == result.error.CONNECTIVITY_ERROR)
+        throw tf2::ConnectivityException(result.error.error_string);
+
+      if(result.error.error == result.error.EXTRAPOLATION_ERROR)
+        throw tf2::ExtrapolationException(result.error.error_string);
+
+      if(result.error.error == result.error.INVALID_ARGUMENT_ERROR)
+        throw tf2::InvalidArgumentException(result.error.error_string);
+
+      if(result.error.error == result.error.TIMEOUT_ERROR)
+        throw tf2::TimeoutException(result.error.error_string);
+
+      throw tf2::TransformException(result.error.error_string);
+    }
+
+    return result.transform;
+  }
+
+  bool BufferClient::canTransform(const std::string& target_frame, const std::string& source_frame, 
+        const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
+  {
+    try
+    {
+      lookupTransform(target_frame, source_frame, time, timeout);
+      return true;
+    }
+    catch(tf2::TransformException& ex)
+    {
+      if(errstr)
+      { 
+        errstr->clear();
+        *errstr = ex.what();
+      }
+      return false;
+    }
+  }
+
+  bool BufferClient::canTransform(const std::string& target_frame, const ros::Time& target_time,
+        const std::string& source_frame, const ros::Time& source_time,
+        const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
+  {
+    try
+    {
+      lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
+      return true;
+    }
+    catch(tf2::TransformException& ex)
+    {
+      if(errstr)
+      {
+        errstr->clear();
+        *errstr = ex.what();
+      }
+      return false;
+    }
+  }
+};
--- a/navigations/geometry2/tf2_ros/src/buffer_server.cpp
+++ b/navigations/geometry2/tf2_ros/src/buffer_server.cpp
@@ -0,0 +1,222 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <tf2_ros/buffer_server.h>
+
+namespace tf2_ros
+{
+  BufferServer::BufferServer(const Buffer& buffer, const std::string& ns, bool auto_start, ros::Duration check_period): 
+    buffer_(buffer),
+    server_(ros::NodeHandle(),
+            ns,
+            boost::bind(&BufferServer::goalCB, this, boost::placeholders::_1),
+            boost::bind(&BufferServer::cancelCB, this, boost::placeholders::_1),
+            auto_start)
+  {
+    ros::NodeHandle n;
+    check_timer_ = n.createTimer(check_period, boost::bind(&BufferServer::checkTransforms, this, boost::placeholders::_1));
+  }
+
+  void BufferServer::checkTransforms(const ros::TimerEvent& e)
+  {
+    (void) e; //Unused
+    boost::mutex::scoped_lock l(mutex_);
+    for(std::list<GoalInfo>::iterator it = active_goals_.begin(); it != active_goals_.end();)
+    {
+      GoalInfo& info = *it;
+
+      //we want to lookup a transform if the time on the goal
+      //has expired, or a transform is available
+      if(canTransform(info.handle) || info.end_time < ros::Time::now())
+      {
+        tf2_msgs::LookupTransformResult result;
+
+        //try to populate the result, catching exceptions if they occur
+        try
+        {
+          result.transform = lookupTransform(info.handle);
+        }
+        catch (tf2::ConnectivityException &ex)
+        {
+          result.error.error = result.error.CONNECTIVITY_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::LookupException &ex)
+        {
+          result.error.error = result.error.LOOKUP_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::ExtrapolationException &ex)
+        {
+          result.error.error = result.error.EXTRAPOLATION_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::InvalidArgumentException &ex)
+        {
+          result.error.error = result.error.INVALID_ARGUMENT_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::TimeoutException &ex)
+        {
+          result.error.error = result.error.TIMEOUT_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::TransformException &ex)
+        {
+          result.error.error = result.error.TRANSFORM_ERROR;
+          result.error.error_string = ex.what();
+        }
+
+        //make sure to pass the result to the client
+        //even failed transforms are considered a success
+        //since the request was successfully processed
+        info.handle.setSucceeded(result);
+        it = active_goals_.erase(it);
+      }
+      else
+        ++it;
+    }
+  }
+
+  void BufferServer::cancelCB(GoalHandle gh)
+  {
+    boost::mutex::scoped_lock l(mutex_);
+    //we need to find the goal in the list and remove it... also setting it as canceled
+    //if its not in the list, we won't do anything since it will have already been set
+    //as completed
+    for(std::list<GoalInfo>::iterator it = active_goals_.begin(); it != active_goals_.end();)
+    {
+      GoalInfo& info = *it;
+      if(info.handle == gh)
+      {
+        info.handle.setCanceled();
+        it = active_goals_.erase(it);
+        return;
+      }
+      else
+        ++it;
+    }
+  }
+
+  void BufferServer::goalCB(GoalHandle gh)
+  {
+    //we'll accept all goals we get
+    gh.setAccepted();
+
+    //if the transform isn't immediately available, we'll push it onto our list to check
+    //along with the time that the goal will end
+    GoalInfo goal_info;
+    goal_info.handle = gh;
+    goal_info.end_time = ros::Time::now() + gh.getGoal()->timeout;
+
+    //we can do a quick check here to see if the transform is valid
+    //we'll also do this if the end time has been reached 
+    if(canTransform(gh) || goal_info.end_time <= ros::Time::now())
+    {
+      tf2_msgs::LookupTransformResult result;
+      try
+      {
+        result.transform = lookupTransform(gh);
+      }
+      catch (tf2::ConnectivityException &ex)
+      {
+        result.error.error = result.error.CONNECTIVITY_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::LookupException &ex)
+      {
+        result.error.error = result.error.LOOKUP_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::ExtrapolationException &ex)
+      {
+        result.error.error = result.error.EXTRAPOLATION_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::InvalidArgumentException &ex)
+      {
+        result.error.error = result.error.INVALID_ARGUMENT_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::TimeoutException &ex)
+      {
+        result.error.error = result.error.TIMEOUT_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::TransformException &ex)
+      {
+        result.error.error = result.error.TRANSFORM_ERROR;
+        result.error.error_string = ex.what();
+      }
+
+      gh.setSucceeded(result);
+      return;
+    }
+
+    boost::mutex::scoped_lock l(mutex_);
+    active_goals_.push_back(goal_info);
+  }
+
+  bool BufferServer::canTransform(GoalHandle gh)
+  {
+    const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal();
+
+    //check whether we need to used the advanced or simple api
+    if(!goal->advanced)
+      return buffer_.canTransform(goal->target_frame, goal->source_frame, goal->source_time);
+
+    return buffer_.canTransform(goal->target_frame, goal->target_time, 
+        goal->source_frame, goal->source_time, goal->fixed_frame);
+  }
+
+  geometry_msgs::TransformStamped BufferServer::lookupTransform(GoalHandle gh)
+  {
+    const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal();
+
+    //check whether we need to used the advanced or simple api
+    if(!goal->advanced)
+      return buffer_.lookupTransform(goal->target_frame, goal->source_frame, goal->source_time);
+
+    return buffer_.lookupTransform(goal->target_frame, goal->target_time, 
+        goal->source_frame, goal->source_time, goal->fixed_frame);
+  }
+
+  void BufferServer::start()
+  {
+    server_.start();
+  }
+
+};
--- a/navigations/geometry2/tf2_ros/src/buffer_server_main.cpp
+++ b/navigations/geometry2/tf2_ros/src/buffer_server_main.cpp
@@ -0,0 +1,71 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Wim Meeussen
+*********************************************************************/
+#include <tf2_ros/buffer_server.h>
+#include <tf2_ros/transform_listener.h>
+#include <ros/ros.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "tf_buffer");
+  ros::NodeHandle nh;  
+
+  double buffer_size;
+  nh.param("buffer_size", buffer_size, 120.0);
+
+  bool publish_frame_service;
+  nh.param("publish_frame_service", publish_frame_service, false);
+
+  // Legacy behavior re: #209
+  bool use_node_namespace;
+  nh.param("use_node_namespace", use_node_namespace, false);
+  std::string node_name;
+  if (use_node_namespace)
+  {
+    node_name = ros::this_node::getName();
+  }
+  else
+  {
+    node_name = "tf2_buffer_server";
+  }
+
+  tf2_ros::Buffer buffer_core(ros::Duration(buffer_size), publish_frame_service);
+  tf2_ros::TransformListener listener(buffer_core);
+  tf2_ros::BufferServer buffer_server(buffer_core, node_name , false);
+  buffer_server.start();
+
+  ros::spin();
+}
--- a/navigations/geometry2/tf2_ros/src/static_transform_broadcaster.cpp
+++ b/navigations/geometry2/tf2_ros/src/static_transform_broadcaster.cpp
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/** \author Tully Foote */
+
+
+#include "ros/ros.h"
+#include "tf2_msgs/TFMessage.h"
+#include "tf2_ros/static_transform_broadcaster.h"
+#include <algorithm>
+
+namespace tf2_ros {
+
+StaticTransformBroadcaster::StaticTransformBroadcaster()
+{
+  publisher_ = node_.advertise<tf2_msgs::TFMessage>("/tf_static", 100, true);
+};
+
+void StaticTransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> & msgtf)
+{
+  for (const geometry_msgs::TransformStamped& input : msgtf)
+  {
+    auto predicate = [&input](const geometry_msgs::TransformStamped existing) {
+      return input.child_frame_id == existing.child_frame_id;
+    };
+    auto existing = std::find_if(net_message_.transforms.begin(), net_message_.transforms.end(), predicate);
+
+    if (existing != net_message_.transforms.end())
+      *existing = input;
+    else
+      net_message_.transforms.push_back(input);
+  }
+
+  publisher_.publish(net_message_);
+}
+
+}
--- a/navigations/geometry2/tf2_ros/src/static_transform_broadcaster_program.cpp
+++ b/navigations/geometry2/tf2_ros/src/static_transform_broadcaster_program.cpp
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <cstdio>
+#include <tf2/LinearMath/Quaternion.h>
+#include "tf2_ros/static_transform_broadcaster.h"
+
+
+bool validateXmlRpcTf(XmlRpc::XmlRpcValue tf_data) {
+  // Validate a TF stored in XML RPC format: ensures the appropriate fields
+  // exist. Note this does not check data types.
+  return tf_data.hasMember("child_frame_id") &&
+         tf_data.hasMember("header") &&
+         tf_data["header"].hasMember("frame_id") &&
+         tf_data.hasMember("transform") &&
+         tf_data["transform"].hasMember("translation") &&
+         tf_data["transform"]["translation"].hasMember("x") &&
+         tf_data["transform"]["translation"].hasMember("y") &&
+         tf_data["transform"]["translation"].hasMember("z") &&
+         tf_data["transform"].hasMember("rotation") &&
+         tf_data["transform"]["rotation"].hasMember("x") &&
+         tf_data["transform"]["rotation"].hasMember("y") &&
+         tf_data["transform"]["rotation"].hasMember("z") &&
+         tf_data["transform"]["rotation"].hasMember("w");
+};
+
+int main(int argc, char ** argv)
+{
+  //Initialize ROS
+  ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName);
+  tf2_ros::StaticTransformBroadcaster broadcaster;
+  geometry_msgs::TransformStamped msg;
+
+  if(argc == 10)
+  {
+    msg.transform.translation.x = atof(argv[1]);
+    msg.transform.translation.y = atof(argv[2]);
+    msg.transform.translation.z = atof(argv[3]);
+    msg.transform.rotation.x = atof(argv[4]);
+    msg.transform.rotation.y = atof(argv[5]);
+    msg.transform.rotation.z = atof(argv[6]);
+    msg.transform.rotation.w = atof(argv[7]);
+    msg.header.stamp = ros::Time::now();
+    msg.header.frame_id = argv[8];
+    msg.child_frame_id = argv[9];
+  }
+  else if (argc == 9)
+  {
+    msg.transform.translation.x = atof(argv[1]);
+    msg.transform.translation.y = atof(argv[2]);
+    msg.transform.translation.z = atof(argv[3]);
+
+    tf2::Quaternion quat;
+    quat.setRPY(atof(argv[6]), atof(argv[5]), atof(argv[4]));
+    msg.transform.rotation.x = quat.x();
+    msg.transform.rotation.y = quat.y();
+    msg.transform.rotation.z = quat.z();
+    msg.transform.rotation.w = quat.w();
+
+    msg.header.stamp = ros::Time::now();
+    msg.header.frame_id = argv[7];
+    msg.child_frame_id = argv[8];
+  }
+  else if (argc == 2) {
+    const std::string param_name = argv[1];
+    ROS_INFO_STREAM("Looking for TF in parameter: " << param_name);
+    XmlRpc::XmlRpcValue tf_data;
+
+    if (!ros::param::has(param_name) || !ros::param::get(param_name, tf_data)) {
+      ROS_FATAL_STREAM("Could not read TF from parameter server: " << param_name);
+      return -1;
+    }
+
+    // Check that all required members are present & of the right type.
+    if (!validateXmlRpcTf(tf_data)) {
+      ROS_FATAL_STREAM("Could not validate XmlRpcC for TF data: " << tf_data);
+      return -1;
+    }
+
+    msg.transform.translation.x = (double) tf_data["transform"]["translation"]["x"];
+    msg.transform.translation.y = (double) tf_data["transform"]["translation"]["y"];
+    msg.transform.translation.z = (double) tf_data["transform"]["translation"]["z"];
+    msg.transform.rotation.x = (double) tf_data["transform"]["rotation"]["x"];
+    msg.transform.rotation.y = (double) tf_data["transform"]["rotation"]["y"];
+    msg.transform.rotation.z = (double) tf_data["transform"]["rotation"]["z"];
+    msg.transform.rotation.w = (double) tf_data["transform"]["rotation"]["w"];
+    msg.header.stamp = ros::Time::now();
+    msg.header.frame_id = (std::string) tf_data["header"]["frame_id"];
+    msg.child_frame_id = (std::string) tf_data["child_frame_id"];
+  }
+  else
+  {
+    printf("A command line utility for manually sending a transform.\n");
+    //printf("It will periodicaly republish the given transform. \n");
+    printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \n");
+    printf("OR \n");
+    printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \n");
+    printf("OR \n");
+    printf("Usage: static_transform_publisher /param_name \n");
+    printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n");
+    printf("of the child_frame_id.  \n");
+    ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments");
+    return -1;
+  }
+
+  // Checks: frames should not be the same.
+  if (msg.header.frame_id == msg.child_frame_id)
+  {
+    ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work",
+              msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
+    return 1;
+  }
+
+  broadcaster.sendTransform(msg);
+  ROS_INFO("Spinning until killed publishing %s to %s",
+           msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
+  ros::spin();
+  return 0;
+};
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/init.py
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/init.py
@@ -0,0 +1,44 @@
+#! /usr/bin/python
+#***********************************************************
+#* Software License Agreement (BSD License)
+#*
+#*  Copyright (c) 2009, Willow Garage, Inc.
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of Willow Garage, Inc. nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#*
+#* Author: Eitan Marder-Eppstein
+#***********************************************************
+from __future__ import absolute_import
+from tf2_py import *
+from .buffer_interface import *
+from .buffer import *
+from .buffer_client import *
+from .transform_listener import *
+from .transform_broadcaster import *
+from .static_transform_broadcaster import *
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/buffer.cpython-38.pyc
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/buffer.cpython-38.pyc
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/buffer_client.cpython-38.pyc
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/buffer_client.cpython-38.pyc
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/buffer_interface.cpython-38.pyc
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/buffer_interface.cpython-38.pyc
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/static_transform_broadcaster.cpython-38.pyc
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/static_transform_broadcaster.cpython-38.pyc
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/transform_broadcaster.cpython-38.pyc
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/transform_broadcaster.cpython-38.pyc
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/transform_listener.cpython-38.pyc
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/pycache/transform_listener.cpython-38.pyc
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/buffer.py
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/buffer.py
@@ -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: Wim Meeussen
+
+import rospy
+import tf2_py as tf2
+import tf2_ros
+from tf2_msgs.srv import FrameGraph, FrameGraphResponse
+import rosgraph.masterapi
+
+DEFAULT_CAN_TRANSFORM_POLLING_SCALE = 0.01
+
+class Buffer(tf2.BufferCore, tf2_ros.BufferInterface):
+    """
+    Standard implementation of the :class:`tf2_ros.BufferInterface` abstract data type.
+
+    Inherits from :class:`tf2_ros.buffer_interface.BufferInterface` and :class:`tf2.BufferCore`.
+
+    Stores known frames and optionally offers a ROS service, "tf2_frames", which responds to client requests
+    with a response containing a :class:`tf2_msgs.FrameGraph` representing the relationship of
+    known frames. 
+    """
+
+    def __init__(self, cache_time = None, debug = True):
+        """
+        .. function:: __init__(cache_time = None, debug = True)
+
+            Constructor.
+
+            :param cache_time: (Optional) How long to retain past information in BufferCore.
+            :param debug: (Optional) If true, check if another tf2_frames service has been advertised.
+        """
+        if cache_time != None:
+            tf2.BufferCore.__init__(self, cache_time)
+        else:
+            tf2.BufferCore.__init__(self)
+        tf2_ros.BufferInterface.__init__(self)
+
+        if debug:
+            #Check to see if the service has already been advertised in this node
+            try:
+                m = rosgraph.masterapi.Master(rospy.get_name())
+                m.lookupService('~tf2_frames')
+            except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure):   
+                self.frame_server = rospy.Service('~tf2_frames', FrameGraph, self.__get_frames)
+
+        self.CAN_TRANSFORM_POLLING_SCALE = DEFAULT_CAN_TRANSFORM_POLLING_SCALE
+
+    def __get_frames(self, req):
+       return FrameGraphResponse(self.all_frames_as_yaml()) 
+
+    def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+
+        self.can_transform(target_frame, source_frame, time, timeout)
+        return self.lookup_transform_core(target_frame, source_frame, time)
+
+    def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame using the advanced API.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        self.can_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout)
+        return self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)
+
+
+    def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0), return_debug_tuple=False):
+        """
+        Check if a transform from the source frame to the target frame is possible.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param return_debug_type: (Optional) If true, return a tuple representing debug information.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        if timeout != rospy.Duration(0.0):
+            start_time = rospy.Time.now()
+            polling_interval = timeout * self.CAN_TRANSFORM_POLLING_SCALE
+            while (rospy.Time.now() < start_time + timeout and 
+                   not self.can_transform_core(target_frame, source_frame, time)[0] and
+                   (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them
+                rospy.sleep(polling_interval)
+        core_result = self.can_transform_core(target_frame, source_frame, time)
+        if return_debug_tuple:
+            return core_result
+        return core_result[0]
+
+    def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0),
+
+                           return_debug_tuple=False):
+        """
+        Check if a transform from the source frame to the target frame is possible (advanced API).
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param return_debug_type: (Optional) If true, return a tuple representing debug information.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        if timeout != rospy.Duration(0.0):
+            start_time = rospy.Time.now()
+            polling_interval = timeout * self.CAN_TRANSFORM_POLLING_SCALE
+            while (rospy.Time.now() < start_time + timeout and 
+                   not self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0] and
+                   (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them
+                rospy.sleep(polling_interval)
+        core_result = self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)
+        if return_debug_tuple:
+            return core_result
+        return core_result[0]
+
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/buffer_client.py
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/buffer_client.py
@@ -0,0 +1,196 @@
+#! /usr/bin/python
+#***********************************************************
+#* Software License Agreement (BSD License)
+#*
+#*  Copyright (c) 2009, Willow Garage, Inc.
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of Willow Garage, Inc. nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#* 
+#* Author: Eitan Marder-Eppstein
+#***********************************************************
+import rospy
+import actionlib
+import tf2_py as tf2
+import tf2_ros
+
+from tf2_msgs.msg import LookupTransformAction, LookupTransformGoal
+from actionlib_msgs.msg import GoalStatus
+
+class BufferClient(tf2_ros.BufferInterface):
+    """
+    Action client-based implementation of BufferInterface.
+    """
+    def __init__(self, ns, check_frequency = None, timeout_padding = rospy.Duration.from_sec(2.0)):
+        """
+        .. function:: __init__(ns, check_frequency = None, timeout_padding = rospy.Duration.from_sec(2.0))
+
+            Constructor.
+
+            :param ns: The namespace in which to look for a BufferServer.
+            :param check_frequency: How frequently to check for updates to known transforms.
+            :param timeout_padding: A constant timeout to add to blocking calls.
+        """
+        tf2_ros.BufferInterface.__init__(self)
+        self.client = actionlib.SimpleActionClient(ns, LookupTransformAction)
+        self.timeout_padding = timeout_padding
+
+        if check_frequency is not None:
+            rospy.logwarn('Argument check_frequency is deprecated and should not be used.')
+
+    def wait_for_server(self, timeout = rospy.Duration()):
+        """
+        Block until the action server is ready to respond to requests. 
+
+        :param timeout: Time to wait for the server.
+        :return: True if the server is ready, false otherwise.
+        :rtype: bool
+        """
+        return self.client.wait_for_server(timeout)
+
+    # lookup, simple api 
+    def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        goal = LookupTransformGoal()
+        goal.target_frame = target_frame;
+        goal.source_frame = source_frame;
+        goal.source_time = time;
+        goal.timeout = timeout;
+        goal.advanced = False;
+
+        return self.__process_goal(goal)
+
+    # lookup, advanced api 
+    def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame using the advanced API.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        goal = LookupTransformGoal()
+        goal.target_frame = target_frame;
+        goal.source_frame = source_frame;
+        goal.source_time = source_time;
+        goal.timeout = timeout;
+        goal.target_time = target_time;
+        goal.fixed_frame = fixed_frame;
+        goal.advanced = True;
+
+        return self.__process_goal(goal)
+
+    # can, simple api
+    def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Check if a transform from the source frame to the target frame is possible.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param return_debug_type: (Optional) If true, return a tuple representing debug information.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        try:
+            self.lookup_transform(target_frame, source_frame, time, timeout)
+            return True
+        except tf2.TransformException:
+            return False
+
+    
+    # can, advanced api
+    def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Check if a transform from the source frame to the target frame is possible (advanced API).
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param return_debug_type: (Optional) If true, return a tuple representing debug information.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        try:
+            self.lookup_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout)
+            return True
+        except tf2.TransformException:
+            return False
+
+    def __process_goal(self, goal):
+        self.client.send_goal(goal)
+
+        if not self.client.wait_for_result(goal.timeout + self.timeout_padding):
+            #This shouldn't happen, but could in rare cases where the server hangs
+            raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server")
+
+        if self.client.get_state() != GoalStatus.SUCCEEDED:
+            raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.")
+
+        return self.__process_result(self.client.get_result())
+
+    def __process_result(self, result):
+        if not result:
+            raise tf2.TransformException("The BufferServer returned None for result!  Something is likely wrong with the server.")
+        if not result.error:
+            raise tf2.TransformException("The BufferServer returned None for result.error!  Something is likely wrong with the server.")
+        if result.error.error != result.error.NO_ERROR:
+            if result.error.error == result.error.LOOKUP_ERROR:
+                raise tf2.LookupException(result.error.error_string)
+            if result.error.error == result.error.CONNECTIVITY_ERROR:
+                raise tf2.ConnectivityException(result.error.error_string)
+            if result.error.error == result.error.EXTRAPOLATION_ERROR:
+                raise tf2.ExtrapolationException(result.error.error_string)
+            if result.error.error == result.error.INVALID_ARGUMENT_ERROR:
+                raise tf2.InvalidArgumentException(result.error.error_string)
+            if result.error.error == result.error.TIMEOUT_ERROR:
+                raise tf2.TimeoutException(result.error.error_string)
+
+            raise tf2.TransformException(result.error.error_string)
+
+        return result.transform
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/buffer_interface.py
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/buffer_interface.py
@@ -0,0 +1,251 @@
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+# 
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# 
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+# 
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# author: Wim Meeussen
+
+from __future__ import print_function
+
+import rospy
+import tf2_py as tf2
+import tf2_ros
+from copy import deepcopy
+from std_msgs.msg import Header
+
+class BufferInterface:
+    """
+    Abstract interface for wrapping the Python bindings for the tf2 library in
+    a ROS-based convenience API.
+    Implementations include :class:tf2_ros.buffer.Buffer and
+    :class:tf2_ros.buffer_client.BufferClient.
+    """
+    def __init__(self):
+        self.registration = tf2_ros.TransformRegistration()
+
+    # transform, simple api
+    def transform(self, object_stamped, target_frame, timeout=rospy.Duration(0.0), new_type = None):
+        """
+        Transform an input into the target frame.
+
+        The input must be a known transformable type (by way of the tf2 data type conversion interface).
+
+        If new_type is not None, the type specified must have a valid conversion from the input type,
+        else the function will raise an exception.
+
+        :param object_stamped: The timestamped object the transform.
+        :param target_frame: Name of the frame to transform the input into.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param new_type: (Optional) Type to convert the object to.
+        :return: The transformed, timestamped output, possibly converted to a new type.
+        """
+        do_transform = self.registration.get(type(object_stamped))
+        res = do_transform(object_stamped, self.lookup_transform(target_frame, object_stamped.header.frame_id,
+                                                                 object_stamped.header.stamp, timeout))
+        if not new_type:
+            return res
+
+        return convert(res, new_type)
+
+    # transform, advanced api
+    def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=rospy.Duration(0.0), new_type = None):
+        """
+        Transform an input into the target frame (advanced API).
+
+        The input must be a known transformable type (by way of the tf2 data type conversion interface).
+
+        If new_type is not None, the type specified must have a valid conversion from the input type,
+        else the function will raise an exception.
+
+        This function follows the advanced API, which allows tranforming between different time points,
+        as well as specifying a frame to be considered fixed in time.
+
+        :param object_stamped: The timestamped object the transform.
+        :param target_frame: Name of the frame to transform the input into.
+        :param target_time: Time to transform the input into.
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param new_type: (Optional) Type to convert the object to.
+        :return: The transformed, timestamped output, possibly converted to a new type.
+        """
+        do_transform = self.registration.get(type(object_stamped))
+        res = do_transform(object_stamped, self.lookup_transform_full(target_frame, target_time,
+                                                                     object_stamped.header.frame_id, object_stamped.header.stamp, 
+                                                                     fixed_frame, timeout))
+        if not new_type:
+            return res
+
+        return convert(res, new_type)
+
+    def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame.
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        raise NotImplementedException()
+
+    def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame using the advanced API.
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        raise NotImplementedException()        
+
+    # can, simple api
+    def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Check if a transform from the source frame to the target frame is possible.
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        raise NotImplementedException()        
+    
+    # can, advanced api
+    def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Check if a transform from the source frame to the target frame is possible (advanced API).
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        raise NotImplementedException()        
+
+
+def Stamped(obj, stamp, frame_id):
+    obj.header = Header(frame_id=frame_id, stamp=stamp)
+    return obj
+
+
+
+class TypeException(Exception):
+    """
+    Raised when an unexpected type is received while registering a transform
+    in :class:`tf2_ros.buffer_interface.BufferInterface`.
+    """
+    def __init__(self, errstr):
+        self.errstr = errstr
+
+class NotImplementedException(Exception):
+    """
+    Raised when can_transform or lookup_transform is not implemented in a
+    subclass of :class:`tf2_ros.buffer_interface.BufferInterface`.
+    """
+    def __init__(self):
+        self.errstr = 'CanTransform or LookupTransform not implemented'
+
+
+class TransformRegistration():
+    __type_map = {}
+    
+    def print_me(self):
+        print(TransformRegistration.__type_map)
+
+    def add(self, key, callback):
+        TransformRegistration.__type_map[key] = callback
+
+    def get(self, key):
+        if not key in TransformRegistration.__type_map:
+            raise TypeException('Type %s if not loaded or supported'% str(key))
+        else:
+            return TransformRegistration.__type_map[key]
+
+class ConvertRegistration():
+    __to_msg_map = {}
+    __from_msg_map = {}
+    __convert_map = {}
+    
+    def add_from_msg(self, key, callback):
+        ConvertRegistration.__from_msg_map[key] = callback
+
+    def add_to_msg(self, key, callback):
+        ConvertRegistration.__to_msg_map[key] = callback
+
+    def add_convert(self, key, callback):
+        ConvertRegistration.__convert_map[key] = callback
+
+    def get_from_msg(self, key):
+        if not key in ConvertRegistration.__from_msg_map:
+            raise TypeException('Type %s if not loaded or supported'% str(key))
+        else:
+            return ConvertRegistration.__from_msg_map[key]
+
+    def get_to_msg(self, key):
+        if not key in ConvertRegistration.__to_msg_map:
+            raise TypeException('Type %s if not loaded or supported'%str(key))
+        else:
+            return ConvertRegistration.__to_msg_map[key]
+
+    def get_convert(self, key):
+        if not key in ConvertRegistration.__convert_map:
+            raise TypeException("Type %s if not loaded or supported" % str(key))
+        else:
+            return ConvertRegistration.__convert_map[key]
+
+def convert(a, b_type):
+    c = ConvertRegistration()
+    #check if an efficient conversion function between the types exists
+    try:
+        f = c.get_convert((type(a), b_type))
+        return f(a)
+    except TypeException:
+        if type(a) == b_type:
+            return deepcopy(a)
+
+        f_to = c.get_to_msg(type(a))
+        f_from = c.get_from_msg(b_type)
+        return f_from(f_to(a))
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/static_transform_broadcaster.py
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/static_transform_broadcaster.py
@@ -0,0 +1,51 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+from tf2_msgs.msg import TFMessage
+from geometry_msgs.msg import TransformStamped
+
+
+class StaticTransformBroadcaster(object):
+    """
+    :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic.
+    """
+
+    def __init__(self):
+        self.pub_tf = rospy.Publisher("/tf_static", TFMessage, queue_size=100, latch=True)
+
+    def sendTransform(self, transform):
+        if not isinstance(transform, list):
+            transform = [transform]
+        self.pub_tf.publish(TFMessage(transform))
+
+
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/transform_broadcaster.py
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/transform_broadcaster.py
@@ -0,0 +1,56 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+from tf2_msgs.msg import TFMessage
+from geometry_msgs.msg import TransformStamped
+
+
+class TransformBroadcaster:
+    """
+    :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic.
+    """
+
+    def __init__(self):
+        self.pub_tf = rospy.Publisher("/tf", TFMessage, queue_size=100)
+
+    def sendTransform(self, transform):
+        """
+        Send a transform, or a list of transforms, to the Buffer associated with this TransformBroadcaster.
+
+        :param transform: A transform or list of transforms to send.
+        """
+        if not isinstance(transform, list):
+            transform = [transform]
+        self.pub_tf.publish(TFMessage(transform))
+
+
--- a/navigations/geometry2/tf2_ros/src/tf2_ros/transform_listener.py
+++ b/navigations/geometry2/tf2_ros/src/tf2_ros/transform_listener.py
@@ -0,0 +1,89 @@
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+# 
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# 
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+# 
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# author: Wim Meeussen
+
+import threading
+
+import rospy
+from tf2_msgs.msg import TFMessage
+
+
+class TransformListener:
+    """
+    :class:`TransformListener` is a convenient way to listen for coordinate frame transformation info.
+    This class takes an object that instantiates the :class:`BufferInterface` interface, to which
+    it propagates changes to the tf frame graph.
+    """
+    def __init__(self, buffer, queue_size=None, buff_size=65536, tcp_nodelay=False):
+        """
+        .. function:: __init__(buffer, queue_size=None, buff_size=65536, tcp_nodelay=False):
+
+            Constructor.
+
+            :param buffer: The buffer to propagate changes to when tf info updates.
+            :param queue_size: Maximum number of messages to receive at a time. This will generally be 1 or None (infinite, default). buff_size should be increased if this parameter is set as incoming data still needs to sit in the incoming buffer before being discarded. Setting queue_size buff_size to a non-default value affects all subscribers to this topic in this process.
+            :param buff_size: Incoming message buffer size in bytes. If queue_size is set, this should be set to a number greater than the queue_size times the average message size. Setting buff_size to a non-default value affects all subscribers to this topic in this process.
+            :param tcp_nodelay: If True, request TCP_NODELAY from publisher. Use of this option is not generally recommended in most cases as it is better to rely on timestamps in message data. Setting tcp_nodelay to True enables TCP_NODELAY for all subscribers in the same python process.
+        """
+        self.buffer = buffer
+        self.last_update = rospy.Time.now()
+        self.last_update_lock = threading.Lock()
+        self.tf_sub = rospy.Subscriber("/tf", TFMessage, self.callback, queue_size=queue_size, buff_size=buff_size, tcp_nodelay=tcp_nodelay)
+        self.tf_static_sub = rospy.Subscriber("/tf_static", TFMessage, self.static_callback, queue_size=queue_size, buff_size=buff_size, tcp_nodelay=tcp_nodelay)
+
+    def __del__(self):
+        self.unregister()
+
+    def unregister(self):
+        """
+        Unregisters all tf subscribers.
+        """
+        self.tf_sub.unregister()
+        self.tf_static_sub.unregister()
+
+    def check_for_reset(self):
+        # Lock to prevent different threads racing on this test and update.
+        # https://github.com/ros/geometry2/issues/341
+        with self.last_update_lock:
+            now = rospy.Time.now()
+            if now < self.last_update:
+                rospy.logwarn("Detected jump back in time of %fs. Clearing TF buffer." % (self.last_update - now).to_sec())
+                self.buffer.clear()
+            self.last_update = now
+
+    def callback(self, data):
+        self.check_for_reset()
+        who = data._connection_header.get('callerid', "default_authority")
+        for transform in data.transforms:
+            self.buffer.set_transform(transform, who)
+
+    def static_callback(self, data):
+        self.check_for_reset()
+        who = data._connection_header.get('callerid', "default_authority")
+        for transform in data.transforms:
+            self.buffer.set_transform_static(transform, who)
--- a/navigations/geometry2/tf2_ros/src/transform_broadcaster.cpp
+++ b/navigations/geometry2/tf2_ros/src/transform_broadcaster.cpp
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/** \author Tully Foote */
+
+
+#include "ros/ros.h"
+#include "tf2_msgs/TFMessage.h"
+#include "tf2_ros/transform_broadcaster.h"
+
+namespace tf2_ros {
+
+TransformBroadcaster::TransformBroadcaster()
+{
+  publisher_ = node_.advertise<tf2_msgs::TFMessage>("/tf", 100);
+};
+
+void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf)
+{
+  std::vector<geometry_msgs::TransformStamped> v1;
+  v1.push_back(msgtf);
+  sendTransform(v1);
+}
+
+
+void TransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> & msgtf)
+{
+  tf2_msgs::TFMessage message;
+  for (std::vector<geometry_msgs::TransformStamped>::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it)
+  {
+    message.transforms.push_back(*it);
+  }
+  publisher_.publish(message);
+}
+
+
+}
+
+
--- a/navigations/geometry2/tf2_ros/src/transform_listener.cpp
+++ b/navigations/geometry2/tf2_ros/src/transform_listener.cpp
@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#include "tf2_ros/transform_listener.h"
+
+
+using namespace tf2_ros;
+
+
+TransformListener::TransformListener(tf2::BufferCore& buffer, bool spin_thread, ros::TransportHints transport_hints):
+  dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false), transport_hints_(transport_hints)
+{
+  if (spin_thread)
+    initWithThread();
+  else
+    init();
+}
+
+TransformListener::TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread,
+    ros::TransportHints transport_hints)
+: dedicated_listener_thread_(NULL)
+, node_(nh)
+, buffer_(buffer)
+, using_dedicated_thread_(false)
+, transport_hints_(transport_hints)
+{
+  if (spin_thread)
+    initWithThread();
+  else
+    init();
+}
+
+TransformListener::~TransformListener()
+{
+  using_dedicated_thread_ = false;
+  if (dedicated_listener_thread_)
+  {
+    dedicated_listener_thread_->join();
+    delete dedicated_listener_thread_;
+  }
+}
+
+void TransformListener::init()
+{
+  message_subscriber_tf_ = node_.subscribe<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, boost::placeholders::_1), ros::VoidConstPtr(), transport_hints_); ///\todo magic number
+  message_subscriber_tf_static_ = node_.subscribe<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, boost::placeholders::_1)); ///\todo magic number
+}
+
+void TransformListener::initWithThread()
+{
+  using_dedicated_thread_ = true;
+  ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, boost::placeholders::_1), ros::VoidPtr(), &tf_message_callback_queue_); ///\todo magic number
+  ops_tf.transport_hints = transport_hints_;
+  message_subscriber_tf_ = node_.subscribe(ops_tf);
+  
+  ros::SubscribeOptions ops_tf_static = ros::SubscribeOptions::create<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, boost::placeholders::_1), ros::VoidPtr(), &tf_message_callback_queue_); ///\todo magic number
+  message_subscriber_tf_static_ = node_.subscribe(ops_tf_static);
+
+  dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this));
+
+  //Tell the buffer we have a dedicated thread to enable timeouts
+  buffer_.setUsingDedicatedThread(true);
+}
+
+
+
+void TransformListener::subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt)
+{
+  subscription_callback_impl(msg_evt, false);
+}
+void TransformListener::static_subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt)
+{
+  subscription_callback_impl(msg_evt, true);
+}
+
+void TransformListener::subscription_callback_impl(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt, bool is_static)
+{
+  ros::Time now = ros::Time::now();
+  if(now < last_update_){
+    ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing TF buffer.");
+    buffer_.clear();
+  }
+  last_update_ = now;
+
+
+
+  const tf2_msgs::TFMessage& msg_in = *(msg_evt.getConstMessage());
+  std::string authority = msg_evt.getPublisherName(); // lookup the authority
+  for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
+  {
+    try
+    {
+      buffer_.setTransform(msg_in.transforms[i], authority, is_static);
+    }
+    
+    catch (tf2::TransformException& ex)
+    {
+      ///\todo Use error reporting
+      std::string temp = ex.what();
+      ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str());
+    }
+  }
+};
+
+
+
+
+
--- a/navigations/geometry2/tf2_ros/test/listener_unittest.cpp
+++ b/navigations/geometry2/tf2_ros/test/listener_unittest.cpp
@@ -0,0 +1,51 @@
+/*
+ * 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_ros/transform_listener.h>
+
+using namespace tf2;
+
+TEST(tf2_ros_transform, transform_listener)
+{
+  tf2_ros::Buffer buffer;
+  tf2_ros::TransformListener tfl(buffer);
+}
+
+TEST(tf2_ros_transform, transform_listener_transport_hints)
+{
+  tf2_ros::Buffer buffer;
+  tf2_ros::TransformListener tfl(buffer, true, ros::TransportHints().tcpNoDelay());
+}
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "transform_listener_unittest");
+  return RUN_ALL_TESTS();
+}
--- a/navigations/geometry2/tf2_ros/test/message_filter_test.cpp
+++ b/navigations/geometry2/tf2_ros/test/message_filter_test.cpp
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2014, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <message_filters/subscriber.h>
+#include <ros/ros.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <gtest/gtest.h>
+#include <thread>
+#include <chrono>
+
+
+void spin_for_a_second()
+{
+  ros::spinOnce();
+  for (uint8_t i = 0; i < 10; ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::microseconds(100));
+    ros::spinOnce();
+  }
+}
+
+bool filter_callback_fired = false;
+void filter_callback(const geometry_msgs::PointStamped& msg)
+{
+  filter_callback_fired = true;
+}
+
+TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance)
+{
+  ros::NodeHandle nh;
+  message_filters::Subscriber<geometry_msgs::PointStamped> sub;
+  sub.subscribe(nh, "point", 10);
+
+  tf2_ros::Buffer buffer;
+  tf2_ros::TransformListener tfl(buffer);
+  tf2_ros::MessageFilter<geometry_msgs::PointStamped> filter(buffer, "map", 10, nh);
+  filter.connectInput(sub);
+  filter.registerCallback(&filter_callback);
+  // Register multiple target frames
+  std::vector<std::string> frames;
+  frames.push_back("odom");
+  frames.push_back("map");
+  filter.setTargetFrames(frames);
+  // Set a non-zero time tolerance
+  filter.setTolerance(ros::Duration(1, 0));
+
+  // Publish static transforms so the frame transformations will always be valid
+  tf2_ros::StaticTransformBroadcaster tfb;
+  geometry_msgs::TransformStamped map_to_odom;
+  map_to_odom.header.stamp = ros::Time(0, 0);
+  map_to_odom.header.frame_id = "map";
+  map_to_odom.child_frame_id = "odom";
+  map_to_odom.transform.translation.x = 0.0;
+  map_to_odom.transform.translation.y = 0.0;
+  map_to_odom.transform.translation.z = 0.0;
+  map_to_odom.transform.rotation.x = 0.0;
+  map_to_odom.transform.rotation.y = 0.0;
+  map_to_odom.transform.rotation.z = 0.0;
+  map_to_odom.transform.rotation.w = 1.0;
+  tfb.sendTransform(map_to_odom);
+
+  geometry_msgs::TransformStamped odom_to_base;
+  odom_to_base.header.stamp = ros::Time(0, 0);
+  odom_to_base.header.frame_id = "odom";
+  odom_to_base.child_frame_id = "base";
+  odom_to_base.transform.translation.x = 0.0;
+  odom_to_base.transform.translation.y = 0.0;
+  odom_to_base.transform.translation.z = 0.0;
+  odom_to_base.transform.rotation.x = 0.0;
+  odom_to_base.transform.rotation.y = 0.0;
+  odom_to_base.transform.rotation.z = 0.0;
+  odom_to_base.transform.rotation.w = 1.0;
+  tfb.sendTransform(odom_to_base);
+
+  // Publish a Point message in the "base" frame
+  ros::Publisher pub = nh.advertise<geometry_msgs::PointStamped>("point", 10);
+  geometry_msgs::PointStamped point;
+  point.header.stamp = ros::Time::now();
+  point.header.frame_id = "base";
+  pub.publish(point);
+
+  // make sure it arrives
+  spin_for_a_second();
+
+  // The filter callback should have been fired because all required transforms are available
+  ASSERT_TRUE(filter_callback_fired);
+}
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "tf2_ros_message_filter");
+  return RUN_ALL_TESTS();
+}
--- a/navigations/geometry2/tf2_ros/test/message_filter_test.launch
+++ b/navigations/geometry2/tf2_ros/test/message_filter_test.launch
@@ -0,0 +1,4 @@
+<launch>
+  <test test-name="message_filter_test" pkg="tf2_ros" type="tf2_ros_test_message_filter" />
+</launch>
+
--- a/navigations/geometry2/tf2_ros/test/time_reset_test.cpp
+++ b/navigations/geometry2/tf2_ros/test/time_reset_test.cpp
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2014, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <gtest/gtest.h>
+#include <thread>
+#include <chrono>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <rosgraph_msgs/Clock.h>
+
+using namespace tf2;
+
+void spin_for_a_second()
+{
+  ros::spinOnce();
+  for (uint8_t i = 0; i < 10; ++i)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    ros::spinOnce();
+  }
+}
+
+TEST(tf2_ros_transform_listener, time_backwards)
+{
+
+  tf2_ros::Buffer buffer;
+  tf2_ros::TransformListener tfl(buffer);
+  tf2_ros::TransformBroadcaster tfb;
+
+  ros::NodeHandle nh = ros::NodeHandle();
+
+  ros::Publisher clock = nh.advertise<rosgraph_msgs::Clock>("/clock", 5);
+
+  rosgraph_msgs::Clock c;
+  c.clock = ros::Time(100);
+  clock.publish(c);
+
+  // basic test
+  ASSERT_FALSE(buffer.canTransform("foo", "bar", ros::Time(101, 0)));
+
+  // set the transform
+  geometry_msgs::TransformStamped msg;
+  msg.header.stamp = ros::Time(100, 0);
+  msg.header.frame_id = "foo";
+  msg.child_frame_id = "bar";
+  msg.transform.rotation.x = 1.0;
+  tfb.sendTransform(msg);
+  msg.header.stamp = ros::Time(102, 0);
+  tfb.sendTransform(msg);
+
+
+  // make sure it arrives
+  spin_for_a_second();
+
+  // verify it's been set
+  ASSERT_TRUE(buffer.canTransform("foo", "bar", ros::Time(101, 0)));
+
+  c.clock = ros::Time(90);
+  clock.publish(c);
+
+  // make sure it arrives
+  spin_for_a_second();
+
+  //Send another message to trigger clock test on an unrelated frame
+  msg.header.stamp = ros::Time(110, 0);
+  msg.header.frame_id = "foo2";
+  msg.child_frame_id = "bar2";
+  tfb.sendTransform(msg);
+
+  // make sure it arrives
+  spin_for_a_second();
+
+  //verify the data's been cleared
+  ASSERT_FALSE(buffer.canTransform("foo", "bar", ros::Time(101, 0)));
+
+}
+
+
+
+
+int main(int argc, char **argv){
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "transform_listener_backwards_reset");
+  return RUN_ALL_TESTS();
+}
--- a/navigations/geometry2/tf2_ros/test/transform_listener_time_reset_test.launch
+++ b/navigations/geometry2/tf2_ros/test/transform_listener_time_reset_test.launch
@@ -0,0 +1,4 @@
+<launch>
+  <test test-name="transform_listener_time_reset_test" pkg="tf2_ros" type="tf2_ros_test_time_reset" />
+  <param name="/use_sim_time" value="True"/>
+</launch>
--- a/navigations/geometry2/tf2_ros/test/transform_listener_unittest.launch
+++ b/navigations/geometry2/tf2_ros/test/transform_listener_unittest.launch
@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="transform_listener_unittest" pkg="tf2_ros" type="tf2_ros_test_listener" />
+</launch>