HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

248
navigations/geometry2/tf2_geometry_msgs/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,248 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package tf2_geometry_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.7.6 (2022-10-11)
------------------
* remove method with misleading doc (`#534 <https://github.com/ros/geometry2/issues/534>`_)
* Contributors: Wellington Castro
0.7.5 (2020-09-01)
------------------
0.7.4 (2020-09-01)
------------------
0.7.3 (2020-08-25)
------------------
* Use list instead of set to make build reproducible (`#473 <https://github.com/ros/geometry2/issues/473>`_)
* Contributors: Jochen Sprickerhof
0.7.2 (2020-06-08)
------------------
0.7.1 (2020-05-13)
------------------
* import setup from setuptools instead of distutils-core (`#449 <https://github.com/ros/geometry2/issues/449>`_)
* Contributors: Alejandro Hernández Cordero
0.7.0 (2020-03-09)
------------------
* Replace kdl packages with rosdep keys (`#447 <https://github.com/ros/geometry2/issues/447>`_)
* Bump CMake version to avoid CMP0048 warning (`#445 <https://github.com/ros/geometry2/issues/445>`_)
* Make kdl headers available (`#419 <https://github.com/ros/geometry2/issues/419>`_)
* FIx python3 compatibility for noetic (`#416 <https://github.com/ros/geometry2/issues/416>`_)
* add <array> from STL (`#366 <https://github.com/ros/geometry2/issues/366>`_)
* use ROS_DEPRECATED macro for portability (`#362 <https://github.com/ros/geometry2/issues/362>`_)
* use ROS_DEPRECATED for better portability
* change ROS_DEPRECATED position (`#5 <https://github.com/ros/geometry2/issues/5>`_)
* Contributors: James Xu, Shane Loretz, Tully Foote
0.6.5 (2018-11-16)
------------------
* Fix python3 import error
* Contributors: Timon Engelke
0.6.4 (2018-11-06)
------------------
0.6.3 (2018-07-09)
------------------
* Changed access to Vector to prevent memory leak (`#305 <https://github.com/ros/geometry2/issues/305>`_)
* Added WrenchStamped transformation (`#302 <https://github.com/ros/geometry2/issues/302>`_)
* Contributors: Denis Štogl, Markus Grimm
0.6.2 (2018-05-02)
------------------
0.6.1 (2018-03-21)
------------------
0.6.0 (2018-03-21)
------------------
* Boilerplate for Sphinx (`#284 <https://github.com/ros/geometry2/issues/284>`_)
Fixes `#264 <https://github.com/ros/geometry2/issues/264>`_
* tf2_geometry_msgs added doTransform implementations for not stamped types (`#262 <https://github.com/ros/geometry2/issues/262>`_)
* tf2_geometry_msgs added doTransform implementations for not stamped Point, Quaterion, Pose and Vector3 message types
* New functionality to transform PoseWithCovarianceStamped messages. (`#282 <https://github.com/ros/geometry2/issues/282>`_)
* New functionality to transform PoseWithCovarianceStamped messages.
* Contributors: Blake Anderson, Tully Foote, cwecht
0.5.17 (2018-01-01)
-------------------
0.5.16 (2017-07-14)
-------------------
* remove explicit templating to standardize on overloading. But provide backwards compatibility with deprecation.
* adding unit tests for conversions
* Copy transform before altering it in do_transform_vector3 [issue 233] (`#235 <https://github.com/ros/geometry2/issues/235>`_)
* store gtest return value as int (`#229 <https://github.com/ros/geometry2/issues/229>`_)
* Document the lifetime of the returned reference for getFrameId and getTimestamp
* tf2_geometry_msgs: using tf2::Transform in doTransform-functions, marked gmTransformToKDL as deprecated
* Switch tf2_geometry_msgs to use package.xml format 2 (`#217 <https://github.com/ros/geometry2/issues/217>`_)
* tf2_geometry_msgs: added missing conversion functions
* Contributors: Christopher Wecht, Sebastian Wagner, Tully Foote, dhood, pAIgn10
0.5.15 (2017-01-24)
-------------------
0.5.14 (2017-01-16)
-------------------
* Add doxygen documentation for tf2_geometry_msgs
* Contributors: Jackie Kay
0.5.13 (2016-03-04)
-------------------
* Add missing python_orocos_kdl dependency
* make example into unit test
* vector3 not affected by translation
* Contributors: Daniel Claes, chapulina
0.5.12 (2015-08-05)
-------------------
* Merge pull request `#112 <https://github.com/ros/geometry_experimental/issues/112>`_ from vrabaud/getYaw
Get yaw
* add toMsg and fromMsg for QuaternionStamped
* Contributors: Tully Foote, Vincent Rabaud
0.5.11 (2015-04-22)
-------------------
0.5.10 (2015-04-21)
-------------------
0.5.9 (2015-03-25)
------------------
0.5.8 (2015-03-17)
------------------
* remove useless Makefile files
* tf2 optimizations
* add conversions of type between tf2 and geometry_msgs
* fix ODR violations
* Contributors: Vincent Rabaud
0.5.7 (2014-12-23)
------------------
* fixing transitive dependency for kdl. Fixes `#53 <https://github.com/ros/geometry_experimental/issues/53>`_
* Contributors: Tully Foote
0.5.6 (2014-09-18)
------------------
0.5.5 (2014-06-23)
------------------
0.5.4 (2014-05-07)
------------------
0.5.3 (2014-02-21)
------------------
0.5.2 (2014-02-20)
------------------
0.5.1 (2014-02-14)
------------------
0.5.0 (2014-02-14)
------------------
0.4.10 (2013-12-26)
-------------------
0.4.9 (2013-11-06)
------------------
0.4.8 (2013-11-06)
------------------
0.4.7 (2013-08-28)
------------------
0.4.6 (2013-08-28)
------------------
0.4.5 (2013-07-11)
------------------
0.4.4 (2013-07-09)
------------------
* making repo use CATKIN_ENABLE_TESTING correctly and switching rostest to be a test_depend with that change.
0.4.3 (2013-07-05)
------------------
0.4.2 (2013-07-05)
------------------
0.4.1 (2013-07-05)
------------------
0.4.0 (2013-06-27)
------------------
* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2
* Cleaning up unnecessary dependency on roscpp
* converting contents of tf2_ros to be properly namespaced in the tf2_ros namespace
* Cleaning up packaging of tf2 including:
removing unused nodehandle
cleaning up a few dependencies and linking
removing old backup of package.xml
making diff minimally different from tf version of library
* Restoring test packages and bullet packages.
reverting 3570e8c42f9b394ecbfd9db076b920b41300ad55 to get back more of the packages previously implemented
reverting 04cf29d1b58c660fdc999ab83563a5d4b76ab331 to fix `#7 <https://github.com/ros/geometry_experimental/issues/7>`_
0.3.6 (2013-03-03)
------------------
0.3.5 (2013-02-15 14:46)
------------------------
* 0.3.4 -> 0.3.5
0.3.4 (2013-02-15 13:14)
------------------------
* 0.3.3 -> 0.3.4
0.3.3 (2013-02-15 11:30)
------------------------
* 0.3.2 -> 0.3.3
0.3.2 (2013-02-15 00:42)
------------------------
* 0.3.1 -> 0.3.2
0.3.1 (2013-02-14)
------------------
* 0.3.0 -> 0.3.1
0.3.0 (2013-02-13)
------------------
* switching to version 0.3.0
* add setup.py
* added setup.py etc to tf2_geometry_msgs
* adding tf2 dependency to tf2_geometry_msgs
* adding tf2_geometry_msgs to groovy-devel (unit tests disabled)
* fixing groovy-devel
* removing bullet and kdl related packages
* disabling tf2_geometry_msgs due to missing kdl dependency
* catkinizing geometry-experimental
* catkinizing tf2_geometry_msgs
* add twist, wrench and pose conversion to kdl, fix message to message conversion by adding specific conversion functions
* merge tf2_cpp and tf2_py into tf2_ros
* Got transform with types working in python
* A working first version of transforming and converting between different types
* Moving from camelCase to undescores to be in line with python style guides
* Fixing tests now that Buffer creates a NodeHandle
* add posestamped
* import vector3stamped
* add support for Vector3Stamped and PoseStamped
* add support for PointStamped geometry_msgs
* add regression tests for geometry_msgs point, vector and pose
* Fixing missing export, compiling version of buffer_client test
* add bullet transforms, and create tests for bullet and kdl
* working transformations of messages
* add support for PoseStamped message
* test for pointstamped
* add PointStamped message transform methods
* transform for vector3stamped message

