From 8763b35e7cf056193f5f860aa4fc1906a576b94a Mon Sep 17 00:00:00 2001 From: Zarrin Bashir Date: Wed, 16 Sep 2026 13:50:05 -0500 Subject: [PATCH 1/7] Adding a tag_found bool variable in the april tag behavior to use in the find world behavior. Also added a publish static transform behavior. Will edit tests once I get the world behavior working with these. --- .../imetro_behavior/apriltag_behaviors.py | 2 ++ .../imetro_behavior/geometry_behaviors.py | 16 ++++++++++++++++ 2 files changed, 18 insertions(+) diff --git a/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py b/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py index 5ee5941..c379a45 100644 --- a/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py @@ -42,6 +42,7 @@ class DetectAprilTag(BehaviourWithPorts): OUTPUT_PORTS = { "tag_pose": PortInformation(data_type=PoseStamped, required=True), + "tag_found": PortInformation(data_type=bool, required=True), } node: Node @@ -113,6 +114,7 @@ def update(self) -> Status: self.node.get_logger().info(f"Apriltag detection total process took {total_time:.4f} seconds!") self._set_output("tag_pose", pose_msg) + self._set_output("tag_found", True) return Status.SUCCESS def shutdown(self) -> None: diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index 0be730c..a318491 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -38,6 +38,7 @@ from scipy.spatial.transform import Rotation as R from std_msgs.msg import Header from tf2_ros import TransformBroadcaster +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster from imetro_behavior.helpers import set_ros_node @@ -668,7 +669,22 @@ def update(self) -> Status: self.tf_broadcaster.sendTransform(transform_stamped) 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.tf_static_broadcaster = StaticTransformBroadcaster(self.node) + + 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. From 90a01dfb1bc6b9c96c56ec33102782a320ca8239 Mon Sep 17 00:00:00 2001 From: Zarrin Bashir Date: Fri, 18 Sep 2026 14:40:13 -0500 Subject: [PATCH 2/7] Reverting the changes to april tag behavior. I was originally planning to use the tag_found bool for the SuccessIfVariableTrue decorator for PB find world tree. But I ended going a different route, and didn't end up using it. The tag_pose output is sufficient. --- src/imetro_behavior/imetro_behavior/apriltag_behaviors.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py b/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py index c379a45..1816170 100644 --- a/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py @@ -42,7 +42,6 @@ class DetectAprilTag(BehaviourWithPorts): OUTPUT_PORTS = { "tag_pose": PortInformation(data_type=PoseStamped, required=True), - "tag_found": PortInformation(data_type=bool, required=True), } node: Node @@ -114,7 +113,6 @@ def update(self) -> Status: self.node.get_logger().info(f"Apriltag detection total process took {total_time:.4f} seconds!") self._set_output("tag_pose", pose_msg) - self._set_output("tag_found", True) return Status.SUCCESS def shutdown(self) -> None: @@ -135,4 +133,4 @@ def shutdown(self) -> None: if destroy is not None: destroy.restype = None destroy(tf) - det.tag_families = {} + det.tag_families = {} \ No newline at end of file From e9f6f8f685d5138615ed4ee12ac3c0ae63679c70 Mon Sep 17 00:00:00 2001 From: Zarrin Bashir Date: Fri, 18 Sep 2026 15:26:32 -0500 Subject: [PATCH 3/7] Addressing PR comment for PublishStaticTransform. Using the shared static transform broadcaster from the blackboard instead of creating a new one. --- .../imetro_behavior/apriltag_behaviors.py | 2 +- src/imetro_behavior/imetro_behavior/executor.py | 3 +++ .../imetro_behavior/geometry_behaviors.py | 10 +++++++--- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py b/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py index 1816170..5ee5941 100644 --- a/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/apriltag_behaviors.py @@ -133,4 +133,4 @@ def shutdown(self) -> None: if destroy is not None: destroy.restype = None destroy(tf) - det.tag_families = {} \ No newline at end of file + det.tag_families = {} diff --git a/src/imetro_behavior/imetro_behavior/executor.py b/src/imetro_behavior/imetro_behavior/executor.py index b3fa9b1..e2fcd4b 100644 --- a/src/imetro_behavior/imetro_behavior/executor.py +++ b/src/imetro_behavior/imetro_behavior/executor.py @@ -34,6 +34,7 @@ from rclpy.duration import Duration from rclpy.node import Node from tf2_ros import Buffer, TransformListener +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster from imetro_behavior_msgs.action import ExecuteBehavior @@ -69,6 +70,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 +83,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.""" diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index a318491..49b08a6 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -38,7 +38,6 @@ from scipy.spatial.transform import Rotation as R from std_msgs.msg import Header from tf2_ros import TransformBroadcaster -from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster from imetro_behavior.helpers import set_ros_node @@ -669,22 +668,27 @@ def update(self) -> Status: self.tf_broadcaster.sendTransform(transform_stamped) 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.tf_static_broadcaster = StaticTransformBroadcaster(self.node) + 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. From e32e6e313d0cbd32ad97dda600176733a269e998 Mon Sep 17 00:00:00 2001 From: Zarrin Bashir Date: Fri, 18 Sep 2026 15:41:00 -0500 Subject: [PATCH 4/7] Updates to the tests accordingly to what I added in the executor + ggeometry_behaviors for publish static transform. --- src/imetro_behavior/tests/test_executor.py | 1 + .../tests/test_geometry_behaviors.py | 25 +++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/src/imetro_behavior/tests/test_executor.py b/src/imetro_behavior/tests/test_executor.py index e0b2703..ff32b02 100644 --- a/src/imetro_behavior/tests/test_executor.py +++ b/src/imetro_behavior/tests/test_executor.py @@ -166,6 +166,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: diff --git a/src/imetro_behavior/tests/test_geometry_behaviors.py b/src/imetro_behavior/tests/test_geometry_behaviors.py index 83ed916..c0cb2a8 100644 --- a/src/imetro_behavior/tests/test_geometry_behaviors.py +++ b/src/imetro_behavior/tests/test_geometry_behaviors.py @@ -29,6 +29,7 @@ LookupTransform, OffsetPoseStamped, PoseStampedToTransformStamped, + PublishStaticTransform, PublishTransform, TransformPose, TransformStampedToPoseStamped, @@ -40,6 +41,7 @@ from rclpy.node import Node from scipy.spatial.transform import Rotation as R from tf2_ros import Buffer +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster @pytest.fixture() @@ -57,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 @@ -395,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() From 5622ebe5ed2e7de66ed317f13af4d68103a69cdf Mon Sep 17 00:00:00 2001 From: Zarrin Bashir Date: Fri, 18 Sep 2026 15:47:43 -0500 Subject: [PATCH 5/7] Importing line tweak. --- src/imetro_behavior/imetro_behavior/executor.py | 3 +-- src/imetro_behavior/tests/test_geometry_behaviors.py | 4 +--- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/imetro_behavior/imetro_behavior/executor.py b/src/imetro_behavior/imetro_behavior/executor.py index e2fcd4b..07b6d5d 100644 --- a/src/imetro_behavior/imetro_behavior/executor.py +++ b/src/imetro_behavior/imetro_behavior/executor.py @@ -33,8 +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.static_transform_broadcaster import StaticTransformBroadcaster +from tf2_ros import Buffer, TransformListener, StaticTransformBroadcaster from imetro_behavior_msgs.action import ExecuteBehavior diff --git a/src/imetro_behavior/tests/test_geometry_behaviors.py b/src/imetro_behavior/tests/test_geometry_behaviors.py index c0cb2a8..7de5b94 100644 --- a/src/imetro_behavior/tests/test_geometry_behaviors.py +++ b/src/imetro_behavior/tests/test_geometry_behaviors.py @@ -40,9 +40,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.static_transform_broadcaster import StaticTransformBroadcaster - +from tf2_ros import Buffer, StaticTransformBroadcaster @pytest.fixture() def tf_buffer() -> Buffer: From 93d100d85b261ae619bd16dc1468beaeb5ad4466 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 18 Sep 2026 17:30:26 -0400 Subject: [PATCH 6/7] fix missing import --- src/imetro_behavior/tests/test_geometry_behaviors.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/imetro_behavior/tests/test_geometry_behaviors.py b/src/imetro_behavior/tests/test_geometry_behaviors.py index 78129f2..a34c49b 100644 --- a/src/imetro_behavior/tests/test_geometry_behaviors.py +++ b/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, From c3ff42c6872f3fe0e7ea3633d9c1b031d5c93e05 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 18 Sep 2026 17:38:48 -0400 Subject: [PATCH 7/7] whitespace fixes --- src/imetro_behavior/imetro_behavior/geometry_behaviors.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index a318491..4288401 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py @@ -669,6 +669,7 @@ def update(self) -> Status: self.tf_broadcaster.sendTransform(transform_stamped) return Status.SUCCESS + class PublishStaticTransform(BehaviourWithPorts): """Publish a permanent static transform stamped message to /tf_static.""" @@ -684,7 +685,8 @@ 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.