Use NamedPoseArray for hand and ee pose topics

Author: sgrizan-nv

examples/teleop_ros2/README.md

@@ -58,12 +58,10 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime.
 
 ## Published Topics
 
-- `xr_teleop/hand` (`teleop_ros2_interfaces/msg/HandJointPoses`)
-  - Named OpenXR hand joint poses; `name`, `pose`, and `is_valid` are index-aligned, names use `left_`/`right_` prefixes, and each available hand contributes 25 entries including `WRIST` and omitting `PALM`
-- `xr_teleop/ee_pose_left` (`geometry_msgs/PoseStamped`)
-  - Left hand/controller EE pose; published when the left EE source is active
-- `xr_teleop/ee_pose_right` (`geometry_msgs/PoseStamped`)
-  - Right hand/controller EE pose; published when the right EE source is active
+- `xr_teleop/hand` (`teleop_ros2_interfaces/msg/NamedPoseArray`)
+  - Named OpenXR hand joint poses; always contains 25 `left_` entries followed by 25 `right_` entries, including `WRIST` and omitting `PALM`; missing or invalid joints use zero poses with `is_valid=false`
+- `xr_teleop/ee_pose` (`teleop_ros2_interfaces/msg/NamedPoseArray`)
+  - Named hand/controller EE poses; always contains `left` and `right` entries, with zero poses and `is_valid=false` for invalid sides
 - `xr_teleop/root_twist` (`geometry_msgs/TwistStamped`)
 - `xr_teleop/root_pose` (`geometry_msgs/PoseStamped`)
 - `xr_teleop/head_pose` (`geometry_msgs/PoseStamped`)
@@ -121,24 +119,23 @@ docker run --rm --gpus all --net=host --ipc=host \
   teleop_ros2_ref --ros-args -p cloudxr_accept_eula:=true \
   -p world_frame:=odom -p rate_hz:=30.0 \
   -r xr_teleop/hand:=my_robot/hand \
-  -r xr_teleop/ee_pose_left:=my_robot/ee_pose_left \
-  -r xr_teleop/ee_pose_right:=my_robot/ee_pose_right
+  -r xr_teleop/ee_pose:=my_robot/ee_pose
 ```
 
 Available parameters: `rate_hz`, `mode`, `hand_retargeter`, `config_asset_root`, `cloudxr_install_dir`, `cloudxr_env_config`, `cloudxr_accept_eula`, `cloudxr_setup_oob`, `cloudxr_usb_local`, `pedal_collection_id`, `world_frame`, `right_wrist_frame`, `left_wrist_frame`, `head_frame`, `left_finger_joint_names`, `right_finger_joint_names`. Use `ros2 param list /teleop_ros2_node` and `ros2 param describe /teleop_ros2_node <param>` (with the node running) for the full set.
 
 By default, `left_finger_joint_names` and `right_finger_joint_names` use the selected mode's retargeter joint names. They can be overridden to publish robot-specific names on `xr_teleop/finger_joints`, but each override must provide the same number of names as the joints emitted by that mode's retargeter.
 
-Available topics for remapping: `xr_teleop/hand`, `xr_teleop/ee_pose_left`, `xr_teleop/ee_pose_right`, `xr_teleop/root_twist`, `xr_teleop/root_pose`, `xr_teleop/head_pose`, `xr_teleop/controller_data`, `xr_teleop/full_body`, `xr_teleop/finger_joints`. Active remaps can be inspected with `ros2 node info /teleop_ros2_node`.
+Available topics for remapping: `xr_teleop/hand`, `xr_teleop/ee_pose`, `xr_teleop/root_twist`, `xr_teleop/root_pose`, `xr_teleop/head_pose`, `xr_teleop/controller_data`, `xr_teleop/full_body`, `xr_teleop/finger_joints`. Active remaps can be inspected with `ros2 node info /teleop_ros2_node`.
 
 ### Mode
 
 The `mode` parameter selects the teleoperation scenario and which topics are published:
 
 | Mode | Topics published |
 |------|------------------|
-| `controller_teleop` (default) | `ee_pose_left`, `ee_pose_right` (from controller aim pose), `root_twist`, `root_pose`, `head_pose`, `finger_joints` (TriHand by default; Sharpa from Manus/OpenXR hands when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`), `controller_data`, `tf` (from controller aim pose and head pose), and `hand` only for the explicit Sharpa retargeter path |
-| `hand_teleop` | `ee_pose_left`, `ee_pose_right` (from hand tracking wrist), `hand` (named left and right joint poses), `finger_joints` (finger joints in joint space), `root_twist`, `root_pose`, `head_pose`, `tf` (from hand tracking wrist and head pose); locomotion comes from the configured foot pedal collection |
+| `controller_teleop` (default) | `ee_pose` (from controller aim poses), `root_twist`, `root_pose`, `head_pose`, `finger_joints` (TriHand by default; Sharpa from Manus/OpenXR hands when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`), `controller_data`, `tf` (from controller aim poses and head pose), and `hand` only for the explicit Sharpa retargeter path |
+| `hand_teleop` | `ee_pose` (from hand tracking wrists), `hand` (named left and right joint poses), `finger_joints` (finger joints in joint space), `root_twist`, `root_pose`, `head_pose`, `tf` (from hand tracking wrists and head pose); locomotion comes from the configured foot pedal collection |
 | `controller_raw` | `controller_data` only |
 | `full_body` | `full_body` and `controller_data` |
 
@@ -193,9 +190,8 @@ coverage.
 ```bash
 docker exec -it teleop_ros2_ref /bin/bash
 
-ros2 topic echo /xr_teleop/hand teleop_ros2_interfaces/msg/HandJointPoses
-ros2 topic echo /xr_teleop/ee_pose_left geometry_msgs/msg/PoseStamped
-ros2 topic echo /xr_teleop/ee_pose_right geometry_msgs/msg/PoseStamped
+ros2 topic echo /xr_teleop/hand teleop_ros2_interfaces/msg/NamedPoseArray
+ros2 topic echo /xr_teleop/ee_pose teleop_ros2_interfaces/msg/NamedPoseArray
 ros2 topic echo /xr_teleop/root_twist geometry_msgs/msg/TwistStamped
 ros2 topic echo /xr_teleop/root_pose geometry_msgs/msg/PoseStamped
 ros2 topic echo /xr_teleop/head_pose geometry_msgs/msg/PoseStamped

examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py

@@ -20,7 +20,7 @@
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
 from std_msgs.msg import ByteMultiArray
-from teleop_ros2_interfaces.msg import HandJointPoses
+from teleop_ros2_interfaces.msg import NamedPoseArray
 from tf2_msgs.msg import TFMessage
 
 
@@ -55,20 +55,17 @@ def _unpack_msgpack(msg: ByteMultiArray) -> dict:
     )
 
 