52
navigations/geometry2/tf2_geometry_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,52 @@
cmake_minimum_required(VERSION 3.0.2)
project(tf2_geometry_msgs)
find_package(orocos_kdl)
find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2_ros tf2)
find_package(Boost COMPONENTS thread REQUIRED)
# Issue #53
find_library(KDL_LIBRARY REQUIRED NAMES orocos-kdl HINTS ${orocos_kdl_LIBRARY_DIRS})
catkin_package(
LIBRARIES ${KDL_LIBRARY}
INCLUDE_DIRS include
DEPENDS orocos_kdl
CATKIN_DEPENDS geometry_msgs tf2_ros tf2)
include_directories(include
${catkin_INCLUDE_DIRS}
)
link_directories(${orocos_kdl_LIBRARY_DIRS})
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
catkin_python_setup()
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(test_tomsg_frommsg test/test_tomsg_frommsg.cpp)
target_include_directories(test_tomsg_frommsg PUBLIC ${orocos_kdl_INCLUDE_DIRS})
target_link_libraries(test_tomsg_frommsg ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES})
find_package(catkin REQUIRED COMPONENTS geometry_msgs rostest tf2_ros tf2)
add_executable(test_geometry_msgs EXCLUDE_FROM_ALL test/test_tf2_geometry_msgs.cpp)
target_include_directories(test_geometry_msgs PUBLIC ${orocos_kdl_INCLUDE_DIRS})
target_link_libraries(test_geometry_msgs ${catkin_LIBRARIES} ${GTEST_LIBRARIES} ${orocos_kdl_LIBRARIES})
add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch)
add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_python.launch)
if(TARGET tests)
add_dependencies(tests test_geometry_msgs)
endif()
endif()

290
navigations/geometry2/tf2_geometry_msgs/conf.py Executable file
View File

@@ -0,0 +1,290 @@
# -*- coding: utf-8 -*-
#
# tf2_geometry_msgs documentation build configuration file, created by
# sphinx-quickstart on Tue Feb 13 15:34:25 2018.
#
# This file is execfile()d with the current directory set to its
# containing dir.
#
# Note that not all possible configuration values are present in this
# autogenerated file.
#
# All configuration values have a default; values that are commented out
# serve to show the default.
import sys
import os
# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
#sys.path.insert(0, os.path.abspath('.'))
# -- General configuration ------------------------------------------------
# If your documentation needs a minimal Sphinx version, state it here.
#needs_sphinx = '1.0'
# Add any Sphinx extension module names here, as strings. They can be
# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
# ones.
extensions = [
'sphinx.ext.autodoc',
'sphinx.ext.doctest',
'sphinx.ext.intersphinx',
'sphinx.ext.todo',
'sphinx.ext.pngmath',
'sphinx.ext.viewcode',
]
# Add any paths that contain templates here, relative to this directory.
templates_path = ['_templates']
# The suffix(es) of source filenames.
# You can specify multiple suffix as a list of string:
# source_suffix = ['.rst', '.md']
source_suffix = '.rst'
# The encoding of source files.
#source_encoding = 'utf-8-sig'
# The master toctree document.
master_doc = 'index'
# General information about the project.
project = u'tf2_geometry_msgs'
copyright = u'2018, Open Source Robotics Foundation, Inc.'
author = u'Tully Foote'
# The version info for the project you're documenting, acts as replacement for
# |version| and |release|, also used in various other places throughout the
# built documents.
#
# The short X.Y version.
version = u'0.1'
# The full version, including alpha/beta/rc tags.
release = u'0.1'
# The language for content autogenerated by Sphinx. Refer to documentation
# for a list of supported languages.
#
# This is also used if you do content translation via gettext catalogs.
# Usually you set "language" from the command line for these cases.
language = None
# There are two options for replacing |today|: either, you set today to some
# non-false value, then it is used:
#today = ''
# Else, today_fmt is used as the format for a strftime call.
#today_fmt = '%B %d, %Y'
# List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files.
exclude_patterns = ['_build']
# The reST default role (used for this markup: `text`) to use for all
# documents.
#default_role = None
# If true, '()' will be appended to :func: etc. cross-reference text.
#add_function_parentheses = True
# If true, the current module name will be prepended to all description
# unit titles (such as .. function::).
#add_module_names = True
# If true, sectionauthor and moduleauthor directives will be shown in the
# output. They are ignored by default.
#show_authors = False
# The name of the Pygments (syntax highlighting) style to use.
pygments_style = 'sphinx'
# A list of ignored prefixes for module index sorting.
#modindex_common_prefix = []
# If true, keep warnings as "system message" paragraphs in the built documents.
#keep_warnings = False
# -- Options for HTML output ---------------------------------------------------
# The theme to use for HTML and HTML Help pages. Major themes that come with
# Sphinx are currently 'default' and 'sphinxdoc'.
html_theme = 'default'
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
# documentation.
#html_theme_options = {}
# Add any paths that contain custom themes here, relative to this directory.
#html_theme_path = []
# The name for this set of Sphinx documents. If None, it defaults to
# "<project> v<release> documentation".
#html_title = None
# A shorter title for the navigation bar. Default is the same as html_title.
#html_short_title = None
# The name of an image file (relative to this directory) to place at the top
# of the sidebar.
#html_logo = None
# The name of an image file (relative to this directory) to use as a favicon of
# the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32
# pixels large.
#html_favicon = None
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ['_static']
# Add any extra paths that contain custom files (such as robots.txt or
# .htaccess) here, relative to this directory. These files are copied
# directly to the root of the documentation.
#html_extra_path = []
# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
# using the given strftime format.
#html_last_updated_fmt = '%b %d, %Y'
# If true, SmartyPants will be used to convert quotes and dashes to
# typographically correct entities.
#html_use_smartypants = True
# Custom sidebar templates, maps document names to template names.
#html_sidebars = {}
# Additional templates that should be rendered to pages, maps page names to
# template names.
#html_additional_pages = {}
# If false, no module index is generated.
#html_domain_indices = True
# If false, no index is generated.
#html_use_index = True
# If true, the index is split into individual pages for each letter.
#html_split_index = False
# If true, links to the reST sources are added to the pages.
#html_show_sourcelink = True
# If true, "Created using Sphinx" is shown in the HTML footer. Default is True.
#html_show_sphinx = True
# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True.
#html_show_copyright = True
# If true, an OpenSearch description file will be output, and all pages will
# contain a <link> tag referring to it. The value of this option must be the
# base URL from which the finished HTML is served.
#html_use_opensearch = ''
# This is the file name suffix for HTML files (e.g. ".xhtml").
#html_file_suffix = None
# Language to be used for generating the HTML full-text search index.
# Sphinx supports the following languages:
# 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja'
# 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr'
#html_search_language = 'en'
# A dictionary with options for the search language support, empty by default.
# Now only 'ja' uses this config value
#html_search_options = {'type': 'default'}
# The name of a javascript file (relative to the configuration directory) that
# implements a search results scorer. If empty, the default will be used.
#html_search_scorer = 'scorer.js'
# Output file base name for HTML help builder.
htmlhelp_basename = 'tf2_geometry_msgsdoc'
# -- Options for LaTeX output ---------------------------------------------
latex_elements = {
# The paper size ('letterpaper' or 'a4paper').
#'papersize': 'letterpaper',
# The font size ('10pt', '11pt' or '12pt').
#'pointsize': '10pt',
# Additional stuff for the LaTeX preamble.
#'preamble': '',
# Latex figure (float) alignment
#'figure_align': 'htbp',
}
# Grouping the document tree into LaTeX files. List of tuples
# (source start file, target name, title,
# author, documentclass [howto, manual, or own class]).
latex_documents = [
(master_doc, 'tf2_geometry_msgs.tex', u'tf2\\_geometry\\_msgs Documentation',
u'Tully Foote', 'manual'),
]
# The name of an image file (relative to this directory) to place at the top of
# the title page.
#latex_logo = None
# For "manual" documents, if this is true, then toplevel headings are parts,
# not chapters.
#latex_use_parts = False
# If true, show page references after internal links.
#latex_show_pagerefs = False
# If true, show URL addresses after external links.
#latex_show_urls = False
# Documents to append as an appendix to all manuals.
#latex_appendices = []
# If false, no module index is generated.
#latex_domain_indices = True
# -- Options for manual page output ---------------------------------------
# One entry per manual page. List of tuples
# (source start file, name, description, authors, manual section).
man_pages = [
(master_doc, 'tf2_geometry_msgs', u'tf2_geometry_msgs Documentation',
[author], 1)
]
# If true, show URL addresses after external links.
#man_show_urls = False
# -- Options for Texinfo output -------------------------------------------
# Grouping the document tree into Texinfo files. List of tuples
# (source start file, target name, title, author,
# dir menu entry, description, category)
texinfo_documents = [
(master_doc, 'tf2_geometry_msgs', u'tf2_geometry_msgs Documentation',
author, 'tf2_geometry_msgs', 'One line description of project.',
'Miscellaneous'),
]
# Documents to append as an appendix to all manuals.
#texinfo_appendices = []
# If false, no module index is generated.
#texinfo_domain_indices = True
# How to display URL addresses: 'footnote', 'no', or 'inline'.
#texinfo_show_urls = 'footnote'
# If true, do not generate a @detailmenu in the "Top" node's menu.
#texinfo_no_detailmenu = False
# Example configuration for intersphinx: refer to the Python standard library.
intersphinx_mapping = {'https://docs.python.org/': None}

