📚 课程目录第4章 ROS2动作通信实战

4. ROS2动作通信实战

任务目标

本案例将实现一个ROS2动作通信（Action）的最小闭环：一个动作服务器执行耗时任务（模拟机器人移动到目标点），一个动作客户端发送目标并异步接收反馈与最终结果。通过该案例理解动作通信中目标（Goal）、反馈（Feedback）、结果（Result）三要素以及可取消（Cancel）机制。

环境与依赖

Ubuntu 22.04 + ROS2 Humble（或更高版本）
已安装 ros-humble-ros2cli 与 ros-humble-example-interfaces（用于复用Fibonacci动作定义，避免自定义接口文件）
工作空间已创建并source：source /opt/ros/humble/setup.bash

实现原理

动作通信基于服务+话题的组合：客户端通过服务发送目标并获取结果，通过话题接收连续反馈，并支持随时取消。服务器端需实现目标回调（接受/拒绝）、执行循环（发布反馈、检查取消请求）和结果返回。本案例复用ROS2标准接口 example_interfaces/action/Fibonacci，其目标为整数order（计算斐波那契数列的阶数），反馈为部分序列partial_sequence，结果为完整序列sequence。

关键完整代码

创建包并编写以下两个Python节点。将代码分别保存为 action_server.py 和 action_client.py，放入 src/action_demo/action_demo/ 目录。

# action_server.py
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from example_interfaces.action import Fibonacci
import time

class FibonacciActionServer(Node):
    def __init__(self):
        super().__init__('fibonacci_action_server')
        self._action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
            execute_callback=self.execute_callback,
            goal_callback=self.goal_callback,
            cancel_callback=self.cancel_callback)

    def goal_callback(self, goal_request):
        # 接受所有目标
        self.get_logger().info(f'收到目标: order={goal_request.order}')
        return rclpy.action.GoalResponse.ACCEPT

    def cancel_callback(self, goal_handle):
        self.get_logger().info('收到取消请求，准备取消')
        return rclpy.action.CancelResponse.ACCEPT

    async def execute_callback(self, goal_handle):
        self.get_logger().info('开始执行目标...')
        feedback_msg = Fibonacci.Feedback()
        sequence = [0, 1]

        for i in range(1, goal_handle.request.order):
            # 模拟长时间计算
            time.sleep(0.5)
            sequence.append(sequence[i] + sequence[i-1])
            feedback_msg.partial_sequence = sequence
            self.get_logger().info(f'反馈: {sequence}')
            goal_handle.publish_feedback(feedback_msg)

            # 检查是否被取消
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('目标已取消')
                return Fibonacci.Result()

        goal_handle.succeed()
        result = Fibonacci.Result()
        result.sequence = sequence
        self.get_logger().info(f'完成，结果: {sequence}')
        return result

def main(args=None):
    rclpy.init(args=args)
    node = FibonacciActionServer()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

# action_client.py
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from example_interfaces.action import Fibonacci

class FibonacciActionClient(Node):
    def __init__(self):
        super().__init__('fibonacci_action_client')
        self._action_client = ActionClient(self, Fibonacci, 'fibonacci')

    def send_goal(self, order):
        goal_msg = Fibonacci.Goal()
        goal_msg.order = order
        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)

    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)

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info(f'最终结果: {result.sequence}')
        rclpy.shutdown()

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(f'收到反馈: {feedback.partial_sequence}')

def main(args=None):
    rclpy.init(args=args)
    node = FibonacciActionClient()
    node.send_goal(5)  # 计算斐波那契前5项
    rclpy.spin(node)

if __name__ == '__main__':
    main()

编译与运行

# 在workspace根目录
colcon build --packages-select action_demo
source install/setup.bash

# 终端1：运行服务器
ros2 run action_demo action_server

# 终端2：运行客户端
ros2 run action_demo action_client

验证方法与判定标准

服务器日志：依次输出“收到目标: order=5”→“开始执行目标”→“反馈: [0, 1, 1]”→“反馈: [0, 1, 1, 2]”…直到“完成，结果: [0, 1, 1, 2, 3, 5]”。
客户端日志：先输出“目标被接受”，随后同步收到多条“收到反馈: [...]”，最后输出“最终结果: [0, 1, 1, 2, 3, 5]”。
取消验证：在服务器执行期间（约2秒内），在第三个终端运行 ros2 action cancel /fibonacci，服务器应输出“收到取消请求，准备取消”和“目标已取消”，客户端不再收到最终结果。

以上日志顺序与内容即为动作通信闭环通过的判定依据。

常见问题

找不到接口：确认已安装 example_interfaces，或使用 ros2 interface show example_interfaces/action/Fibonacci 验证。
客户端卡住：确认服务器已启动且动作名称一致（均为 fibonacci）。
取消不生效：取消回调需在服务器执行循环中通过 is_cancel_requested 主动检查，本案例已在每次迭代中检查。