【ROS2机器人开发实战】Python动作通信：RCLPY ActionServer与Client详解

1. ROS2动作通信基础概念

动作（Action）是ROS2中一种重要的通信机制，它结合了话题（Topic）和服务（Service）的优点，特别适合需要长时间执行并反馈进度的任务场景。想象一下你点了一份外卖：下单相当于服务调用，骑手实时位置更新相当于话题发布，而整个送餐过程就是一个典型的动作交互。

在机器人开发中，动作通信最常见的应用场景包括：

• 机械臂运动控制
• 导航任务执行
• 物体抓取操作
• 长时间运行的算法任务

与ROS1相比，ROS2的动作系统有了显著改进：

1. 取消机制：支持随时中断正在执行的任务
2. 反馈机制：实时返回任务执行进度
3. 超时处理：内置完善的超时管理
4. 多语言支持：Python和C++实现更加统一

RCLPY是ROS2的Python客户端库，它提供了简洁的API来实现动作通信。相比RCLCPP（C++库），RCLPY的代码更加简洁，特别适合快速原型开发和教育演示。不过要注意，Python在实时性要求高的场景下可能不如C++高效。

2. 环境准备与工程创建

2.1 开发环境配置

在开始之前，确保你已经完成以下准备工作：

1. 安装Ubuntu 22.04（推荐）或20.04
2. 安装ROS2 Humble或Foxy版本
3. 配置Python 3.8+环境
4. 安装必要的开发工具：

bash复制sudo apt install python3-pip python3-rosdep2
pip install setuptools==58.2.0

2.2 创建工作空间和功能包

让我们从创建一个全新的工作空间开始：

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

创建Python功能包时，我们需要指定正确的依赖项：

bash复制ros2 pkg create example_action_rclpy \
  --build-type ament_python \
  --dependencies rclpy robot_control_interfaces \
  --node-name action_server \
  --maintainer-name "your_name" \
  --maintainer-email "your_email@example.com"

这个命令会自动生成包的基本结构，但我们需要手动添加一些关键文件：

bash复制touch ~/action_ws/src/example_action_rclpy/example_action_rclpy/action_server.py
touch ~/action_ws/src/example_action_rclpy/example_action_rclpy/action_client.py
touch ~/action_ws/src/example_action_rclpy/example_action_rclpy/robot_simulator.py

2.3 配置package.xml和setup.py

确保package.xml包含所有必要的依赖：

xml复制<depend>rclpy</depend>
<depend>robot_control_interfaces</depend>
<depend>action_msgs</depend>

在setup.py中正确配置入口点：

python复制entry_points={
    'console_scripts': [
        'action_server = example_action_rclpy.action_server:main',
        'action_client = example_action_rclpy.action_client:main',
    ],
},

3. 动作服务端实现详解

3.1 机器人模拟器开发

我们先创建一个简单的机器人模拟器，用于演示动作通信：

python复制from robot_control_interfaces.action import MoveRobot
import math

class RobotSimulator:
    """模拟机器人移动的虚拟类"""
    def __init__(self):
        self.current_pose = 0.0
        self.target_pose = 0.0
        self.status = MoveRobot.Feedback.STATUS_READY
        
    def move_step(self):
        """模拟机器人单步移动"""
        step_size = 0.1 * (self.target_pose - self.current_pose)
        self.current_pose += step_size
        return self.current_pose
        
    def set_goal(self, distance):
        """设置移动目标"""
        self.target_pose = distance
        self.status = MoveRobot.Feedback.STATUS_MOVING
        return True
        
    def get_status(self):
        """获取当前状态"""
        if abs(self.current_pose - self.target_pose) < 0.01:
            self.status = MoveRobot.Feedback.STATUS_COMPLETED
        return self.status

3.2 动作服务端核心实现

动作服务端的主要任务是接收客户端请求并执行相应操作：

python复制import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from robot_control_interfaces.action import MoveRobot

class RobotActionServer(Node):
    def __init__(self):
        super().__init__('robot_action_server')
        self.robot = RobotSimulator()
        
        self.action_server = ActionServer(
            self,
            MoveRobot,
            'move_robot',
            self.execute_callback
        )
        self.get_logger().info('动作服务端已启动...')

    async def execute_callback(self, goal_handle):
        """执行回调函数"""
        self.get_logger().info('开始执行移动命令...')
        
        # 设置目标位置
        target_distance = goal_handle.request.distance
        self.robot.set_goal(target_distance)
        
        # 初始化反馈消息
        feedback = MoveRobot.Feedback()
        
        # 执行移动循环
        while not self.robot.get_status() == MoveRobot.Feedback.STATUS_COMPLETED:
            # 检查是否收到取消请求
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('移动任务被取消')
                return MoveRobot.Result()
            
            # 执行单步移动
            current_pose = self.robot.move_step()
            
            # 发布反馈
            feedback.current_pose = current_pose
            feedback.status = self.robot.get_status()
            goal_handle.publish_feedback(feedback)
            
            # 控制循环频率
            await asyncio.sleep(0.5)
        
        # 任务完成
        goal_handle.succeed()
        result = MoveRobot.Result()
        result.final_pose = self.robot.current_pose
        return result