1057
navigations/geometry2/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h Executable file
View File

File diff suppressed because it is too large Load Diff

17
navigations/geometry2/tf2_geometry_msgs/index.rst Executable file
View File

@@ -0,0 +1,17 @@
Welcome to tf2_geometry_msgs's documentation!
=============================================
Contents:
.. toctree::
:maxdepth: 2
Indices and tables
==================
* :ref:`genindex`
* :ref:`modindex`
* :ref:`search`

19
navigations/geometry2/tf2_geometry_msgs/mainpage.dox Executable file
View File

@@ -0,0 +1,19 @@
/**
\mainpage
\htmlinclude manifest.html
\b tf2_geometry_msgs contains functions for converting between various geometry_msgs data types.
This library is an implementation of the templated conversion interface specified in tf/convert.h.
It offers conversion and transform convenience functions between various geometry_msgs data types,
such as Vector, Point, Pose, Transform, Quaternion, etc.
See the <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">Conversions overview</A>
wiki page for more information about datatype conversion in tf2.
\section codeapi Code API
This library consists of one header only, tf2_geometry_msgs/tf2_geometry_msgs.h, which consists mostly of
specializations of template functions defined in tf2/convert.h.
*/

27
navigations/geometry2/tf2_geometry_msgs/package.xml Executable file
View File

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

7
navigations/geometry2/tf2_geometry_msgs/rosdoc.yaml Executable file
View File

@@ -0,0 +1,7 @@
- builder: doxygen
name: C++ API
output_dir: c++
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
- builder: sphinx
name: Python API
output_dir: python

100
navigations/geometry2/tf2_geometry_msgs/scripts/test.py Executable file
View File

@@ -0,0 +1,100 @@
#!/usr/bin/env python
import unittest
import rospy
import PyKDL
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped, WrenchStamped
class GeometryMsgs(unittest.TestCase):
def test_transform(self):
b = tf2_ros.Buffer()
t = TransformStamped()
t.transform.translation.x = 1
t.transform.rotation.x = 1
t.header.stamp = rospy.Time(2.0)
t.header.frame_id = 'a'
t.child_frame_id = 'b'
b.set_transform(t, 'eitan_rocks')
out = b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0))
self.assertEqual(out.transform.translation.x, 1)
self.assertEqual(out.transform.rotation.x, 1)
self.assertEqual(out.header.frame_id, 'a')
self.assertEqual(out.child_frame_id, 'b')
v = PointStamped()
v.header.stamp = rospy.Time(2)
v.header.frame_id = 'a'
v.point.x = 1
v.point.y = 2
v.point.z = 3
out = b.transform(v, 'b')
self.assertEqual(out.point.x, 0)
self.assertEqual(out.point.y, -2)
self.assertEqual(out.point.z, -3)
v = PoseStamped()
v.header.stamp = rospy.Time(2)
v.header.frame_id = 'a'
v.pose.position.x = 1
v.pose.position.y = 2
v.pose.position.z = 3
v.pose.orientation.x = 1
out = b.transform(v, 'b')
self.assertEqual(out.pose.position.x, 0)
self.assertEqual(out.pose.position.y, -2)
self.assertEqual(out.pose.position.z, -3)
# Translation shouldn't affect Vector3
t = TransformStamped()
t.transform.translation.x = 1
t.transform.translation.y = 2
t.transform.translation.z = 3
t.transform.rotation.w = 1
v = Vector3Stamped()
v.vector.x = 1
v.vector.y = 0
v.vector.z = 0
out = tf2_geometry_msgs.do_transform_vector3(v, t)
self.assertEqual(out.vector.x, 1)
self.assertEqual(out.vector.y, 0)
self.assertEqual(out.vector.z, 0)
# Rotate Vector3 180 deg about y
t = TransformStamped()
t.transform.translation.x = 1
t.transform.translation.y = 2
t.transform.translation.z = 3
t.transform.rotation.y = 1
v = Vector3Stamped()
v.vector.x = 1
v.vector.y = 0
v.vector.z = 0
out = tf2_geometry_msgs.do_transform_vector3(v, t)
self.assertEqual(out.vector.x, -1)
self.assertEqual(out.vector.y, 0)
self.assertEqual(out.vector.z, 0)
v = WrenchStamped()
v.wrench.force.x = 1
v.wrench.force.y = 0
v.wrench.force.z = 0
v.wrench.torque.x = 1
v.wrench.torque.y = 0
v.wrench.torque.z = 0
out = tf2_geometry_msgs.do_transform_wrench(v, t)
self.assertEqual(out.wrench.force.x, -1)
self.assertEqual(out.wrench.force.y, 0)
self.assertEqual(out.wrench.force.z, 0)
self.assertEqual(out.wrench.torque.x, -1)
self.assertEqual(out.wrench.torque.y, 0)
self.assertEqual(out.wrench.torque.z, 0)
if __name__ == '__main__':
import rosunit
rospy.init_node('test_tf2_geometry_msgs_python')
rosunit.unitrun("test_tf2_geometry_msgs", "test_tf2_geometry_msgs_python", GeometryMsgs)

13
navigations/geometry2/tf2_geometry_msgs/setup.py Executable file
View File

@@ -0,0 +1,13 @@
#!/usr/bin/env python
from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['tf2_geometry_msgs'],
package_dir={'': 'src'},
requires=['rospy','geometry_msgs','tf2_ros','orocos_kdl']
)
setup(**d)

1
navigations/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/__init__.py Executable file
View File

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

110
navigations/geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py Executable file
View File

@@ -0,0 +1,110 @@
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
# author: Wim Meeussen
from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped, WrenchStamped
import PyKDL
import rospy
import tf2_ros
import copy
def to_msg_msg(msg):
return msg
tf2_ros.ConvertRegistration().add_to_msg(Vector3Stamped, to_msg_msg)
tf2_ros.ConvertRegistration().add_to_msg(PoseStamped, to_msg_msg)
tf2_ros.ConvertRegistration().add_to_msg(PointStamped, to_msg_msg)
def from_msg_msg(msg):
return msg
tf2_ros.ConvertRegistration().add_from_msg(Vector3Stamped, from_msg_msg)
tf2_ros.ConvertRegistration().add_from_msg(PoseStamped, from_msg_msg)
tf2_ros.ConvertRegistration().add_from_msg(PointStamped, from_msg_msg)
def transform_to_kdl(t):
return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
t.transform.rotation.z, t.transform.rotation.w),
PyKDL.Vector(t.transform.translation.x,
t.transform.translation.y,
t.transform.translation.z))
# PointStamped
def do_transform_point(point, transform):
p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, point.point.y, point.point.z)
res = PointStamped()
res.point.x = p[0]
res.point.y = p[1]
res.point.z = p[2]
res.header = transform.header
return res
tf2_ros.TransformRegistration().add(PointStamped, do_transform_point)
# Vector3Stamped
def do_transform_vector3(vector3, transform):
transform = copy.deepcopy(transform)
transform.transform.translation.x = 0;
transform.transform.translation.y = 0;
transform.transform.translation.z = 0;
p = transform_to_kdl(transform) * PyKDL.Vector(vector3.vector.x, vector3.vector.y, vector3.vector.z)
res = Vector3Stamped()
res.vector.x = p[0]
res.vector.y = p[1]
res.vector.z = p[2]
res.header = transform.header
return res
tf2_ros.TransformRegistration().add(Vector3Stamped, do_transform_vector3)
# PoseStamped
def do_transform_pose(pose, transform):
f = transform_to_kdl(transform) * PyKDL.Frame(PyKDL.Rotation.Quaternion(pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w),
PyKDL.Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z))
res = PoseStamped()
res.pose.position.x = f[(0, 3)]
res.pose.position.y = f[(1, 3)]
res.pose.position.z = f[(2, 3)]
(res.pose.orientation.x, res.pose.orientation.y, res.pose.orientation.z, res.pose.orientation.w) = f.M.GetQuaternion()
res.header = transform.header
return res
tf2_ros.TransformRegistration().add(PoseStamped, do_transform_pose)
# WrenchStamped
def do_transform_wrench(wrench, transform):
force = Vector3Stamped()
torque = Vector3Stamped()
force.vector = wrench.wrench.force
torque.vector = wrench.wrench.torque
res = WrenchStamped()
res.wrench.force = do_transform_vector3(force, transform).vector
res.wrench.torque = do_transform_vector3(torque, transform).vector
res.header = transform.header
return res
tf2_ros.TransformRegistration().add(WrenchStamped, do_transform_wrench)