-def _assert_hand_joint_poses(msg: HandJointPoses) -> None:
+def _assert_named_pose_array(msg: NamedPoseArray, expected_names: list[str]) -> None:
     if msg.header.frame_id != "world":
         raise ValueError(f"unexpected frame_id {msg.header.frame_id!r}")
     names = list(msg.name)
-    if names != _EXPECTED_HAND_POSE_NAMES:
-        raise ValueError("hand joint pose names do not match the expected order")
+    if names != expected_names:
+        raise ValueError("named pose entries do not match the expected order")
     if len(set(names)) != len(names):
-        raise ValueError("hand joint pose names are not unique")
+        raise ValueError("named pose entries are not unique")
     if len(msg.pose) != len(names) or len(msg.is_valid) != len(names):
-        raise ValueError("hand joint pose arrays differ in length")
-    if not any(bool(is_valid) for is_valid in msg.is_valid):
-        raise ValueError("all hand joint poses are invalid")
+        raise ValueError("named pose arrays differ in length")
 
-    positions = []
     for pose in msg.pose:
         position = (pose.position.x, pose.position.y, pose.position.z)
         orientation = (
@@ -78,12 +75,32 @@ def _assert_hand_joint_poses(msg: HandJointPoses) -> None:
             pose.orientation.w,
         )
         if not _is_finite_sequence(position):
-            raise ValueError("hand joint pose position contains non-finite values")
+            raise ValueError("named pose position contains non-finite values")
         if not _is_finite_sequence(orientation):
