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 <[email protected]>

Author: clalancette

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)