diff --git a/tf2_kdl/docs/source/conf.py b/tf2_kdl/docs/source/conf.py index 8fd31a968..42482be37 100644 --- a/tf2_kdl/docs/source/conf.py +++ b/tf2_kdl/docs/source/conf.py @@ -1,9 +1,40 @@ # -*- coding: utf-8 -*- + +# Copyright 2020 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. + + # # tf2 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. +# 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. @@ -11,18 +42,18 @@ # 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('./src/tf2_kdl')) -# -- General configuration ----------------------------------------------------- +# -- General configuration ---------------------------------------------------- -# Add any Sphinx extension module names here, as strings. They can be extensions +# 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'] +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'] @@ -31,14 +62,14 @@ source_suffix = '.rst' # The encoding of source files. -#source_encoding = 'utf-8' +# source_encoding = 'utf-8' # The master toctree document. master_doc = 'index' # General information about the project. project = u'tf2_kdl' -copyright = u'2016, Open Source Robotics Foundation' +copyright = u'2016, Open Source Robotics Foundation' # noqa(A001) # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the @@ -51,16 +82,16 @@ # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. -#language = None +# language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: -#today = '' +# today = '' # Else, today_fmt is used as the format for a strftime call. -#today_fmt = '%B %d, %Y' +# today_fmt = '%B %d, %Y' # List of documents that shouldn't be included in the build. -#unused_docs = [] +# unused_docs = [] # List of directories, relative to source directory, that shouldn't be searched # for source files. @@ -68,28 +99,29 @@ exclude_patterns = ['_CHANGELOG.rst'] -# The reST default role (used for this markup: `text`) to use for all documents. -#default_role = None +# 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 +# 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 +# add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. -#show_authors = False +# 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 = [] +# modindex_common_prefix = [] -# -- Options for HTML output --------------------------------------------------- +# -- 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'. @@ -98,81 +130,82 @@ # 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 = {} +# html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. -#html_theme_path = [] +# html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". -#html_title = None +# html_title = None # A shorter title for the navigation bar. Default is the same as html_title. -#html_short_title = None +# 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 +# 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 +# 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'] +# 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' +# 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 +# html_use_smartypants = True # Custom sidebar templates, maps document names to template names. -#html_sidebars = {} +# html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. -#html_additional_pages = {} +# html_additional_pages = {} # If false, no module index is generated. -#html_use_modindex = True +# html_use_modindex = True # If false, no index is generated. -#html_use_index = True +# html_use_index = True # If true, the index is split into individual pages for each letter. -#html_split_index = False +# html_split_index = False # If true, links to the reST sources are added to the pages. -#html_show_sourcelink = True +# 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 = '' +# html_use_opensearch = '' # If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml"). -#html_file_suffix = '' +# html_file_suffix = '' # Output file base name for HTML help builder. htmlhelp_basename = 'tfdoc' -# -- Options for LaTeX output -------------------------------------------------- +# -- Options for LaTeX output ------------------------------------------------- # The paper size ('letter' or 'a4'). -#latex_paper_size = 'letter' +# latex_paper_size = 'letter' # The font size ('10pt', '11pt' or '12pt'). -#latex_font_size = '10pt' +# latex_font_size = '10pt' # Grouping the document tree into LaTeX files. List of tuples -# (source start file, target name, title, author, documentclass [howto/manual]). +# (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'), @@ -180,25 +213,25 @@ # The name of an image file (relative to this directory) to place at the top of # the title page. -#latex_logo = None +# latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. -#latex_use_parts = False +# latex_use_parts = False # Additional stuff for the LaTeX preamble. -#latex_preamble = '' +# latex_preamble = '' # Documents to append as an appendix to all manuals. -#latex_appendices = [] +# latex_appendices = [] # If false, no module index is generated. -#latex_use_modindex = True +# 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 + 'http://docs.scipy.org/doc/numpy': None } diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index 1adc9524e..4a440063e 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -1,39 +1,39 @@ -/* - * 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. - */ +// 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 */ -#ifndef TF2_KDL_H -#define TF2_KDL_H +#ifndef TF2_KDL__TF2_KDL_H_ +#define TF2_KDL__TF2_KDL_H_ #warning This header is obsolete, please include tf2_kdl/tf2_kdl.hpp instead #include -#endif // TF2_KDL_H +#endif // TF2_KDL__TF2_KDL_H_ diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp index 707781734..b183b564b 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.hpp @@ -1,31 +1,31 @@ -/* - * 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. - */ +// 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 */ diff --git a/tf2_kdl/scripts/test.py b/tf2_kdl/scripts/test.py index 8ecc490cb..521defc2b 100755 --- a/tf2_kdl/scripts/test.py +++ b/tf2_kdl/scripts/test.py @@ -1,15 +1,46 @@ #!/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 rclpy + +import builtin_interfaces +from geometry_msgs.msg import Quaternion, TransformStamped import PyKDL +import rclpy import tf2_ros -import tf2_kdl -from geometry_msgs.msg import TransformStamped, Quaternion -from copy import deepcopy -import builtin_interfaces +import tf2_kdl # noqa(F401) + class KDLConversions(unittest.TestCase): + def test_transform(self): b = tf2_ros.Buffer() t = TransformStamped() @@ -19,27 +50,32 @@ def test_transform(self): 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)) + 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') + 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') + 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') + 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) @@ -47,8 +83,9 @@ def test_transform(self): 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') + 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) @@ -57,7 +94,7 @@ def test_transform(self): self.assertEqual(out.torque.z(), -4) def test_convert(self): - v = PyKDL.Vector(1,2,3) + 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) diff --git a/tf2_kdl/src/tf2_kdl/__init__.py b/tf2_kdl/src/tf2_kdl/__init__.py index 97eef01b2..e6b5dc616 100644 --- a/tf2_kdl/src/tf2_kdl/__init__.py +++ b/tf2_kdl/src/tf2_kdl/__init__.py @@ -1 +1,30 @@ -from .tf2_kdl import * +# 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. + + +from .tf2_kdl import * # noqa diff --git a/tf2_kdl/src/tf2_kdl/tf2_kdl.py b/tf2_kdl/src/tf2_kdl/tf2_kdl.py index 477e2b916..6c4b4e04a 100644 --- a/tf2_kdl/src/tf2_kdl/tf2_kdl.py +++ b/tf2_kdl/src/tf2_kdl/tf2_kdl.py @@ -1,22 +1,23 @@ -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# +# 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. -# +# +# * 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 +# 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 @@ -25,31 +26,35 @@ # 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 rclpy import tf2_ros -from geometry_msgs.msg import PointStamped + def transform_to_kdl(t): - """Convert a geometry_msgs Transform message to a PyKDL Frame. + """ + 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), + 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. + """ + 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 @@ -62,10 +67,13 @@ def do_transform_vector(vector, transform): 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. + """ + Convert a PyKDL Vector to a geometry_msgs PointStamped message. :param vector: The vector to convert. :type vector: PyKDL.Vector @@ -79,10 +87,13 @@ def to_msg_vector(vector): 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. + """ + Convert a PointStamped message to a stamped PyKDL Vector. :param msg: The PointStamped message to convert. :type msg: geometry_msgs.msg.PointStamped @@ -92,21 +103,30 @@ def from_msg_vector(msg): 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. + """ + 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) + 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) -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. + """ + 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 @@ -118,10 +138,14 @@ def do_transform_frame(frame, transform): 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. + """ + 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 @@ -133,12 +157,15 @@ def do_transform_twist(twist, transform): 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. + """ + 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 @@ -150,4 +177,6 @@ def do_transform_wrench(wrench, transform): 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_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index c3319ca88..33d083a8e 100644 --- a/tf2_kdl/test/test_tf2_kdl.cpp +++ b/tf2_kdl/test/test_tf2_kdl.cpp @@ -1,31 +1,31 @@ -/* - * 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. - */ +// 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 */