diff --git a/examples/teleop_ros2/python/isaaccapture_examples/teleop_ros2/messages.py b/examples/teleop_ros2/python/isaaccapture_examples/teleop_ros2/messages.py index 58935bd8d3..ef33dca8e0 100644 --- a/examples/teleop_ros2/python/isaaccapture_examples/teleop_ros2/messages.py +++ b/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 diff --git a/examples/teleop_ros2/python/isaaccapture_examples/teleop_ros2/tensor_group_helpers.py b/examples/teleop_ros2/python/isaaccapture_examples/teleop_ros2/tensor_group_helpers.py index 9fb25ea46c..607a699093 100644 --- a/examples/teleop_ros2/python/isaaccapture_examples/teleop_ros2/tensor_group_helpers.py +++ b/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] diff --git a/tests/python/examples/teleop_ros2/test_messages.py b/tests/python/examples/teleop_ros2/test_messages.py index fff0557e1d..51d231c3b8 100644 --- a/tests/python/examples/teleop_ros2/test_messages.py +++ b/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 ):