ROS2机械臂开发：环境搭建与节点创建指南

1. ROS2机械臂开发环境搭建与基础节点创建

在开始机械臂控制系统的开发前，我们需要先搭建好ROS2的开发环境。这里假设你已经安装好了ROS2 Humble版本（推荐使用Ubuntu 22.04系统）。让我们从创建工作空间开始：

bash复制mkdir -p ~/dev_ws/src
cd ~/dev_ws/src

接下来创建机械臂控制包。ROS2中创建Python包的完整命令如下：

bash复制ros2 pkg create --build-type ament_python --node-name arm_joint_node arm_pkg

这个命令会创建一个名为arm_pkg的Python包，并自动生成一个名为arm_joint_node的节点模板。创建完成后，目录结构应该是这样的：

code复制dev_ws/
└── src/
    └── arm_pkg/
        ├── arm_pkg/
        │   ├── __init__.py
        │   ├── arm_joint_node.py  # 自动生成的节点文件
        │   └── ...
        ├── package.xml
        ├── resource/
        ├── setup.cfg
        └── setup.py

提示：如果你需要同时创建C++节点，可以使用--node-name参数多次，或者手动添加节点文件。但在本项目中我们统一使用Python开发。

2. 机械臂关节角度读取与消息定义

2.1 自定义消息类型

机械臂控制需要特定的消息类型来传递关节角度信息。我们需要先创建一个独立的包来存放自定义消息：

bash复制cd ~/dev_ws/src
ros2 pkg create --build-type ament_cmake arm_msg

在arm_msg包中创建消息定义文件：

code复制arm_msg/
└── msg/
    └── ArmJointAngles.msg

ArmJointAngles.msg内容如下：

code复制float32[6] angles  # 假设是6轴机械臂
string timestamp

然后修改package.xml和CMakeLists.txt以支持消息生成。在package.xml中添加：

xml复制<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

在CMakeLists.txt中添加：

cmake复制find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/ArmJointAngles.msg"
)

2.2 关节角度读取实现

在arm_joint_node.py中，我们需要实现关节角度的读取和发布。以下是核心代码框架：

python复制import rclpy
from rclpy.node import Node
from arm_msg.msg import ArmJointAngles

class ArmJointNode(Node):
    def __init__(self):
        super().__init__('arm_joint_node')
        self.publisher = self.create_publisher(ArmJointAngles, 'joint_angles', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)  # 10Hz
        
        # 初始化机械臂硬件接口
        self.arm_interface = ArmInterface()  # 假设的机械臂接口类
        
    def timer_callback(self):
        msg = ArmJointAngles()
        msg.angles = self.arm_interface.read_joint_angles()  # 读取实际关节角度
        msg.timestamp = str(self.get_clock().now().to_msg())
        self.publisher.publish(msg)
        self.get_logger().info(f'Publishing: {msg.angles}')

注意：实际的ArmInterface类需要根据你使用的具体机械臂SDK实现。常见机械臂如UR、Franka等都有对应的ROS2驱动包。

3. 机械臂控制节点开发

3.1 添加订阅者功能

为了让机械臂节点能够接收控制指令，我们需要添加订阅者功能。修改ArmJointNode类：

python复制def __init__(self):
    # ...原有代码...
    self.subscription = self.create_subscription(
        ArmJointAngles,
        'cmd_angles',
        self.cmd_callback,
        10)
        
def cmd_callback(self, msg):
    self.get_logger().info(f'Received command: {msg.angles}')
    # 将角度指令发送给机械臂
    success = self.arm_interface.move_to_joint_angles(msg.angles)
    if not success:
        self.get_logger().error('Failed to execute joint movement!')

3.2 电机驱动实现

电机驱动的具体实现高度依赖于硬件接口。这里给出一个通用框架：

python复制class ArmInterface:
    def __init__(self):
        # 初始化硬件连接
        self.serial_port = serial.Serial('/dev/ttyUSB0', 115200)  # 示例
        
    def read_joint_angles(self):
        # 发送读取指令
        self.serial_port.write(b'GET_ANGLES\n')
        # 读取返回数据
        response = self.serial_port.readline().decode().strip()
        return [float(x) for x in response.split(',')]
        
    def move_to_joint_angles(self, angles):
        cmd = f"MOVE {','.join(str(a) for a in angles)}\n"
        self.serial_port.write(cmd.encode())
        response = self.serial_port.readline().decode().strip()
        return response == "OK"

实际项目中，你可能需要使用厂商提供的SDK，或者更复杂的通信协议。务必添加充分的错误处理和超时机制。

4. 机械臂动作录制与回放系统

4.1 录制节点实现

创建arm_record_node.py实现动作录制：

python复制import rclpy
from rclpy.node import Node
from arm_msg.msg import ArmJointAngles
import json
import time

class ArmRecordNode(Node):
    def __init__(self):
        super().__init__('arm_record_node')
        self.subscription = self.create_subscription(
            ArmJointAngles,
            'joint_angles',
            self.listener_callback,
            10)
        self.recording = False
        self.record_data = []
        
        # 服务定义
        self.start_service = self.create_service(
            StartRecording, 
            'start_recording',
            self.start_recording_callback)
            
        self.stop_service = self.create_service(
            StopRecording,
            'stop_recording',
            self.stop_recording_callback)
    
    def listener_callback(self, msg):
        if self.recording:
            self.record_data.append({
                'timestamp': msg.timestamp,
                'angles': list(msg.angles)
            })
            
    def start_recording_callback(self, request, response):
        if not self.recording:
            self.record_data = []
            self.recording = True
            response.success = True
            response.message = "Recording started"
        else:
            response.success = False
            response.message = "Already recording"
        return response
        
    def stop_recording_callback(self, request, response):
        if self.recording:
            self.recording = False
            # 保存到文件
            with open(request.filename, 'w') as f:
                json.dump(self.record_data, f)
            response.success = True
            response.message = f"Recording saved to {request.filename}"
        else:
            response.success = False
            response.message = "Not recording"
        return response

