Adding Static Transform Broadcaster Behavior

src/imetro_behavior/imetro_behavior/executor.py

@@ -33,7 +33,7 @@
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.duration import Duration
 from rclpy.node import Node
-from tf2_ros import Buffer, TransformListener
+from tf2_ros import Buffer, StaticTransformBroadcaster, TransformListener
 
 from imetro_behavior_msgs.action import ExecuteBehavior
 
@@ -69,6 +69,7 @@ def __init__(self, node: Node, config: dict | Path | str | None = None):
         # Create a global TF buffer to share across behaviors.
         self._tf_buffer = Buffer()
         self._tf_listener = TransformListener(self._tf_buffer, node)
+        self._tf_static_broadcaster = StaticTransformBroadcaster(node)
         self._init_global_blackboard()
 
         # Initialize state.
@@ -81,6 +82,7 @@ def _init_global_blackboard(self):
         """Initializes the blackboard and places shared resources on it as necessary."""
         Blackboard.clear()
         Blackboard.set("/ros/tf_buffer", self._tf_buffer)
+        Blackboard.set("/ros/tf_static_broadcaster", self._tf_static_broadcaster)
 
     def _load_config(self, config: dict | Path | str):
         """Loads parameters, imports, and tree search paths from a dictionary or YAML configuration file."""

src/imetro_behavior/imetro_behavior/geometry_behaviors.py

@@ -669,6 +669,26 @@ def update(self) -> Status:
         return Status.SUCCESS
 
 
+class PublishStaticTransform(BehaviourWithPorts):
+    """Publish a permanent static transform stamped message to /tf_static."""
+
+    INPUT_PORTS = {"transform_stamped": PortInformation(data_type=TransformStamped, required=True)}
+
+    OUTPUT_PORTS = {}
+
+    node: Node
+
+    def setup(self, **kwargs):
+        set_ros_node(self, **kwargs)
+        self.blackboard_client.register_key(key="/ros/tf_static_broadcaster", access=Access.READ)
+        self.tf_static_broadcaster = self.blackboard_client.get("/ros/tf_static_broadcaster")
+
+    def update(self) -> Status:
+        transform_stamped = self.get_input("transform_stamped")
+        self.tf_static_broadcaster.sendTransform(transform_stamped)
+        return Status.SUCCESS
+
+
 class PublishTwist(BehaviourWithPorts):
     """
     Takes in `linear` and `angular` velocities and converts them geometry_msgs/msg/Twist.

src/imetro_behavior/tests/test_executor.py

@@ -167,6 +167,7 @@ def test_run_tree_resets_blackboard(executor: BehaviorTreeExecutor, tmp_path: Pa
     executor.run_tree(tree_path)
     assert "/leftover_data" not in Blackboard.storage
     assert Blackboard.get("/ros/tf_buffer") is executor._tf_buffer
+    assert Blackboard.get("/ros/tf_static_broadcaster") is executor._tf_static_broadcaster
 
 
 def test_load_tree_from_search_path(ros_node: Node) -> None:

src/imetro_behavior/tests/test_geometry_behaviors.py

@@ -24,7 +24,7 @@
 from py_trees.common import Status
 from rclpy.node import Node
 from scipy.spatial.transform import Rotation as R
-from tf2_ros import Buffer
+from tf2_ros import Buffer, StaticTransformBroadcaster
 
 from imetro_behavior.geometry_behaviors import (
     AlignPoseToNearestAxis,
@@ -35,6 +35,7 @@
     LookupTransform,
     OffsetPoseStamped,
     PoseStampedToTransformStamped,
+    PublishStaticTransform,
     PublishTransform,
     TransformPose,
     TransformStampedToPoseStamped,
@@ -58,6 +59,14 @@ def tf_buffer() -> Buffer:
     return buffer
 
 
+@pytest.fixture()
+def tf_static_broadcaster(ros_node: Node) -> StaticTransformBroadcaster:
+    """A static transform broadcaster set on the blackboard."""
+    broadcaster = StaticTransformBroadcaster(ros_node)
+    Blackboard.set("/ros/tf_static_broadcaster", broadcaster)
+    return broadcaster
+
+
 def make_pose(position_xyz: list[float], orientation_xyzw: list[float], frame_id: str = "") -> PoseStamped:
     pose = PoseStamped()
     pose.header.frame_id = frame_id
@@ -396,6 +405,21 @@ def test_publish_transform(ros_node: Node) -> None:
     assert behavior.status == Status.SUCCESS
 
 
+def test_publish_static_transform(ros_node: Node, tf_static_broadcaster: StaticTransformBroadcaster) -> None:
+    behavior = PublishStaticTransform(name="publish_static_transform")
+    behavior.setup_ports()
+    behavior.setup(node=ros_node)
+
+    tform = TransformStamped()
+    tform.header.frame_id = "map"
+    tform.child_frame_id = "static_object"
+    tform.transform.rotation.w = 1.0
+    Blackboard.set(behavior._get_blackboard_key("transform_stamped"), tform)
+
+    behavior.tick_once()
+    assert behavior.status == Status.SUCCESS
+
+
 def test_transform_stamped_to_pose_stamped() -> None:
     behavior = TransformStampedToPoseStamped(name="tf_to_pose")
     behavior.setup_ports()