Adding StaticTransformListener in Python (#719)

Signed-off-by: CursedRock17 <[email protected]>

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',

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)

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)