From 84dd504fdfe1e3e0257e07e9a05e5b73a8ba2261 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 3 Dec 2024 08:41:38 -0500 Subject: [PATCH] Add in the linters for tf2_ros_py. (#740) Signed-off-by: Chris Lalancette --- tf2_ros_py/doc/source/conf.py | 2 +- tf2_ros_py/test/test_buffer.py | 51 ++++--- tf2_ros_py/test/test_buffer_client.py | 78 +++++------ tf2_ros_py/test/test_copyright.py | 36 +++++ tf2_ros_py/test/test_flake8.py | 39 ++++++ .../test/test_listener_and_broadcaster.py | 76 +++++------ tf2_ros_py/test/test_pep257.py | 36 +++++ tf2_ros_py/test/test_xmllint.py | 37 +++-- tf2_ros_py/tf2_ros/__init__.py | 81 ++++++----- tf2_ros_py/tf2_ros/buffer.py | 109 ++++++++------- tf2_ros_py/tf2_ros/buffer_client.py | 126 +++++++++--------- tf2_ros_py/tf2_ros/buffer_interface.py | 114 +++++++++------- .../tf2_ros/static_transform_broadcaster.py | 61 +++++---- .../tf2_ros/static_transform_listener.py | 41 +++--- tf2_ros_py/tf2_ros/transform_broadcaster.py | 70 +++++----- tf2_ros_py/tf2_ros/transform_listener.py | 46 ++++--- 16 files changed, 572 insertions(+), 431 deletions(-) create mode 100644 tf2_ros_py/test/test_copyright.py create mode 100644 tf2_ros_py/test/test_flake8.py create mode 100644 tf2_ros_py/test/test_pep257.py diff --git a/tf2_ros_py/doc/source/conf.py b/tf2_ros_py/doc/source/conf.py index 7928c1dbe..48f687f9c 100644 --- a/tf2_ros_py/doc/source/conf.py +++ b/tf2_ros_py/doc/source/conf.py @@ -35,7 +35,7 @@ # -- Project information ----------------------------------------------------- project = 'tf2_ros_py' -copyright = '2023, Open Source Robotics Foundation, Inc.' +copyright = '2023, Open Source Robotics Foundation, Inc.' # noqa: A001 author = 'Open Source Robotics Foundation, Inc.' # The full version, including alpha/beta/rc tags diff --git a/tf2_ros_py/test/test_buffer.py b/tf2_ros_py/test/test_buffer.py index f805f0e7c..add27a066 100644 --- a/tf2_ros_py/test/test_buffer.py +++ b/tf2_ros_py/test/test_buffer.py @@ -1,40 +1,39 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# All rights reserved. +# Copyright (c) 2019 Open Source Robotics Foundation, 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: +# 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. +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. # -# 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 +# * 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 copyright holder 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 geometry_msgs.msg import TransformStamped import pytest import rclpy - from tf2_ros.buffer import Buffer -from geometry_msgs.msg import TransformStamped, PointStamped + class TestBuffer: + def build_transform(self, target, source, rclpy_time): transform = TransformStamped() transform.header.frame_id = target diff --git a/tf2_ros_py/test/test_buffer_client.py b/tf2_ros_py/test/test_buffer_client.py index 7e1674526..6071a032e 100644 --- a/tf2_ros_py/test/test_buffer_client.py +++ b/tf2_ros_py/test/test_buffer_client.py @@ -1,46 +1,42 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# All rights reserved. +# Copyright (c) 2019 Open Source Robotics Foundation, 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: +# 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. +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. # -# 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 +# * 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 copyright holder 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 import threading -import time - -import rclpy -from tf2_ros.buffer_client import BufferClient from geometry_msgs.msg import TransformStamped -from tf2_msgs.action import LookupTransform -from tf2_py import BufferCore, LookupException +import pytest +import rclpy from rclpy.executors import SingleThreadedExecutor from rclpy.time import Time +from tf2_msgs.action import LookupTransform from tf2_msgs.msg import TF2Error +from tf2_py import BufferCore, LookupException +from tf2_ros.buffer_client import BufferClient def build_transform(target_frame, source_frame, stamp): @@ -61,8 +57,10 @@ def build_transform(target_frame, source_frame, stamp): class MockBufferServer(): + def __init__(self, node, buffer_core): - self.action_server = rclpy.action.ActionServer(node, LookupTransform, 'lookup_transform', self.execute_callback) + self.action_server = rclpy.action.ActionServer( + node, LookupTransform, 'lookup_transform', self.execute_callback) self.buffer_core = buffer_core def execute_callback(self, goal_handle): @@ -72,9 +70,10 @@ def execute_callback(self, goal_handle): try: if not goal_handle.request.advanced: - transform = self.buffer_core.lookup_transform_core(target_frame=goal_handle.request.target_frame, - source_frame=goal_handle.request.source_frame, - time=Time.from_msg(goal_handle.request.source_time)) + transform = self.buffer_core.lookup_transform_core( + target_frame=goal_handle.request.target_frame, + source_frame=goal_handle.request.source_frame, + time=Time.from_msg(goal_handle.request.source_time)) else: transform = self.buffer_core.lookup_transform_full_core( target_frame=goal_handle.request.target_frame, @@ -85,7 +84,7 @@ def execute_callback(self, goal_handle): ) response.transform = transform goal_handle.succeed() - except LookupException as e: + except LookupException: response.error.error = TF2Error.LOOKUP_ERROR goal_handle.abort() @@ -96,6 +95,7 @@ def destroy(self): class TestBufferClient: + @classmethod def setup_class(cls): cls.context = rclpy.context.Context() @@ -133,7 +133,8 @@ def spin(self): def test_lookup_transform_true(self): buffer_client = BufferClient( - self.node, 'lookup_transform', check_frequency=10.0, timeout_padding=rclpy.duration.Duration(seconds=0.0)) + self.node, 'lookup_transform', check_frequency=10.0, + timeout_padding=rclpy.duration.Duration(seconds=0.0)) result = buffer_client.lookup_transform( 'foo', 'bar', rclpy.time.Time(), rclpy.duration.Duration(seconds=5.0)) @@ -144,7 +145,8 @@ def test_lookup_transform_true(self): def test_lookup_transform_fail(self): buffer_client = BufferClient( - self.node, 'lookup_transform', check_frequency=10.0, timeout_padding=rclpy.duration.Duration(seconds=0.0)) + self.node, 'lookup_transform', check_frequency=10.0, + timeout_padding=rclpy.duration.Duration(seconds=0.0)) with pytest.raises(LookupException) as excinfo: buffer_client.lookup_transform( diff --git a/tf2_ros_py/test/test_copyright.py b/tf2_ros_py/test/test_copyright.py new file mode 100644 index 000000000..e6f0d080a --- /dev/null +++ b/tf2_ros_py/test/test_copyright.py @@ -0,0 +1,36 @@ +# Copyright (c) 2024 Open Source Robotics Foundation, 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 copyright holder 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + assert main(argv=['.', 'test']) == 0, 'Found errors' diff --git a/tf2_ros_py/test/test_flake8.py b/tf2_ros_py/test/test_flake8.py new file mode 100644 index 000000000..6026be197 --- /dev/null +++ b/tf2_ros_py/test/test_flake8.py @@ -0,0 +1,39 @@ +# Copyright (c) 2024 Open Source Robotics Foundation, 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 copyright holder 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/tf2_ros_py/test/test_listener_and_broadcaster.py b/tf2_ros_py/test/test_listener_and_broadcaster.py index 60c31546e..75d76901c 100644 --- a/tf2_ros_py/test/test_listener_and_broadcaster.py +++ b/tf2_ros_py/test/test_listener_and_broadcaster.py @@ -1,43 +1,40 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# All rights reserved. +# Copyright (c) 2019 Open Source Robotics Foundation, 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: +# 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. +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. # -# 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 +# * 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 copyright holder 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 geometry_msgs.msg import TransformStamped import pytest import rclpy - -from geometry_msgs.msg import TransformStamped +from tf2_ros import ExtrapolationException from tf2_ros.buffer import Buffer -from tf2_ros.transform_broadcaster import TransformBroadcaster from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster -from tf2_ros.transform_listener import TransformListener from tf2_ros.static_transform_listener import StaticTransformListener -from tf2_ros import ExtrapolationException +from tf2_ros.transform_broadcaster import TransformBroadcaster +from tf2_ros.transform_listener import TransformListener def build_transform(target_frame, source_frame, stamp): @@ -58,6 +55,7 @@ def build_transform(target_frame, source_frame, stamp): class TestBroadcasterAndListener: + @classmethod def setup_class(cls): rclpy.init() @@ -160,21 +158,21 @@ def test_broadcaster_and_listener(self): assert broadcasted_transform == listened_transform_async def test_static_broadcaster_and_listener(self): - broadcasted_transform = self.broadcast_static_transform( + broadcasted_tf = self.broadcast_static_transform( target_frame='foo', source_frame='bar', time_stamp=rclpy.time.Time(seconds=1.1, nanoseconds=0).to_msg()) - listened_transform = self.buffer.lookup_transform( + listened_tf = self.buffer.lookup_transform( target_frame='foo', source_frame='bar', time=rclpy.time.Time(seconds=1.5, nanoseconds=0).to_msg()) - assert broadcasted_transform.header.stamp.sec == 1 - assert broadcasted_transform.header.stamp.nanosec == 100000000 + assert broadcasted_tf.header.stamp.sec == 1 + assert broadcasted_tf.header.stamp.nanosec == 100000000 - assert listened_transform.header.stamp.sec == 1 - assert listened_transform.header.stamp.nanosec == 500000000 + assert listened_tf.header.stamp.sec == 1 + assert listened_tf.header.stamp.nanosec == 500000000 - assert broadcasted_transform.header.frame_id == listened_transform.header.frame_id - assert broadcasted_transform.child_frame_id == listened_transform.child_frame_id - assert broadcasted_transform.transform.translation == listened_transform.transform.translation - assert broadcasted_transform.transform.rotation == listened_transform.transform.rotation + assert broadcasted_tf.header.frame_id == listened_tf.header.frame_id + assert broadcasted_tf.child_frame_id == listened_tf.child_frame_id + assert broadcasted_tf.transform.translation == listened_tf.transform.translation + assert broadcasted_tf.transform.rotation == listened_tf.transform.rotation diff --git a/tf2_ros_py/test/test_pep257.py b/tf2_ros_py/test/test_pep257.py new file mode 100644 index 000000000..17d725b9a --- /dev/null +++ b/tf2_ros_py/test/test_pep257.py @@ -0,0 +1,36 @@ +# Copyright (c) 2024 Open Source Robotics Foundation, 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 copyright holder 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + assert main(argv=['.', 'test']) == 0, 'Found code style errors / warnings' diff --git a/tf2_ros_py/test/test_xmllint.py b/tf2_ros_py/test/test_xmllint.py index f46285e71..be5fac7f6 100644 --- a/tf2_ros_py/test/test_xmllint.py +++ b/tf2_ros_py/test/test_xmllint.py @@ -1,16 +1,30 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. +# Copyright (c) 2024 Open Source Robotics Foundation, Inc. All rights reserved. # -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: # -# http://www.apache.org/licenses/LICENSE-2.0 +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. # -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. +# * 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 copyright holder 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 ament_xmllint.main import main import pytest @@ -19,5 +33,4 @@ @pytest.mark.linter @pytest.mark.xmllint def test_xmllint(): - rc = main(argv=[]) - assert rc == 0, 'Found errors' + assert main(argv=[]) == 0, 'Found errors' diff --git a/tf2_ros_py/tf2_ros/__init__.py b/tf2_ros_py/tf2_ros/__init__.py index 018aa88ae..28bc68a64 100644 --- a/tf2_ros_py/tf2_ros/__init__.py +++ b/tf2_ros_py/tf2_ros/__init__.py @@ -1,43 +1,38 @@ -#! /usr/bin/python -#*********************************************************** -#* Software License Agreement (BSD License) -#* -#* Copyright (c) 2009, Willow Garage, Inc. -#* All rights reserved. -#* -#* Redistribution and use in source and binary forms, with or without -#* modification, are permitted provided that the following conditions -#* are met: -#* -#* * Redistributions of source code must retain the above copyright -#* notice, this list of conditions and the following disclaimer. -#* * Redistributions in binary form must reproduce the above -#* copyright notice, this list of conditions and the following -#* disclaimer in the documentation and/or other materials provided -#* with the distribution. -#* * Neither the name of Willow Garage, Inc. nor the names of its -#* contributors may be used to endorse or promote products derived -#* from this software without specific prior written permission. -#* -#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -#* POSSIBILITY OF SUCH DAMAGE. -#* -#* Author: Eitan Marder-Eppstein -#*********************************************************** -from tf2_py import * -from .buffer_interface import * -from .buffer import * -from .buffer_client import * -from .transform_listener import * -from .transform_broadcaster import * -from .static_transform_broadcaster import * +# Copyright (c) 2009 Willow Garage, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder 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: Eitan Marder-Eppstein + +from tf2_py import * # noqa: F401, F403 + +from .buffer import * # noqa: F401, F403 +from .buffer_client import * # noqa: F401, F403 +from .buffer_interface import * # noqa: F401, F403 +from .static_transform_broadcaster import * # noqa: F401, F403 +from .transform_broadcaster import * # noqa: F401, F403 +from .transform_listener import * # noqa: F401, F403 diff --git a/tf2_ros_py/tf2_ros/buffer.py b/tf2_ros_py/tf2_ros/buffer.py index 8fe69ca41..52fcf07a6 100644 --- a/tf2_ros_py/tf2_ros/buffer.py +++ b/tf2_ros_py/tf2_ros/buffer.py @@ -1,22 +1,23 @@ -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. +# 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. +# * 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 copyright holder 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 @@ -26,34 +27,31 @@ # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen -from typing import TypeVar -from typing import Optional -from typing import Any -from typing import List -from typing import Tuple -from typing import Callable import threading +from time import sleep +from typing import Callable +from typing import List +from typing import Optional +from typing import TypeVar -import rclpy -import tf2_py as tf2 -import tf2_ros -from tf2_msgs.srv import FrameGraph from geometry_msgs.msg import TransformStamped -# TODO(vinnamkim): It seems rosgraph is not ready -# import rosgraph.masterapi -from time import sleep +import rclpy from rclpy.clock import JumpThreshold, TimeJump -from rclpy.node import Node -from rclpy.time import Time from rclpy.duration import Duration +from rclpy.node import Node from rclpy.task import Future +from rclpy.time import Time +from tf2_msgs.srv import FrameGraph +import tf2_py as tf2 + +from .buffer_interface import BufferInterface FrameGraphSrvRequest = TypeVar('FrameGraphSrvRequest') FrameGraphSrvResponse = TypeVar('FrameGraphSrvResponse') -class Buffer(tf2.BufferCore, tf2_ros.BufferInterface): +class Buffer(tf2.BufferCore, BufferInterface): """ Standard implementation of the :class:`tf2_ros.BufferInterface` abstract data type. @@ -70,16 +68,16 @@ def __init__( node: Optional[Node] = None ) -> None: """ - Constructor. + Construct a Buffer. - :param cache_time: Duration object describing how long to retain past information in BufferCore. + :param cache_time: Duration describing how long to retain past information in BufferCore. :param node: Create a tf2_frames service that returns all frames as a yaml document. """ if cache_time is not None: tf2.BufferCore.__init__(self, cache_time) else: tf2.BufferCore.__init__(self) - tf2_ros.BufferInterface.__init__(self) + BufferInterface.__init__(self) self._new_data_callbacks: List[Callable[[], None]] = [] self._callbacks_to_remove: List[Callable[[], None]] = [] @@ -91,11 +89,13 @@ def __init__( else: self.clock = rclpy.clock.Clock() - # create a jump callback so as to clear the buffer if use_sim_true is true and there is a jump in time + # create a jump callback to clear the buffer if use_sim_true is true and there is a + # jump in time threshold = JumpThreshold(min_forward=None, min_backward=Duration(seconds=-1), on_clock_change=True) - self.jump_handle = self.clock.create_jump_callback(threshold, post_callback=self.time_jump_callback) + self.jump_handle = self.clock.create_jump_callback( + threshold, post_callback=self.time_jump_callback) def __get_frames( self, @@ -105,7 +105,8 @@ def __get_frames( return FrameGraph.Response(frame_yaml=self.all_frames_as_yaml()) def time_jump_callback(self, time_jump: TimeJump): - rclpy.logging.get_logger("tf2_buffer").warning("Detected jump back in time. Clearing tf buffer.") + rclpy.logging.get_logger('tf2_buffer').warning( + 'Detected jump back in time. Clearing tf buffer.') self.clear() def set_transform(self, transform: TransformStamped, authority: str) -> None: @@ -181,12 +182,13 @@ def lookup_transform_full( :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to (0 will get the latest). :param source_frame: Name of the input frame. - :param source_time: The time at which source_frame will be evaluated (0 will get the latest). + :param source_time: The time at which source_frame will be evaluated (0 gets the latest). :param fixed_frame: Name of the frame to consider constant in time. :param timeout: Time to wait for the target frame to become available. :return: The transform between the frames. """ - self.can_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout) + self.can_transform_full( + target_frame, target_time, source_frame, source_time, fixed_frame, timeout) return self.lookup_transform_full_core( target_frame, target_time, source_frame, source_time, fixed_frame) @@ -199,16 +201,17 @@ async def lookup_transform_full_async( fixed_frame: str ) -> TransformStamped: """ - Get the transform from the source frame to the target frame using the advanced API asyncronously. + Get transform from source frame to target frame using the advanced API asyncronously. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to (0 will get the latest). :param source_frame: Name of the input frame. - :param source_time: The time at which source_frame will be evaluated (0 will get the latest). + :param source_time: The time at which source_frame will be evaluated (0 gets the latest). :param fixed_frame: Name of the frame to consider constant in time. :return: The transform between the frames. """ - await self.wait_for_transform_full_async(target_frame, target_time, source_frame, source_time, fixed_frame) + await self.wait_for_transform_full_async( + target_frame, target_time, source_frame, source_time, fixed_frame) return self.lookup_transform_full_core( target_frame, target_time, source_frame, source_time, fixed_frame) @@ -235,7 +238,8 @@ def can_transform( start_time = clock.now() while (clock.now() < start_time + timeout and not self.can_transform_core(target_frame, source_frame, time)[0] and - (clock.now() + Duration(seconds=3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them + (clock.now() + Duration(seconds=3.0)) >= start_time): + # big jumps in time are likely bag loops, so break for them # TODO(Anyone): We can't use Rate here because it would never expire # with a single-threaded executor. # See https://github.com/ros2/geometry2/issues/327 for ideas on @@ -265,7 +269,7 @@ def can_transform_full( :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to (0 will get the latest). :param source_frame: Name of the input frame. - :param source_time: The time at which source_frame will be evaluated (0 will get the latest). + :param source_time: The time at which source_frame will be evaluated (0 gets the latest). :param fixed_frame: Name of the frame to consider constant in time. :param timeout: Time to wait for the target frame to become available. :param return_debug_type: If true, return a tuple representing debug information. @@ -275,14 +279,17 @@ def can_transform_full( if timeout != Duration(): start_time = clock.now() while (clock.now() < start_time + timeout and - not self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0] and - (clock.now() + Duration(seconds=3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them + not self.can_transform_full_core( + target_frame, target_time, source_frame, source_time, fixed_frame)[0] and + (clock.now() + Duration(seconds=3.0)) >= start_time): + # big jumps in time are likely bag loops, so break for them # TODO(Anyone): We can't use Rate here because it would never expire # with a single-threaded executor. # See https://github.com/ros2/geometry2/issues/327 for ideas on # how to timeout waiting for transforms that don't block the executor. sleep(0.02) - core_result = self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame) + core_result = self.can_transform_full_core( + target_frame, target_time, source_frame, source_time, fixed_frame) if return_debug_tuple: return core_result return core_result[0] @@ -333,20 +340,26 @@ def wait_for_transform_full_async( :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to (0 will get the latest). :param source_frame: Name of the input frame. - :param source_time: The time at which source_frame will be evaluated (0 will get the latest). + :param source_time: The time at which source_frame will be evaluated (0 gets the latest). :param fixed_frame: Name of the frame to consider constant in time. :return: A future that becomes true when the transform is available. """ fut = rclpy.task.Future() - if self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0]: + if self.can_transform_full_core( + target_frame, target_time, source_frame, source_time, fixed_frame)[0]: # Short cut, the transform is available - fut.set_result(self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)) + fut.set_result( + self.lookup_transform_full_core( + target_frame, target_time, source_frame, source_time, fixed_frame)) return fut def _on_new_data(): try: - if self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0]: - fut.set_result(self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)) + if self.can_transform_full_core( + target_frame, target_time, source_frame, source_time, fixed_frame)[0]: + fut.set_result( + self.lookup_transform_full_core( + target_frame, target_time, source_frame, source_time, fixed_frame)) except BaseException as e: fut.set_exception(e) diff --git a/tf2_ros_py/tf2_ros/buffer_client.py b/tf2_ros_py/tf2_ros/buffer_client.py index a8d38022f..02c5243f6 100644 --- a/tf2_ros_py/tf2_ros/buffer_client.py +++ b/tf2_ros_py/tf2_ros/buffer_client.py @@ -1,67 +1,58 @@ -#! /usr/bin/python -#*********************************************************** -#* Software License Agreement (BSD License) -#* -#* Copyright (c) 2009, Willow Garage, Inc. -#* All rights reserved. -#* -#* Redistribution and use in source and binary forms, with or without -#* modification, are permitted provided that the following conditions -#* are met: -#* -#* * Redistributions of source code must retain the above copyright -#* notice, this list of conditions and the following disclaimer. -#* * Redistributions in binary form must reproduce the above -#* copyright notice, this list of conditions and the following -#* disclaimer in the documentation and/or other materials provided -#* with the distribution. -#* * Neither the name of Willow Garage, Inc. nor the names of its -#* contributors may be used to endorse or promote products derived -#* from this software without specific prior written permission. -#* -#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -#* POSSIBILITY OF SUCH DAMAGE. -#* -#* Author: Eitan Marder-Eppstein -#*********************************************************** +# Copyright (c) 2009 Willow Garage, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder 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: Eitan Marder-Eppstein + +import threading +from time import sleep from typing import TypeVar +import warnings +import builtin_interfaces.msg from geometry_msgs.msg import TransformStamped - -from rclpy.node import Node from rclpy.action.client import ActionClient +from rclpy.clock import Clock from rclpy.duration import Duration +from rclpy.node import Node from rclpy.time import Time -from rclpy.clock import Clock -from time import sleep - -import builtin_interfaces.msg +from tf2_msgs.action import LookupTransform import tf2_py as tf2 -import tf2_ros -import threading -import warnings -from tf2_msgs.action import LookupTransform +from .buffer_interface import BufferInterface # Used for documentation purposes only LookupTransformGoal = TypeVar('LookupTransformGoal') LookupTransformResult = TypeVar('LookupTransformResult') -class BufferClient(tf2_ros.BufferInterface): - """ - Action client-based implementation of BufferInterface. - """ +class BufferClient(BufferInterface): + """Action client-based implementation of BufferInterface.""" + def __init__( self, node: Node, @@ -70,14 +61,14 @@ def __init__( timeout_padding: Duration = Duration(seconds=2.0) ) -> None: """ - Constructor. + Construct a BufferClient. - :param node: The ROS2 node. + :param node: The ROS 2 node. :param ns: The namespace in which to look for a BufferServer. :param check_frequency: How frequently to check for updates to known transforms. :param timeout_padding: A constant timeout to add to blocking calls. """ - tf2_ros.BufferInterface.__init__(self) + BufferInterface.__init__(self) self.node = node self.action_client = ActionClient(node, LookupTransform, action_name=ns) self.check_frequency = check_frequency @@ -103,8 +94,8 @@ def lookup_transform( if isinstance(time, builtin_interfaces.msg.Time): source_time = Time.from_msg(time) warnings.warn( - 'Passing a builtin_interfaces.msg.Time argument is deprecated, and will be removed in the near future. ' - 'Use rclpy.time.Time instead.') + 'Passing a builtin_interfaces.msg.Time argument is deprecated, ' + 'and will be removed in the near future. Use rclpy.time.Time instead.') elif isinstance(time, Time): source_time = time else: @@ -135,7 +126,7 @@ def lookup_transform_full( :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. - :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param source_time: The time at which source_frame will be evaluated. (0 gets the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: Time to wait for the target frame to become available. :return: The transform between the frames. @@ -193,14 +184,15 @@ def can_transform_full( :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. - :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) + :param source_time: The time at which source_frame will be evaluated. (0 gets the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: Time to wait for the target frame to become available. :param return_debug_type: If true, return a tuple representing debug information. :return: True if the transform is possible, false otherwise. """ try: - self.lookup_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout) + self.lookup_transform_full( + target_frame, target_time, source_frame, source_time, fixed_frame, timeout) return True except tf2.TransformException: return False @@ -208,7 +200,7 @@ def can_transform_full( def __process_goal(self, goal: LookupTransformGoal) -> TransformStamped: # TODO(sloretz) why is this an action client? Service seems more appropriate. if not self.action_client.server_is_ready(): - raise tf2.TimeoutException("The BufferServer is not ready.") + raise tf2.TimeoutException('The BufferServer is not ready.') event = threading.Event() @@ -243,21 +235,28 @@ def unblock_by_timeout(): # This shouldn't happen, but could in rare cases where the server hangs if not send_goal_future.done(): - raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.") + raise tf2.TimeoutException( + 'The LookupTransform goal sent to the BufferServer did not come back in the ' + 'specified time. Something is likely wrong with the server.') # Raises if future was given an exception goal_handle = send_goal_future.result() if not goal_handle.accepted: - raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with accepted status. Something is likely wrong with the server.") + raise tf2.TimeoutException( + 'The LookupTransform goal sent to the BufferServer did not come back with ' + 'accepted status. Something is likely wrong with the server.') response = self.action_client._get_result(goal_handle) return self.__process_result(response.result) def __process_result(self, result: LookupTransformResult) -> TransformStamped: - if result == None or result.error == None: - raise tf2.TransformException("The BufferServer returned None for result or result.error! Something is likely wrong with the server.") + if result is None or result.error is None: + raise tf2.TransformException( + 'The BufferServer returned None for result or result.error! ' + 'Something is likely wrong with the server.') + if result.error.error != result.error.NO_ERROR: if result.error.error == result.error.LOOKUP_ERROR: raise tf2.LookupException(result.error.error_string) @@ -276,5 +275,4 @@ def __process_result(self, result: LookupTransformResult) -> TransformStamped: def destroy(self) -> None: """Cleanup resources associated with this BufferClient.""" - self.action_client.destroy() diff --git a/tf2_ros_py/tf2_ros/buffer_interface.py b/tf2_ros_py/tf2_ros/buffer_interface.py index 933d4b29d..35c5385e5 100644 --- a/tf2_ros_py/tf2_ros/buffer_interface.py +++ b/tf2_ros_py/tf2_ros/buffer_interface.py @@ -1,22 +1,23 @@ -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. +# 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. +# * 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 copyright holder 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 @@ -26,27 +27,26 @@ # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen + +from copy import deepcopy +from typing import Any +from typing import Callable from typing import Optional +from typing import Tuple from typing import TypeVar from typing import Union -from typing import Callable -from typing import Tuple -from typing import Any -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 PointStamped from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import TransformStamped from geometry_msgs.msg import Vector3Stamped +from rclpy.duration import Duration +from rclpy.time import Time from sensor_msgs.msg import PointCloud2 +from std_msgs.msg import Header +import tf2_ros -from rclpy.time import Time -from rclpy.duration import Duration MsgStamped = Union[ PointStamped, @@ -55,18 +55,19 @@ Vector3Stamped, PointCloud2 ] -PyKDLType = TypeVar("PyKDLType") +PyKDLType = TypeVar('PyKDLType') TransformableObject = Union[MsgStamped, PyKDLType] -TransformableObjectType = TypeVar("TransformableObjectType") +TransformableObjectType = TypeVar('TransformableObjectType') class BufferInterface: """ - Abstract interface for wrapping the Python bindings for the tf2 library in - a ROS-based convenience API. + Abstract interface for wrapping the Python tf2 library in a ROS convenience API. + Implementations include :class:tf2_ros.buffer.Buffer and :class:tf2_ros.buffer_client.BufferClient. """ + def __init__(self) -> None: self.registration = tf2_ros.TransformRegistration() @@ -81,10 +82,11 @@ def transform( """ Transform an input into the target frame. - The input must be a known transformable type (by way of the tf2 data type conversion interface). + The input must be a known transformable type (by way of the tf2 data type conversion + interface). - If new_type is not None, the type specified must have a valid conversion from the input type, - else the function will raise an exception. + If new_type is not None, the type specified must have a valid conversion from the input + type, else the function will raise an exception. :param object_stamped: The timestamped object to transform. :param target_frame: Name of the frame to transform the input into. @@ -93,9 +95,12 @@ def transform( :return: The transformed, timestamped output, possibly converted to a new type. """ do_transform = self.registration.get(type(object_stamped)) - res = do_transform(object_stamped, self.lookup_transform(target_frame, object_stamped.header.frame_id, - object_stamped.header.stamp, timeout)) - if new_type == None: + res = do_transform( + object_stamped, + self.lookup_transform( + target_frame, object_stamped.header.frame_id, + object_stamped.header.stamp, timeout)) + if new_type is None: return res return convert(res, new_type) @@ -113,13 +118,14 @@ def transform_full( """ Transform an input into the target frame (advanced API). - The input must be a known transformable type (by way of the tf2 data type conversion interface). + The input must be a known transformable type (by way of the tf2 data type conversion + interface). - If new_type is not None, the type specified must have a valid conversion from the input type, - else the function will raise an exception. + If new_type is not None, the type specified must have a valid conversion from the input + type, else the function will raise an exception. - This function follows the advanced API, which allows tranforming between different time points, - as well as specifying a frame to be considered fixed in time. + This function follows the advanced API, which allows tranforming between different time + points, as well as specifying a frame to be considered fixed in time. :param object_stamped: The timestamped object to transform. :param target_frame: Name of the frame to transform the input into. @@ -130,10 +136,11 @@ def transform_full( :return: The transformed, timestamped output, possibly converted to a new type. """ do_transform = self.registration.get(type(object_stamped)) - res = do_transform(object_stamped, self.lookup_transform_full(target_frame, target_time, - object_stamped.header.frame_id, object_stamped.header.stamp, - fixed_frame, timeout)) - if new_type == None: + res = do_transform(object_stamped, self.lookup_transform_full( + target_frame, target_time, + object_stamped.header.frame_id, object_stamped.header.stamp, + fixed_frame, timeout)) + if new_type is None: return res return convert(res, new_type) @@ -175,7 +182,7 @@ def lookup_transform_full( :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to (0 will get the latest). :param source_frame: Name of the input frame. - :param source_time: The time at which source_frame will be evaluated (0 will get the latest). + :param source_time: The time at which source_frame will be evaluated (0 gets the latest). :param fixed_frame: Name of the frame to consider constant in time. :param timeout: Time to wait for the target frame to become available. :return: The transform between the frames. @@ -221,7 +228,7 @@ def can_transform_full( :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to (0 will get the latest). :param source_frame: Name of the input frame. - :param source_time: The time at which source_frame will be evaluated (0 will get the latest). + :param source_time: The time at which source_frame will be evaluated (0 gets the latest). :param fixed_frame: Name of the frame to consider constant in time. :param timeout: Time to wait for the target frame to become available. :return: True if the transform is possible, false otherwise. @@ -238,21 +245,26 @@ def Stamped( return obj - class TypeException(Exception): """ + The TypeException class. + Raised when an unexpected type is received while registering a transform in :class:`tf2_ros.buffer_interface.BufferInterface`. """ + def __init__(self, errstr: str) -> None: self.errstr = errstr class NotImplementedException(Exception): """ + The NotImplementedException class. + Raised when can_transform or lookup_transform is not implemented in a subclass of :class:`tf2_ros.buffer_interface.BufferInterface`. """ + def __init__(self) -> None: self.errstr = 'CanTransform or LookupTransform not implemented' @@ -274,7 +286,7 @@ def get( self, key: TransformableObjectType ) -> Callable[[TransformableObject, TransformStamped], TransformableObject]: - if not key in TransformRegistration.__type_map: + if key not in TransformRegistration.__type_map: raise TypeException('Type %s is not loaded or supported' % str(key)) else: return TransformRegistration.__type_map[key] @@ -310,7 +322,7 @@ def get_from_msg( self, key: TransformableObjectType ) -> Callable[[MsgStamped], TransformableObject]: - if not key in ConvertRegistration.__from_msg_map: + if key not in ConvertRegistration.__from_msg_map: raise TypeException('Type %s is not loaded or supported' % str(key)) else: return ConvertRegistration.__from_msg_map[key] @@ -319,7 +331,7 @@ def get_to_msg( self, key: TransformableObjectType ) -> Callable[[TransformableObject], MsgStamped]: - if not key in ConvertRegistration.__to_msg_map: + if key not in ConvertRegistration.__to_msg_map: raise TypeException('Type %s is not loaded or supported' % str(key)) else: return ConvertRegistration.__to_msg_map[key] @@ -328,20 +340,20 @@ def get_convert( self, key: Tuple[TransformableObjectType, TransformableObjectType] ) -> Callable[[Any], TransformableObject]: - if not key in ConvertRegistration.__convert_map: - raise TypeException("Type %s is not loaded or supported" % str(key)) + if key not in ConvertRegistration.__convert_map: + raise TypeException('Type %s is not loaded or supported' % str(key)) else: return ConvertRegistration.__convert_map[key] def convert(a: TransformableObject, b_type: TransformableObjectType) -> TransformableObject: c = ConvertRegistration() - #check if an efficient conversion function between the types exists + # check if an efficient conversion function between the types exists try: f = c.get_convert((type(a), b_type)) return f(a) except TypeException: - if type(a) == b_type: + if isinstance(a, b_type): return deepcopy(a) f_to = c.get_to_msg(type(a)) diff --git a/tf2_ros_py/tf2_ros/static_transform_broadcaster.py b/tf2_ros_py/tf2_ros/static_transform_broadcaster.py index c05a73e22..ae32834a2 100644 --- a/tf2_ros_py/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros_py/tf2_ros/static_transform_broadcaster.py @@ -1,54 +1,53 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. +# 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: +# 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. # -# * 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. +# * Neither the name of the copyright holder 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 +# 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 typing import List from typing import Optional from typing import Union -from typing import List +from geometry_msgs.msg import TransformStamped from rclpy.node import Node from rclpy.qos import DurabilityPolicy from rclpy.qos import HistoryPolicy from rclpy.qos import QoSProfile from tf2_msgs.msg import TFMessage -from geometry_msgs.msg import TransformStamped class StaticTransformBroadcaster: """ - :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic. + :class:`StaticTransformBroadcaster` sends static transforms. + + This is a convenient way to send static transforms on the `/tf_static` message topic. """ def __init__(self, node: Node, qos: Optional[Union[QoSProfile, int]] = None) -> None: """ - Constructor. + Construct the StaticTransformBroadcaster. :param node: The ROS2 node. :param qos: A QoSProfile or a history depth to apply to the publisher. @@ -59,7 +58,7 @@ def __init__(self, node: Node, qos: Optional[Union[QoSProfile, int]] = None) -> durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, ) - self.pub_tf = node.create_publisher(TFMessage, "/tf_static", qos) + self.pub_tf = node.create_publisher(TFMessage, '/tf_static', qos) self.net_message = TFMessage() self._child_frame_ids = set() diff --git a/tf2_ros_py/tf2_ros/static_transform_listener.py b/tf2_ros_py/tf2_ros/static_transform_listener.py index 07cee7eaa..93ebc16bc 100644 --- a/tf2_ros_py/tf2_ros/static_transform_listener.py +++ b/tf2_ros_py/tf2_ros/static_transform_listener.py @@ -1,21 +1,23 @@ -# Copyright 2024, Open Source Robotics Foundation, Inc. All rights reserved. +# Copyright (c) 2024 Open Source Robotics Foundation, 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. +# * 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 copyright holder 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 @@ -24,27 +26,25 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from threading import Thread from typing import Optional from typing import Union -from rclpy.node import Node from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node from rclpy.qos import DurabilityPolicy from rclpy.qos import HistoryPolicy from rclpy.qos import QoSProfile -from tf2_ros.buffer import Buffer from tf2_msgs.msg import TFMessage -from threading import Thread +from tf2_ros.buffer import Buffer DEFAULT_TF_TOPIC = '/tf' DEFAULT_STATIC_TF_TOPIC = '/tf_static' class StaticTransformListener: - """ - :class:`StaticTransformListener` is a convenient way to establish a TransformListener on only static topics. - """ + """:class:`StaticTransformListener` receives transforms for static topics.""" def __init__( self, @@ -56,7 +56,7 @@ def __init__( tf_static_topic: str = DEFAULT_STATIC_TF_TOPIC ) -> None: """ - Constructor. + Construct a StaticTransformListener. :param buffer: The buffer to propagate changes to when tf info updates. :param node: The ROS 2 node. @@ -77,7 +77,8 @@ def __init__( self.group = ReentrantCallbackGroup() self.tf_static_sub = node.create_subscription( - TFMessage, tf_static_topic, self.static_callback, static_qos, callback_group=self.group) + TFMessage, tf_static_topic, self.static_callback, static_qos, + callback_group=self.group) if spin_thread: self.executor = SingleThreadedExecutor() @@ -98,9 +99,7 @@ def __del__(self) -> None: self.unregister() def unregister(self) -> None: - """ - Unregisters all tf_static subscribers. - """ + """Unregisters all tf_static subscribers.""" self.node.destroy_subscription(self.tf_static_sub) def static_callback(self, data: TFMessage) -> None: diff --git a/tf2_ros_py/tf2_ros/transform_broadcaster.py b/tf2_ros_py/tf2_ros/transform_broadcaster.py index c7e19f5e4..dbfd790de 100644 --- a/tf2_ros_py/tf2_ros/transform_broadcaster.py +++ b/tf2_ros_py/tf2_ros/transform_broadcaster.py @@ -1,71 +1,69 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. +# 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: +# 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. # -# * 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. +# * Neither the name of the copyright holder 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 +# 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 typing import List from typing import Optional from typing import Union -from typing import List +from geometry_msgs.msg import TransformStamped from rclpy.node import Node from rclpy.qos import QoSProfile from tf2_msgs.msg import TFMessage -from geometry_msgs.msg import TransformStamped class TransformBroadcaster: """ - :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. + :class:`TransformBroadcaster` sends transforms. + + This is a convenient way to send transform updates on the `/tf` message topic. """ + def __init__( self, node: Node, qos: Optional[Union[QoSProfile, int]] = None ) -> None: """ - .. function:: __init__(node, qos=None) - - Constructor. + Construct the TransformBroadcaster. - :param node: The ROS2 node. - :param qos: A QoSProfile or a history depth to apply to the publisher. + :param node: The ROS 2 node. + :param qos: A QoSProfile or a history depth to apply to the publisher. """ if qos is None: qos = QoSProfile(depth=100) - self.pub_tf = node.create_publisher(TFMessage, "/tf", qos) + self.pub_tf = node.create_publisher(TFMessage, '/tf', qos) def sendTransform( self, transform: Union[TransformStamped, List[TransformStamped]] ) -> None: """ - Send a transform, or a list of transforms, to the Buffer associated with this TransformBroadcaster. + Send a transform, or a list of transforms, to the associated Buffer. :param transform: A transform or list of transforms to send. """ diff --git a/tf2_ros_py/tf2_ros/transform_listener.py b/tf2_ros_py/tf2_ros/transform_listener.py index 57163a65f..1806ec94e 100644 --- a/tf2_ros_py/tf2_ros/transform_listener.py +++ b/tf2_ros_py/tf2_ros/transform_listener.py @@ -1,22 +1,23 @@ -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. +# 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. +# * 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 copyright holder 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 @@ -26,26 +27,30 @@ # POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen + +from threading import Thread + from typing import Optional from typing import Union -from rclpy.node import Node from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node from rclpy.qos import DurabilityPolicy from rclpy.qos import HistoryPolicy from rclpy.qos import QoSProfile -import tf2_ros -from tf2_ros.buffer import Buffer from tf2_msgs.msg import TFMessage -from threading import Thread +from tf2_ros.buffer import Buffer DEFAULT_TF_TOPIC = '/tf' DEFAULT_STATIC_TF_TOPIC = '/tf_static' + class TransformListener: """ - :class:`TransformListener` is a convenient way to listen for coordinate frame transformation info. + :class:`TransformListener` receives transforms. + + It is a convenient way to listen for coordinate frame transformation info. This class takes an object that instantiates the :class:`BufferInterface` interface, to which it propagates changes to the tf frame graph. It listens to both static and dynamic transforms. @@ -64,7 +69,7 @@ def __init__( static_only: bool = False ) -> None: """ - Constructor. + Construct the TransformListener. :param buffer: The buffer to propagate changes to when tf info updates. :param node: The ROS2 node. @@ -99,7 +104,8 @@ def __init__( TFMessage, tf_topic, self.callback, qos, callback_group=self.group) self.tf_static_sub = node.create_subscription( - TFMessage, tf_static_topic, self.static_callback, static_qos, callback_group=self.group) + TFMessage, tf_static_topic, self.static_callback, static_qos, + callback_group=self.group) if spin_thread: self.executor = SingleThreadedExecutor() @@ -120,9 +126,7 @@ def __del__(self) -> None: self.unregister() def unregister(self) -> None: - """ - Unregisters all tf subscribers. - """ + """Unregisters all tf subscribers.""" self.node.destroy_subscription(self.tf_sub) self.node.destroy_subscription(self.tf_static_sub)