ROS: Standardize hand and EE pose topics with NamedPoseArray

examples/teleop_ros2/AGENTS.md

@@ -12,7 +12,7 @@ SPDX-License-Identifier: Apache-2.0
 - The image build disables some CI gates (`-DENABLE_CLANG_FORMAT_CHECK=OFF`, `-DBUILD_TESTING=OFF`), so a green Docker build does not mean C++ formatting or ctest pass. Validate those separately.
 - When creating temporary Docker images for `examples/teleop_ros2` validation, remove them before finishing the task unless the user explicitly asks to keep them.
 
-## Python Node Layout
+## Source Layout
 
-- In `python/teleop_ros2_node.py`, preserve the existing grouped/sorted organization for global non-member helpers and `TeleopRos2Node` member functions: scan the surrounding order before inserting, and do not place helpers near call sites when the existing section is sorted.
+- In source code files under this example, preserve the existing grouped/sorted organization for helpers, message builders, classes, and member functions: scan the surrounding order before inserting, and do not place helpers near call sites when the existing section is sorted.
 - In Python integration test verifier code, do not use bare `assert` for runtime validation; Python optimization can disable it, so raise explicit exceptions from validators.

examples/teleop_ros2/CMakeLists.txt

@@ -18,3 +18,7 @@ install(FILES README.md Dockerfile
 install(DIRECTORY configs assets
     DESTINATION examples/teleop_ros2
 )
+
+install(DIRECTORY teleop_ros2_interfaces
+    DESTINATION examples/teleop_ros2
+)

examples/teleop_ros2/Dockerfile

@@ -46,6 +46,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     libxxf86vm-dev \
     swig \
     ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
+    ros-${ROS_DISTRO}-rosidl-default-runtime \
     && rm -rf /var/lib/apt/lists/*
 
 # Install uv via standalone script (avoids PEP 668 "externally managed" pip on Ubuntu 24.04 / Jazzy)
@@ -113,6 +114,9 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     git \
     patchelf \
     pkg-config \
+    python3-colcon-common-extensions \
+    ros-${ROS_DISTRO}-ament-cmake-auto \
+    ros-${ROS_DISTRO}-rosidl-default-generators \
     && rm -rf /var/lib/apt/lists/*
 
 WORKDIR /opt/isaacteleop
@@ -124,7 +128,8 @@ COPY . /opt/isaacteleop
 # produces; a stale build/ would skip a clean CMake reconfigure. The build tree
 # is rebuilt from scratch every time -- ccache keeps recompiles cheap -- so each
 # build reconfigures against the current image rather than trusting cached
-# configure results.
+# configure results. Ament's Python tools come from apt, so the interface
+# package build below pins the ROS distro's system Python instead of uv's.
 RUN --mount=type=cache,target=/ccache,id=isaacteleop-teleop-ros2-ccache-${ROS_DISTRO}-${TARGETARCH} \
     rm -rf /opt/isaacteleop/install /opt/isaacteleop/build \
     && cmake -B build -S . \
@@ -143,10 +148,22 @@ RUN --mount=type=cache,target=/ccache,id=isaacteleop-teleop-ros2-ccache-${ROS_DI
         -DISAAC_TELEOP_PYTHON_VERSION=${PYTHON_VERSION} \
     && cmake --build build --config Release -j$(nproc) \
     && cmake --install build --config Release \
+    && bash -lc "source /opt/ros/${ROS_DISTRO}/setup.bash \
+        && colcon build \
+            --base-paths examples/teleop_ros2/teleop_ros2_interfaces \
+            --build-base build/teleop_ros2_interfaces \
+            --install-base /opt/isaacteleop/install \
+            --merge-install \
+            --cmake-args \
+                -DCMAKE_BUILD_TYPE=Release \
+                -DPython3_EXECUTABLE=/usr/bin/python3" \
     && ccache --show-stats
 
 COPY --chmod=644 <<EOF /usr/local/bin/teleop-env-setup
 source /opt/ros/${ROS_DISTRO}/setup.bash
+if [[ -f /opt/isaacteleop/install/local_setup.bash ]]; then
+    source /opt/isaacteleop/install/local_setup.bash
+fi
 EOF
 
 COPY --chmod=755 <<'EOF' /usr/local/bin/teleop-entrypoint

examples/teleop_ros2/README.md

@@ -58,16 +58,14 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime.
 
 ## Published Topics
 
-- `xr_teleop/hand` (`geometry_msgs/PoseArray`)
-  - `poses`: Hand joint poses (all joints except palm, left then right); published by `hand_teleop` and by `controller_teleop` when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`
-- `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`)
-  - Head pose; published in `controller_teleop` and `hand_teleop` modes when head tracking is valid
+  - Head pose
 - `xr_teleop/controller_data` (`std_msgs/ByteMultiArray`, msgpack-encoded dictionary)
 - `xr_teleop/finger_joints` (`sensor_msgs/JointState`)
   - Retargeted finger joint angles for the robot; contains joint names and position arrays corresponding to the robot finger joints (TriHand in default `controller_teleop`, selected Sharpa retargeter in `hand_teleop`, or explicit Sharpa retargeter in `controller_teleop`)
@@ -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` (finger joints in pose space), `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 geometry_msgs/msg/PoseArray
-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/constants.py

@@ -6,7 +6,10 @@
 
 from enum import Enum
 
-from isaacteleop.retargeting_engine.tensor_types.indices import BodyJointPicoIndex
+from isaacteleop.retargeting_engine.tensor_types.indices import (
+    BodyJointPicoIndex,
+    HandJointIndex,
+)
 
 try:
     from enum import StrEnum
@@ -25,6 +28,11 @@ class HandRetargeter(StrEnum):
 
 
 BODY_JOINT_NAMES = [e.name for e in BodyJointPicoIndex]
+HAND_POSE_JOINT_INDICES = tuple(
+    HandJointIndex(i)
+    for i in range(HandJointIndex.WRIST, HandJointIndex.LITTLE_TIP + 1)
+)
+HAND_POSE_NAMES = [joint.name for joint in HAND_POSE_JOINT_INDICES]
 HAND_RETARGETERS = tuple(retargeter.value for retargeter in HandRetargeter)
 SHARPA_HAND_RETARGETERS = (HandRetargeter.PINK_IK, HandRetargeter.DEXPILOT)
 TELEOP_MODES = ("controller_teleop", "hand_teleop", "controller_raw", "full_body")

examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py

@@ -15,15 +15,24 @@
 
 import msgpack
 import rclpy
-from geometry_msgs.msg import PoseArray, PoseStamped, TwistStamped
+from geometry_msgs.msg import PoseStamped, TwistStamped
+from isaacteleop.retargeting_engine.tensor_types.indices import HandJointIndex
 from rclpy.node import Node
 from sensor_msgs.msg import JointState
 from std_msgs.msg import ByteMultiArray
+from teleop_ros2_interfaces.msg import NamedPoseArray
 from tf2_msgs.msg import TFMessage
 
 
 _MODES = ("controller_teleop", "hand_teleop", "controller_raw", "full_body")
 _EXPECTED_TF_FRAMES = {"right_wrist", "left_wrist", "head"}
+_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:
@@ -46,13 +55,18 @@ def _unpack_msgpack(msg: ByteMultiArray) -> dict:
     )
 
 
-def _assert_pose_array(msg: PoseArray, *, expected_count: int) -> 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}")
-    if len(msg.poses) != expected_count:
-        raise ValueError(f"expected {expected_count} poses, got {len(msg.poses)}")
-    positions = []
-    for pose in msg.poses:
+    names = list(msg.name)
+    if names != expected_names:
+        raise ValueError("named pose entries do not match the expected order")
+    if len(set(names)) != len(names):
+        raise ValueError("named pose entries are not unique")
+    if len(msg.pose) != len(names) or len(msg.is_valid) != len(names):
+        raise ValueError("named pose arrays differ in length")
+
+    for pose in msg.pose:
         position = (pose.position.x, pose.position.y, pose.position.z)
         orientation = (
             pose.orientation.x,
@@ -61,12 +75,32 @@ def _assert_pose_array(msg: PoseArray, *, expected_count: int) -> None:
             pose.orientation.w,
         )
         if not _is_finite_sequence(position):
-            raise ValueError("pose position contains non-finite values")
+            raise ValueError("named pose position contains non-finite values")
         if not _is_finite_sequence(orientation):
-            raise ValueError("pose orientation contains non-finite values")
-        positions.extend(position)
-    if not any(abs(value) > 1e-6 for value in positions):
-        raise ValueError("pose array 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(
@@ -229,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),
@@ -265,24 +289,14 @@ def _expected_subscriptions(
                 (
                     "hand",
                     "xr_teleop/hand",
-                    PoseArray,
-                    lambda msg: _assert_pose_array(msg, expected_count=50),
-                ),
-                (
-                    "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),