3.3 服务端高级特性

RCLPY提供了几个有用的默认回调函数，可以简化开发：

1. 目标接受回调：决定是否接受新目标

python复制def goal_callback(self, goal_request):
    """自定义目标接受策略"""
    if goal_request.distance > 10.0:  # 拒绝过大移动距离
        return GoalResponse.REJECT
    return GoalResponse.ACCEPT

2. 取消请求回调：处理取消请求

python复制def cancel_callback(self, cancel_request):
    """自定义取消策略"""
    return CancelResponse.ACCEPT  # 总是允许取消

3. 多线程处理：使用回调组提高并发性

python复制from rclpy.callback_groups import ReentrantCallbackGroup

# 在初始化ActionServer时指定
self.action_server = ActionServer(
    self,
    MoveRobot,
    'move_robot',
    self.execute_callback,
    callback_group=ReentrantCallbackGroup()
)

4. 动作客户端实现详解

4.1 客户端基础实现

动作客户端负责向服务端发送目标请求并处理反馈：

python复制import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from robot_control_interfaces.action import MoveRobot

class RobotActionClient(Node):
    def __init__(self):
        super().__init__('robot_action_client')
        self.action_client = ActionClient(self, MoveRobot, 'move_robot')
        self.get_logger().info('动作客户端已启动...')
        
    def send_goal(self, distance):
        """发送移动目标"""
        goal_msg = MoveRobot.Goal()
        goal_msg.distance = distance
        
        self.action_client.wait_for_server()
        
        self._send_goal_future = self.action_client.send_goal_async(
            goal_msg,
            feedback_callback=self.feedback_callback
        )
        self._send_goal_future.add_done_callback(self.goal_response_callback)

4.2 回调函数实现

动作客户端需要实现三个关键回调函数：

1. 目标响应回调：

python复制def goal_response_callback(self, future):
    """处理目标接受/拒绝响应"""
    goal_handle = future.result()
    if not goal_handle.accepted:
        self.get_logger().info('目标被拒绝')
        return
        
    self.get_logger().info('目标已接受，开始执行...')
    self._get_result_future = goal_handle.get_result_async()
    self._get_result_future.add_done_callback(self.get_result_callback)

2. 结果回调：

python复制def get_result_callback(self, future):
    """处理最终结果"""
    result = future.result().result
    self.get_logger().info(f'移动完成，最终位置: {result.final_pose}')

3. 反馈回调：

python复制def feedback_callback(self, feedback_msg):
    """处理进度反馈"""
    feedback = feedback_msg.feedback
    self.get_logger().info(
        f'当前位置: {feedback.current_pose:.2f}, 状态: {feedback.status}'
    )

4.3 客户端高级功能

1. 超时设置：

python复制# 等待服务端时设置超时
if not self.action_client.wait_for_server(timeout_sec=5.0):
    self.get_logger().error('服务端未响应')
    return

2. 取消请求发送：

python复制def cancel_goal(self):
    """取消当前目标"""
    if hasattr(self, '_goal_handle'):
        future = self._goal_handle.cancel_goal_async()
        future.add_done_callback(self.cancel_done_callback)

3. 结果轮询：

python复制def check_result(self):
    """轮询检查结果"""
    if self._get_result_future.done():
        result = self._get_result_future.result().result
        self.process_result(result)

5. 实战测试与调试技巧

5.1 编译与运行

1. 编译功能包：

bash复制cd ~/action_ws
colcon build --packages-select example_action_rclpy
source install/setup.bash

2. 启动动作服务端：

bash复制ros2 run example_action_rclpy action_server

3. 启动动作客户端：

bash复制ros2 run example_action_rclpy action_client

5.2 常用调试命令

1. 查看动作列表：

bash复制ros2 action list

2. 查看动作信息：

bash复制ros2 action info /move_robot

3. 手动发送动作目标：

bash复制ros2 action send_goal /move_robot robot_control_interfaces/action/MoveRobot "{distance: 5.0}"

5.3 常见问题解决

1. 服务端未响应：

• 检查是否正确定义了动作类型
• 确认服务端节点已正常运行
• 检查网络连接和ROS2域名设置

2. 反馈信息不更新：

• 确保在execute_callback中正确调用publish_feedback
• 检查反馈消息类型是否匹配
• 确认客户端反馈回调函数已注册

3. 取消请求无效：

• 检查服务端是否实现了取消回调
• 确认客户端正确发送了取消请求
• 检查回调组设置是否允许并发处理

6. 性能优化与最佳实践