3
navigations/geometry2/tf2_geometry_msgs/test/test.launch Executable file
View File

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

3
navigations/geometry2/tf2_geometry_msgs/test/test_python.launch Executable file
View File

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

380
navigations/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Executable file
View File

@@ -0,0 +1,380 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Wim Meeussen */
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <ros/ros.h>
#include <gtest/gtest.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2_ros::Buffer* tf_buffer;
geometry_msgs::TransformStamped t;
static const double EPS = 1e-3;
TEST(TfGeometry, Frame)
{
geometry_msgs::PoseStamped v1;
v1.pose.position.x = 1;
v1.pose.position.y = 2;
v1.pose.position.z = 3;
v1.pose.orientation.x = 1;
v1.header.stamp = ros::Time(2);
v1.header.frame_id = "A";
// simple api
geometry_msgs::PoseStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
EXPECT_NEAR(v_simple.pose.position.x, -9, EPS);
EXPECT_NEAR(v_simple.pose.position.y, 18, EPS);
EXPECT_NEAR(v_simple.pose.position.z, 27, EPS);
EXPECT_NEAR(v_simple.pose.orientation.x, 0.0, EPS);
EXPECT_NEAR(v_simple.pose.orientation.y, 0.0, EPS);
EXPECT_NEAR(v_simple.pose.orientation.z, 0.0, EPS);
EXPECT_NEAR(v_simple.pose.orientation.w, 1.0, EPS);
// advanced api
geometry_msgs::PoseStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
"A", ros::Duration(3.0));
EXPECT_NEAR(v_advanced.pose.position.x, -9, EPS);
EXPECT_NEAR(v_advanced.pose.position.y, 18, EPS);
EXPECT_NEAR(v_advanced.pose.position.z, 27, EPS);
EXPECT_NEAR(v_advanced.pose.orientation.x, 0.0, EPS);
EXPECT_NEAR(v_advanced.pose.orientation.y, 0.0, EPS);
EXPECT_NEAR(v_advanced.pose.orientation.z, 0.0, EPS);
EXPECT_NEAR(v_advanced.pose.orientation.w, 1.0, EPS);
}
TEST(TfGeometry, PoseWithCovarianceStamped)
{
geometry_msgs::PoseWithCovarianceStamped v1;
v1.pose.pose.position.x = 1;
v1.pose.pose.position.y = 2;
v1.pose.pose.position.z = 3;
v1.pose.pose.orientation.x = 1;
v1.header.stamp = ros::Time(2);
v1.header.frame_id = "A";
v1.pose.covariance[0] = 1;
v1.pose.covariance[7] = 1;
v1.pose.covariance[14] = 1;
v1.pose.covariance[21] = 1;
v1.pose.covariance[28] = 1;
v1.pose.covariance[35] = 1;
// simple api
const geometry_msgs::PoseWithCovarianceStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
EXPECT_NEAR(v_simple.pose.pose.position.x, -9, EPS);
EXPECT_NEAR(v_simple.pose.pose.position.y, 18, EPS);
EXPECT_NEAR(v_simple.pose.pose.position.z, 27, EPS);
EXPECT_NEAR(v_simple.pose.pose.orientation.x, 0.0, EPS);
EXPECT_NEAR(v_simple.pose.pose.orientation.y, 0.0, EPS);
EXPECT_NEAR(v_simple.pose.pose.orientation.z, 0.0, EPS);
EXPECT_NEAR(v_simple.pose.pose.orientation.w, 1.0, EPS);
// no rotation in this transformation, so no change to covariance
EXPECT_NEAR(v_simple.pose.covariance[0], 1.0, EPS);
EXPECT_NEAR(v_simple.pose.covariance[7], 1.0, EPS);
EXPECT_NEAR(v_simple.pose.covariance[14], 1.0, EPS);
EXPECT_NEAR(v_simple.pose.covariance[21], 1.0, EPS);
EXPECT_NEAR(v_simple.pose.covariance[28], 1.0, EPS);
EXPECT_NEAR(v_simple.pose.covariance[35], 1.0, EPS);
// advanced api
const geometry_msgs::PoseWithCovarianceStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
"A", ros::Duration(3.0));
EXPECT_NEAR(v_advanced.pose.pose.position.x, -9, EPS);
EXPECT_NEAR(v_advanced.pose.pose.position.y, 18, EPS);
EXPECT_NEAR(v_advanced.pose.pose.position.z, 27, EPS);
EXPECT_NEAR(v_advanced.pose.pose.orientation.x, 0.0, EPS);
EXPECT_NEAR(v_advanced.pose.pose.orientation.y, 0.0, EPS);
EXPECT_NEAR(v_advanced.pose.pose.orientation.z, 0.0, EPS);
EXPECT_NEAR(v_advanced.pose.pose.orientation.w, 1.0, EPS);
// no rotation in this transformation, so no change to covariance
EXPECT_NEAR(v_advanced.pose.covariance[0], 1.0, EPS);
EXPECT_NEAR(v_advanced.pose.covariance[7], 1.0, EPS);
EXPECT_NEAR(v_advanced.pose.covariance[14], 1.0, EPS);
EXPECT_NEAR(v_advanced.pose.covariance[21], 1.0, EPS);
EXPECT_NEAR(v_advanced.pose.covariance[28], 1.0, EPS);
EXPECT_NEAR(v_advanced.pose.covariance[35], 1.0, EPS);
/** now add rotation to transform to test the effect on covariance **/
// rotate pi/2 radians about x-axis
geometry_msgs::TransformStamped t_rot;
t_rot.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(1,0,0), M_PI/2));
t_rot.header.stamp = ros::Time(2.0);
t_rot.header.frame_id = "A";
t_rot.child_frame_id = "rotated";
tf_buffer->setTransform(t_rot, "rotation_test");
// need to put some covariance in the matrix
v1.pose.covariance[1] = 1;
v1.pose.covariance[6] = 1;
v1.pose.covariance[12] = 1;
// perform rotation
const geometry_msgs::PoseWithCovarianceStamped v_rotated = tf_buffer->transform(v1, "rotated", ros::Duration(2.0));
// the covariance matrix should now be transformed
EXPECT_NEAR(v_rotated.pose.covariance[0], 1.0, EPS);
EXPECT_NEAR(v_rotated.pose.covariance[1], 0.0, EPS);
EXPECT_NEAR(v_rotated.pose.covariance[2],-1.0, EPS);
EXPECT_NEAR(v_rotated.pose.covariance[6], 1.0, EPS);
EXPECT_NEAR(v_rotated.pose.covariance[7], 1.0, EPS);
EXPECT_NEAR(v_rotated.pose.covariance[8], 0.0, EPS);
EXPECT_NEAR(v_rotated.pose.covariance[12],-1.0, EPS);
EXPECT_NEAR(v_rotated.pose.covariance[13], 0.0, EPS);
EXPECT_NEAR(v_rotated.pose.covariance[14], 1.0, EPS);
// set buffer back to original transform
tf_buffer->setTransform(t, "test");
}
TEST(TfGeometry, Transform)
{
geometry_msgs::TransformStamped v1;
v1.transform.translation.x = 1;
v1.transform.translation.y = 2;
v1.transform.translation.z = 3;
v1.transform.rotation.x = 1;
v1.header.stamp = ros::Time(2);
v1.header.frame_id = "A";
// simple api
geometry_msgs::TransformStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
EXPECT_NEAR(v_simple.transform.translation.x, -9, EPS);
EXPECT_NEAR(v_simple.transform.translation.y, 18, EPS);
EXPECT_NEAR(v_simple.transform.translation.z, 27, EPS);
EXPECT_NEAR(v_simple.transform.rotation.x, 0.0, EPS);
EXPECT_NEAR(v_simple.transform.rotation.y, 0.0, EPS);
EXPECT_NEAR(v_simple.transform.rotation.z, 0.0, EPS);
EXPECT_NEAR(v_simple.transform.rotation.w, 1.0, EPS);
// advanced api
geometry_msgs::TransformStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
"A", ros::Duration(3.0));
EXPECT_NEAR(v_advanced.transform.translation.x, -9, EPS);
EXPECT_NEAR(v_advanced.transform.translation.y, 18, EPS);
EXPECT_NEAR(v_advanced.transform.translation.z, 27, EPS);
EXPECT_NEAR(v_advanced.transform.rotation.x, 0.0, EPS);
EXPECT_NEAR(v_advanced.transform.rotation.y, 0.0, EPS);
EXPECT_NEAR(v_advanced.transform.rotation.z, 0.0, EPS);
EXPECT_NEAR(v_advanced.transform.rotation.w, 1.0, EPS);
}
TEST(TfGeometry, Vector)
{
geometry_msgs::Vector3Stamped v1, res;
v1.vector.x = 1;
v1.vector.y = 2;
v1.vector.z = 3;
v1.header.stamp = ros::Time(2.0);
v1.header.frame_id = "A";
// simple api
geometry_msgs::Vector3Stamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
EXPECT_NEAR(v_simple.vector.x, 1, EPS);
EXPECT_NEAR(v_simple.vector.y, -2, EPS);
EXPECT_NEAR(v_simple.vector.z, -3, EPS);
// advanced api
geometry_msgs::Vector3Stamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
"A", ros::Duration(3.0));
EXPECT_NEAR(v_advanced.vector.x, 1, EPS);
EXPECT_NEAR(v_advanced.vector.y, -2, EPS);
EXPECT_NEAR(v_advanced.vector.z, -3, EPS);
}
TEST(TfGeometry, Point)
{
geometry_msgs::PointStamped v1, res;
v1.point.x = 1;
v1.point.y = 2;
v1.point.z = 3;
v1.header.stamp = ros::Time(2.0);
v1.header.frame_id = "A";
// simple api
geometry_msgs::PointStamped v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
EXPECT_NEAR(v_simple.point.x, -9, EPS);
EXPECT_NEAR(v_simple.point.y, 18, EPS);
EXPECT_NEAR(v_simple.point.z, 27, EPS);
// advanced api
geometry_msgs::PointStamped v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
"A", ros::Duration(3.0));
EXPECT_NEAR(v_advanced.point.x, -9, EPS);
EXPECT_NEAR(v_advanced.point.y, 18, EPS);
EXPECT_NEAR(v_advanced.point.z, 27, EPS);
}
TEST(TfGeometry, doTransformPoint)
{
geometry_msgs::Point v1, res;
v1.x = 2;
v1.y = 1;
v1.z = 3;
geometry_msgs::TransformStamped trafo;
trafo.transform.translation.x = -1;
trafo.transform.translation.y = 2;
trafo.transform.translation.z = -3;
trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
tf2::doTransform(v1, res, trafo);
EXPECT_NEAR(res.x, 0, EPS);
EXPECT_NEAR(res.y, 0, EPS);
EXPECT_NEAR(res.z, 0, EPS);
}
TEST(TfGeometry, doTransformQuaterion)
{
geometry_msgs::Quaternion v1, res;
v1.w = 1;
geometry_msgs::TransformStamped trafo;
trafo.transform.translation.x = -1;
trafo.transform.translation.y = 2;
trafo.transform.translation.z = -3;
trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
tf2::doTransform(v1, res, trafo);
EXPECT_NEAR(res.x, trafo.transform.rotation.x, EPS);
EXPECT_NEAR(res.y, trafo.transform.rotation.y, EPS);
EXPECT_NEAR(res.z, trafo.transform.rotation.z, EPS);
EXPECT_NEAR(res.w, trafo.transform.rotation.w, EPS);
}
TEST(TfGeometry, doTransformPose)
{
geometry_msgs::Pose v1, res;
v1.position.x = 2;
v1.position.y = 1;
v1.position.z = 3;
v1.orientation.w = 1;
geometry_msgs::TransformStamped trafo;
trafo.transform.translation.x = -1;
trafo.transform.translation.y = 2;
trafo.transform.translation.z = -3;
trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
tf2::doTransform(v1, res, trafo);
EXPECT_NEAR(res.position.x, 0, EPS);
EXPECT_NEAR(res.position.y, 0, EPS);
EXPECT_NEAR(res.position.z, 0, EPS);
EXPECT_NEAR(res.orientation.x, trafo.transform.rotation.x, EPS);
EXPECT_NEAR(res.orientation.y, trafo.transform.rotation.y, EPS);
EXPECT_NEAR(res.orientation.z, trafo.transform.rotation.z, EPS);
EXPECT_NEAR(res.orientation.w, trafo.transform.rotation.w, EPS);
}
TEST(TfGeometry, doTransformVector3)
{
geometry_msgs::Vector3 v1, res;
v1.x = 2;
v1.y = 1;
v1.z = 3;
geometry_msgs::TransformStamped trafo;
trafo.transform.translation.x = -1;
trafo.transform.translation.y = 2;
trafo.transform.translation.z = -3;
trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
tf2::doTransform(v1, res, trafo);
EXPECT_NEAR(res.x, 1, EPS);
EXPECT_NEAR(res.y, -2, EPS);
EXPECT_NEAR(res.z, 3, EPS);
}
TEST(TfGeometry, doTransformWrench)
{
geometry_msgs::Wrench v1, res;
v1.force.x = 2;
v1.force.y = 1;
v1.force.z = 3;
v1.torque.x = 2;
v1.torque.y = 1;
v1.torque.z = 3;
geometry_msgs::TransformStamped trafo;
trafo.transform.translation.x = -1;
trafo.transform.translation.y = 2;
trafo.transform.translation.z = -3;
trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0));
tf2::doTransform(v1, res, trafo);
EXPECT_NEAR(res.force.x, 1, EPS);
EXPECT_NEAR(res.force.y, -2, EPS);
EXPECT_NEAR(res.force.z, 3, EPS);
EXPECT_NEAR(res.torque.x, 1, EPS);
EXPECT_NEAR(res.torque.y, -2, EPS);
EXPECT_NEAR(res.torque.z, 3, EPS);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test");
ros::NodeHandle n;
tf_buffer = new tf2_ros::Buffer();
tf_buffer->setUsingDedicatedThread(true);
// populate buffer
t.transform.translation.x = 10;
t.transform.translation.y = 20;
t.transform.translation.z = 30;
t.transform.rotation.x = 1;
t.header.stamp = ros::Time(2.0);
t.header.frame_id = "A";
t.child_frame_id = "B";
tf_buffer->setTransform(t, "test");
int ret = RUN_ALL_TESTS();
delete tf_buffer;
return ret;
}

