Consolidate left and right hand topics into a single topic

Author: sgrizan-nv

examples/teleop_ros2/README.md

@@ -44,8 +44,7 @@ parameter:
 In `controller_teleop`, explicitly setting `hand_retargeter:=dexpilot` or
 `hand_retargeter:=pink_ik` keeps XR controllers responsible for EE poses, wrist
 TFs, locomotion, and `controller_data`, while Manus/OpenXR hand data drives
-`xr_teleop/hand_left`, `xr_teleop/hand_right`, and Sharpa
-`xr_teleop/finger_joints`.
+`xr_teleop/hand` and Sharpa `xr_teleop/finger_joints`.
 
 The Docker build fetches the pinned official Sharpa Wave URDFs and installs them
 at `/opt/isaacteleop/install/examples/teleop_ros2/assets/urdf/sharpa_standalone/`.
@@ -59,10 +58,8 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime.
 
 ## Published Topics
 
-- `xr_teleop/hand_left` (`teleop_ros2_interfaces/msg/HandJointPoses`)
-  - Named left OpenXR hand joint poses; `name`, `pose`, and `is_valid` each have 25 index-aligned entries; includes `WRIST`, omits `PALM`
-- `xr_teleop/hand_right` (`teleop_ros2_interfaces/msg/HandJointPoses`)
-  - Named right OpenXR hand joint poses; `name`, `pose`, and `is_valid` each have 25 index-aligned entries; includes `WRIST`, omits `PALM`
+- `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`)
@@ -123,8 +120,7 @@ docker run --rm --gpus all --net=host --ipc=host \
   --name teleop_ros2_ref \
   teleop_ros2_ref --ros-args -p cloudxr_accept_eula:=true \
   -p world_frame:=odom -p rate_hz:=30.0 \
-  -r xr_teleop/hand_left:=my_robot/hand_left \
-  -r xr_teleop/hand_right:=my_robot/hand_right \
+  -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
 ```
@@ -133,16 +129,16 @@ Available parameters: `rate_hz`, `mode`, `hand_retargeter`, `config_asset_root`,
 
 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_left`, `xr_teleop/hand_right`, `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_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`.
 
 ### 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_left`/`hand_right` only for the explicit Sharpa retargeter path |
-| `hand_teleop` | `ee_pose_left`, `ee_pose_right` (from hand tracking wrist), `hand_left`/`hand_right` (named per-hand 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_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_raw` | `controller_data` only |
 | `full_body` | `full_body` and `controller_data` |
 
@@ -197,8 +193,7 @@ coverage.
 ```bash
 docker exec -it teleop_ros2_ref /bin/bash
 
-ros2 topic echo /xr_teleop/hand_left teleop_ros2_interfaces/msg/HandJointPoses
-ros2 topic echo /xr_teleop/hand_right teleop_ros2_interfaces/msg/HandJointPoses
+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/root_twist geometry_msgs/msg/TwistStamped

examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py

@@ -26,10 +26,13 @@
 
 _MODES = ("controller_teleop", "hand_teleop", "controller_raw", "full_body")
 _EXPECTED_TF_FRAMES = {"right_wrist", "left_wrist", "head"}
-_EXPECTED_HAND_POSE_NAMES = [
+_HAND_POSE_NAMES = [
     HandJointIndex(i).name
     for i in range(HandJointIndex.WRIST, HandJointIndex.LITTLE_TIP + 1)
 ]
+_EXPECTED_HAND_POSE_NAMES = [
+    f"{side}_{name}" for side in ("left", "right") for name in _HAND_POSE_NAMES
+]
 
 
 def _is_finite_sequence(values) -> bool:
@@ -277,14 +280,8 @@ def _expected_subscriptions(
         if mode == "hand_teleop":
             return [
                 (
-                    "hand_left",
-                    "xr_teleop/hand_left",
-                    HandJointPoses,
-                    _assert_hand_joint_poses,
-                ),
-                (
-                    "hand_right",
-                    "xr_teleop/hand_right",
+                    "hand",
+                    "xr_teleop/hand",
                     HandJointPoses,
                     _assert_hand_joint_poses,
                 ),

examples/teleop_ros2/python/messages.py

@@ -214,35 +214,39 @@ def build_full_body_payload(full_body: OptionalTensorGroup) -> Dict:
     }
 
 
-def build_hand_msg_from_hand(
-    hand: OptionalTensorGroup,
+def build_hand_msg_from_hands(
+    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 one hand."""
-    if hand.is_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()
     msg.header.stamp = now
     msg.header.frame_id = frame_id
 
-    positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS])
-    orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS])
-    joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID])
-    for joint_idx, joint_name in zip(HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES):
-        joint_is_valid = bool(joint_valid[joint_idx])
-        if joint_is_valid:
-            pose = to_pose(positions[joint_idx], orientations[joint_idx])
-            if transform_rot is not None or transform_trans is not None:
-                pose = apply_transform_to_pose(pose, transform_rot, transform_trans)
-        else:
-            pose = to_pose([0.0, 0.0, 0.0])
-        msg.name.append(joint_name)
-        msg.pose.append(pose)
-        msg.is_valid.append(joint_is_valid)
+    for side, hand in (("left", left_hand), ("right", right_hand)):
+        if hand.is_none:
+            continue
+        positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS])
+        orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS])
+        joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID])
+        for joint_idx, joint_name in zip(HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES):
+            joint_is_valid = bool(joint_valid[joint_idx])
+            if joint_is_valid:
+                pose = to_pose(positions[joint_idx], orientations[joint_idx])
+                if transform_rot is not None or transform_trans is not None:
+                    pose = apply_transform_to_pose(pose, transform_rot, transform_trans)
+            else:
+                pose = to_pose([0.0, 0.0, 0.0])
+            msg.name.append(f"{side}_{joint_name}")
+            msg.pose.append(pose)
+            msg.is_valid.append(joint_is_valid)
     return msg
 
 

examples/teleop_ros2/python/teleop_ros2_node.py

@@ -15,16 +15,15 @@
                        (retargeted TriHand angles), controller_data, head_pose,
                        and TF transforms for left/right wrists and head
   - hand_teleop: ee_pose_left/ee_pose_right (from hand tracking wrist),
-                 hand_left/hand_right (named per-hand joint poses),
+                 hand (named left and right joint poses),
                  finger_joints (retargeted Sharpa joint angles),
                  root_twist/root_pose (from foot pedal locomotion), head_pose,
                  and TF transforms for left/right wrists and head
   - controller_raw: controller_data only
   - full_body: full_body and controller_data
 
 Topic names (remappable via ROS 2 remapping):
-  - xr_teleop/hand_left (HandJointPoses): named left hand joint poses
-  - xr_teleop/hand_right (HandJointPoses): named right hand joint poses
+  - xr_teleop/hand (HandJointPoses): named left and right hand joint poses
   - xr_teleop/ee_pose_left (PoseStamped): left hand/controller EE pose
   - xr_teleop/ee_pose_right (PoseStamped): right hand/controller EE pose
   - xr_teleop/root_twist (TwistStamped): root velocity command
@@ -68,7 +67,7 @@
     build_ee_msg_from_hand,
     build_finger_joints_msg,
     build_full_body_payload,
-    build_hand_msg_from_hand,
+    build_hand_msg_from_hands,
     build_head_msg,
 )
 from node_parameters import (
@@ -134,12 +133,7 @@ def _get_orientation(pose: Pose) -> List[float]:
         return tfs
 
     def _create_publishers(self) -> None:
-        self._pub_hand_left = self.create_publisher(
-            HandJointPoses, "xr_teleop/hand_left", 10
-        )
-        self._pub_hand_right = self.create_publisher(
-            HandJointPoses, "xr_teleop/hand_right", 10
-        )
+        self._pub_hand = self.create_publisher(HandJointPoses, "xr_teleop/hand", 10)
         self._pub_ee_pose_left = self.create_publisher(
             PoseStamped, "xr_teleop/ee_pose_left", 10
         )
@@ -196,24 +190,16 @@ def _publish_controller_outputs(self, result: dict, now) -> None:
         if wrist_tfs:
             self._tf_broadcaster.sendTransform(wrist_tfs)
         if self._params.controller_uses_hands_source:
-            left_hand_msg = build_hand_msg_from_hand(
+            hand_msg = build_hand_msg_from_hands(
                 result["hand_left"],
-                now,
-                self._params.world_frame,
-                self._params.transform_rotation,
-                self._params.transform_translation,
-            )
-            right_hand_msg = build_hand_msg_from_hand(
                 result["hand_right"],
                 now,
                 self._params.world_frame,
                 self._params.transform_rotation,
                 self._params.transform_translation,
             )
-            if left_hand_msg is not None:
-                self._pub_hand_left.publish(left_hand_msg)
-            if right_hand_msg is not None:
-                self._pub_hand_right.publish(right_hand_msg)
+            if hand_msg is not None:
+                self._pub_hand.publish(hand_msg)
 
     def _publish_controller_payload(self, result: dict) -> None:
         if self._params.mode not in (
@@ -264,24 +250,16 @@ def _publish_full_body_payload(self, result: dict) -> None:
     def _publish_hand_tracking_outputs(self, result: dict, now) -> None:
         left_hand = result["hand_left"]
         right_hand = result["hand_right"]
-        left_hand_msg = build_hand_msg_from_hand(
+        hand_msg = build_hand_msg_from_hands(
             left_hand,
-            now,
-            self._params.world_frame,
-            self._params.transform_rotation,
-            self._params.transform_translation,
-        )
-        right_hand_msg = build_hand_msg_from_hand(
             right_hand,
             now,
             self._params.world_frame,
             self._params.transform_rotation,
             self._params.transform_translation,
         )
-        if left_hand_msg is not None:
-            self._pub_hand_left.publish(left_hand_msg)
-        if right_hand_msg is not None:
-            self._pub_hand_right.publish(right_hand_msg)
+        if hand_msg is not None:
+            self._pub_hand.publish(hand_msg)
 
         left_ee_msg = build_ee_msg_from_hand(
             left_hand,