diff --git a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py index 59d60b1..e07540e 100644 --- a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py +++ b/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) diff --git a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_publisher.py b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_publisher.py index 34fb679..8071d8f 100644 --- a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_publisher.py +++ b/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: diff --git a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_subscriber.py b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_subscriber.py index ee2e6ee..a530180 100644 --- a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_subscriber.py +++ b/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