405
navigations/geometry2/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp Executable file
View File

@@ -0,0 +1,405 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/** \author Wim Meeussen */
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <gtest/gtest.h>
static const double EPS = 1e-6;
tf2::Vector3 get_tf2_vector()
{
return tf2::Vector3(1.0, 2.0, 3.0);
}
geometry_msgs::Vector3& value_initialize(geometry_msgs::Vector3 &m1)
{
m1.x = 1;
m1.y = 2;
m1.z = 3;
return m1;
}
std_msgs::Header& value_initialize(std_msgs::Header & h)
{
h.stamp = ros::Time(10);
h.frame_id = "foobar";
return h;
}
geometry_msgs::Vector3Stamped& value_initialize(geometry_msgs::Vector3Stamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.vector);
return m1;
}
geometry_msgs::Point& value_initialize(geometry_msgs::Point &m1)
{
m1.x = 1;
m1.y = 2;
m1.z = 3;
return m1;
}
geometry_msgs::PointStamped& value_initialize(geometry_msgs::PointStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.point);
return m1;
}
geometry_msgs::Quaternion & value_initialize(geometry_msgs::Quaternion &m1)
{
m1.x = 0;
m1.y = 0;
m1.z = 0.7071067811;
m1.w = 0.7071067811;
return m1;
}
geometry_msgs::QuaternionStamped& value_initialize(geometry_msgs::QuaternionStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.quaternion);
return m1;
}
geometry_msgs::Pose & value_initialize(geometry_msgs::Pose & m1)
{
value_initialize(m1.position);
value_initialize(m1.orientation);
return m1;
}
geometry_msgs::PoseStamped& value_initialize(geometry_msgs::PoseStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.pose);
return m1;
}
geometry_msgs::Transform & value_initialize(geometry_msgs::Transform & m1)
{
value_initialize(m1.translation);
value_initialize(m1.rotation);
return m1;
}
geometry_msgs::TransformStamped& value_initialize(geometry_msgs::TransformStamped &m1)
{
value_initialize(m1.header);
value_initialize(m1.transform);
return m1;
}
void expect_near(const std_msgs::Header & h1, const std_msgs::Header & h2)
{
EXPECT_NEAR(h1.stamp.toSec(), h2.stamp.toSec(), EPS);
EXPECT_STREQ(h1.frame_id.c_str(), h2.frame_id.c_str());
}
/*
* Vector3
*/
void expect_near(const geometry_msgs::Vector3 & v1, const tf2::Vector3 & v2)
{
EXPECT_NEAR(v1.x, v2.x(), EPS);
EXPECT_NEAR(v1.y, v2.y(), EPS);
EXPECT_NEAR(v1.z, v2.z(), EPS);
}
void expect_near(const geometry_msgs::Vector3 & v1, const geometry_msgs::Vector3 & v2)
{
EXPECT_NEAR(v1.x, v2.x, EPS);
EXPECT_NEAR(v1.y, v2.y, EPS);
EXPECT_NEAR(v1.z, v2.z, EPS);
}
void expect_near(const tf2::Vector3 & v1, const tf2::Vector3 & v2)
{
EXPECT_NEAR(v1.x(), v2.x(), EPS);
EXPECT_NEAR(v1.y(), v2.y(), EPS);
EXPECT_NEAR(v1.z(), v2.z(), EPS);
}
void expect_near(const geometry_msgs::Vector3Stamped & p1, const geometry_msgs::Vector3Stamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.vector, p2.vector);
}
/*
* Point
*/
void expect_near(const geometry_msgs::Point & p1, const tf2::Vector3 & v2)
{
EXPECT_NEAR(p1.x, v2.x(), EPS);
EXPECT_NEAR(p1.y, v2.y(), EPS);
EXPECT_NEAR(p1.z, v2.z(), EPS);
}
void expect_near(const geometry_msgs::Point & p1, const geometry_msgs::Point & v2)
{
EXPECT_NEAR(p1.x, v2.x, EPS);
EXPECT_NEAR(p1.y, v2.y, EPS);
EXPECT_NEAR(p1.z, v2.z, EPS);
}
void expect_near(const geometry_msgs::PointStamped & p1, const geometry_msgs::PointStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.point, p2.point);
}
/*
* Quaternion
*/
void expect_near(const geometry_msgs::Quaternion & q1, const tf2::Quaternion & v2)
{
EXPECT_NEAR(q1.x, v2.x(), EPS);
EXPECT_NEAR(q1.y, v2.y(), EPS);
EXPECT_NEAR(q1.z, v2.z(), EPS);
}
void expect_near(const geometry_msgs::Quaternion & q1, const geometry_msgs::Quaternion & v2)
{
EXPECT_NEAR(q1.x, v2.x, EPS);
EXPECT_NEAR(q1.y, v2.y, EPS);
EXPECT_NEAR(q1.z, v2.z, EPS);
}
void expect_near(const geometry_msgs::QuaternionStamped & p1, const geometry_msgs::QuaternionStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.quaternion, p2.quaternion);
}
/*
* Pose
*/
void expect_near(const geometry_msgs::Pose & p, const tf2::Transform & t)
{
expect_near(p.position, t.getOrigin());
expect_near(p.orientation, t.getRotation());
}
void expect_near(const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2)
{
expect_near(p1.position, p2.position);
expect_near(p1.orientation, p2.orientation);
}
void expect_near(const geometry_msgs::PoseStamped & p1, const geometry_msgs::PoseStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.pose, p2.pose);
}
/*
* Transform
*/
void expect_near(const geometry_msgs::Transform & p, const tf2::Transform & t)
{
expect_near(p.translation, t.getOrigin());
expect_near(p.rotation, t.getRotation());
}
void expect_near(const geometry_msgs::Transform & p1, const geometry_msgs::Transform & p2)
{
expect_near(p1.translation, p2.translation);
expect_near(p1.rotation, p2.rotation);
}
void expect_near(const geometry_msgs::TransformStamped & p1, const geometry_msgs::TransformStamped & p2)
{
expect_near(p1.header, p2.header);
expect_near(p1.transform, p2.transform);
}
/*
* Stamped templated expect_near
*/
template <typename T>
void expect_near(const tf2::Stamped<T>& s1, const tf2::Stamped<T>& s2)
{
expect_near((T)s1, (T)s2);
}
/*********************
* Tests
*********************/
TEST(tf2_geometry_msgs, Vector3)
{
geometry_msgs::Vector3 m1;
value_initialize(m1);
tf2::Vector3 v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
expect_near(m1, v1);
geometry_msgs::Vector3 m2 = toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, Point)
{
geometry_msgs::Point m1;
value_initialize(m1);
tf2::Vector3 v1;
SCOPED_TRACE("m1 v1");
fromMsg(m1, v1);
expect_near(m1, v1);
geometry_msgs::Point m2 = toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, Quaternion)
{
geometry_msgs::Quaternion m1;
value_initialize(m1);
tf2::Quaternion q1;
SCOPED_TRACE("m1 q1");
fromMsg(m1, q1);
expect_near(m1, q1);
geometry_msgs::Quaternion m2 = toMsg(q1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, Pose)
{
geometry_msgs::Pose m1;
value_initialize(m1);
tf2::Transform t1;
fromMsg(m1, t1);
SCOPED_TRACE("m1 t1");
expect_near(m1, t1);
geometry_msgs::Pose m2 = toMsg(t1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, Transform)
{
geometry_msgs::Transform m1;
value_initialize(m1);
tf2::Transform t1;
fromMsg(m1, t1);
SCOPED_TRACE("m1 t1");
expect_near(m1, t1);
geometry_msgs::Transform m2 = toMsg(t1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, Vector3Stamped)
{
geometry_msgs::Vector3Stamped m1;
value_initialize(m1);
tf2::Stamped<tf2::Vector3> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1);
geometry_msgs::Vector3Stamped m2;
m2 = toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, PointStamped)
{
geometry_msgs::PointStamped m1;
value_initialize(m1);
tf2::Stamped<tf2::Vector3> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::PointStamped m2;
m2 = toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, QuaternionStamped)
{
geometry_msgs::QuaternionStamped m1;
value_initialize(m1);
tf2::Stamped<tf2::Quaternion> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::QuaternionStamped m2;
m2 = tf2::toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, PoseStamped)
{
geometry_msgs::PoseStamped m1;
value_initialize(m1);
tf2::Stamped<tf2::Transform> v1;
SCOPED_TRACE("m1 v1");
fromMsg(m1, v1);
// expect_near(m1, v1); //TODO implement cross verification explicityly
geometry_msgs::PoseStamped m2;
m2 = tf2::toMsg(v1, m2);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
TEST(tf2_geometry_msgs, TransformStamped)
{
geometry_msgs::TransformStamped m1;
value_initialize(m1);
tf2::Stamped<tf2::Transform> v1;
fromMsg(m1, v1);
SCOPED_TRACE("m1 v1");
// expect_near(m1, v1);
geometry_msgs::TransformStamped m2;
m2 = tf2::toMsg(v1);
SCOPED_TRACE("m1 m2");
expect_near(m1, m2);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}

371
navigations/geometry2/tf2_ros/CHANGELOG.rst Executable file
View File

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

153
navigations/geometry2/tf2_ros/CMakeLists.txt Executable file
View File

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

204
navigations/geometry2/tf2_ros/doc/conf.py Executable file
View File

@@ -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"

48
navigations/geometry2/tf2_ros/doc/index.rst Executable file
View File

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

41
navigations/geometry2/tf2_ros/doc/mainpage.dox Executable file
View File

@@ -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.
*/

73
navigations/geometry2/tf2_ros/doc/tf2_ros.rst Executable file
View File

@@ -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:

144
navigations/geometry2/tf2_ros/include/tf2_ros/buffer.h Executable file
View File

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

139
navigations/geometry2/tf2_ros/include/tf2_ros/buffer_client.h Executable file
View File

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

267
navigations/geometry2/tf2_ros/include/tf2_ros/buffer_interface.h Executable file
View File

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

92
navigations/geometry2/tf2_ros/include/tf2_ros/buffer_server.h Executable file
View File

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

716
navigations/geometry2/tf2_ros/include/tf2_ros/message_filter.h Executable file
View File

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

75
navigations/geometry2/tf2_ros/include/tf2_ros/static_transform_broadcaster.h Executable file
View File

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

78
navigations/geometry2/tf2_ros/include/tf2_ros/transform_broadcaster.h Executable file
View File

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

92
navigations/geometry2/tf2_ros/include/tf2_ros/transform_listener.h Executable file
View File

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

43
navigations/geometry2/tf2_ros/package.xml Executable file
View File

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

8
navigations/geometry2/tf2_ros/rosdoc.yaml Executable file
View File

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

13
navigations/geometry2/tf2_ros/setup.py Executable file
View File

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

201
navigations/geometry2/tf2_ros/src/buffer.cpp Executable file
View File

@@ -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;
}
}

162
navigations/geometry2/tf2_ros/src/buffer_client.cpp Executable file
View File

@@ -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;
}
}
};

