diff --git a/src/imetro_behavior/imetro_behavior/executor.py b/src/imetro_behavior/imetro_behavior/executor.py index b3fa9b1..1976573 100644 --- a/src/imetro_behavior/imetro_behavior/executor.py +++ b/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.""" diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index 0be730c..49b08a6 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/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. diff --git a/src/imetro_behavior/tests/test_executor.py b/src/imetro_behavior/tests/test_executor.py index 8333051..7577f6e 100644 --- a/src/imetro_behavior/tests/test_executor.py +++ b/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: diff --git a/src/imetro_behavior/tests/test_geometry_behaviors.py b/src/imetro_behavior/tests/test_geometry_behaviors.py index b267b41..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, @@ -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()