diff --git a/geometry2/package.xml b/geometry2/package.xml index 504ed3ff8..c3798dafb 100644 --- a/geometry2/package.xml +++ b/geometry2/package.xml @@ -27,9 +27,11 @@ tf2_eigen_kdl tf2_geometry_msgs tf2_kdl + tf2_kdl_py tf2_msgs tf2_py tf2_ros + tf2_ros_py tf2_sensor_msgs tf2_tools diff --git a/tf2_kdl/CMakeLists.txt b/tf2_kdl/CMakeLists.txt index fb82f9ee0..09541b82e 100644 --- a/tf2_kdl/CMakeLists.txt +++ b/tf2_kdl/CMakeLists.txt @@ -19,9 +19,6 @@ find_package(orocos_kdl REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) -ament_python_install_package(${PROJECT_NAME} - PACKAGE_DIR src/${PROJECT_NAME}) - add_library(tf2_kdl INTERFACE) target_link_libraries(tf2_kdl INTERFACE ${builtin_interfaces_TARGETS} @@ -37,11 +34,6 @@ install(TARGETS tf2_kdl EXPORT export_tf2_kdl) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) -# TODO(ahcorde): Port python once https://github.com/ros2/orocos_kinematics_dynamics/pull/4 is merged -# install(PROGRAMS scripts/test.py -# DESTINATION lib/${PROJECT_NAME} -# ) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(rclcpp REQUIRED) diff --git a/tf2_kdl/package.xml b/tf2_kdl/package.xml index 8c9b14cdf..e487aaa40 100644 --- a/tf2_kdl/package.xml +++ b/tf2_kdl/package.xml @@ -22,8 +22,6 @@ tf2 tf2_ros - tf2_ros_py - ament_cmake_gtest rclcpp tf2_msgs diff --git a/tf2_kdl/scripts/test.py b/tf2_kdl/scripts/test.py deleted file mode 100755 index 521defc2b..000000000 --- a/tf2_kdl/scripts/test.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/python - -# Copyright 2008 Willow Garage, Inc. -# -# 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 HOLDER 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 unittest - -import builtin_interfaces -from geometry_msgs.msg import Quaternion, TransformStamped -import PyKDL -import rclpy -import tf2_ros -import tf2_kdl # noqa(F401) - - -class KDLConversions(unittest.TestCase): - - def test_transform(self): - b = tf2_ros.Buffer() - t = TransformStamped() - t.transform.translation.x = 1.0 - t.transform.rotation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0) - t.header.stamp = builtin_interfaces.msg.Time(sec=2) - t.header.frame_id = 'a' - t.child_frame_id = 'b' - b.set_transform(t, 'eitan_rocks') - out = b.lookup_transform('a', 'b', - rclpy.time.Time(seconds=2), - rclpy.time.Duration(seconds=2)) - self.assertEqual(out.transform.translation.x, 1) - self.assertEqual(out.transform.rotation.x, 1) - self.assertEqual(out.header.frame_id, 'a') - self.assertEqual(out.child_frame_id, 'b') - - v = PyKDL.Vector(1, 2, 3) - out = b.transform( - tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') - self.assertEqual(out.x(), 0) - self.assertEqual(out.y(), -2) - self.assertEqual(out.z(), -3) - - f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) - out = b.transform( - tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') - self.assertEqual(out.p.x(), 0) - self.assertEqual(out.p.y(), -2) - self.assertEqual(out.p.z(), -3) - # TODO(tfoote) check values of rotation - - t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) - out = b.transform( - tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') - self.assertEqual(out.vel.x(), 1) - self.assertEqual(out.vel.y(), -8) - self.assertEqual(out.vel.z(), 2) - self.assertEqual(out.rot.x(), 4) - self.assertEqual(out.rot.y(), -5) - self.assertEqual(out.rot.z(), -6) - - w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) - out = b.transform( - tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') - self.assertEqual(out.force.x(), 1) - self.assertEqual(out.force.y(), -2) - self.assertEqual(out.force.z(), -3) - self.assertEqual(out.torque.x(), 4) - self.assertEqual(out.torque.y(), -8) - self.assertEqual(out.torque.z(), -4) - - def test_convert(self): - v = PyKDL.Vector(1, 2, 3) - vs = tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a') - vs2 = tf2_ros.convert(vs, PyKDL.Vector) - self.assertEqual(vs.x(), 1) - self.assertEqual(vs.y(), 2) - self.assertEqual(vs.z(), 3) - self.assertEqual(vs2.x(), 1) - self.assertEqual(vs2.y(), 2) - self.assertEqual(vs2.z(), 3) - - -if __name__ == '__main__': - rclpy.init(args=None) - node = rclpy.create_node('test_tf2_kdl_python') - unittest.main() diff --git a/tf2_kdl/src/tf2_kdl/tf2_kdl.py b/tf2_kdl/src/tf2_kdl/tf2_kdl.py deleted file mode 100644 index 6c4b4e04a..000000000 --- a/tf2_kdl/src/tf2_kdl/tf2_kdl.py +++ /dev/null @@ -1,182 +0,0 @@ -# Copyright 2008 Willow Garage, Inc. -# -# 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 HOLDER 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 PointStamped -import PyKDL -import tf2_ros - - -def transform_to_kdl(t): - """ - Convert a geometry_msgs Transform message to a PyKDL Frame. - - :param t: The Transform message to convert. - :type t: geometry_msgs.msg.TransformStamped - :return: The converted PyKDL frame. - :rtype: PyKDL.Frame - """ - return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, - t.transform.rotation.y, - t.transform.rotation.z, - t.transform.rotation.w), - PyKDL.Vector(t.transform.translation.x, - t.transform.translation.y, - t.transform.translation.z)) - - -def do_transform_vector(vector, transform): - """ - Apply a transform in the form of a geometry_msgs message to a PyKDL vector. - - :param vector: The PyKDL vector to transform. - :type vector: PyKDL.Vector - :param transform: The transform to apply. - :type transform: geometry_msgs.msg.TransformStamped - :return: The transformed vector. - :rtype: PyKDL.Vector - """ - res = transform_to_kdl(transform) * vector - res.header = transform.header - return res - - -tf2_ros.TransformRegistration().add(PyKDL.Vector, do_transform_vector) - - -def to_msg_vector(vector): - """ - Convert a PyKDL Vector to a geometry_msgs PointStamped message. - - :param vector: The vector to convert. - :type vector: PyKDL.Vector - :return: The converted vector/point. - :rtype: geometry_msgs.msg.PointStamped - """ - msg = PointStamped() - msg.header = vector.header - msg.point.x = vector[0] - msg.point.y = vector[1] - msg.point.z = vector[2] - return msg - - -tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Vector, to_msg_vector) - - -def from_msg_vector(msg): - """ - Convert a PointStamped message to a stamped PyKDL Vector. - - :param msg: The PointStamped message to convert. - :type msg: geometry_msgs.msg.PointStamped - :return: The timestamped converted PyKDL vector. - :rtype: PyKDL.Vector - """ - vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z) - return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id) - - -tf2_ros.ConvertRegistration().add_from_msg(PyKDL.Vector, from_msg_vector) - - -def convert_vector(vector): - """ - Convert a generic stamped triplet message to a stamped PyKDL Vector. - - :param vector: The message to convert. - :return: The timestamped converted PyKDL vector. - :rtype: PyKDL.Vector - """ - return tf2_ros.Stamped(PyKDL.Vector(vector), - vector.header.stamp, - vector.header.frame_id) - - -tf2_ros.ConvertRegistration().add_convert((PyKDL.Vector, PyKDL.Vector), - convert_vector) - - -def do_transform_frame(frame, transform): - """ - Apply a transform in the form of a geometry_msgs message to a PyKDL Frame. - - :param frame: The PyKDL frame to transform. - :type frame: PyKDL.Frame - :param transform: The transform to apply. - :type transform: geometry_msgs.msg.TransformStamped - :return: The transformed PyKDL frame. - :rtype: PyKDL.Frame - """ - res = transform_to_kdl(transform) * frame - res.header = transform.header - return res - - -tf2_ros.TransformRegistration().add(PyKDL.Frame, do_transform_frame) - - -def do_transform_twist(twist, transform): - """ - Apply a transform in the form of a geometry_msgs message to a PyKDL Twist. - - :param twist: The PyKDL twist to transform. - :type twist: PyKDL.Twist - :param transform: The transform to apply. - :type transform: geometry_msgs.msg.TransformStamped - :return: The transformed PyKDL twist. - :rtype: PyKDL.Twist - """ - res = transform_to_kdl(transform) * twist - res.header = transform.header - return res - - -tf2_ros.TransformRegistration().add(PyKDL.Twist, do_transform_twist) - - -# Wrench -def do_transform_wrench(wrench, transform): - """ - Apply a transform in the form of a geometry_msgs message to a PyKDL Wrench. - - :param wrench: The PyKDL wrench to transform. - :type wrench: PyKDL.Wrench - :param transform: The transform to apply. - :type transform: geometry_msgs.msg.TransformStamped - :return: The transformed PyKDL wrench. - :rtype: PyKDL.Wrench - """ - res = transform_to_kdl(transform) * wrench - res.header = transform.header - return res - - -tf2_ros.TransformRegistration().add(PyKDL.Wrench, do_transform_wrench) diff --git a/tf2_kdl/test/test_python.launch b/tf2_kdl/test/test_python.launch deleted file mode 100644 index d43a4c790..000000000 --- a/tf2_kdl/test/test_python.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/tf2_kdl_py/CHANGELOG.rst b/tf2_kdl_py/CHANGELOG.rst new file mode 100644 index 000000000..999991f7f --- /dev/null +++ b/tf2_kdl_py/CHANGELOG.rst @@ -0,0 +1,3 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tf2_kdl_py +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/tf2_kdl_py/doc/conf.py b/tf2_kdl_py/doc/conf.py new file mode 100644 index 000000000..24effb031 --- /dev/null +++ b/tf2_kdl_py/doc/conf.py @@ -0,0 +1,255 @@ +# Copyright 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 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 HOLDER 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. + +# -*- coding: utf-8 -*- +# +# tf2_kdl_py 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', 'sphinx_rtd_theme'] + +# 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. +from datetime import datetime +now = datetime.now() # current date and time +year = now.strftime("%Y") + +project = 'tf2_kdl_py' +copyright = '2008-' + year + ', Open Source Robotics Foundation, Inc.' # noqa +author = 'Open Source Robotics Foundation, 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 = '4.12.0' +# The full version, including alpha/beta/rc tags. +release = '4.12.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 = 'sphinx_rtd_theme' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +#html_static_path = ['_static'] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_use_modindex = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = '' + +# Output file base name for HTML help builder. +htmlhelp_basename = 'tf2_kdl_py_doc' + + +# -- 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', 'tf2_kdl_py.tex', u'stereo\\_utils Documentation', + [copyright], '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 + +# -- 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_kdl_py', 'tf2_kdl_py Documentation', + [copyright], 1) +] + +# -- 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_kdl_py', 'tf2_kdl_py Documentation', + copyright, 'tf2_kdl_py', 'ROS 2 ', + 'Miscellaneous'), +] + + +autoclass_content = "both" + +autodoc_default_options = { + 'members': True, # document members + 'undoc-members': True, # also document members without documentation +} diff --git a/tf2_kdl_py/doc/index.rst b/tf2_kdl_py/doc/index.rst new file mode 100644 index 000000000..5a2368590 --- /dev/null +++ b/tf2_kdl_py/doc/index.rst @@ -0,0 +1,28 @@ +tf2_kdl_py +========== + +:mod:`tf2_kdl_py` is a set of ``PyKDL`` bindings written for the tf2 system for Python. + +1.1 Converting Types +-------------------- + +To make these types univsersal with other ``tf2`` types, they must have conversion methods, +the easiest way to change to geometry_msgs types or back to ``PyKDL`` types is by calling: + +* :meth:`tf2_ros.convert(TransformableObject, TransformableObjectType)` + +Essentially take any valid ``geometry_msgs`` object or ``PyKDL`` object to be TransformableObject, +and the desired converted type to be TransformableObjectType, and the system will automatically +convert. + +.. toctree:: + :maxdepth: 2 + + self + Python Modules + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` diff --git a/tf2_kdl_py/package.xml b/tf2_kdl_py/package.xml new file mode 100644 index 000000000..28a2169d8 --- /dev/null +++ b/tf2_kdl_py/package.xml @@ -0,0 +1,25 @@ + + tf2_kdl_py + 0.36.1 + + PyKDL bindings for tf2 + + + Alejandro Hernandez Cordero + Chris Lalancette + + Apache-2.0 + + geometry_msgs + python_orocos_kdl_vendor + tf2_ros_py + + builtin_interfaces + python3-pytest + rclpy + + + ament_python + rosdoc2.yaml + + diff --git a/tf2_kdl_py/resource/tf2_kdl_py b/tf2_kdl_py/resource/tf2_kdl_py new file mode 100644 index 000000000..e69de29bb diff --git a/tf2_kdl_py/rosdoc2.yaml b/tf2_kdl_py/rosdoc2.yaml new file mode 100644 index 000000000..08402fd81 --- /dev/null +++ b/tf2_kdl_py/rosdoc2.yaml @@ -0,0 +1,62 @@ +## Default configuration, generated by rosdoc2. + +## This 'attic section' self-documents this file's type and version. +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + ## If this is true, a standard index page is generated in the output directory. + ## It uses the package information from the 'package.xml' to show details + ## about the package, creates a table of contents for the various builders + ## that were run, and may contain links to things like build farm jobs for + ## this package or links to other versions of this package. + + ## If false, you can still include content that would have been in the index + ## into one of your '.rst' files from your Sphinx project, using the + ## '.. include::' directive in Sphinx. + ## For example, you could include it in a custom 'index.rst' so you can have + ## the standard information followed by custom content. + + ## TODO(wjwwood): provide a concrete example of this (relative path?) + + ## If this is not specified explicitly, it defaults to 'true'. + generate_package_index: true + + ## This setting is relevant mostly if the standard Python package layout cannot + ## be assumed for 'sphinx-apidoc' invocation. The user can provide the path + ## (relative to the 'package.xml' file) where the Python modules defined by this + ## package are located. + python_source: 'tf2_kdl_py' + + ## This setting, if true, attempts to run `doxygen` and the `breathe`/`exhale` + ## extensions to `sphinx` regardless of build type. This is most useful if the + ## user would like to generate C/C++ API documentation for a package that is not + ## of the `ament_cmake/cmake` build type. + always_run_doxygen: false + + ## This setting, if true, attempts to run `sphinx-apidoc` regardless of build + ## type. This is most useful if the user would like to generate Python API + ## documentation for a package that is not of the `ament_python` build type. + always_run_sphinx_apidoc: false + + # This setting, if provided, will override the build_type of this package + # for documentation purposes only. If not provided, documentation will be + # generated assuming the build_type in package.xml. + # override_build_type: 'ament_python' +builders: + ## Each stanza represents a separate build step, performed by a specific 'builder'. + ## The key of each stanza is the builder to use; this must be one of the + ## available builders. + ## The value of each stanza is a dictionary of settings for the builder that + ## outputs to that directory. + ## Required keys in the settings dictionary are: + ## * 'output_dir' - determines output subdirectory for builder instance + ## relative to --output-directory + ## * 'name' - used when referencing the built docs from the index. + - sphinx: { + name: 'tf2_kdl_py', + doxygen_xml_directory: 'generated/doxygen/xml', + output_dir: '' + } diff --git a/tf2_kdl_py/setup.cfg b/tf2_kdl_py/setup.cfg new file mode 100644 index 000000000..c0fb9c8c7 --- /dev/null +++ b/tf2_kdl_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/tf2_kdl_py +[install] +install_scripts=$base/lib/tf2_kdl_py diff --git a/tf2_kdl_py/setup.py b/tf2_kdl_py/setup.py new file mode 100644 index 000000000..444c5bad5 --- /dev/null +++ b/tf2_kdl_py/setup.py @@ -0,0 +1,21 @@ +from setuptools import find_packages, setup + +package_name = 'tf2_kdl_py' + +setup( + name=package_name, + version='0.36.1', + packages=find_packages([package_name]), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Alejandro Hernandez Cordero, Chris Lalancette', + maintainer_email='alejandro@openrobotics.org, clalancette@openrobotics.org', + description='PyKDL bindings for tf2', + license='Apache-2.0', + tests_require=['pytest'], +) diff --git a/tf2_kdl_py/tests/test_kdl.py b/tf2_kdl_py/tests/test_kdl.py new file mode 100644 index 000000000..209f3597e --- /dev/null +++ b/tf2_kdl_py/tests/test_kdl.py @@ -0,0 +1,291 @@ +# Copyright 2008 Willow Garage, Inc. +# +# 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 HOLDER 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 pytest # noqa: F401 + +import builtin_interfaces +from geometry_msgs.msg import Vector3, Quaternion, TransformStamped, TwistStamped +from geometry_msgs.msg import Vector3Stamped, PointStamped, PoseStamped, Pose, WrenchStamped +import PyKDL +import rclpy +import tf2_ros +from rclpy.time import Time +import tf2_kdl_py # noqa: F401 + + +class TestKDLConversions: + def test_transform(self): + tf_buffer = tf2_ros.Buffer() + t = TransformStamped() + t.transform.translation = Vector3(x=2.0, y=4.0, z=6.0) + t.transform.rotation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0) + t.header.stamp = builtin_interfaces.msg.Time(sec=2) + t.header.frame_id = 'a' + t.child_frame_id = 'b' + tf_buffer.set_transform(t, 'test') + out = tf_buffer.lookup_transform('a', 'b', + Time(seconds=2), + rclpy.time.Duration(seconds=2)) + assert out.transform.translation.x == 2 + assert out.transform.rotation.x == 1 + assert out.header.frame_id == 'a' + assert out.child_frame_id == 'b' + + # Vector + v = PyKDL.Vector(1, 2, 3) + out = tf_buffer.transform( + tf2_kdl_py.StampedVector(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') + assert out.x() == -1 + assert out.y() == 2 + assert out.z() == 3 + assert out.header.stamp.sec == 2 + + f = PyKDL.Frame(PyKDL.Rotation.RPY(0, 90, 180), PyKDL.Vector(1, 2, 3)) + out = tf_buffer.transform( + tf2_kdl_py.StampedFrame(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') + assert out.p.x() == -1 + assert out.p.y() == 2 + assert out.p.z() == 3 + # TODO(CursedRock17) ensure Rotations are correct + # assert out.M.GetRPY()[0] == 0 + # assert out.M.GetRPY()[1] == 1 + # assert out.M.GetRPY()[2] == 2 + assert out.header.stamp.sec == 2 + + t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) + out = tf_buffer.transform( + tf2_kdl_py.StampedTwist(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') + assert out.vel.x() == 7 + assert out.vel.y() == 10 + assert out.vel.z() == -9 + assert out.rot.x() == 4 + assert out.rot.y() == -5 + assert out.rot.z() == -6 + assert out.header.stamp.sec == 2 + + w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) + out = tf_buffer.transform( + tf2_kdl_py.StampedWrench(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') + assert out.force.x() == 1 + assert out.force.y() == -2 + assert out.force.z() == -3 + assert out.torque.x() == 4 + assert out.torque.y() == -5 + assert out.torque.z() == -6 + assert out.header.stamp.sec == 2 + + def test_convert_vector(self): + # Create the PyKDL and geometry_msgs objects + v = PyKDL.Vector(1, 2, 3) + vs = tf2_kdl_py.StampedVector(v, builtin_interfaces.msg.Time(sec=1.0), 'a') + ros_v = PointStamped() + ros_v.point.x = 1 + ros_v.point.y = 2 + ros_v.point.z = 3 + ros_v.header.stamp = builtin_interfaces.msg.Time(sec=1.0) + ros_v.header.frame_id = 'a' + + # Test each form of conversion between PyKDL and geometry_msgs types + vs2 = tf2_ros.convert(vs, tf2_kdl_py.StampedVector) + vs3 = tf2_ros.convert(vs, PointStamped) + vs4 = tf2_ros.convert(ros_v, tf2_kdl_py.StampedVector) + vs5 = tf2_ros.convert(ros_v, PointStamped) + + # Ensure each set of conversions remains constant + assert vs2.x() == 1 + assert vs2.y() == 2 + assert vs2.z() == 3 + + assert vs3.point.x == 1 + assert vs3.point.y == 2 + assert vs3.point.z == 3 + + assert vs4.x() == 1 + assert vs4.y() == 2 + assert vs4.z() == 3 + + assert vs5.point.x == 1 + assert vs5.point.y == 2 + assert vs5.point.z == 3 + + def test_convert_frame(self): + # Create the PyKDL and geometry_msgs objects + v = PyKDL.Vector(1, 2, 3) + r = PyKDL.Rotation.Quaternion(0, 0, 0, 1) + fs = tf2_kdl_py.StampedFrame(PyKDL.Frame(r, v), builtin_interfaces.msg.Time(sec=1.0), 'a') + ros_p = PoseStamped() + ros_p.pose.position.x = 1 + ros_p.pose.position.y = 2 + ros_p.pose.position.z = 3 + ros_p.pose.orientation.x = 0 + ros_p.pose.orientation.y = 0 + ros_p.pose.orientation.z = 0 + ros_p.pose.orientation.w = 1 + ros_p.header.stamp = builtin_interfaces.msg.Time(sec=1.0) + ros_p.header.frame_id = 'a' + + # Test each form of conversion between PyKDL and geometry_msgs types + fs2 = tf2_ros.convert(fs, tf2_kdl_py.StampedFrame) + fs3 = tf2_ros.convert(fs, PoseStamped) + fs4 = tf2_ros.convert(ros_p, tf2_kdl_py.StampedFrame) + fs5 = tf2_ros.convert(ros_p, PoseStamped) + + # Ensure each set of conversions remains constant + assert fs2.p[0] == 1 + assert fs2.p[1] == 2 + assert fs2.p[2] == 3 + assert fs2.M.GetQuaternion()[0] == 0 + assert fs2.M.GetQuaternion()[1] == 0 + assert fs2.M.GetQuaternion()[2] == 0 + assert fs2.M.GetQuaternion()[3] == 1 + + assert fs3.pose.position.x == 1 + assert fs3.pose.position.y == 2 + assert fs3.pose.position.z == 3 + assert fs3.pose.orientation.x == 0 + assert fs3.pose.orientation.y == 0 + assert fs3.pose.orientation.z == 0 + assert fs3.pose.orientation.w == 1 + + assert fs4.p[0] == 1 + assert fs4.p[1] == 2 + assert fs4.p[2] == 3 + assert fs4.M.GetQuaternion()[0] == 0 + assert fs4.M.GetQuaternion()[1] == 0 + assert fs4.M.GetQuaternion()[2] == 0 + assert fs4.M.GetQuaternion()[3] == 1 + + assert fs5.pose.position.x == 1 + assert fs5.pose.position.y == 2 + assert fs5.pose.position.z == 3 + assert fs5.pose.orientation.x == 0 + assert fs5.pose.orientation.y == 0 + assert fs5.pose.orientation.z == 0 + assert fs5.pose.orientation.w == 1 + + def test_confert_twist(self): + # Create the PyKDL and geometry_msgs objects + v = PyKDL.Vector(1, 2, 3) + v2 = PyKDL.Vector(1, 2, 3) + ts = tf2_kdl_py.StampedTwist(PyKDL.Twist(v, v2), builtin_interfaces.msg.Time(sec=1.0), 'a') + ros_t = TwistStamped() + ros_t.twist.linear.x = 1 + ros_t.twist.linear.y = 2 + ros_t.twist.linear.z = 3 + ros_t.twist.angular.x = 1 + ros_t.twist.angular.y = 2 + ros_t.twist.angular.z = 3 + ros_t.header.stamp = builtin_interfaces.msg.Time(sec=1.0) + ros_t.header.frame_id = 'a' + + # Test each form of conversion between PyKDL and geometry_msgs types + ts2 = tf2_ros.convert(ts, tf2_kdl_py.StampedTwist) + ts3 = tf2_ros.convert(ts, TwistStamped) + ts4 = tf2_ros.convert(ros_t, tf2_kdl_py.StampedTwist) + ts5 = tf2_ros.convert(ros_t, TwistStamped) + + # Ensure each set of conversions remains constant + assert ts2.vel[0] == 1 + assert ts2.vel[1] == 2 + assert ts2.vel[2] == 3 + assert ts2.rot[0] == 1 + assert ts2.rot[1] == 2 + assert ts2.rot[2] == 3 + + assert ts3.twist.linear.x == 1 + assert ts3.twist.linear.y == 2 + assert ts3.twist.linear.z == 3 + assert ts3.twist.angular.x == 1 + assert ts3.twist.angular.y == 2 + assert ts3.twist.angular.z == 3 + + assert ts4.vel[0] == 1 + assert ts4.vel[1] == 2 + assert ts4.vel[2] == 3 + assert ts4.rot[0] == 1 + assert ts4.rot[1] == 2 + assert ts4.rot[2] == 3 + + assert ts5.twist.linear.x == 1 + assert ts5.twist.linear.y == 2 + assert ts5.twist.linear.z == 3 + assert ts5.twist.angular.x == 1 + assert ts5.twist.angular.y == 2 + assert ts5.twist.angular.z == 3 + + def test_convert_wrench(self): + # Create the PyKDL and geometry_msgs objects + v = PyKDL.Vector(1, 2, 3) + v2 = PyKDL.Vector(1, 2, 3) + w = PyKDL.Wrench(v, v2) + ws = tf2_kdl_py.StampedWrench(w, builtin_interfaces.msg.Time(sec=1.0), 'a') + ros_w = WrenchStamped() + ros_w.wrench.force.x = 1 + ros_w.wrench.force.y = 2 + ros_w.wrench.force.z = 3 + ros_w.wrench.torque.x = 1 + ros_w.wrench.torque.y = 2 + ros_w.wrench.torque.z = 3 + ros_w.header.stamp = builtin_interfaces.msg.Time(sec=1.0) + ros_w.header.frame_id = 'a' + + # Tesw each form of conversion between PyKDL and geometry_msgs types + ws2 = tf2_ros.convert(ws, tf2_kdl_py.StampedWrench) + ws3 = tf2_ros.convert(ws, WrenchStamped) + ws4 = tf2_ros.convert(ros_w, tf2_kdl_py.StampedWrench) + ws5 = tf2_ros.convert(ros_w, WrenchStamped) + + # Ensure each set of conversions remains constant + assert ws2.force[0] == 1 + assert ws2.force[1] == 2 + assert ws2.force[2] == 3 + assert ws2.torque[0] == 1 + assert ws2.torque[1] == 2 + assert ws2.torque[2] == 3 + + assert ws3.wrench.force.x == 1 + assert ws3.wrench.force.y == 2 + assert ws3.wrench.force.z == 3 + assert ws3.wrench.torque.x == 1 + assert ws3.wrench.torque.y == 2 + assert ws3.wrench.torque.z == 3 + + assert ws4.force[0] == 1 + assert ws4.force[1] == 2 + assert ws4.force[2] == 3 + assert ws4.torque[0] == 1 + assert ws4.torque[1] == 2 + assert ws4.torque[2] == 3 + + assert ws5.wrench.force.x == 1 + assert ws5.wrench.force.y == 2 + assert ws5.wrench.force.z == 3 + assert ws5.wrench.torque.x == 1 + assert ws5.wrench.torque.y == 2 + assert ws5.wrench.torque.z == 3 diff --git a/tf2_kdl/src/tf2_kdl/__init__.py b/tf2_kdl_py/tf2_kdl_py/__init__.py similarity index 94% rename from tf2_kdl/src/tf2_kdl/__init__.py rename to tf2_kdl_py/tf2_kdl_py/__init__.py index e6b5dc616..691d78d65 100644 --- a/tf2_kdl/src/tf2_kdl/__init__.py +++ b/tf2_kdl_py/tf2_kdl_py/__init__.py @@ -27,4 +27,5 @@ # POSSIBILITY OF SUCH DAMAGE. -from .tf2_kdl import * # noqa +from .tf2_kdl import * # noqa: F401 +from .transform_datatypes import * # noga: F401 diff --git a/tf2_kdl_py/tf2_kdl_py/tf2_kdl.py b/tf2_kdl_py/tf2_kdl_py/tf2_kdl.py new file mode 100644 index 000000000..9c3fffea0 --- /dev/null +++ b/tf2_kdl_py/tf2_kdl_py/tf2_kdl.py @@ -0,0 +1,388 @@ +# Copyright 2008 Willow Garage, Inc. +# +# 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 HOLDER 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 PointStamped, TransformStamped, Pose, PoseStamped +from geometry_msgs.msg import TwistStamped, WrenchStamped +import PyKDL +import tf2_ros +import tf2_kdl_py.transform_datatypes + + +def transform_to_kdl(t): + """ + Convert a geometry_msgs TransformStamped message to a PyKDL Frame. + + :param t: The Transform message to convert. + :type t: geometry_msgs.msg.TransformStamped + :return: The converted PyKDL frame. + :rtype: PyKDL.Frame + """ + return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + t.transform.rotation.w), + PyKDL.Vector(t.transform.translation.x, + t.transform.translation.y, + t.transform.translation.z)) + + +def kdl_to_transform(k): + """ + Convert a PyKDL Frame to the equivalent geometry_msgs message type. + + :param k: The Frame to convert. + :type t: PyKDL.Frame + :return: The converted geometry_msgs transform. + :rtype: geometry_msgs.msg.TransformStamped + """ + t = TransformStamped() + t.transform.translation.x = k.p.x() + t.transform.translation.y = k.p.y() + t.transform.translation.z = k.p.z() + k.M.GetQuaternion(t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w) + return t + +########### +# Vector # +########### + + +def do_transform_vector(vector_in, transform_in): + """ + Apply a transform in the form of a geometry_msgs message to a PyKDL vector. + + :param vector_in: The stamped PyKDL vector to transform. + :type vector_in: tf2_kdl_py.transform_datatypes.StampedVector + :param transform_in: The transform to apply. + :type transform_in: geometry_msgs.msg.TransformStamped + :return: The stamped transformed vector. + :rtype: tf2_kdl_py.transform_datatypes.StampedVector + """ + vector_out = transform_to_kdl(transform_in) * vector_in + return tf2_kdl_py.transform_datatypes.StampedVector(vector_out, transform_in.header.stamp, transform_in.header.frame_id) + + +tf2_ros.TransformRegistration().add(tf2_kdl_py.transform_datatypes.StampedVector, do_transform_vector) + + +def to_msg_vector(vector_in): + """ + Convert a stamped PyKDL Vector to a geometry_msgs PointStamped message. + + :param vector_in: The vector to convert. + :type vector_in: tf2_kdl_py.transform_datatypes.StampedVector + :return: The converted vector/point. + :rtype: geometry_msgs.msg.PointStamped + """ + msg = PointStamped() + msg.header.stamp = vector_in.header.stamp + msg.header.frame_id = vector_in.header.frame_id + msg.point.x = vector_in.vector[0] + msg.point.y = vector_in.vector[1] + msg.point.z = vector_in.vector[2] + return msg + + +tf2_ros.ConvertRegistration().add_to_msg(tf2_kdl_py.transform_datatypes.StampedVector, to_msg_vector) + + +def from_msg_vector(msg): + """ + Convert a PointStamped message to a stamped PyKDL Vector. + + :param msg: The PointStamped message to convert. + :type msg: geometry_msgs.msg.PointStamped + :return: The timestamped converted PyKDL vector. + :rtype: tf2_kdl_py.transform_datatypes.StampedVector + """ + vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z) + return tf2_kdl_py.transform_datatypes.StampedVector(vector, msg.header.stamp, msg.header.frame_id) + + +tf2_ros.ConvertRegistration().add_from_msg(PointStamped, from_msg_vector) + + +def convert_vector(vector_in): + """ + Convert a generic stamped triplet message to a stamped PyKDL Vector. + + :param vector_in: The message to convert. + :return: The timestamped converted PyKDL vector. + :rtype: tf2_kdl_py.transform_datatypes.StampedVector(PyKDL.Vector) + """ + return tf2_kdl_py.transform_datatypes.StampedVetor(PyKDL.Vector(vector_in), + vector_in.header.stamp, + vector_in.header.frame_id) + + +tf2_ros.ConvertRegistration().add_convert((PyKDL.Vector, PyKDL.Vector), + convert_vector) + + +########### +# Frame # +########### + + +def do_transform_frame(frame_in, transform_in): + """ + Apply a transform in the form of a geometry_msgs TransformStamped to a PyKDL Frame. + + :param frame_in: The stamped PyKDL frame to transform. + :type frame_in: tf2_kdl_py.transform_datatypes.StampedFrame + :param transform_in: The stamped transform to apply. + :type transform_in: geometry_msgs.msg.TransformStamped + :return: The stamped transformed PyKDL frame. + :rtype: tf2_kdl_py.transform_datatypes.StampedFrame + """ + frame_out = transform_to_kdl(transform_in) * frame_in + return tf2_kdl_py.transform_datatypes.StampedFrame(frame_out, transform_in.header.stamp, transform_in.header.frame_id) + + +tf2_ros.TransformRegistration().add(tf2_kdl_py.transform_datatypes.StampedFrame, do_transform_frame) + + +def to_msg_frame(frame_in): + """ + Convert a PyKDL Frame to a geometry_msgs Pose message. + + :param frame_in: The frame to convert. + :type frame_in: PyKDL.Frame + :return: The converted pose. + :rtype: geometry_msgs.Pose + """ + msg = Pose() + msg.position.x = frame_in.p[0] + msg.position.y = frame_in.p[1] + msg.position.z = frame_in.p[2] + frame_in.M.GetQuaternion(msg.orientation.x, msg.orientation.y, + msg.orientation.z, msg.orientation.w) + return msg + + +tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Frame, to_msg_frame) + + +def from_msg_frame(msg): + """ + Convert a Pose message to a PyKDL frame. + + :param msg: The Pose message to convert. + :type msg: geometry_msgs.Pose + :return: The converted PyKDL frame. + :rtype: PyKDL.Frame + """ + frame = PyKDL.Frame() + frame.p[0] = msg.position.x + frame.p[1] = msg.position.y + frame.p[2] = msg.position.z + frame.M = PyKDL.Rotation.Quaternion(msg.orientation.x, msg.orientation.y, + msg.orientation.z, msg.orientation.w) + + return frame + + +tf2_ros.ConvertRegistration().add_from_msg(PyKDL.Frame, from_msg_frame) + + +def to_msg_frame_stamped(frame_in): + """ + Convert a stamped PyKDL Frame to a geometry_msgs PoseStamped message. + + :param frame_in: The stamped frame to convert. + :type frame_in: tf2_kdl_py.transform_datatypes.StampedFrame + :return: The converted stamped pose. + :rtype: geometry_msgs.PoseStamped + """ + msg = PoseStamped() + msg.header.stamp = frame_in.header.stamp + msg.header.frame_id = frame_in.header.frame_id + msg.pose.position.x = frame_in.frame.p[0] + msg.pose.position.y = frame_in.frame.p[1] + msg.pose.position.z = frame_in.frame.p[2] + orientation = frame_in.frame.M.GetQuaternion() + msg.pose.orientation.x = orientation[0] + msg.pose.orientation.y = orientation[1] + msg.pose.orientation.z = orientation[2] + msg.pose.orientation.w = orientation[3] + return msg + + +tf2_ros.ConvertRegistration().add_to_msg(tf2_kdl_py.transform_datatypes.StampedFrame, to_msg_frame_stamped) + + +def from_msg_frame_stamped(msg): + """ + Convert a PoseStamped message to a stamped PyKDL frame. + + :param msg: The Pose message to convert. + :type msg: geometry_msgs.PoseStamped + :return: The converted PyKDL stamped frame. + :rtype: tf2_kdl_py.transform_datatypes.StampedFrame + """ + rotation = PyKDL.Rotation.Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z, msg.pose.orientation.w) + vector = PyKDL.Vector(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) + frame = PyKDL.Frame(rotation, vector) + return tf2_kdl_py.transform_datatypes.StampedFrame(frame, msg.header.stamp, msg.header.frame_id) + + +tf2_ros.ConvertRegistration().add_from_msg(PoseStamped, from_msg_frame_stamped) + + +########### +# Twist # +########### + +def do_transform_twist(twist_in, transform_in): + """ + Apply a transform in the form of a geometry_msgs TransformStamped to a stamped PyKDL Twist. + + :param twist_in: The stamped PyKDL twist to transform. + :type twist_in: tf2_kdl_py.transform_datatypes.StampedTwist + :param transform_in: The transform to apply. + :type transform_in: geometry_msgs.msg.TransformStamped + :return: The stamped transformed PyKDL twist. + :rtype: tf2_kdl_py.transform_datatypes.StampedTwist + """ + twist_out = transform_to_kdl(transform_in) * twist_in + return tf2_kdl_py.transform_datatypes.StampedTwist(twist_out, transform_in.header.stamp, transform_in.header.frame_id) + + +tf2_ros.TransformRegistration().add(tf2_kdl_py.transform_datatypes.StampedTwist, do_transform_twist) + + +def to_msg_twist_stamped(twist_in): + """ + Convert a stamped PyKDL Twist to a geometry_msgs TwistStamped message. + + :param twist_in: The stamped twist to convert. + :type twist_in: tf2_kdl_py.transform_datatypes.StampedTwist + :return: The converted stamped twist. + :rtype: geometry_msgs.TwistStamped + """ + msg = TwistStamped() + msg.header.stamp = twist_in.header.stamp + msg.header.frame_id = twist_in.header.frame_id + msg.twist.linear.x = twist_in.twist.vel[0] + msg.twist.linear.y = twist_in.twist.vel[1] + msg.twist.linear.z = twist_in.twist.vel[2] + msg.twist.angular.x = twist_in.twist.rot[0] + msg.twist.angular.y = twist_in.twist.rot[1] + msg.twist.angular.z = twist_in.twist.rot[2] + return msg + + +tf2_ros.ConvertRegistration().add_to_msg(tf2_kdl_py.transform_datatypes.StampedTwist, to_msg_twist_stamped) + + +def from_msg_twist_stamped(msg): + """ + Convert a TwistStamped message to a stamped PyKDL twist. + + :param msg: The TwistStamped message to convert. + :type msg: geometry_msgs.TwistStamped + :return: The converted PyKDL stamped twist. + :rtype: tf2_kdl_py.transform_datatypes.StampedTwist + """ + velocity = PyKDL.Vector(msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z) + rotation = PyKDL.Vector(msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z) + twist = PyKDL.Twist(velocity, rotation) + return tf2_kdl_py.transform_datatypes.StampedTwist(twist, msg.header.stamp, msg.header.frame_id) + + +tf2_ros.ConvertRegistration().add_from_msg(TwistStamped, from_msg_twist_stamped) + + +########### +# Wrench # +########### + + +def do_transform_wrench(wrench_in, transform_in): + """ + Apply a transform in the form of a geometry_msgs TransformStamped to a PyKDL Wrench. + + :param wrench_in: The Stamped PyKDL wrench to transform. + :type wrench_in: tf2_kdl_py.transform_datatypes.StampedWrench + :param transform_in: The transform to apply. + :type transform_in: geometry_msgs.msg.TransformStamped + :return: The transformed stamped PyKDL wrench. + :rtype: tf2_kdl_py.transform_datatypes.StampedWrench + """ + wrench_out = transform_to_kdl(transform_in) * wrench_in + return tf2_kdl_py.transform_datatypes.StampedWrench(wrench_out, transform_in.header.stamp, transform_in.header.frame_id) + + +tf2_ros.TransformRegistration().add(tf2_kdl_py.transform_datatypes.StampedWrench, do_transform_wrench) + + +def to_msg_wrench_stamped(wrench_in): + """ + Convert a stamped PyKDL Wrench to a geometry_msgs WrenchStamped message. + + :param wrench_in: The stamped wrench to convert. + :type wrench_in: tf2_kdl_py.transform_datatypes.StampedWrench + :return: The converted stamped wrench. + :rtype: geometry_msgs.WrenchStamped + """ + msg = WrenchStamped() + msg.header.stamp = wrench_in.header.stamp + msg.header.frame_id = wrench_in.header.frame_id + msg.wrench.force.x = wrench_in.wrench.force[0] + msg.wrench.force.y = wrench_in.wrench.force[1] + msg.wrench.force.z = wrench_in.wrench.force[2] + msg.wrench.torque.x = wrench_in.wrench.torque[0] + msg.wrench.torque.y = wrench_in.wrench.torque[1] + msg.wrench.torque.z = wrench_in.wrench.torque[2] + return msg + + +tf2_ros.ConvertRegistration().add_to_msg(tf2_kdl_py.transform_datatypes.StampedWrench, to_msg_wrench_stamped) + + +def from_msg_wrench_stamped(msg): + """ + Convert a WrenchStamped message to a stamped PyKDL wrench. + + :param msg: The WrenchStamped message to convert. + :type msg: geometry_msgs.WrenchStamped + :return: The converted PyKDL stamped wrench. + :rtype: tf2_kdl_py.transform_datatypes.StampedWrench + """ + force = PyKDL.Vector(msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z) + torque = PyKDL.Vector(msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z) + wrench = PyKDL.Wrench(force, torque) + return tf2_kdl_py.transform_datatypes.StampedWrench(wrench, msg.header.stamp, msg.header.frame_id) + + +tf2_ros.ConvertRegistration().add_from_msg(WrenchStamped, from_msg_wrench_stamped) diff --git a/tf2_kdl_py/tf2_kdl_py/transform_datatypes.py b/tf2_kdl_py/tf2_kdl_py/transform_datatypes.py new file mode 100644 index 000000000..5eb12453d --- /dev/null +++ b/tf2_kdl_py/tf2_kdl_py/transform_datatypes.py @@ -0,0 +1,76 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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 Open Source Robotics Foundation, 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 HOLDER 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 rclpy.time import Time +import PyKDL + + +class Header: + def __init__(self, stamp_, frame_id_): + self.stamp = stamp_ + self.frame_id = frame_id_ + + +class StampedVector(PyKDL.Vector): + def __init__(self, vector, timestamp: Time, frame_id: str): + super().__init__(vector) + self.vector = vector + self.header = Header(timestamp, frame_id) + + def set_data(self, vector): + self.vector = vector + + +class StampedFrame(PyKDL.Frame): + def __init__(self, frame, timestamp: Time, frame_id: str): + super().__init__(frame) + self.frame = frame + self.header = Header(timestamp, frame_id) + + def set_data(self, frame): + self.frame = frame + + +class StampedTwist(PyKDL.Twist): + def __init__(self, twist, timestamp: Time, frame_id: str): + super().__init__(twist) + self.twist = twist + self.header = Header(timestamp, frame_id) + + def set_data(self, twist): + self.twist = twist + + +class StampedWrench(PyKDL.Wrench): + def __init__(self, wrench, timestamp: Time, frame_id: str): + super().__init__(wrench) + self.wrench = wrench + self.header = Header(timestamp, frame_id) + + def set_data(self, wrench): + self.wrench = wrench diff --git a/tf2_ros_py/tf2_ros/__init__.py b/tf2_ros_py/tf2_ros/__init__.py index 018aa88ae..14c9eb639 100644 --- a/tf2_ros_py/tf2_ros/__init__.py +++ b/tf2_ros_py/tf2_ros/__init__.py @@ -31,7 +31,7 @@ #* 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 tf2_py import * @@ -39,5 +39,6 @@ from .buffer import * from .buffer_client import * from .transform_listener import * +from .transform_datatypes import * from .transform_broadcaster import * from .static_transform_broadcaster import * diff --git a/tf2_ros_py/tf2_ros/buffer_interface.py b/tf2_ros_py/tf2_ros/buffer_interface.py index 933d4b29d..ad7d61e8b 100644 --- a/tf2_ros_py/tf2_ros/buffer_interface.py +++ b/tf2_ros_py/tf2_ros/buffer_interface.py @@ -32,6 +32,7 @@ from typing import Callable from typing import Tuple from typing import Any +from typing import get_args import rclpy import tf2_py as tf2 @@ -39,10 +40,12 @@ from copy import deepcopy from std_msgs.msg import Header from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import TwistStamped from geometry_msgs.msg import PointStamped from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped from geometry_msgs.msg import Vector3Stamped +from geometry_msgs.msg import WrenchStamped from sensor_msgs.msg import PointCloud2 from rclpy.time import Time @@ -53,10 +56,13 @@ PoseStamped, PoseWithCovarianceStamped, Vector3Stamped, - PointCloud2 + PointCloud2, + TwistStamped, + WrenchStamped ] PyKDLType = TypeVar("PyKDLType") -TransformableObject = Union[MsgStamped, PyKDLType] +StampedPyKDLType = TypeVar("StampedPyKDLType") +TransformableObject = Union[MsgStamped, PyKDLType, StampedPyKDLType] TransformableObjectType = TypeVar("TransformableObjectType") @@ -67,6 +73,7 @@ class BufferInterface: Implementations include :class:tf2_ros.buffer.Buffer and :class:tf2_ros.buffer_client.BufferClient. """ + def __init__(self) -> None: self.registration = tf2_ros.TransformRegistration() @@ -229,16 +236,6 @@ def can_transform_full( raise NotImplementedException() -def Stamped( - obj: TransformableObject, - stamp: Time, - frame_id: str -) -> TransformableObject: - 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 @@ -343,7 +340,9 @@ def convert(a: TransformableObject, b_type: TransformableObjectType) -> Transfor 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)) + if b_type in get_args(MsgStamped): + f_to = c.get_to_msg(type(a)) + return f_to(a) + else: + f_from = c.get_from_msg(type(a)) + return f_from(a)