ROS: Swap left and right EE in EE topic (#385)

Author: sgrizan-nv

examples/teleop_ros2/README.md

@@ -28,8 +28,8 @@ NV_CXR_ENABLE_PUSH_DEVICES=0
 - `xr_teleop/hand` (`geometry_msgs/PoseArray`)
   - `poses`: Finger joint poses (all joints except palm/wrist, right then left)
 - `xr_teleop/ee_poses` (`geometry_msgs/PoseArray`)
-  - `poses[0]`: Right hand/controller EE pose (if active)
-  - `poses[1]`: Left hand/controller EE pose (if active)
+  - `poses[0]`: Left hand/controller EE pose (if active)
+  - `poses[1]`: Right hand/controller EE pose (if active)
 - `xr_teleop/root_twist` (`geometry_msgs/TwistStamped`)
 - `xr_teleop/root_pose` (`geometry_msgs/PoseStamped`)
 - `xr_teleop/controller_data` (`std_msgs/ByteMultiArray`, msgpack-encoded dictionary)

examples/teleop_ros2/python/teleop_ros2_node.py

@@ -20,7 +20,7 @@
 
 Topic names (remappable via ROS 2 remapping):
   - xr_teleop/hand (PoseArray): [finger_joint_poses...]
-  - xr_teleop/ee_poses (PoseArray): [right_ee, left_ee]
+  - xr_teleop/ee_poses (PoseArray): [left_ee, right_ee]
   - xr_teleop/root_twist (TwistStamped): root velocity command
   - xr_teleop/root_pose (PoseStamped): root pose command (height only)
   - xr_teleop/controller_data (ByteMultiArray): msgpack-encoded controller data
@@ -241,23 +241,23 @@ def _build_ee_msg_from_controllers(
     transform_rot: Rotation | None = None,
     transform_trans: Sequence[float] | None = None,
 ) -> PoseArray:
-    """Build a PoseArray with right then left controller aim poses (wrist proxy)."""
+    """Build a PoseArray with left then right controller aim poses (wrist proxy)."""
     msg = PoseArray()
     msg.header.stamp = now
     msg.header.frame_id = frame_id
-    if not right_ctrl.is_none:
-        pos = [float(x) for x in right_ctrl[ControllerInputIndex.AIM_POSITION]]
-        ori = [float(x) for x in right_ctrl[ControllerInputIndex.AIM_ORIENTATION]]
+    if not left_ctrl.is_none:
+        pos = [float(x) for x in left_ctrl[ControllerInputIndex.AIM_POSITION]]
+        ori = [float(x) for x in left_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)
         msg.poses.append(pose)
     else:
         msg.poses.append(_to_pose([0.0, 0.0, 0.0]))
 
-    if not left_ctrl.is_none:
-        pos = [float(x) for x in left_ctrl[ControllerInputIndex.AIM_POSITION]]
-        ori = [float(x) for x in left_ctrl[ControllerInputIndex.AIM_ORIENTATION]]
+    if not right_ctrl.is_none:
+        pos = [float(x) for x in right_ctrl[ControllerInputIndex.AIM_POSITION]]
+        ori = [float(x) for x in right_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)
@@ -276,30 +276,30 @@ def _build_ee_msg_from_hands(
     transform_rot: Rotation | None = None,
     transform_trans: Sequence[float] | None = None,
 ) -> PoseArray:
-    """Build a PoseArray with right then left hand wrist poses (EE proxy)."""
+    """Build a PoseArray with left then right hand wrist poses (EE proxy)."""
     msg = PoseArray()
     msg.header.stamp = now
     msg.header.frame_id = frame_id
 
-    if not right_hand.is_none:
-        right_positions = np.asarray(right_hand[HandInputIndex.JOINT_POSITIONS])
-        right_orientations = np.asarray(right_hand[HandInputIndex.JOINT_ORIENTATIONS])
+    if not left_hand.is_none:
+        left_positions = np.asarray(left_hand[HandInputIndex.JOINT_POSITIONS])
+        left_orientations = np.asarray(left_hand[HandInputIndex.JOINT_ORIENTATIONS])
         pose = _to_pose(
-            right_positions[HandJointIndex.WRIST],
-            right_orientations[HandJointIndex.WRIST],
+            left_positions[HandJointIndex.WRIST],
+            left_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)
         msg.poses.append(pose)
     else:
         msg.poses.append(_to_pose([0.0, 0.0, 0.0]))
 
-    if not left_hand.is_none:
-        left_positions = np.asarray(left_hand[HandInputIndex.JOINT_POSITIONS])
-        left_orientations = np.asarray(left_hand[HandInputIndex.JOINT_ORIENTATIONS])
+    if not right_hand.is_none:
+        right_positions = np.asarray(right_hand[HandInputIndex.JOINT_POSITIONS])
+        right_orientations = np.asarray(right_hand[HandInputIndex.JOINT_ORIENTATIONS])
         pose = _to_pose(
-            left_positions[HandJointIndex.WRIST],
-            left_orientations[HandJointIndex.WRIST],
+            right_positions[HandJointIndex.WRIST],
+            right_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)
@@ -705,7 +705,7 @@ def _build_wrist_tfs(
         left_available: bool,
         now,
     ) -> List[TransformStamped]:
-        """Build wrist TF transforms from a pre-built ee_poses PoseArray (right pose at index 0, left at index 1)."""
+        """Build wrist TF transforms from a pre-built ee_poses PoseArray (left pose at index 0, right at index 1)."""
         tfs = []
 
         def _get_orientation(pose: Pose) -> List[float]:
@@ -716,24 +716,24 @@ def _get_orientation(pose: Pose) -> List[float]:
                 pose.orientation.w,
             ]
 
-        if right_available:
+        if left_available:
             pose = ee_msg.poses[0]
             tfs.append(
                 _make_transform(
                     now,
                     self._world_frame,
-                    self._right_wrist_frame,
+                    self._left_wrist_frame,
                     [pose.position.x, pose.position.y, pose.position.z],
                     _get_orientation(pose),
                 )
             )
-        if left_available:
+        if right_available:
             pose = ee_msg.poses[1]
             tfs.append(
                 _make_transform(
                     now,
                     self._world_frame,
-                    self._left_wrist_frame,
+                    self._right_wrist_frame,
                     [pose.position.x, pose.position.y, pose.position.z],
                     _get_orientation(pose),
                 )