Have a functioning tf2_kdl_py

Signed-off-by: CursedRock17 <mtglucas1@gmail.com>

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
+# "<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 = '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
+}

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 <modules>
+
+Indices and tables
+==================
+
+* :ref:`genindex`
+* :ref:`search`

tf2_kdl_py/package.xml

@@ -2,7 +2,7 @@
   <name>tf2_kdl_py</name>
   <version>0.36.1</version>
   <description>
-    PyKDL binding for tf2
+    PyKDL bindings for tf2
   </description>
 
   <maintainer email="alejandro@openrobotics.org">Alejandro Hernandez Cordero</maintainer>
@@ -20,6 +20,6 @@
 
   <export>
     <build_type>ament_python</build_type>
+    <rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
-
 </package>

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: ''
+    }

tf2_kdl_py/setup.py

@@ -5,7 +5,7 @@
 setup(
     name=package_name,
     version='0.36.1',
-    packages=find_packages(exclude=['test']),
+    packages=find_packages([package_name]),
     data_files=[
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
@@ -15,7 +15,7 @@
     zip_safe=True,
     maintainer='Alejandro Hernandez Cordero, Chris Lalancette',
     maintainer_email='alejandro@openrobotics.org, clalancette@openrobotics.org',
-    description='PyKDL binding for tf2',
+    description='PyKDL bindings for tf2',
     license='Apache-2.0',
     tests_require=['pytest'],
 )

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

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

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

tf2_ros_py/tf2_ros/__init__.py

@@ -31,13 +31,14 @@
 #*  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 *
 from .buffer_interface import *
 from .buffer import *
 from .buffer_client import *
 from .transform_listener import *
+from .transform_datatypes import *
 from .transform_broadcaster import *
 from .static_transform_broadcaster import *

tf2_ros_py/tf2_ros/buffer_interface.py

@@ -32,17 +32,20 @@
 from typing import Callable
 from typing import Tuple
 from typing import Any
+from typing import get_args
 
 import rclpy
 import tf2_py as tf2
 import tf2_ros
 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)