fix(ros2): reject untracked head-relative targets

examples/teleop_ros2/python/isaaccapture_examples/teleop_ros2/messages.py

@@ -41,6 +41,7 @@
 from .tensor_group_helpers import (
     controller_aim_is_valid,
     hand_wrist_is_valid,
+    head_is_tracked,
     head_is_valid,
     joint_names_from_group_type,
 )
@@ -142,7 +143,11 @@ def _finalize_ee_output(
 ) -> tuple[NamedPoseArray, list[TransformStamped]]:
     if ee_poses_frame == EePoseFrame.HEAD:
         frame_id = head_frame
-        head_pose = _compute_head_pose(head, transform_rot, transform_trans)
+        head_pose = (
+            _compute_head_pose(head, transform_rot, transform_trans)
+            if head is not None and head_is_tracked(head)
+            else None
+        )
         if head_pose is None:
             # Tracking loss must not change coordinates or reparent wrist TFs.
             left_pose = right_pose = None

examples/teleop_ros2/python/isaaccapture_examples/teleop_ros2/tensor_group_helpers.py

@@ -34,5 +34,9 @@ def head_is_valid(head: OptionalTensorGroup) -> bool:
     return _flag_is_valid(head, HeadInputIndex.IS_VALID)
 
 
+def head_is_tracked(head: OptionalTensorGroup) -> bool:
+    return _flag_is_valid(head, HeadInputIndex.IS_TRACKED)
+
+
 def joint_names_from_group_type(group_type) -> list[str]:
     return [tensor_type.name for tensor_type in group_type.types]

tests/python/examples/teleop_ros2/test_messages.py

@@ -304,6 +304,28 @@ def test_build_ee_output_from_controllers_in_the_head_frame() -> None:
     ]
 
 
+def test_build_ee_output_from_controllers_rejects_untracked_head() -> None:
+    head = _active_head()
+    head[HeadInputIndex.IS_TRACKED] = False
+
+    msg, transforms = build_ee_output_from_controllers(
+        _active_controller(),
+        _active_controller(),
+        Time(),
+        "world",
+        "left_wrist",
+        "right_wrist",
+        head=head,
+        head_frame="headset",
+        ee_poses_frame=EePoseFrame.HEAD,
+    )
+
+    assert msg.header.frame_id == "headset"
+    assert list(msg.is_valid) == [False, False]
+    assert all(_position(pose) == [0.0, 0.0, 0.0] for pose in msg.pose)
+    assert transforms == []
+
+
 def test_build_ee_output_from_hands_in_head_frame_keeps_untracked_sides_invalid() -> (
     None
 ):