云端机器人研发：在 AWS 实现 ROS 2 设备与 Isaac Sim 的 Lerobot 仿真及数据流

# 本地控制节点实现 (lerobot_so101_ros2_bridge_remote.py)
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist
import websocket
import json
import serial
import time
import struct
import argparse
import sys

class JointStateReader2Remote(Node):
    def __init__(self, usb_port, rosbridge_ws, device_id=""):
        super().__init__('joint_state_reader2remote')

        self.usb_port = usb_port
        self.rosbridge_ws = rosbridge_ws
        self.device_id = device_id

        # Configure topic names
        prefix = f"/{device_id}" if device_id else ""
        self.joint_states_topic = f"{prefix}/joint_states"
        self.cmd_pose_topic = f"{prefix}/robot/cmd_pose"

        # SO100 joint configuration
        self.joint_names = ['shoulder_pan', 'shoulder_lift', 'elbow_flex', 'wrist_flex', 'wrist_roll', 'gripper']

        # Initialize connections
        self.serial_port = None
        self.ws = None
        self.connect_to_robot()
        self.connect_to_websocket()

        # State tracking
        self.last_positions = [0.0] * len(self.joint_names)
        self.read_errors = [0] * len(self.joint_names)
        self.total_reads = 0
        self.consecutive_errors = 0

        # Start reading timer (20Hz)
        self.timer = self.create_timer(0.05, self.read_and_publish)

        self.get_logger().info("SO100 Bridge started")
        self.get_logger().info(f"USB: {self.usb_port}")
        self.get_logger().info(f"WebSocket: {self.rosbridge_ws}")
        self.get_logger().info(f"Topics: {self.joint_states_topic}, {self.cmd_pose_topic}")

    def connect_to_robot(self):
        """Connect to SO100 robot via USB"""
        try:
            self.serial_port = serial.Serial(self.usb_port, 1000000, timeout=0.1)
            self.serial_port.reset_input_buffer()
            self.serial_port.reset_output_buffer()
            time.sleep(0.1)
            self.get_logger().info("Connected to SO100 robot")
        except Exception as e:
            self.get_logger().error(f"Failed to connect to robot: {e}")
            self.serial_port = None

    def connect_to_websocket(self):
        """Connect to remote WebSocket bridge"""
        try:
            self.ws = websocket.WebSocket()
            self.ws.connect(self.rosbridge_ws)

            # Advertise topics
            topics = [
                (self.joint_states_topic, "sensor_msgs/JointState"),
                (self.cmd_pose_topic, "geometry_msgs/Twist")
            ]

            for topic, msg_type in topics:
                advertise_msg = {"op": "advertise", "topic": topic, "type": msg_type}
                self.ws.send(json.dumps(advertise_msg))

            self.get_logger().info("Connected to WebSocket bridge")
        except Exception as e:
            self.get_logger().error(f"Failed to connect to WebSocket: {e}")
            self.ws = None

    def read_servo_position(self, servo_id):
        """Read position from STS3215 servo"""
        if not self.serial_port:
            return None

        try:
            # Build command packet
            length = 4
            instruction = 0x02
            address = 0x38
            read_length = 0x02
            checksum = (~(servo_id + length + instruction + address + read_length)) & 0xFF
            cmd = bytes([0xFF, 0xFF, servo_id, length, instruction, address, read_length, checksum])

            # Send command and read response
            self.serial_port.reset_input_buffer()
            self.serial_port.write(cmd)
            time.sleep(0.002)
            response = self.serial_port.read(8)

            # Parse response
            if len(response) >= 7 and response[0] == 0xFF and response[1] == 0xFF and response[2] == servo_id:
                pos = struct.unpack('<H', response[5:7])[0]
                if 0 <= pos <= 4095:
                    return pos
            return None

        except Exception:
            self.read_errors[servo_id - 1] += 1
            return None

    def ticks_to_radians(self, ticks, joint_idx):
        """Convert servo ticks to radians"""
        if ticks is None:
            return self.last_positions[joint_idx]
        normalized = (ticks - 2048) / 2048.0
        return normalized * 3.14159

    def send_to_websocket(self, topic, msg_data):
        """Send message to WebSocket"""
        if not self.ws:
            return

        try:
            data = {"op": "publish", "topic": topic, "msg": msg_data}
            self.ws.send(json.dumps(data))
        except Exception as e:
            self.get_logger().error(f"WebSocket send error: {e}")
            self.ws = None

    def read_and_publish(self):
        """Main loop: read joint positions and publish to WebSocket"""
        # Reconnect if needed
        if not self.serial_port:
            self.connect_to_robot()
            return
        if not self.ws:
            self.connect_to_websocket()
            return

        self.total_reads += 1

        # Read all joint positions
        positions = []
        successful_reads = 0

        for i in range(len(self.joint_names)):
            ticks = self.read_servo_position(i + 1)
            radians = self.ticks_to_radians(ticks, i)
            positions.append(radians)

            if ticks is not None:
                successful_reads += 1
            time.sleep(0.01)

        # Handle connection errors
        if successful_reads == 0:
            self.consecutive_errors += 1
            if self.consecutive_errors > 10:
                self.serial_port = None
                self.consecutive_errors = 0
                return
        else:
            self.consecutive_errors = 0

        self.last_positions = positions

        # Publish joint states
        current_time = self.get_clock().now().to_msg()
        joint_msg = {
            "header": {
                "stamp": {"sec": current_time.sec, "nanosec": current_time.nanosec},
                "frame_id": ""
            },
            "name": self.joint_names,
            "position": positions,
            "velocity": [],
            "effort": []
        }
        self.send_to_websocket(self.joint_states_topic, joint_msg)

        # Publish command pose (empty twist)
        twist_msg = {
            "linear": {"x": 0.0, "y": 0.0, "z": 0.0},
            "angular": {"x": 0.0, "y": 0.0, "z": 0.0}
        }
        self.send_to_websocket(self.cmd_pose_topic, twist_msg)

        # Status logging
        if self.total_reads % 100 == 0:
            self.get_logger().info(f"Read #{self.total_reads}: {successful_reads}/{len(self.joint_names)} servos OK")

def main():
    parser = argparse.ArgumentParser(description='SO101 ROS2 Bridge Remote')
    parser.add_argument('--usb_port', default='', help='USB port (e.g., /dev/tty.usbmodemxxx)')
    parser.add_argument('--rosbridge_ws', default='', help='WebSocket URL (e.g., ws://ip:port)')
    parser.add_argument('--device_id', default='', help='Device ID for topic prefix')

    args = parser.parse_args()

    # Validate required parameters
    if not args.usb_port:
        print("Error: --usb_port is required")
        print("Example: --usb_port /dev/tty.usbmodemxxx")
        sys.exit(1)

    if not args.rosbridge_ws:
        print("Error: --rosbridge_ws is required")
        print("Example: --rosbridge_ws ws://ip:port")
        sys.exit(1)

    rclpy.init()

    try:
        reader = JointStateReader2Remote(args.usb_port, args.rosbridge_ws, args.device_id)
        rclpy.spin(reader)
    except KeyboardInterrupt:
        print("\nShutting down...")
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()