6.1 回调组策略

ROS2提供了多种回调组类型，合理选择可以显著提高性能：

1. MutuallyExclusiveCallbackGroup（默认）：

• 串行执行回调
• 保证线程安全
• 适合简单应用

2. ReentrantCallbackGroup：

• 并行执行回调
• 需要开发者自行处理线程安全
• 适合高性能需求场景

使用示例：

python复制from rclpy.callback_groups import ReentrantCallbackGroup

callback_group = ReentrantCallbackGroup()

self.action_server = ActionServer(
    self,
    MoveRobot,
    'move_robot',
    self.execute_callback,
    callback_group=callback_group
)

6.2 执行器选择

根据应用场景选择合适的执行器：

1. 单线程执行器：

python复制rclpy.spin(node)

• 简单可靠
• 适合大多数基础应用

2. 多线程执行器：

python复制from rclpy.executors import MultiThreadedExecutor

executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(node)
executor.spin()

• 提高并发性能
• 需要处理线程安全问题
• 适合高负载场景

6.3 动作设计建议

1. 消息设计原则：

• 目标消息应只包含必要参数
• 反馈消息应提供有意义的进度信息
• 结果消息应包含完整执行摘要

2. 超时处理：

python复制# 客户端设置发送超时
send_goal_future = self.action_client.send_goal_async(goal_msg)
rclpy.spin_until_future_complete(self, send_goal_future, timeout_sec=3.0)

# 服务端设置执行超时
start_time = self.get_clock().now()
while not done:
    if (self.get_clock().now() - start_time).nanoseconds > 1e9 * 30:
        return Result(status=Result.STATUS_TIMEOUT)

3. 资源清理：

python复制def destroy_node(self):
    self.action_server.destroy()
    super().destroy_node()

7. 扩展应用与进阶话题

7.1 复合动作实现

通过组合多个基本动作实现复杂任务：

python复制class ComplexActionServer(Node):
    def __init__(self):
        super().__init__('complex_action_server')
        self.move_client = ActionClient(self, MoveRobot, 'move_robot')
        self.gripper_client = ActionClient(self, GripperAction, 'gripper_control')
        
    async def execute_pick_and_place(self, goal_handle):
        # 第一步：打开夹爪
        gripper_goal = GripperAction.Goal(position=100)
        await self.gripper_client.send_goal_async(gripper_goal)
        
        # 第二步：移动到目标位置
        move_goal = MoveRobot.Goal(distance=2.0)
        await self.move_client.send_goal_async(move_goal)
        
        # 第三步：关闭夹爪
        gripper_goal.position = 0
        await self.gripper_client.send_goal_async(gripper_goal)
        
        # 第四步：返回起始位置
        move_goal.distance = -2.0
        await self.move_client.send_goal_async(move_goal)

7.2 动作与行为树集成

将动作服务作为行为树的叶子节点：

python复制import py_trees

class MoveRobotAction(py_trees.behaviour.Behaviour):
    def __init__(self, name, action_client, distance):
        super().__init__(name)
        self.action_client = action_client
        self.distance = distance
        
    def update(self):
        if not self.action_client.server_is_ready():
            return py_trees.common.Status.RUNNING
            
        if not hasattr(self, 'goal_handle'):
            goal_msg = MoveRobot.Goal(distance=self.distance)
            self.future = self.action_client.send_goal_async(goal_msg)
            return py_trees.common.Status.RUNNING
            
        if not self.future.done():
            return py_trees.common.Status.RUNNING
            
        self.goal_handle = self.future.result()
        if not self.goal_handle.accepted:
            return py_trees.common.Status.FAILURE
            
        result_future = self.goal_handle.get_result_async()
        if not result_future.done():
            return py_trees.common.Status.RUNNING
            
        return py_trees.common.Status.SUCCESS

7.3 动作的单元测试

使用ROS2测试框架验证动作功能：

python复制import unittest
from rclpy.action import ActionClient
from robot_control_interfaces.action import MoveRobot

class TestMoveRobotAction(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node('test_node')
        cls.client = ActionClient(cls.node, MoveRobot, 'move_robot')
        
    def test_action_success(self):
        goal_msg = MoveRobot.Goal(distance=2.0)
        future = self.client.send_goal_async(goal_msg)
        rclpy.spin_until_future_complete(self.node, future)
        
        self.assertTrue(future.result().accepted)
        
    @classmethod
    def tearDownClass(cls):
        cls.node.destroy_node()
        rclpy.shutdown()

在实际项目中，我发现动作通信的稳定性很大程度上取决于超时设置和错误处理的完善程度。特别是在机器人应用中，一定要考虑网络中断、节点崩溃等异常情况。一个实用的技巧是为所有动作调用添加适当的超时检查，并在客户端实现重试机制。另外，动作服务的回调函数中应该包含充分的日志输出，这对后期调试非常有帮助。