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

4. 动作通信实现

任务目标

实现一个ROS2动作通信（Action）的最小可运行闭环：动作服务器接收“移动指定距离”的目标，在移动过程中周期性反馈进度，动作客户端发送目标并实时显示进度与最终结果。本案例以机器人底盘直线移动为场景，用仿真速度指令代替真实电机控制，便于独立验证。

环境与依赖

Ubuntu 22.04 + ROS2 Humble（或更高版本）
已创建ROS2工作空间 ~/ros2_ws/src
依赖包：rclpy、example_interfaces（含Action定义）

实现方案

使用ROS2内置的 example_interfaces/action/Fibonacci 接口作为动作定义（其结构为 goal: int32 order、feedback: int32[] sequence、result: int32[] sequence）。将“移动距离”映射为 order 字段（单位：米），服务器每0.5秒模拟移动0.5米并反馈当前已移动距离，全部完成后返回总距离。客户端发送目标并订阅反馈与结果。

关键完整代码

创建 move_action_server.py：

import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from example_interfaces.action import Fibonacci
import time

class MoveActionServer(Node):
    def __init__(self):
        super().__init__('move_action_server')
        self._action_server = ActionServer(
            self,
            Fibonacci,
            'move_robot',
            execute_callback=self.execute_callback)

    async def execute_callback(self, goal_handle):
        self.get_logger().info(f'收到目标：移动 {goal_handle.request.order} 米')
        feedback_msg = Fibonacci.Feedback()
        result_msg = Fibonacci.Result()

        distance_moved = 0.0
        total_distance = float(goal_handle.request.order)
        step = 0.5  # 每次移动0.5米

        while distance_moved < total_distance:
            distance_moved = min(distance_moved + step, total_distance)
            feedback_msg.sequence = [int(distance_moved * 10)]  # 用整数表示厘米
            self.get_logger().info(f'进度：{distance_moved:.1f} / {total_distance:.1f} 米')
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(0.5)

        goal_handle.succeed()
        result_msg.sequence = [int(total_distance * 10)]
        self.get_logger().info('移动完成')
        return result_msg

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

if __name__ == '__main__':
    main()

创建 move_action_client.py：

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from example_interfaces.action import Fibonacci

class MoveActionClient(Node):
    def __init__(self):
        super().__init__('move_action_client')
        self._action_client = ActionClient(self, Fibonacci, 'move_robot')

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

    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 feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        moved_cm = feedback.sequence[0] if feedback.sequence else 0
        self.get_logger().info(f'客户端收到反馈：已移动 {moved_cm / 10.0:.1f} 米')

    def get_result_callback(self, future):
        result = future.result().result
        total_cm = result.sequence[0] if result.sequence else 0
        self.get_logger().info(f'最终结果：移动完成，总距离 {total_cm / 10.0:.1f} 米')
        rclpy.shutdown()

def main(args=None):
    rclpy.init(args=args)
    node = MoveActionClient()
    node.send_goal(3.0)  # 请求移动3米
    rclpy.spin(node)

if __name__ == '__main__':
    main()

运行与验证

将两个文件放入 ~/ros2_ws/src/action_demo/action_demo/，并在 setup.py 的 entry_points 中添加：

'move_server = action_demo.move_action_server:main',
'move_client = action_demo.move_action_client:main',

编译并运行：

cd ~/ros2_ws && colcon build --packages-select action_demo
source install/setup.bash
ros2 run action_demo move_server
# 另开终端：
source ~/ros2_ws/install/setup.bash
ros2 run action_demo move_client

也可用命令行直接验证动作通信：

ros2 action send_goal /move_robot example_interfaces/action/Fibonacci "{order: 3}"

通过条件：服务器终端依次输出“收到目标”“进度：0.5/3.0”“进度：1.0/3.0”……直至“移动完成”；客户端终端同步显示反馈进度，并在结束时输出“最终结果：移动完成，总距离 3.0 米”。若使用命令行发送目标，终端会显示反馈序列与最终结果，即验证通过。