From 247fd08ac99ed043036067ce681adaa7e238dd10 Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Thu, 7 Nov 2024 08:30:17 -0500 Subject: [PATCH] Adding StaticTransformListener in Python (#719) Signed-off-by: CursedRock17 --- .../test/test_listener_and_broadcaster.py | 57 +++++---- .../tf2_ros/static_transform_listener.py | 109 ++++++++++++++++++ tf2_ros_py/tf2_ros/transform_listener.py | 25 ++-- 3 files changed, 159 insertions(+), 32 deletions(-) create mode 100644 tf2_ros_py/tf2_ros/static_transform_listener.py diff --git a/tf2_ros_py/test/test_listener_and_broadcaster.py b/tf2_ros_py/test/test_listener_and_broadcaster.py index fa0b6c0f9..60c31546e 100644 --- a/tf2_ros_py/test/test_listener_and_broadcaster.py +++ b/tf2_ros_py/test/test_listener_and_broadcaster.py @@ -36,6 +36,7 @@ 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 @@ -67,6 +68,8 @@ def setup_class(cls): cls.static_broadcaster = StaticTransformBroadcaster(cls.node) cls.listener = TransformListener( buffer=cls.buffer, node=cls.node, spin_thread=False) + cls.static_listener = StaticTransformListener( + buffer=cls.buffer, node=cls.node, spin_thread=False) cls.executor = rclpy.executors.SingleThreadedExecutor() cls.executor.add_node(cls.node) @@ -80,6 +83,7 @@ def teardown_class(cls): def setup_method(self, method): self.buffer = Buffer() self.listener.buffer = self.buffer + self.static_listener.buffer = self.buffer def broadcast_transform(self, target_frame, source_frame, time_stamp): broadcast_transform = build_transform( @@ -101,6 +105,36 @@ def broadcast_static_transform(self, target_frame, source_frame, time_stamp): return broadcast_transform + def test_extrapolation_exception(self): + self.broadcast_transform( + target_frame='foo', source_frame='bar', + time_stamp=rclpy.time.Time(seconds=0.3, nanoseconds=0).to_msg()) + + self.broadcast_transform( + target_frame='foo', source_frame='bar', + time_stamp=rclpy.time.Time(seconds=0.2, nanoseconds=0).to_msg()) + + with pytest.raises(ExtrapolationException) as excinfo: + self.buffer.lookup_transform( + target_frame='foo', source_frame='bar', + time=rclpy.time.Time(seconds=0.1, nanoseconds=0).to_msg()) + + assert 'Lookup would require extrapolation into the past' in str(excinfo.value) + + with pytest.raises(ExtrapolationException) as excinfo: + self.buffer.lookup_transform( + target_frame='foo', source_frame='bar', + time=rclpy.time.Time(seconds=0.4, nanoseconds=0).to_msg()) + + assert 'Lookup would require extrapolation into the future' in str(excinfo.value) + + def static_transfrom_listener_rclpy_node(self): + node = rclpy.create_node('test_broadcaster_listener') + buffer = Buffer() + + tfl = StaticTransformListener(buffer=buffer, node=node, spin_thread=False) + assert tfl == self.static_listener + def test_broadcaster_and_listener(self): time_stamp = rclpy.time.Time(seconds=1, nanoseconds=0).to_msg() @@ -125,29 +159,6 @@ def test_broadcaster_and_listener(self): assert broadcasted_transform == listened_transform_async - def test_extrapolation_exception(self): - self.broadcast_transform( - target_frame='foo', source_frame='bar', - time_stamp=rclpy.time.Time(seconds=0.3, nanoseconds=0).to_msg()) - - self.broadcast_transform( - target_frame='foo', source_frame='bar', - time_stamp=rclpy.time.Time(seconds=0.2, nanoseconds=0).to_msg()) - - with pytest.raises(ExtrapolationException) as excinfo: - self.buffer.lookup_transform( - target_frame='foo', source_frame='bar', - time=rclpy.time.Time(seconds=0.1, nanoseconds=0).to_msg()) - - assert 'Lookup would require extrapolation into the past' in str(excinfo.value) - - with pytest.raises(ExtrapolationException) as excinfo: - self.buffer.lookup_transform( - target_frame='foo', source_frame='bar', - time=rclpy.time.Time(seconds=0.4, nanoseconds=0).to_msg()) - - assert 'Lookup would require extrapolation into the future' in str(excinfo.value) - def test_static_broadcaster_and_listener(self): broadcasted_transform = self.broadcast_static_transform( target_frame='foo', source_frame='bar', diff --git a/tf2_ros_py/tf2_ros/static_transform_listener.py b/tf2_ros_py/tf2_ros/static_transform_listener.py new file mode 100644 index 000000000..07cee7eaa --- /dev/null +++ b/tf2_ros_py/tf2_ros/static_transform_listener.py @@ -0,0 +1,109 @@ +# Copyright 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. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from 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.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 + +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. + """ + + def __init__( + self, + buffer: Buffer, + node: Node, + *, + spin_thread: bool = False, + static_qos: Optional[Union[QoSProfile, int]] = None, + tf_static_topic: str = DEFAULT_STATIC_TF_TOPIC + ) -> None: + """ + Constructor. + + :param buffer: The buffer to propagate changes to when tf info updates. + :param node: The ROS 2 node. + :param spin_thread: Whether to create a dedidcated thread to spin this node. + :param static_qos: A QoSProfile or a history depth to apply to tf_static subscribers. + :param tf_static_topic: Which topic to listen to for static transforms. + """ + if static_qos is None: + static_qos = QoSProfile( + depth=100, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + ) + self.buffer = buffer + self.node = node + # Default callback group is mutually exclusive, which would prevent waiting for transforms + # from another callback in the same group. + self.group = ReentrantCallbackGroup() + + self.tf_static_sub = node.create_subscription( + TFMessage, tf_static_topic, self.static_callback, static_qos, callback_group=self.group) + + if spin_thread: + self.executor = SingleThreadedExecutor() + + def run_func(): + self.executor.add_node(self.node) + self.executor.spin() + self.executor.remove_node(self.node) + + self.dedicated_listener_thread = Thread(target=run_func) + self.dedicated_listener_thread.start() + + def __del__(self) -> None: + if hasattr(self, 'dedicated_listener_thread') and hasattr(self, 'executor'): + self.executor.shutdown() + self.dedicated_listener_thread.join() + + self.unregister() + + def unregister(self) -> None: + """ + Unregisters all tf_static subscribers. + """ + self.node.destroy_subscription(self.tf_static_sub) + + def static_callback(self, data: TFMessage) -> None: + who = 'default_authority' + for transform in data.transforms: + self.buffer.set_transform_static(transform, who) diff --git a/tf2_ros_py/tf2_ros/transform_listener.py b/tf2_ros_py/tf2_ros/transform_listener.py index 81a3545f5..57163a65f 100644 --- a/tf2_ros_py/tf2_ros/transform_listener.py +++ b/tf2_ros_py/tf2_ros/transform_listener.py @@ -50,6 +50,7 @@ class TransformListener: it propagates changes to the tf frame graph. It listens to both static and dynamic transforms. """ + def __init__( self, buffer: Buffer, @@ -59,7 +60,8 @@ def __init__( qos: Optional[Union[QoSProfile, int]] = None, static_qos: Optional[Union[QoSProfile, int]] = None, tf_topic: str = DEFAULT_TF_TOPIC, - tf_static_topic: str = DEFAULT_STATIC_TF_TOPIC + tf_static_topic: str = DEFAULT_STATIC_TF_TOPIC, + static_only: bool = False ) -> None: """ Constructor. @@ -71,13 +73,8 @@ def __init__( :param static_qos: A QoSProfile or a history depth to apply to tf_static subscribers. :param tf_topic: Which topic to listen to for dynamic transforms. :param tf_static_topic: Which topic to listen to for static transforms. + :param static_only: A bool which allows the listener to be strictly static. """ - if qos is None: - qos = QoSProfile( - depth=100, - durability=DurabilityPolicy.VOLATILE, - history=HistoryPolicy.KEEP_LAST, - ) if static_qos is None: static_qos = QoSProfile( depth=100, @@ -89,8 +86,18 @@ def __init__( # Default callback group is mutually exclusive, which would prevent waiting for transforms # from another callback in the same group. self.group = ReentrantCallbackGroup() - self.tf_sub = node.create_subscription( - TFMessage, tf_topic, self.callback, qos, callback_group=self.group) + + # Turn the class into a StaticTransformListener by enabling static_only. + if static_only is False: + if qos is None: + qos = QoSProfile( + depth=100, + durability=DurabilityPolicy.VOLATILE, + history=HistoryPolicy.KEEP_LAST, + ) + self.tf_sub = node.create_subscription( + 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)