222
navigations/geometry2/tf2_ros/src/buffer_server.cpp Executable file
View File

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

71
navigations/geometry2/tf2_ros/src/buffer_server_main.cpp Executable file
View File

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

64
navigations/geometry2/tf2_ros/src/static_transform_broadcaster.cpp Executable file
View File

@@ -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_);
}
}

145
navigations/geometry2/tf2_ros/src/static_transform_broadcaster_program.cpp Executable file
View File

@@ -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;
};

44
navigations/geometry2/tf2_ros/src/tf2_ros/__init__.py Executable file
View File

@@ -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 *

BIN
navigations/geometry2/tf2_ros/src/tf2_ros/__pycache__/buffer.cpython-38.pyc Normal file
View File

Binary file not shown.

BIN
navigations/geometry2/tf2_ros/src/tf2_ros/__pycache__/buffer_client.cpython-38.pyc Normal file
View File

Binary file not shown.

BIN
navigations/geometry2/tf2_ros/src/tf2_ros/__pycache__/buffer_interface.cpython-38.pyc Normal file
View File

Binary file not shown.

BIN
navigations/geometry2/tf2_ros/src/tf2_ros/__pycache__/static_transform_broadcaster.cpython-38.pyc Normal file
View File

Binary file not shown.

BIN
navigations/geometry2/tf2_ros/src/tf2_ros/__pycache__/transform_broadcaster.cpython-38.pyc Normal file
View File

