Fix republishing behavior in PublishStaticTransform

pyproject.toml

@@ -19,6 +19,7 @@ scripts = ["scripts/entrypoint.sh"]
 COLCON_DEFAULT_OUTPUT_STYLE = "catkin_tools"
 COLCON_DEFAULTS_FILE = "${PIXI_PROJECT_ROOT}/config/colcon-defaults.yaml"
 RCUTILS_COLORIZED_OUTPUT = "1"
+PYTHONNOUSERSITE = "1"  # Never pick up packages from the user site-packages
 
 [tool.pixi.tasks]
 build = "colcon build --cmake-args"

src/imetro_behavior/imetro_behavior/geometry_behaviors.py

@@ -685,7 +685,17 @@ def setup(self, **kwargs):
 
     def update(self) -> Status:
         transform_stamped = self.get_input("transform_stamped")
-        self.tf_static_broadcaster.sendTransform(transform_stamped)
+        # The Python StaticTransformBroadcaster silently ignores transforms whose child frame was already
+        # sent, so replace any existing entry in its accumulated message ourselves before publishing.
+        transforms = self.tf_static_broadcaster.net_message.transforms
+        for i, existing in enumerate(transforms):
+            if existing.child_frame_id == transform_stamped.child_frame_id:
+                transforms[i] = transform_stamped
+                break
+        else:
+            transforms.append(transform_stamped)
+            self.tf_static_broadcaster._child_frame_ids.add(transform_stamped.child_frame_id)
+        self.tf_static_broadcaster.pub_tf.publish(self.tf_static_broadcaster.net_message)
         return Status.SUCCESS
 
 

src/imetro_behavior/tests/test_geometry_behaviors.py

@@ -420,6 +420,36 @@ def test_publish_static_transform(ros_node: Node, tf_static_broadcaster: StaticT
     assert behavior.status == Status.SUCCESS
 
 
+def test_publish_static_transform_updates_existing_frame(
+    ros_node: Node, tf_static_broadcaster: StaticTransformBroadcaster
+) -> None:
+    behavior = PublishStaticTransform(name="publish_static_transform")
+    behavior.setup_ports()
+    behavior.setup(node=ros_node)
+
+    other = TransformStamped()
+    other.header.frame_id = "map"
+    other.child_frame_id = "other_object"
+    other.transform.rotation.w = 1.0
+    tf_static_broadcaster.sendTransform(other)
+
+    for x in [1.0, 2.0]:
+        tform = TransformStamped()
+        tform.header.frame_id = "map"
+        tform.child_frame_id = "static_object"
+        tform.transform.translation.x = x
+        tform.transform.rotation.w = 1.0
+        Blackboard.set(behavior._get_blackboard_key("transform_stamped"), tform)
+
+        behavior.tick_once()
+        assert behavior.status == Status.SUCCESS
+
+    transforms = {t.child_frame_id: t for t in tf_static_broadcaster.net_message.transforms}
+    assert len(tf_static_broadcaster.net_message.transforms) == 2
+    assert transforms["static_object"].transform.translation.x == 2.0
+    assert "other_object" in transforms
+
+
 def test_transform_stamped_to_pose_stamped() -> None:
     behavior = TransformStampedToPoseStamped(name="tf_to_pose")
     behavior.setup_ports()