Subscribe to ROS joint position node and stream to Isaac Sim

autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py

@@ -1,14 +1,3 @@
-from test_subscriber import HandPoseSubscriber
-from test_publisher import TestFloatPublisher
-from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR
-from HumanoidRL.HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG
-from isaaclab.utils import configclass
-from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
-from isaaclab.managers import SceneEntityCfg
-from isaaclab.assets import AssetBaseCfg
-import isaaclab.sim as sim_utils
-import rclpy
-import torch
 import argparse
 from isaaclab.app import AppLauncher
 
@@ -19,6 +8,17 @@
 app_launcher = AppLauncher(args_cli)
 simulation_app = app_launcher.app
 
+import rclpy
+import torch
+from isaaclab.utils import configclass
+from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
+from isaaclab.managers import SceneEntityCfg
+from isaaclab.assets import AssetBaseCfg
+import isaaclab.sim as sim_utils
+from HumanoidRL.HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG
+from test_subscriber import HandPoseSubscriber
+from test_publisher import TestVRHandPublisher
+
 
 @configclass
 class HandSceneCfg(InteractiveSceneCfg):
@@ -97,7 +97,7 @@ def main():
     # Initialize ROS2
     rclpy.init()
     node = HandPoseSubscriber()
-    test_node = TestFloatPublisher()
+    test_node = TestVRHandPublisher()
 
     # Initialize simulation
     sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)

autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_publisher.py

@@ -1,27 +1,43 @@
 import rclpy
 from rclpy.node import Node
-from std_msgs.msg import Float32MultiArray
+from common_msgs.msg import VRHandState, VRFingerFeatures
 
 
-class TestFloatPublisher(Node):
+# TODO: confirm topic name with Yichen
+TOPIC_NAME = '/vr_hand_state'
+
+
+def make_finger(curl, flexion=0.0, abduction=0.0, opposition=0.0):
+    f = VRFingerFeatures()
+    f.curl = curl
+    f.flexion = flexion
+    f.abduction = abduction
+    f.opposition = opposition
+    return f
+
+
+class TestVRHandPublisher(Node):
     def __init__(self):
-        super().__init__('test_float_publisher')
-        self.publisher_ = self.create_publisher(
-            Float32MultiArray, '/hand_joint_positions', 10)
+        super().__init__('test_vr_hand_publisher')
+        self.publisher_ = self.create_publisher(VRHandState, TOPIC_NAME, 10)
         self.timer = self.create_timer(0.1, self.timer_callback)
-        self.get_logger().info('Test Float Publisher initialized')
+        self.get_logger().info('Test VR Hand Publisher initialized')
 
     def timer_callback(self):
-        msg = Float32MultiArray()
-        # Publish a list of 15 floats (e.g., all 1.0 for testing)
-        msg.data = [2.0] * 15
+        msg = VRHandState()
+        # Publish test values: all fingers curled at 1.0
+        msg.thumb = make_finger(curl=1.0)
+        msg.index = make_finger(curl=1.0, flexion=1.0, abduction=0.5)
+        msg.middle = make_finger(curl=1.0, flexion=1.0, abduction=0.5)
+        msg.ring = make_finger(curl=1.0, flexion=1.0, abduction=0.5)
+        msg.pinky = make_finger(curl=1.0, flexion=1.0)
         self.publisher_.publish(msg)
-        self.get_logger().info(f'Published joint positions: {msg.data}')
+        self.get_logger().info('Published VR hand state')
 
 
 def main(args=None):
     rclpy.init(args=args)
-    node = TestFloatPublisher()
+    node = TestVRHandPublisher()
     try:
         rclpy.spin(node)
     except KeyboardInterrupt:

autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_subscriber.py

@@ -1,27 +1,56 @@
 import rclpy
 from rclpy.node import Node
-from std_msgs.msg import Float32MultiArray
+from common_msgs.msg import VRHandState
+
+
+# TODO: confirm topic name with Yichen
+TOPIC_NAME = '/vr_hand_state'
 
 
 class HandPoseSubscriber(Node):
     def __init__(self):
         super().__init__('hand_pose_subscriber')
         self.subscription = self.create_subscription(
-            Float32MultiArray,
-            '/hand_joint_positions',
+            VRHandState,
+            TOPIC_NAME,
             self.listener_callback,
             10)
         self.joint_positions = None
         self.get_logger().info('Hand Pose Subscriber initialized')
 
     def listener_callback(self, msg):
-        if len(msg.data) == 15:
-            self.joint_positions = list(msg.data)
-            self.get_logger().info(
-                f'Received joint positions: {self.joint_positions}')
-        else:
-            self.get_logger().warn(
-                f'Received invalid joint positions: expected 15 floats, got {len(msg.data)}')
+        # Map VRHandState fields to 15 joint positions.
+        # Joint order matches HAND_CFG (Revolute_1 to Revolute_15) as loaded
+        # from hand.usd. TODO: verify exact ordering with simulation.
+        #
+        # Mapping:
+        #   index:  mcp=curl, pip=flexion, dip=flexion
+        #   middle: mcp=curl, pip=flexion, dip=flexion
+        #   ring:   mcp=curl, pip=flexion, dip=flexion
+        #   pinky:  mcp=curl, pip=flexion, dip=flexion
+        #   thumb:  cmc=curl, mcp=curl,    ip=curl
+        #
+        # abduction and opposition are not used (no corresponding joints in
+        # the 15-DOF hand model)
+        self.joint_positions = [
+            msg.index.curl,
+            msg.index.flexion,
+            msg.index.flexion,
+            msg.middle.curl,
+            msg.middle.flexion,
+            msg.middle.flexion,
+            msg.ring.curl,
+            msg.ring.flexion,
+            msg.ring.flexion,
+            msg.pinky.curl,
+            msg.pinky.flexion,
+            msg.pinky.flexion,
+            msg.thumb.curl,
+            msg.thumb.curl,
+            msg.thumb.curl,
+        ]
+        self.get_logger().info(
+            f'Received joint positions: {self.joint_positions}')
 
     def get_latest_joint_positions(self):
         return self.joint_positions