Binary file not shown.

BIN
navigations/geometry2/tf2_ros/src/tf2_ros/__pycache__/transform_listener.cpython-38.pyc Normal file
View File

Binary file not shown.

162
navigations/geometry2/tf2_ros/src/tf2_ros/buffer.py Executable file
View File

@@ -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]

196
navigations/geometry2/tf2_ros/src/tf2_ros/buffer_client.py Executable file
View File

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

251
navigations/geometry2/tf2_ros/src/tf2_ros/buffer_interface.py Executable file
View File

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

51
navigations/geometry2/tf2_ros/src/tf2_ros/static_transform_broadcaster.py Executable file
View File

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

56
navigations/geometry2/tf2_ros/src/tf2_ros/transform_broadcaster.py Executable file
View File

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

89
navigations/geometry2/tf2_ros/src/tf2_ros/transform_listener.py Executable file
View File

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

66
navigations/geometry2/tf2_ros/src/transform_broadcaster.cpp Executable file
View File

@@ -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);
}
}

136
navigations/geometry2/tf2_ros/src/transform_listener.cpp Executable file
View File

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

51
navigations/geometry2/tf2_ros/test/listener_unittest.cpp Executable file
View File

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

125
navigations/geometry2/tf2_ros/test/message_filter_test.cpp Executable file
View File

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

4
navigations/geometry2/tf2_ros/test/message_filter_test.launch Executable file
View File

@@ -0,0 +1,4 @@
<launch>
<test test-name="message_filter_test" pkg="tf2_ros" type="tf2_ros_test_message_filter" />
</launch>

111
navigations/geometry2/tf2_ros/test/time_reset_test.cpp Executable file
View File

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

4
navigations/geometry2/tf2_ros/test/transform_listener_time_reset_test.launch Executable file
View File

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

3
navigations/geometry2/tf2_ros/test/transform_listener_unittest.launch Executable file
View File

@@ -0,0 +1,3 @@
<launch>
<test test-name="transform_listener_unittest" pkg="tf2_ros" type="tf2_ros_test_listener" />
</launch>

110
navigations/geometry2/tf2_sensor_msgs/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,110 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package tf2_sensor_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.7.6 (2022-10-11)
------------------
0.7.5 (2020-09-01)
------------------
0.7.4 (2020-09-01)
------------------
0.7.3 (2020-08-25)
------------------
* Use list instead of set to make build reproducible (`#473 <https://github.com/ros/geometry2/issues/473>`_)
* Contributors: Jochen Sprickerhof
0.7.2 (2020-06-08)
------------------
0.7.1 (2020-05-13)
------------------
* import setup from setuptools instead of distutils-core (`#449 <https://github.com/ros/geometry2/issues/449>`_)
* Contributors: Alejandro Hernández Cordero
0.7.0 (2020-03-09)
------------------
* Replace kdl packages with rosdep keys (`#447 <https://github.com/ros/geometry2/issues/447>`_)
* Bump CMake version to avoid CMP0048 warning (`#445 <https://github.com/ros/geometry2/issues/445>`_)
* Merge pull request `#378 <https://github.com/ros/geometry2/issues/378>`_ from peci1/tf2_sensor_msgs_isometry
Affine->Isometry
* Python 3 compatibility: relative imports and print statement
* Contributors: Martin Pecka, Shane Loretz, Timon Engelke, Tully Foote
0.6.5 (2018-11-16)
------------------
0.6.4 (2018-11-06)
------------------
0.6.3 (2018-07-09)
------------------
0.6.2 (2018-05-02)
------------------
0.6.1 (2018-03-21)
------------------
0.6.0 (2018-03-21)
------------------
0.5.17 (2018-01-01)
-------------------
* Merge pull request `#257 <https://github.com/ros/geometry2/issues/257>`_ from delftrobotics-forks/python3
Make tf2_py python3 compatible again
* Use python3 print function.
* Contributors: Maarten de Vries, Tully Foote
0.5.16 (2017-07-14)
-------------------
* Fix do_transform_cloud for multi-channelled pointcloud2. (`#241 <https://github.com/ros/geometry2/issues/241>`_)
* store gtest return value as int (`#229 <https://github.com/ros/geometry2/issues/229>`_)
* Document the lifetime of the returned reference for getFrameId and getTimestamp
* Find eigen in a much nicer way.
* Switch tf2_sensor_msgs over to package format 2.
* Contributors: Atsushi Watanabe, Chris Lalancette, dhood
0.5.15 (2017-01-24)
-------------------
0.5.14 (2017-01-16)
-------------------
0.5.13 (2016-03-04)
-------------------
* add missing Python runtime dependency
* fix wrong comment
* Adding tests to package
* Fixing do_transform_cloud for python
The previous code was not used at all (it was a mistake in the __init_\_.py so
the do_transform_cloud was not available to the python users).
The python code need some little correction (e.g there is no method named
read_cloud but it's read_points for instance, and as we are in python we can't
use the same trick as in c++ when we got an immutable)
* Contributors: Laurent GEORGE, Vincent Rabaud
0.5.12 (2015-08-05)
-------------------
0.5.11 (2015-04-22)
-------------------
0.5.10 (2015-04-21)
-------------------
0.5.9 (2015-03-25)
------------------
0.5.8 (2015-03-17)
------------------
* ODR violation fixes and more conversions
* Fix keeping original pointcloud header in transformed pointcloud
* Contributors: Paul Bovbel, Tully Foote, Vincent Rabaud
0.5.7 (2014-12-23)
------------------
* add support for transforming sensor_msgs::PointCloud2
* Contributors: Vincent Rabaud

56
navigations/geometry2/tf2_sensor_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,56 @@
cmake_minimum_required(VERSION 3.0.2)
project(tf2_sensor_msgs)
find_package(catkin REQUIRED COMPONENTS cmake_modules sensor_msgs tf2_ros tf2)
find_package(Boost COMPONENTS thread REQUIRED)
# Finding Eigen is somewhat complicated because of our need to support Ubuntu
# all the way back to saucy. First we look for the Eigen3 cmake module
# provided by the libeigen3-dev on newer Ubuntu. If that fails, then we
# fall-back to the version provided by cmake_modules, which is a stand-in.
find_package(Eigen3 QUIET)
if(NOT EIGEN3_FOUND)
find_package(cmake_modules REQUIRED)
find_package(Eigen REQUIRED)
set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
endif()
# Note that eigen 3.2 (on Ubuntu Wily) only provides EIGEN3_INCLUDE_DIR,
# not EIGEN3_INCLUDE_DIRS, so we have to set the latter from the former.
if(NOT EIGEN3_INCLUDE_DIRS)
set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
endif()
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS sensor_msgs tf2_ros tf2
DEPENDS EIGEN3
)
include_directories(include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
catkin_python_setup()
if(CATKIN_ENABLE_TESTING)
catkin_add_nosetests(test/test_tf2_sensor_msgs.py)
find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs rostest tf2_ros tf2)
add_executable(test_tf2_sensor_msgs_cpp EXCLUDE_FROM_ALL test/test_tf2_sensor_msgs.cpp)
target_link_libraries(test_tf2_sensor_msgs_cpp ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
if(TARGET tests)
add_dependencies(tests test_tf2_sensor_msgs_cpp)
endif()
add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test.launch)
endif()

107
navigations/geometry2/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h Executable file
View File