-            raise ValueError("hand joint pose orientation contains non-finite values")
-        positions.extend(position)
-    if not any(abs(value) > 1e-6 for value in positions):
-        raise ValueError("hand joint pose positions are all zero")
+            raise ValueError("named pose orientation contains non-finite values")
+
+
+def _assert_nonzero_pose_positions(msg: NamedPoseArray, label: str) -> None:
+    if not any(
+        abs(value) > 1e-6
+        for pose in msg.pose
+        for value in (pose.position.x, pose.position.y, pose.position.z)
+    ):
+        raise ValueError(f"{label} positions are all zero")
+
+
+def _assert_ee_pose_array(msg: NamedPoseArray) -> None:
+    _assert_named_pose_array(msg, ["left", "right"])
+    if not all(bool(is_valid) for is_valid in msg.is_valid):
+        raise ValueError("EE pose array contains an invalid entry")
+    _assert_nonzero_pose_positions(msg, "EE pose")
+
+
+def _assert_hand_pose_array(msg: NamedPoseArray) -> None:
+    _assert_named_pose_array(msg, _EXPECTED_HAND_POSE_NAMES)
+    if not any(bool(is_valid) for is_valid in msg.is_valid):
+        raise ValueError("all hand joint poses are invalid")
+    _assert_nonzero_pose_positions(msg, "hand joint pose")
 
 
 def _assert_pose_stamped(
@@ -246,20 +263,10 @@ def _expected_subscriptions(
         if mode == "controller_teleop":
             return [
                 (
-                    "ee_pose_left",
-                    "xr_teleop/ee_pose_left",
-                    PoseStamped,
-                    lambda msg: _assert_pose_stamped(
-                        msg, require_nonzero_position=True
-                    ),
-                ),
-                (
-                    "ee_pose_right",
-                    "xr_teleop/ee_pose_right",
-                    PoseStamped,
-                    lambda msg: _assert_pose_stamped(
-                        msg, require_nonzero_position=True
-                    ),
+                    "ee_pose",
+                    "xr_teleop/ee_pose",
+                    NamedPoseArray,
+                    _assert_ee_pose_array,
                 ),
                 ("root_twist", "xr_teleop/root_twist", TwistStamped, _assert_twist),
                 ("root_pose", "xr_teleop/root_pose", PoseStamped, _assert_pose_stamped),
@@ -282,24 +289,14 @@ def _expected_subscriptions(
                 (
                     "hand",
                     "xr_teleop/hand",
-                    HandJointPoses,
-                    _assert_hand_joint_poses,
-                ),
-                (
-                    "ee_pose_left",
-                    "xr_teleop/ee_pose_left",
-                    PoseStamped,
-                    lambda msg: _assert_pose_stamped(
-                        msg, require_nonzero_position=True
-                    ),
+                    NamedPoseArray,
+                    _assert_hand_pose_array,
                 ),
                 (
-                    "ee_pose_right",
-                    "xr_teleop/ee_pose_right",
-                    PoseStamped,
-                    lambda msg: _assert_pose_stamped(
-                        msg, require_nonzero_position=True
-                    ),
+                    "ee_pose",
+                    "xr_teleop/ee_pose",
+                    NamedPoseArray,
+                    _assert_ee_pose_array,
                 ),
                 ("root_twist", "xr_teleop/root_twist", TwistStamped, _assert_twist),
                 ("root_pose", "xr_teleop/root_pose", PoseStamped, _assert_pose_stamped),

examples/teleop_ros2/python/messages.py

@@ -11,7 +11,7 @@
 from geometry_msgs.msg import Pose, PoseStamped
 from scipy.spatial.transform import Rotation
 from sensor_msgs.msg import JointState
-from teleop_ros2_interfaces.msg import HandJointPoses
+from teleop_ros2_interfaces.msg import NamedPoseArray
 
 from isaacteleop.retargeting_engine.interface import OptionalTensorGroup
 from isaacteleop.retargeting_engine.tensor_types.indices import (
@@ -36,6 +36,70 @@
 )
 
 
+def _compose_ee_msg(
+    left_pose: Pose | None,
+    right_pose: Pose | None,
+    now,
+    frame_id: str,
+) -> NamedPoseArray:
+    msg = NamedPoseArray()
+    msg.header.stamp = now
+    msg.header.frame_id = frame_id
+    for side, pose in (("left", left_pose), ("right", right_pose)):
+        # Set the pose and validity together so the parallel fields cannot diverge.
+        if pose is None:
+            pose = to_pose([0.0, 0.0, 0.0])
+            pose_is_valid = False
+        else:
+            pose_is_valid = True
+        msg.name.append(side)
+        msg.pose.append(pose)
+        msg.is_valid.append(pose_is_valid)
+    return msg
+
+
+def _compute_ee_pose_from_controller(
+    ctrl: OptionalTensorGroup,
+    side: str,
+    transform_rot: Rotation | None = None,
+    transform_trans: Sequence[float] | None = None,
+    controller_uses_hands_source: bool = False,
+) -> Pose | None:
+    if not controller_aim_is_valid(ctrl):
+        return None
+
+    pos = [float(x) for x in ctrl[ControllerInputIndex.AIM_POSITION]]
+    ori = [float(x) for x in ctrl[ControllerInputIndex.AIM_ORIENTATION]]
+    pose = to_pose(pos, ori)
+
+    if transform_rot is not None or transform_trans is not None:
+        pose = apply_transform_to_pose(pose, transform_rot, transform_trans)
+
+    if controller_uses_hands_source:
+        pose = apply_manus_controller_to_hand_pose(pose, side)
+
+    return pose
+
+
+def _compute_ee_pose_from_hand(
+    hand: OptionalTensorGroup,
+    transform_rot: Rotation | None = None,
+    transform_trans: Sequence[float] | None = None,
+) -> Pose | None:
+    if not hand_wrist_is_valid(hand):
+        return None
+
+    positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS])
+    orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS])
+    pose = to_pose(
+        positions[HandJointIndex.WRIST],
+        orientations[HandJointIndex.WRIST],
+    )
+    if transform_rot is not None or transform_trans is not None:
+        pose = apply_transform_to_pose(pose, transform_rot, transform_trans)
+    return pose
+
+
 def _to_pose_stamped(pose: Pose, now, frame_id: str) -> PoseStamped:
     msg = PoseStamped()
     msg.header.stamp = now
@@ -119,50 +183,53 @@ def _as_float(ctrl, index):
     }
 
 
-def build_ee_msg_from_controller(
-    ctrl: OptionalTensorGroup,
-    side: str,
+def build_ee_msg_from_controllers(
+    left_ctrl: OptionalTensorGroup,
+    right_ctrl: OptionalTensorGroup,
     now,
     frame_id: str,
     transform_rot: Rotation | None = None,
     transform_trans: Sequence[float] | None = None,
     controller_uses_hands_source: bool = False,
-) -> PoseStamped | None:
-    if not controller_aim_is_valid(ctrl):
-        return None
-
-    pos = [float(x) for x in ctrl[ControllerInputIndex.AIM_POSITION]]
-    ori = [float(x) for x in ctrl[ControllerInputIndex.AIM_ORIENTATION]]
-    pose = to_pose(pos, ori)
-
-    if transform_rot is not None or transform_trans is not None:
-        pose = apply_transform_to_pose(pose, transform_rot, transform_trans)
-
-    if controller_uses_hands_source:
-        pose = apply_manus_controller_to_hand_pose(pose, side)
-
-    return _to_pose_stamped(pose, now, frame_id)
+) -> NamedPoseArray:
+    """Build fixed left/right EE entries from controller aim poses."""
+    left_pose = _compute_ee_pose_from_controller(
+        left_ctrl,
+        "left",
+        transform_rot,
+        transform_trans,
+        controller_uses_hands_source,
+    )
+    right_pose = _compute_ee_pose_from_controller(
+        right_ctrl,
+        "right",
+        transform_rot,
+        transform_trans,
+        controller_uses_hands_source,
+    )
+    return _compose_ee_msg(left_pose, right_pose, now, frame_id)
 
 
-def build_ee_msg_from_hand(
-    hand: OptionalTensorGroup,
+def build_ee_msg_from_hands(
+    left_hand: OptionalTensorGroup,
+    right_hand: OptionalTensorGroup,
     now,
     frame_id: str,
     transform_rot: Rotation | None = None,
     transform_trans: Sequence[float] | None = None,
-) -> PoseStamped | None:
-    if not hand_wrist_is_valid(hand):
-        return None
-
-    positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS])
-    orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS])
-    pose = to_pose(
-        positions[HandJointIndex.WRIST],
-        orientations[HandJointIndex.WRIST],
+) -> NamedPoseArray:
+    """Build fixed left/right EE entries from hand wrist poses."""
+    left_pose = _compute_ee_pose_from_hand(
+        left_hand,
+        transform_rot,
+        transform_trans,
     )
-    if transform_rot is not None or transform_trans is not None:
-        pose = apply_transform_to_pose(pose, transform_rot, transform_trans)
-    return _to_pose_stamped(pose, now, frame_id)
+    right_pose = _compute_ee_pose_from_hand(
+        right_hand,
+        transform_rot,
+        transform_trans,
+    )
+    return _compose_ee_msg(left_pose, right_pose, now, frame_id)
 
 
 def build_finger_joints_msg(
@@ -214,24 +281,25 @@ def build_full_body_payload(full_body: OptionalTensorGroup) -> Dict:
     }
 
 
-def build_hand_msg_from_hands(
+def build_hand_msg(
     left_hand: OptionalTensorGroup,
     right_hand: OptionalTensorGroup,
     now,
     frame_id: str,
     transform_rot: Rotation | None = None,
     transform_trans: Sequence[float] | None = None,
-) -> HandJointPoses | None:
-    """Build named hand joint poses for available hands, left then right."""
-    if left_hand.is_none and right_hand.is_none:
-        return None
-
-    msg = HandJointPoses()
+) -> NamedPoseArray:
+    """Build fixed left/right hand joint entries with invalid placeholders."""
+    msg = NamedPoseArray()
     msg.header.stamp = now
     msg.header.frame_id = frame_id
 
     for side, hand in (("left", left_hand), ("right", right_hand)):
         if hand.is_none:
+            for joint_name in HAND_POSE_NAMES:
+                msg.name.append(f"{side}_{joint_name}")
+                msg.pose.append(to_pose([0.0, 0.0, 0.0]))
+                msg.is_valid.append(False)
             continue
         positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS])
         orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS])