From 97be01b505ce2c881a2db40674eaf7a3273d14b9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 24 Oct 2023 16:53:02 -0500 Subject: [PATCH] Make sure to cache transforms in tf2_ros_py. (#634) That way if you publish two separate transforms, they will *both* be published (not just the latest one). This is a lot more intuitive and matches the behavior of the C++ static transform broadcaster. Signed-off-by: Chris Lalancette --- tf2_ros_py/tf2_ros/static_transform_broadcaster.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/tf2_ros_py/tf2_ros/static_transform_broadcaster.py b/tf2_ros_py/tf2_ros/static_transform_broadcaster.py index ed179cddb..c05a73e22 100644 --- a/tf2_ros_py/tf2_ros/static_transform_broadcaster.py +++ b/tf2_ros_py/tf2_ros/static_transform_broadcaster.py @@ -61,10 +61,19 @@ def __init__(self, node: Node, qos: Optional[Union[QoSProfile, int]] = None) -> ) self.pub_tf = node.create_publisher(TFMessage, "/tf_static", qos) + self.net_message = TFMessage() + self._child_frame_ids = set() + def sendTransform(self, transform: Union[TransformStamped, List[TransformStamped]]) -> None: if not isinstance(transform, list): if hasattr(transform, '__iter__'): transform = list(transform) else: transform = [transform] - self.pub_tf.publish(TFMessage(transforms=transform)) + + for t_in in transform: + if t_in.child_frame_id not in self._child_frame_ids: + self._child_frame_ids.add(t_in.child_frame_id) + self.net_message.transforms.append(t_in) + + self.pub_tf.publish(self.net_message)