4.2 回放节点实现

创建arm_playback_node.py实现动作回放：

python复制import rclpy
from rclpy.node import Node
from arm_msg.msg import ArmJointAngles
import json
import time

class ArmPlaybackNode(Node):
    def __init__(self):
        super().__init__('arm_playback_node')
        self.publisher = self.create_publisher(ArmJointAngles, 'cmd_angles', 10)
        
        self.playback_service = self.create_service(
            PlaybackRecording,
            'playback_recording',
            self.playback_callback)
            
    def playback_callback(self, request, response):
        try:
            with open(request.filename, 'r') as f:
                data = json.load(f)
                
            start_time = time.time()
            for frame in data:
                # 等待正确的时间点
                while (time.time() - start_time) < float(frame['timestamp']):
                    time.sleep(0.001)
                    
                msg = ArmJointAngles()
                msg.angles = frame['angles']
                self.publisher.publish(msg)
                
            response.success = True
            response.message = "Playback completed"
        except Exception as e:
            response.success = False
            response.message = str(e)
        return response

4.3 功能整合与状态管理

将录制和回放功能整合到单个节点arm_record_playback_node.py：

python复制class ArmRecordPlaybackNode(Node):
    def __init__(self):
        super().__init__('arm_record_playback_node')
        # 状态变量
        self.current_mode = 'idle'  # 'idle', 'recording', 'playing'
        self.record_data = []
        
        # 服务定义
        self.create_service(StartRecording, ...)
        self.create_service(StopRecording, ...)
        self.create_service(StartPlayback, ...)
        self.create_service(StopPlayback, ...)
        self.create_service(GetStatus, ...)
        
    def validate_mode_switch(self, target_mode):
        if self.current_mode == 'idle':
            return True
        if self.current_mode == target_mode:
            return True
        return False

5. GUI界面与一键启动配置

5.1 GUI节点实现

创建gui_record_playback_node.py：

python复制import rclpy
from rclpy.node import Node
from PyQt5.QtWidgets import QApplication, QMainWindow, QPushButton, QLineEdit, QLabel

class ControlGUI(Node):
    def __init__(self):
        super().__init__('gui_record_playback_node')
        self.app = QApplication([])
        self.window = QMainWindow()
        
        # UI元素初始化
        self.record_button = QPushButton('Start Recording', self.window)
        self.playback_button = QPushButton('Start Playback', self.window)
        self.filename_input = QLineEdit(self.window)
        self.status_label = QLabel('Status: Idle', self.window)
        
        # 设置客户端
        self.record_client = self.create_client(StartRecording, 'start_recording')
        self.playback_client = self.create_client(StartPlayback, 'start_playback')
        
    def run(self):
        self.window.show()
        self.app.exec_()

5.2 ROS2 Launch文件配置

创建arm_record_playback.launch.py：

python复制from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='arm_pkg',
            executable='arm_joint_node',
            name='arm_joint_node'
        ),
        Node(
            package='arm_pkg',
            executable='arm_record_playback_node',
            name='arm_record_playback_node'
        ),
        Node(
            package='arm_pkg',
            executable='gui_record_playback_node',
            name='gui_record_playback_node'
        )
    ])

6. 系统构建与测试

完成所有代码后，构建并测试系统：

bash复制cd ~/dev_ws
colcon build --packages-select arm_msg arm_pkg
source install/setup.bash
ros2 launch arm_pkg arm_record_playback.launch.py

测试流程建议：

1. 先手动移动机械臂，确认arm_joint_node能正确读取和发布关节角度
2. 测试录制功能，录制一段动作后检查生成的文件
3. 测试回放功能，确认机械臂能复现录制的动作
4. 最后测试GUI界面，验证所有功能都能通过界面控制

7. 实际开发中的经验分享

在开发ROS2机械臂控制系统时，有几个关键点需要注意：

1. 实时性问题：机械臂控制对实时性要求较高，Python节点可能无法满足毫秒级的实时需求。对于高性能要求的场景，建议使用C++实现核心控制节点。

2. 硬件接口稳定性：机械臂硬件接口常常会出现连接不稳定、响应超时等问题。务必添加充分的错误处理和重试机制。

3. 安全考虑：机械臂是强动力设备，必须添加急停功能和运动范围限制。可以在arm_joint_node中添加安全监控线程。

4. 录制数据优化：长时间录制会产生大量数据，可以考虑：

   • 只在角度变化超过阈值时记录
   • 使用二进制格式代替JSON减少文件大小
   • 添加数据压缩功能

5. 回放平滑处理：直接按记录的时间戳回放可能会导致机械臂运动不流畅。可以：

   • 添加运动插值算法
   • 实现速度规划
   • 添加过渡缓冲

6. 多机械臂协调：如果需要控制多个机械臂，可以考虑：

   • 为每个机械臂创建独立的命名空间
   • 使用robot_state_publisher发布TF信息
   • 设计协调控制消息格式

这个项目展示了ROS2在机械臂控制中的典型应用模式。通过将系统分解为独立的节点，我们可以实现模块化开发和灵活的功能组合。录制回放功能在工业应用中非常实用，可以用于：

• 生产工艺的保存和复现
• 操作员动作的教学和回放
• 自动化测试场景的构建