@@ -0,0 +1,107 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef TF2_SENSOR_MSGS_H
#define TF2_SENSOR_MSGS_H
#include <tf2/convert.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
namespace tf2
{
/********************/
/** PointCloud2 **/
/********************/
/** \brief Extract a timestamp from the header of a PointCloud2 message.
* This function is a specialization of the getTimestamp template defined in tf2/convert.h.
* \param t PointCloud2 message to extract the timestamp from.
* \return The timestamp of the message. The lifetime of the returned reference
* is bound to the lifetime of the argument.
*/
template <>
inline
const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;}
/** \brief Extract a frame ID from the header of a PointCloud2 message.
* This function is a specialization of the getFrameId template defined in tf2/convert.h.
* \param t PointCloud2 message to extract the frame ID from.
* \return A string containing the frame ID of the message. The lifetime of the
* returned reference is bound to the lifetime of the argument.
*/
template <>
inline
const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}
// this method needs to be implemented by client library developers
template <>
inline
void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)
{
p_out = p_in;
p_out.header = t_in.header;
Eigen::Transform<float,3,Eigen::Isometry> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y,
t_in.transform.translation.z) * Eigen::Quaternion<float>(
t_in.transform.rotation.w, t_in.transform.rotation.x,
t_in.transform.rotation.y, t_in.transform.rotation.z);
sensor_msgs::PointCloud2ConstIterator<float> x_in(p_in, "x");
sensor_msgs::PointCloud2ConstIterator<float> y_in(p_in, "y");
sensor_msgs::PointCloud2ConstIterator<float> z_in(p_in, "z");
sensor_msgs::PointCloud2Iterator<float> x_out(p_out, "x");
sensor_msgs::PointCloud2Iterator<float> y_out(p_out, "y");
sensor_msgs::PointCloud2Iterator<float> z_out(p_out, "z");
Eigen::Vector3f point;
for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);
*x_out = point.x();
*y_out = point.y();
*z_out = point.z();
}
}
inline
sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in)
{
return in;
}
inline
void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out)
{
out = msg;
}
} // namespace
#endif // TF2_SENSOR_MSGS_H

31
navigations/geometry2/tf2_sensor_msgs/package.xml Executable file
View File

@@ -0,0 +1,31 @@
<package format="2">
<name>tf2_sensor_msgs</name>
<version>0.7.6</version>
<description>
Small lib to transform sensor_msgs with tf. Most notably, PointCloud2
</description>
<author>Vincent Rabaud</author>
<maintainer email="vincent.rabaud@gmail.com">Vincent Rabaud</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/tf2_ros</url>
<buildtool_depend version_gte="0.5.6">catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>eigen</build_depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<exec_depend>python3-pykdl</exec_depend>
<exec_depend>rospy</exec_depend>
<build_export_depend>eigen</build_export_depend>
<test_depend>rostest</test_depend>
<test_depend>geometry_msgs</test_depend>
</package>

13
navigations/geometry2/tf2_sensor_msgs/setup.py Executable file
View File

@@ -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_sensor_msgs'],
package_dir={'': 'src'},
requires=['rospy','sensor_msgs','tf2_ros','orocos_kdl']
)
setup(**d)

1
navigations/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py Executable file
View File

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

60
navigations/geometry2/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py Executable file
View File

@@ -0,0 +1,60 @@
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
from sensor_msgs.msg import PointCloud2
from sensor_msgs.point_cloud2 import read_points, create_cloud
import PyKDL
import rospy
import tf2_ros
def to_msg_msg(msg):
return msg
tf2_ros.ConvertRegistration().add_to_msg(PointCloud2, to_msg_msg)
def from_msg_msg(msg):
return msg
tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg)
def transform_to_kdl(t):
return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
t.transform.rotation.z, t.transform.rotation.w),
PyKDL.Vector(t.transform.translation.x,
t.transform.translation.y,
t.transform.translation.z))
# PointStamped
def do_transform_cloud(cloud, transform):
t_kdl = transform_to_kdl(transform)
points_out = []
for p_in in read_points(cloud):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append((p_out[0], p_out[1], p_out[2]) + p_in[3:])
res = create_cloud(transform.header, cloud.fields, points_out)
return res
tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud)

3
navigations/geometry2/tf2_sensor_msgs/test/test.launch Executable file
View File

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

104
navigations/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.cpp Executable file
View File

@@ -0,0 +1,104 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/transform_listener.h>
#include <ros/ros.h>
#include <gtest/gtest.h>
#include <tf2_ros/buffer.h>
tf2_ros::Buffer* tf_buffer;
static const double EPS = 1e-3;
TEST(Tf2Sensor, PointCloud2)
{
sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(1);
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
*iter_x = 1;
*iter_y = 2;
*iter_z = 3;
cloud.header.stamp = ros::Time(2);
cloud.header.frame_id = "A";
// simple api
sensor_msgs::PointCloud2 cloud_simple = tf_buffer->transform(cloud, "B", ros::Duration(2.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_after(cloud_simple, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_after(cloud_simple, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_after(cloud_simple, "z");
EXPECT_NEAR(*iter_x_after, -9, EPS);
EXPECT_NEAR(*iter_y_after, 18, EPS);
EXPECT_NEAR(*iter_z_after, 27, EPS);
// advanced api
sensor_msgs::PointCloud2 cloud_advanced = tf_buffer->transform(cloud, "B", ros::Time(2.0),
"A", ros::Duration(3.0));
sensor_msgs::PointCloud2Iterator<float> iter_x_advanced(cloud_advanced, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y_advanced(cloud_advanced, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z_advanced(cloud_advanced, "z");
EXPECT_NEAR(*iter_x_advanced, -9, EPS);
EXPECT_NEAR(*iter_y_advanced, 18, EPS);
EXPECT_NEAR(*iter_z_advanced, 27, EPS);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test");
ros::NodeHandle n;
tf_buffer = new tf2_ros::Buffer();
// populate buffer
geometry_msgs::TransformStamped t;
t.transform.translation.x = 10;
t.transform.translation.y = 20;
t.transform.translation.z = 30;
t.transform.rotation.x = 1;
t.transform.rotation.y = 0;
t.transform.rotation.z = 0;
t.transform.rotation.w = 0;
t.header.stamp = ros::Time(2.0);
t.header.frame_id = "A";
t.child_frame_id = "B";
tf_buffer->setTransform(t, "test");
int ret = RUN_ALL_TESTS();
delete tf_buffer;
return ret;
}

103
navigations/geometry2/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py Executable file
View File

@@ -0,0 +1,103 @@
#!/usr/bin/env python
from __future__ import print_function
import unittest
import struct
import tf2_sensor_msgs
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointField
from tf2_ros import TransformStamped
import copy
## A sample python unit test
class PointCloudConversions(unittest.TestCase):
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)]
self.point_cloud_in.point_step = 4 * 3
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
points = [1, 2, 0, 10, 20, 30]
self.point_cloud_in.data = struct.pack('%sf' % len(points), *points)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = 300
self.transform_translate_xyz_300.transform.translation.y = 300
self.transform_translate_xyz_300.transform.translation.z = 300
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)])
def test_simple_transform(self):
old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now
point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
k = 300
expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)]
new_points = list(point_cloud2.read_points(point_cloud_transformed))
print("new_points are %s" % new_points)
assert(expected_coordinates == new_points)
assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud
## A simple unit test for tf2_sensor_msgs.do_transform_cloud (multi channel version)
class PointCloudConversionsMultichannel(unittest.TestCase):
TRANSFORM_OFFSET_DISTANCE = 300
def setUp(self):
self.point_cloud_in = point_cloud2.PointCloud2()
self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('index', 12, PointField.INT32, 1)]
self.point_cloud_in.point_step = 4 * 4
self.point_cloud_in.height = 1
# we add two points (with x, y, z to the cloud)
self.point_cloud_in.width = 2
self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width
self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)]
for point in self.points:
self.point_cloud_in.data += struct.pack('3fi', *point)
self.transform_translate_xyz_300 = TransformStamped()
self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE
self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w
assert(list(point_cloud2.read_points(self.point_cloud_in)) == self.points)
def test_simple_transform_multichannel(self):
old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now
point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300)
expected_coordinates = []
for point in self.points:
expected_coordinates += [(
point[0] + self.TRANSFORM_OFFSET_DISTANCE,
point[1] + self.TRANSFORM_OFFSET_DISTANCE,
point[2] + self.TRANSFORM_OFFSET_DISTANCE,
point[3] # index channel must be kept same
)]
new_points = list(point_cloud2.read_points(point_cloud_transformed))
print("new_points are %s" % new_points)
assert(expected_coordinates == new_points)
assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud
if __name__ == '__main__':
import rosunit
rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions)
rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversionsMultichannel)