diff --git a/examples/teleop_ros2/AGENTS.md b/examples/teleop_ros2/AGENTS.md index 8954adfa2..a25a5032a 100644 --- a/examples/teleop_ros2/AGENTS.md +++ b/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. diff --git a/examples/teleop_ros2/CMakeLists.txt b/examples/teleop_ros2/CMakeLists.txt index 29dc30328..cdab1df63 100644 --- a/examples/teleop_ros2/CMakeLists.txt +++ b/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 +) diff --git a/examples/teleop_ros2/Dockerfile b/examples/teleop_ros2/Dockerfile index 47921599f..3384863f9 100644 --- a/examples/teleop_ros2/Dockerfile +++ b/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 <` (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 @@ -137,8 +134,8 @@ The `mode` parameter selects the teleoperation scenario and which topics are pub | 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 diff --git a/examples/teleop_ros2/python/constants.py b/examples/teleop_ros2/python/constants.py index 668b59559..22556d5a6 100644 --- a/examples/teleop_ros2/python/constants.py +++ b/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") diff --git a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py index 8f93cf250..e2ba06526 100755 --- a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py +++ b/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), diff --git a/examples/teleop_ros2/python/messages.py b/examples/teleop_ros2/python/messages.py index 60037cfd1..d52b68b4b 100644 --- a/examples/teleop_ros2/python/messages.py +++ b/examples/teleop_ros2/python/messages.py @@ -8,9 +8,10 @@ from typing import Dict, Sequence import numpy as np -from geometry_msgs.msg import Pose, PoseArray, PoseStamped +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 NamedPoseArray from isaacteleop.retargeting_engine.interface import OptionalTensorGroup from isaacteleop.retargeting_engine.tensor_types.indices import ( @@ -21,9 +22,8 @@ HeadPoseIndex, ) -from constants import BODY_JOINT_NAMES +from constants import BODY_JOINT_NAMES, HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES from geometry import ( - append_hand_poses, apply_manus_controller_to_hand_pose, apply_transform_to_pose, to_pose, @@ -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,51 +281,40 @@ 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, -) -> PoseArray: - """Build a PoseArray with finger joint poses, left hand then right hand.""" - msg = PoseArray() +) -> NamedPoseArray: + """Build fixed left/right hand joint entries with invalid placeholders.""" + msg = NamedPoseArray() msg.header.stamp = now msg.header.frame_id = frame_id - if not left_hand.is_none: - left_positions = np.asarray(left_hand[HandInputIndex.JOINT_POSITIONS]) - left_orientations = np.asarray(left_hand[HandInputIndex.JOINT_ORIENTATIONS]) - left_valid = np.asarray(left_hand[HandInputIndex.JOINT_VALID]) - append_hand_poses( - msg.poses, - left_positions, - left_orientations, - left_valid, - transform_rot, - transform_trans, - ) - else: - for _ in range(HandJointIndex.WRIST, HandJointIndex.LITTLE_TIP + 1): - msg.poses.append(to_pose([0.0, 0.0, 0.0])) - - if not right_hand.is_none: - right_positions = np.asarray(right_hand[HandInputIndex.JOINT_POSITIONS]) - right_orientations = np.asarray(right_hand[HandInputIndex.JOINT_ORIENTATIONS]) - right_valid = np.asarray(right_hand[HandInputIndex.JOINT_VALID]) - append_hand_poses( - msg.poses, - right_positions, - right_orientations, - right_valid, - transform_rot, - transform_trans, - ) - else: - for _ in range(HandJointIndex.WRIST, HandJointIndex.LITTLE_TIP + 1): - msg.poses.append(to_pose([0.0, 0.0, 0.0])) - + 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]) + 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 diff --git a/examples/teleop_ros2/python/teleop_ros2_node.py b/examples/teleop_ros2/python/teleop_ros2_node.py index 928943f20..52aea971f 100755 --- a/examples/teleop_ros2/python/teleop_ros2_node.py +++ b/examples/teleop_ros2/python/teleop_ros2_node.py @@ -10,21 +10,21 @@ The `mode` parameter selects the teleoperation scenario and which topics are published: - - controller_teleop (default): ee_pose_left/ee_pose_right (from controller - aim pose), root_twist, root_pose, finger_joints + - controller_teleop (default): ee_pose (from controller aim poses), + root_twist, root_pose, finger_joints (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 - (finger 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 + - hand_teleop: ee_pose (from hand tracking wrists), 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 (PoseArray): [finger_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/hand (NamedPoseArray): named left and right hand joint poses + - xr_teleop/ee_pose (NamedPoseArray): named left and right EE poses - xr_teleop/root_twist (TwistStamped): root velocity command - xr_teleop/root_pose (PoseStamped): root pose command (height only) - xr_teleop/head_pose (PoseStamped): head pose @@ -47,7 +47,6 @@ import rclpy from geometry_msgs.msg import ( Pose, - PoseArray, PoseStamped, TransformStamped, TwistStamped, @@ -55,6 +54,7 @@ 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_ros import TransformBroadcaster from isaacteleop.cloudxr import CloudXRLauncher @@ -62,11 +62,11 @@ from geometry import make_transform from messages import ( build_controller_payload, - build_ee_msg_from_controller, - build_ee_msg_from_hand, + build_ee_msg_from_controllers, + build_ee_msg_from_hands, build_finger_joints_msg, build_full_body_payload, - build_hand_msg_from_hands, + build_hand_msg, build_head_msg, ) from node_parameters import ( @@ -91,13 +91,14 @@ def __init__(self) -> None: def _build_wrist_tfs( self, - left_ee_msg: PoseStamped | None, - right_ee_msg: PoseStamped | None, - *, - now, + ee_msg: NamedPoseArray, ) -> List[TransformStamped]: - """Build wrist TF transforms from side-specific EE pose messages.""" + """Build wrist TF transforms from valid EE pose entries.""" tfs = [] + wrist_frames = { + "left": self._params.left_wrist_frame, + "right": self._params.right_wrist_frame, + } def _get_orientation(pose: Pose) -> List[float]: return [ @@ -107,24 +108,14 @@ def _get_orientation(pose: Pose) -> List[float]: pose.orientation.w, ] - if left_ee_msg is not None: - pose = left_ee_msg.pose + for side, pose, is_valid in zip(ee_msg.name, ee_msg.pose, ee_msg.is_valid): + if not is_valid: + continue tfs.append( make_transform( - now, - self._params.world_frame, - self._params.left_wrist_frame, - [pose.position.x, pose.position.y, pose.position.z], - _get_orientation(pose), - ) - ) - if right_ee_msg is not None: - pose = right_ee_msg.pose - tfs.append( - make_transform( - now, - self._params.world_frame, - self._params.right_wrist_frame, + ee_msg.header.stamp, + ee_msg.header.frame_id, + wrist_frames[side], [pose.position.x, pose.position.y, pose.position.z], _get_orientation(pose), ) @@ -132,12 +123,9 @@ def _get_orientation(pose: Pose) -> List[float]: return tfs def _create_publishers(self) -> None: - self._pub_hand = self.create_publisher(PoseArray, "xr_teleop/hand", 10) - self._pub_ee_pose_left = self.create_publisher( - PoseStamped, "xr_teleop/ee_pose_left", 10 - ) - self._pub_ee_pose_right = self.create_publisher( - PoseStamped, "xr_teleop/ee_pose_right", 10 + self._pub_hand = self.create_publisher(NamedPoseArray, "xr_teleop/hand", 10) + self._pub_ee_pose = self.create_publisher( + NamedPoseArray, "xr_teleop/ee_pose", 10 ) self._pub_root_twist = self.create_publisher( TwistStamped, "xr_teleop/root_twist", 10 @@ -156,49 +144,22 @@ def _create_publishers(self) -> None: ) self._pub_head = self.create_publisher(PoseStamped, "xr_teleop/head_pose", 10) - def _publish_controller_outputs(self, result: dict, now) -> None: + def _publish_ee_pose_from_controllers(self, result: dict, now) -> None: left_ctrl = result["controller_left"] right_ctrl = result["controller_right"] - left_ee_msg = build_ee_msg_from_controller( + ee_pose_msg = build_ee_msg_from_controllers( left_ctrl, - "left", - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - self._params.controller_uses_hands_source, - ) - right_ee_msg = build_ee_msg_from_controller( right_ctrl, - "right", now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, self._params.controller_uses_hands_source, ) - if left_ee_msg is not None: - self._pub_ee_pose_left.publish(left_ee_msg) - if right_ee_msg is not None: - self._pub_ee_pose_right.publish(right_ee_msg) - wrist_tfs = self._build_wrist_tfs( - left_ee_msg, - right_ee_msg, - now=now, - ) + self._pub_ee_pose.publish(ee_pose_msg) + wrist_tfs = self._build_wrist_tfs(ee_pose_msg) if wrist_tfs: self._tf_broadcaster.sendTransform(wrist_tfs) - if self._params.controller_uses_hands_source: - hand_msg = build_hand_msg_from_hands( - result["hand_left"], - result["hand_right"], - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - ) - if hand_msg.poses: - self._pub_hand.publish(hand_msg) def _publish_controller_payload(self, result: dict) -> None: if self._params.mode not in ( @@ -246,43 +207,30 @@ def _publish_full_body_payload(self, result: dict) -> None: body_msg.data = tuple(bytes([a]) for a in payload) self._pub_full_body.publish(body_msg) - def _publish_hand_tracking_outputs(self, result: dict, now) -> None: - left_hand = result["hand_left"] - right_hand = result["hand_right"] - hand_msg = build_hand_msg_from_hands( - left_hand, - right_hand, + def _publish_hand_poses(self, result: dict, now) -> None: + hand_msg = build_hand_msg( + result["hand_left"], + result["hand_right"], now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, ) - if hand_msg.poses: - self._pub_hand.publish(hand_msg) + self._pub_hand.publish(hand_msg) - left_ee_msg = build_ee_msg_from_hand( + def _publish_ee_pose_from_hands(self, result: dict, now) -> None: + left_hand = result["hand_left"] + right_hand = result["hand_right"] + ee_pose_msg = build_ee_msg_from_hands( left_hand, - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - ) - right_ee_msg = build_ee_msg_from_hand( right_hand, now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, ) - if left_ee_msg is not None: - self._pub_ee_pose_left.publish(left_ee_msg) - if right_ee_msg is not None: - self._pub_ee_pose_right.publish(right_ee_msg) - wrist_tfs = self._build_wrist_tfs( - left_ee_msg, - right_ee_msg, - now=now, - ) + self._pub_ee_pose.publish(ee_pose_msg) + wrist_tfs = self._build_wrist_tfs(ee_pose_msg) if wrist_tfs: self._tf_broadcaster.sendTransform(wrist_tfs) @@ -373,9 +321,14 @@ def _run_session_loop(self, launcher: CloudXRLauncher | None = None) -> int: now = self.get_clock().now().to_msg() if self._params.mode == "hand_teleop": - self._publish_hand_tracking_outputs(result, now) + self._publish_ee_pose_from_hands(result, now) elif self._params.mode == "controller_teleop": - self._publish_controller_outputs(result, now) + self._publish_ee_pose_from_controllers(result, now) + if ( + self._params.mode == "hand_teleop" + or self._params.controller_uses_hands_source + ): + self._publish_hand_poses(result, now) self._publish_root_command(result, now) self._publish_finger_joints(result, now) diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt b/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt new file mode 100644 index 000000000..365ac95ac --- /dev/null +++ b/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.8) +project(teleop_ros2_interfaces) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/NamedPoseArray.msg" + DEPENDENCIES + geometry_msgs + std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_auto_package() diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/msg/NamedPoseArray.msg b/examples/teleop_ros2/teleop_ros2_interfaces/msg/NamedPoseArray.msg new file mode 100644 index 000000000..609966abd --- /dev/null +++ b/examples/teleop_ros2/teleop_ros2_interfaces/msg/NamedPoseArray.msg @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. +# SPDX-License-Identifier: Apache-2.0 + +# Named poses in the same frame. +std_msgs/Header header +string[] name +geometry_msgs/Pose[] pose +bool[] is_valid diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/package.xml b/examples/teleop_ros2/teleop_ros2_interfaces/package.xml new file mode 100644 index 000000000..64b5b2f31 --- /dev/null +++ b/examples/teleop_ros2/teleop_ros2_interfaces/package.xml @@ -0,0 +1,22 @@ + + + + teleop_ros2_interfaces + 0.1.0 + Custom ROS 2 interfaces for Isaac Teleop. + Sergey Grizan + Apache-2.0 + + ament_cmake_auto + + geometry_msgs + rosidl_default_generators + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages +