diff --git a/pyproject.toml b/pyproject.toml index c7b19df..36ce37c 100644 --- a/pyproject.toml +++ b/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" diff --git a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py b/src/imetro_behavior/imetro_behavior/geometry_behaviors.py index 49b08a6..1790804 100644 --- a/src/imetro_behavior/imetro_behavior/geometry_behaviors.py +++ b/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 diff --git a/src/imetro_behavior/tests/test_geometry_behaviors.py b/src/imetro_behavior/tests/test_geometry_behaviors.py index a34c49b..392d26e 100644 --- a/src/imetro_behavior/tests/test_geometry_behaviors.py +++ b/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()