From 504748c430d9b9ae110c149cb7116d38183368a3 Mon Sep 17 00:00:00 2001 From: Yoi Hibino Date: Fri, 11 Apr 2025 22:25:03 -0700 Subject: [PATCH] ros2 teleoperate --- examples/12_ros2_distributed_teleoperation.md | 148 +++++++++++++ lerobot/common/robot_devices/ros2_teleop.py | 203 ++++++++++++++++++ lerobot/scripts/run_ros2_follower.py | 64 ++++++ lerobot/scripts/run_ros2_leader.py | 66 ++++++ 4 files changed, 481 insertions(+) create mode 100644 examples/12_ros2_distributed_teleoperation.md create mode 100644 lerobot/common/robot_devices/ros2_teleop.py create mode 100644 lerobot/scripts/run_ros2_follower.py create mode 100644 lerobot/scripts/run_ros2_leader.py diff --git a/examples/12_ros2_distributed_teleoperation.md b/examples/12_ros2_distributed_teleoperation.md new file mode 100644 index 00000000..e8179dc5 --- /dev/null +++ b/examples/12_ros2_distributed_teleoperation.md @@ -0,0 +1,148 @@ +# ROS2 Distributed Teleoperation + +This tutorial explains how to set up a distributed teleoperation system using ROS2, allowing a leader robot on one computer to control a follower robot on another computer over a network. + +## Prerequisites + +1. ROS2 (Foxy or newer) installed on both computers +2. Network connectivity between the leader and follower computers +3. lerobot package installed on both computers + +## Installation + +Ensure you have the required ROS2 packages for sensor messages: + +```bash +pip install sensor_msgs +``` + +## Setup + +### 1. Configure ROS2 Domain ID (optional but recommended) + +To isolate your ROS2 network from others, set the same domain ID on both computers: + +```bash +# On both leader and follower computers +export ROS_DOMAIN_ID=42 # Choose any number between 0-232 +``` + +### 2. Network Configuration + +Ensure both computers can communicate over the network: + +1. Connect both computers to the same network +2. Verify they can ping each other +3. Make sure any firewalls allow ROS2 communication (default ports) + +## Running the Distributed Teleoperation + +### 1. Leader Robot Computer + +On the computer connected to the leader robot: + +```bash +python lerobot/scripts/run_ros2_leader.py \ + --robot.type=koch \ + --robot.leader_arms.main.port=/dev/ttyUSB0 \ + --ros2_config.node_name=lerobot_teleop \ + --ros2_config.topic_name=joint_states \ + --ros2_config.publish_rate_hz=200.0 +``` + +Replace `/dev/ttyUSB0` with the actual port of your leader robot. + +### 2. Follower Robot Computer + +On the computer connected to the follower robot: + +```bash +python lerobot/scripts/run_ros2_follower.py \ + --robot.type=koch \ + --robot.follower_arms.main.port=/dev/ttyUSB0 \ + --ros2_config.node_name=lerobot_teleop \ + --ros2_config.topic_name=joint_states +``` + +Replace `/dev/ttyUSB0` with the actual port of your follower robot. + +### Note on Topic Names + +Make sure both leader and follower use the same topic name (default is `joint_states`). + +## Troubleshooting + +### 1. Check ROS2 Discovery + +Verify nodes can discover each other: + +```bash +# On either computer +ros2 topic list +``` + +You should see `/joint_states` in the list. + +### 2. Monitor Message Flow + +Check if messages are being published: + +```bash +ros2 topic echo /joint_states +``` + +### 3. Network Issues + +If nodes can't discover each other: +- Ensure both computers are on the same network +- Check if ROS_DOMAIN_ID is set to the same value on both computers +- Verify firewall settings +- Try using the ROS_LOCALHOST_ONLY=0 environment variable + +### 4. Debug Node Status + +```bash +ros2 node list +ros2 node info /lerobot_teleop_leader # or follower +``` + +## Advanced Configuration + +### Quality of Service Settings + +For teleoperation over unreliable networks, you can adjust QoS settings: + +```bash +# Use reliable QoS instead of best effort (might increase latency but improve reliability) +--ros2_config.use_best_effort_qos=false +``` + +### Publishing Rate + +Adjust the rate at which joint states are published: + +```bash +# On leader robot, set a lower rate for unreliable networks +--ros2_config.publish_rate_hz=100.0 +``` + +## Examples + +### Using with myCobot 280 + +For MyCobot robots, ensure you've set up the correct motor types: + +Leader: +```bash +python lerobot/scripts/run_ros2_leader.py \ + --robot.type=mycobot \ + --robot.leader_arms.main.port=/dev/ttyUSB0 \ + --ros2_config.publish_rate_hz=100.0 +``` + +Follower: +```bash +python lerobot/scripts/run_ros2_follower.py \ + --robot.type=mycobot \ + --robot.follower_arms.main.port=/dev/ttyUSB0 +``` diff --git a/lerobot/common/robot_devices/ros2_teleop.py b/lerobot/common/robot_devices/ros2_teleop.py new file mode 100644 index 00000000..3ce5634b --- /dev/null +++ b/lerobot/common/robot_devices/ros2_teleop.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 +# Copyright 2024 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""ROS2 nodes for robot teleoperation between leader and follower robots.""" + +import time +from dataclasses import dataclass +from typing import Dict, List, Optional + +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy +from sensor_msgs.msg import JointState +import torch + +from lerobot.common.robot_devices.control_utils import busy_wait +from lerobot.common.robot_devices.robots.manipulator import ManipulatorRobot +from lerobot.common.robot_devices.robots.errors import RobotDeviceNotConnectedError + + +@dataclass +class ROS2TeleopConfig: + """Configuration for ROS2 teleoperation.""" + node_name: str = "lerobot_teleop" + topic_name: str = "joint_states" + publish_rate_hz: float = 200.0 # Default publish rate + use_best_effort_qos: bool = True # Use best effort QoS for real-time performance + + +class LeaderNode(Node): + """ROS2 node for the leader robot that publishes joint states.""" + + def __init__(self, robot: ManipulatorRobot, config: ROS2TeleopConfig): + super().__init__(config.node_name + "_leader") + + self.robot = robot + self.config = config + + # Set up QoS profile - for real-time control, best effort is often better than reliable + qos = QoSProfile(depth=10) + if config.use_best_effort_qos: + qos.reliability = QoSReliabilityPolicy.BEST_EFFORT + + # Create publisher + self.publisher = self.create_publisher( + JointState, + config.topic_name, + qos + ) + + # Create timer for publishing at specified rate + period = 1.0 / config.publish_rate_hz + self.timer = self.create_timer(period, self.publish_joint_states) + + self.get_logger().info(f"Leader node initialized, publishing to {config.topic_name}") + + def publish_joint_states(self): + """Read joint states from leader robot and publish them.""" + if not self.robot.is_connected: + self.get_logger().error("Robot is not connected") + return + + # Read positions from all leader arms + leader_pos = {} + joint_names = [] + positions = [] + + try: + for name in self.robot.leader_arms: + pos = self.robot.leader_arms[name].read("Present_Position") + leader_pos[name] = pos + + # Extend the joint names and positions lists + motor_names = self.robot.leader_arms[name].motor_names + joint_names.extend([f"{name}_{motor_name}" for motor_name in motor_names]) + positions.extend(pos.tolist()) + + # Create and publish the message + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.name = joint_names + msg.position = positions + + self.publisher.publish(msg) + except Exception as e: + self.get_logger().error(f"Failed to publish joint states: {e}") + + +class FollowerNode(Node): + """ROS2 node for the follower robot that subscribes to joint states.""" + + def __init__(self, robot: ManipulatorRobot, config: ROS2TeleopConfig): + super().__init__(config.node_name + "_follower") + + self.robot = robot + self.config = config + self.joint_positions = {} # Store the latest joint positions + + # Set up QoS profile + qos = QoSProfile(depth=10) + if config.use_best_effort_qos: + qos.reliability = QoSReliabilityPolicy.BEST_EFFORT + + # Create subscription + self.subscription = self.create_subscription( + JointState, + config.topic_name, + self.joint_states_callback, + qos + ) + + self.get_logger().info(f"Follower node initialized, subscribing to {config.topic_name}") + + def joint_states_callback(self, msg: JointState): + """Process incoming joint states and command the follower robot.""" + if not self.robot.is_connected: + self.get_logger().error("Robot is not connected") + return + + try: + # Parse the message and organize by arm name + arm_positions = {} + + for i, joint_name in enumerate(msg.name): + # Joint names are expected to be in format: "arm_name_motor_name" + parts = joint_name.split("_", 1) + if len(parts) < 2: + continue + + arm_name, _ = parts + + if arm_name not in arm_positions: + arm_positions[arm_name] = [] + + arm_positions[arm_name].append(msg.position[i]) + + # Send positions to follower arms that match leader arms + for name, positions in arm_positions.items(): + if name in self.robot.follower_arms: + # Convert to numpy array + pos_array = np.array(positions, dtype=np.float32) + + # Store the latest positions + self.joint_positions[name] = pos_array + + # Send to the follower robot + self.robot.follower_arms[name].write("Goal_Position", pos_array) + + except Exception as e: + self.get_logger().error(f"Failed to process joint states: {e}") + + +def run_ros2_leader(robot: ManipulatorRobot, config: Optional[ROS2TeleopConfig] = None): + """Run the ROS2 leader node until interrupted.""" + if not robot.is_connected: + robot.connect() + + if config is None: + config = ROS2TeleopConfig() + + rclpy.init() + node = LeaderNode(robot, config) + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +def run_ros2_follower(robot: ManipulatorRobot, config: Optional[ROS2TeleopConfig] = None): + """Run the ROS2 follower node until interrupted.""" + if not robot.is_connected: + robot.connect() + + if config is None: + config = ROS2TeleopConfig() + + rclpy.init() + node = FollowerNode(robot, config) + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/lerobot/scripts/run_ros2_follower.py b/lerobot/scripts/run_ros2_follower.py new file mode 100644 index 00000000..4f12057f --- /dev/null +++ b/lerobot/scripts/run_ros2_follower.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +# Copyright 2024 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import logging +from pprint import pformat +from dataclasses import asdict, dataclass + +from lerobot.common.config import parse_and_overload, parser +from lerobot.common.robot_devices.control_configs import ControlPipelineConfig +from lerobot.common.robot_devices.robots.utils import make_robot_from_config +from lerobot.common.robot_devices.ros2_teleop import ROS2TeleopConfig, run_ros2_follower +from lerobot.common.logging_utils import init_logging + +@dataclass +class ROS2FollowerConfig: + """Configuration for ROS2 follower teleoperation.""" + node_name: str = "lerobot_teleop" + topic_name: str = "joint_states" + use_best_effort_qos: bool = True + +@parser.wrap() +def run_follower(cfg: ControlPipelineConfig, ros2_config: ROS2FollowerConfig = None): + """Run a robot as a ROS2 follower that subscribes to joint states.""" + init_logging() + + if ros2_config is None: + ros2_config = ROS2FollowerConfig() + + teleop_config = ROS2TeleopConfig( + node_name=ros2_config.node_name, + topic_name=ros2_config.topic_name, + use_best_effort_qos=ros2_config.use_best_effort_qos + ) + + logging.info("ROS2 Follower Configuration:") + logging.info(pformat(asdict(teleop_config))) + logging.info("Robot Configuration:") + logging.info(pformat(asdict(cfg.robot))) + + robot = make_robot_from_config(cfg.robot) + + try: + run_ros2_follower(robot, teleop_config) + except KeyboardInterrupt: + logging.info("Follower node stopped by user") + finally: + if robot.is_connected: + robot.disconnect() + +if __name__ == "__main__": + run_follower() diff --git a/lerobot/scripts/run_ros2_leader.py b/lerobot/scripts/run_ros2_leader.py new file mode 100644 index 00000000..80b63700 --- /dev/null +++ b/lerobot/scripts/run_ros2_leader.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +# Copyright 2024 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import logging +from pprint import pformat +from dataclasses import asdict + +from lerobot.common.config import parse_and_overload, parser +from lerobot.common.robot_devices.control_configs import ControlPipelineConfig +from lerobot.common.robot_devices.robots.utils import make_robot_from_config +from lerobot.common.robot_devices.ros2_teleop import ROS2TeleopConfig, run_ros2_leader +from lerobot.common.logging_utils import init_logging + +@dataclass +class ROS2LeaderConfig: + """Configuration for ROS2 leader teleoperation.""" + node_name: str = "lerobot_teleop" + topic_name: str = "joint_states" + publish_rate_hz: float = 200.0 + use_best_effort_qos: bool = True + +@parser.wrap() +def run_leader(cfg: ControlPipelineConfig, ros2_config: ROS2LeaderConfig = None): + """Run a robot as a ROS2 leader that publishes joint states.""" + init_logging() + + if ros2_config is None: + ros2_config = ROS2LeaderConfig() + + teleop_config = ROS2TeleopConfig( + node_name=ros2_config.node_name, + topic_name=ros2_config.topic_name, + publish_rate_hz=ros2_config.publish_rate_hz, + use_best_effort_qos=ros2_config.use_best_effort_qos + ) + + logging.info("ROS2 Leader Configuration:") + logging.info(pformat(asdict(teleop_config))) + logging.info("Robot Configuration:") + logging.info(pformat(asdict(cfg.robot))) + + robot = make_robot_from_config(cfg.robot) + + try: + run_ros2_leader(robot, teleop_config) + except KeyboardInterrupt: + logging.info("Leader node stopped by user") + finally: + if robot.is_connected: + robot.disconnect() + +if __name__ == "__main__": + run_leader()