📚 课程目录第3章动作通信执行复杂任务

3. 动作通信执行复杂任务

任务目标

本案例实现两台机器人之间的动作通信：机器人A作为动作客户端，向机器人B发送导航目标点；机器人B作为动作服务端，执行导航任务并通过反馈消息实时上报进度（如当前坐标、剩余距离），任务完成后返回最终结果。通过该案例掌握ROS2动作通信的完整链路：目标下发、进度反馈、结果返回。

环境依赖

需要ROS2（Humble或更高版本）、turtlesim包（用于模拟机器人运动）。无需额外安装动作相关依赖，ROS2核心已包含rclpy动作库。

实现方案

使用turtlesim模拟机器人B的运动。动作服务端接收目标坐标后，控制乌龟以恒定速度直线移动，每移动一小段距离发布一次反馈（包含当前位置与剩余距离），到达目标后返回成功结果。机器人A通过动作客户端发送目标并订阅反馈。

关键完整代码

创建两个Python文件：action_server.py（机器人B）与action_client.py（机器人A）。

# action_server.py — 机器人B：动作服务端
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from nav_msgs.action import NavigateToPose  # 使用标准导航动作接口
import math

class NavigateServer(Node):
    def __init__(self):
        super().__init__('navigate_server')
        self.publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self.pose_sub = self.create_subscription(Pose, '/turtle1/pose', self.pose_cb, 10)
        self.current_pose = Pose()
        self._action_server = ActionServer(
            self, NavigateToPose, 'navigate', self.execute_callback)

    def pose_cb(self, msg):
        self.current_pose = msg

    async def execute_callback(self, goal_handle):
        target = goal_handle.request.pose.pose.position
        feedback_msg = NavigateToPose.Feedback()
        result_msg = NavigateToPose.Result()

        while rclpy.ok():
            dx = target.x - self.current_pose.x
            dy = target.y - self.current_pose.y
            distance = math.sqrt(dx*dx + dy*dy)
            if distance < 0.1:
                break
            # 发布反馈：当前位置与剩余距离
            feedback_msg.current_pose = self.current_pose
            feedback_msg.remaining_distance = distance
            goal_handle.publish_feedback(feedback_msg)
            # 控制乌龟向目标移动
            twist = Twist()
            twist.linear.x = min(2.0, distance)
            twist.angular.z = math.atan2(dy, dx) - self.current_pose.theta
            self.publisher.publish(twist)
            self.get_clock().sleep_for(rclpy.duration.Duration(seconds=0.1))

        goal_handle.succeed()
        result_msg.result = True
        return result_msg

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

if __name__ == '__main__':
    main()

# action_client.py — 机器人A：动作客户端
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav_msgs.action import NavigateToPose
from geometry_msgs.msg import PoseStamped

class NavigateClient(Node):
    def __init__(self):
        super().__init__('navigate_client')
        self._client = ActionClient(self, NavigateToPose, 'navigate')

    def send_goal(self, x, y):
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = PoseStamped()
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        self._client.wait_for_server()
        self._send_goal_future = self._client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_cb)
        self._send_goal_future.add_done_callback(self.goal_response_cb)

    def feedback_cb(self, feedback_msg):
        fb = feedback_msg.feedback
        print(f'进度: 位置({fb.current_pose.x:.2f}, {fb.current_pose.y:.2f}), '
              f'剩余距离 {fb.remaining_distance:.2f}')

    def goal_response_cb(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            print('目标被拒绝')
            return
        print('目标已接受')
        self._result_future = goal_handle.get_result_async()
        self._result_future.add_done_callback(self.result_cb)

    def result_cb(self, future):
        result = future.result().result
        print(f'导航完成: {result.result}')

def main(args=None):
    rclpy.init(args=args)
    node = NavigateClient()
    node.send_goal(5.0, 5.0)  # 请求机器人B移动到(5,5)
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

运行与验证

打开三个终端，依次执行：

# 终端1：启动乌龟模拟器
ros2 run turtlesim turtlesim_node

# 终端2：启动动作服务端（机器人B）
python3 action_server.py

# 终端3：启动动作客户端（机器人A）
python3 action_client.py

验证方法：观察终端3输出，应依次出现“目标已接受”、多条“进度”反馈（位置坐标逐渐接近目标、剩余距离递减），最后输出“导航完成: True”。同时终端1的乌龟画面中，乌龟从初始位置移动到(5,5)附近。若终端3收到反馈且乌龟到达目标点，则动作通信闭环验证通过。

常见问题

找不到nav_msgs动作：执行sudo apt install ros-$ROS_DISTRO-nav-msgs安装。
乌龟不移动：检查服务端是否成功订阅/turtle1/pose话题，且目标坐标在乌龟窗口范围内（0~11）。
反馈频率过高：可通过增大sleep_for时长降低反馈频率。