From 67c3166cc5b0a236e0c0f88ec7ed6897c786257c Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Wed, 21 Oct 2026 18:30:36 +0000 Subject: [PATCH] fix(ros2): reject untracked head-relative targets CloudXR can leave a disconnected headset pose valid while clearing its tracked state. Require active head tracking before publishing head-relative end-effector targets so stale poses cannot be used as robot commands. Signed-off-by: Sergey Grizan --- .../teleop_ros2/messages.py | 7 +++++- .../teleop_ros2/tensor_group_helpers.py | 4 ++++ .../examples/teleop_ros2/test_messages.py | 22 +++++++++++++++++++ 3 files changed, 32 insertions(+), 1 deletion(-) 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 ):