⚡ 第05课：动作(Action)

ROS2基础 ✅ Docker验证通过

📋 课程目标

🧠 Action通信模型

Action是ROS2中最复杂的通信方式，专为长时间运行的异步任务设计。它结合了Topic和Service的优点：支持目标下发（类似Service）、进度反馈（类似Topic）、结果返回（类似Service）。

Action Client Action Server ┌──────────────────┐ ┌──────────────────┐ │ │ ─── Goal ────────► │ │ │ send_goal() │ │ execute_callback│ │ │ ◄── Goal Response─ │ │ │ │ (accepted/ │ ┌────────────┐ │ │ │ rejected) │ │ 执行任务 │ │ │ │ │ │ (长时间) │ │ │ 接收反馈 │ ◄── Feedback ───── │ │ │ │ │ 更新UI/状态 │ (周期性) │ │ │ │ │ │ │ └────────────┘ │ │ │ ◄── Result ─────── │ │ │ 处理结果 │ (最终结果) │ │ │ │ │ │ │ cancel_goal() │ ─── Cancel ──────►│ 取消执行 │ └──────────────────┘ └──────────────────┘ 三大核心：Goal（目标）→ Feedback（反馈）→ Result（结果）

📊 三种通信方式对比

特性TopicServiceAction
模型发布/订阅请求/响应目标/反馈/结果
方向单向双向双向
异步✅❌（同步等待）✅
反馈❌❌✅
可取消❌❌✅
适用场景传感器数据快速查询/操作导航/运动/长时间任务
典型例子/scan, /image/enable, /get_status/navigate, /move_arm

🐍 Python：Action Server（导航模拟）

#!/usr/bin/env python3
"""导航Action服务端 - 模拟机器人导航到目标点"""

import math
import time
import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import PoseStamped


class NavigationActionServer(Node):
    """导航Action服务端"""

    def __init__(self):
        super().__init__('navigation_action_server')

        self._action_server = ActionServer(
            self,
            NavigateToPose,
            'navigate_to_pose',
            execute_callback=self.execute_callback,
            goal_callback=self.goal_callback,
            handle_accepted_callback=self.handle_accepted_callback,
            cancel_callback=self.cancel_callback,
            callback_group=ReentrantCallbackGroup()
        )

        # 当前机器人位置（模拟）
        self.current_x = 0.0
        self.current_y = 0.0
        self.current_theta = 0.0

        self.get_logger().info('🧭 导航Action服务端已启动')

    def goal_callback(self, goal_request):
        """决定是否接受目标"""
        pose = goal_request.pose
        self.get_logger().info(
            f'收到导航目标: ({pose.pose.position.x:.2f}, '
            f'{pose.pose.position.y:.2f})'
        )
        return GoalResponse.ACCEPT

    def handle_accepted_callback(self, goal_handle):
        """目标被接受后开始执行"""
        self.get_logger().info('目标已接受，开始导航...')
        goal_handle.execute()

    def cancel_callback(self, goal_handle):
        """处理取消请求"""
        self.get_logger().warn('⚠️ 收到取消导航请求')
        return CancelResponse.ACCEPT

    async def execute_callback(self, goal_handle):
        """执行导航（模拟）"""
        target = goal_handle.request.pose.pose.position
        target_x, target_y = target.x, target.y

        # 计算总距离
        dx = target_x - self.current_x
        dy = target_y - self.current_y
        total_dist = math.sqrt(dx**2 + dy**2)
        step_size = 0.1  # 每步移动0.1m
        num_steps = max(int(total_dist / step_size), 1)

        self.get_logger().info(f'导航距离: {total_dist:.2f}m, 预计步数: {num_steps}')

        result = NavigateToPose.Result()
        feedback = NavigateToPose.Feedback()

        for step in range(num_steps):
            # 检查是否被取消
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                result.result = PoseStamped()
                self.get_logger().info('导航已取消')
                return result

            # 模拟移动
            progress = (step + 1) / num_steps
            self.current_x += dx / num_steps
            self.current_y += dy / num_steps

            # 计算剩余距离
            remaining = total_dist * (1.0 - progress)

            # 发布反馈
            feedback.current_pose = PoseStamped()
            feedback.current_pose.pose.position.x = self.current_x
            feedback.current_pose.pose.position.y = self.current_y
            feedback.distance_remaining = remaining
            feedback.navigation_time.sec = int(step * 0.1)
            feedback.number_of_recoveries = 0

            goal_handle.publish_feedback(feedback)

            if step % 10 == 0:
                self.get_logger().info(
                    f'导航进度: {progress*100:.0f}%, '
                    f'剩余: {remaining:.2f}m'
                )

            time.sleep(0.05)  # 模拟耗时

        # 导航完成
        goal_handle.succeed()
        result.result = PoseStamped()
        result.result.pose.position.x = target_x
        result.result.pose.position.y = target_y

        self.get_logger().info(f'✅ 导航完成！到达 ({target_x:.2f}, {target_y:.2f})')
        return result


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

if __name__ == '__main__':
    main()

🐍 Python：Action Client

#!/usr/bin/env python3
"""导航Action客户端 - 发送导航目标"""

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import PoseStamped


class NavigationActionClient(Node):
    """导航Action客户端"""

    def __init__(self):
        super().__init__('navigation_action_client')
        self._action_client = ActionClient(
            self, NavigateToPose, 'navigate_to_pose'
        )
        self._get_result_future = None
        self._send_goal_future = None

    def send_goal(self, x, y, theta=0.0):
        """发送导航目标"""
        self.get_logger().info(f'发送导航目标: ({x}, {y})')

        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = PoseStamped()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.pose.position.x = float(x)
        goal_msg.pose.pose.position.y = float(y)
        # 设置朝向
        import math
        goal_msg.pose.pose.orientation.z = math.sin(theta / 2)
        goal_msg.pose.pose.orientation.w = math.cos(theta / 2)

        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().error('❌ 目标被拒绝')
            return

        self.get_logger().info('✅ 目标已接受')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.result_callback)

    def feedback_callback(self, feedback_msg):
        """反馈回调"""
        feedback = feedback_msg.feedback
        self.get_logger().info(
            f'📍 反馈: 剩余距离 {feedback.distance_remaining:.2f}m, '
            f'当前位置 ({feedback.current_pose.pose.position.x:.2f}, '
            f'{feedback.current_pose.pose.position.y:.2f})'
        )

    def result_callback(self, future):
        """结果回调"""
        result = future.result().result
        status = future.result().status
        if status == 4:  # SUCCEEDED
            self.get_logger().info('🎉 导航成功完成！')
        elif status == 5:  # CANCELED
            self.get_logger().warn('⚠️ 导航被取消')
        else:
            self.get_logger().error(f'❌ 导航失败，状态: {status}')


def main(args=None):
    rclpy.init(args=None)
    client = NavigationActionClient()

    # 发送导航目标
    client.send_goal(3.0, 2.0, theta=1.57)

    rclpy.spin(client)
    client.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

📦 自定义Action类型

# my_interfaces/action/MoveRobot.action
# Goal（目标）
float64 target_x
float64 target_y
float64 target_theta
float64 max_speed
---
# Result（结果）
float64 final_x
float64 final_y
float64 final_theta
float64 distance_traveled
float64 time_elapsed
bool success
string message
---
# Feedback（反馈）
float64 current_x
float64 current_y
float64 current_theta
float64 distance_remaining
float64 speed
float64 progress  # 0.0 ~ 1.0

🔄 Action目标管理

目标状态机: ┌───────────┐ │ UNKNOWN │ └─────┬─────┘ │ send_goal ▼ ┌───────────┐ accepted ┌───────────┐ │ ACCEPTING │─────────────────►│ EXECUTING │ └─────┬─────┘ └─────┬─────┘ │ rejected │ succeed ▼ ▼ ┌───────────┐ ┌───────────┐ │ REJECTED │ │ SUCCEEDED │ └───────────┘ └───────────┘ ▲ ┌───────────┐ cancel_request │ │ CANCELING │────────────────────────┘ └─────┬─────┘ canceled │ ▼ ┌───────────┐ │ CANCELED │ └───────────┘ ▲ ┌───────────┐ abort │ │ EXECUTING │────────────────────────┘ └───────────┘ ABORTED

🔧 C++ Action Server

// move_robot_action_server.cpp
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "my_interfaces/action/move_robot.hpp"

#include <memory>
#include <thread>
#include <cmath>
#include <chrono>

using MoveRobot = my_interfaces::action::MoveRobot;
using GoalHandleMoveRobot = rclcpp_action::ServerGoalHandle<MoveRobot>;

class MoveRobotActionServer : public rclcpp::Node {
public:
    MoveRobotActionServer() : Node("move_robot_action_server") {
        action_server_ = rclcpp_action::create_server<MoveRobot>(
            this, "move_robot",
            std::bind(&MoveRobotActionServer::handle_goal, this,
                      std::placeholders::_1, std::placeholders::_2),
            std::bind(&MoveRobotActionServer::handle_cancel, this,
                      std::placeholders::_1),
            std::bind(&MoveRobotActionServer::handle_accepted, this,
                      std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "C++ MoveRobot Action服务端已启动");
    }

private:
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID&,
        std::shared_ptr<const MoveRobot::Goal> goal) {

        RCLCPP_INFO(this->get_logger(),
            "收到目标: (%.2f, %.2f, %.2f)",
            goal->target_x, goal->target_y, goal->target_theta);
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandleMoveRobot>) {
        RCLCPP_INFO(this->get_logger(), "收到取消请求");
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(
        const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
        std::thread{std::bind(&MoveRobotActionServer::execute, this,
                              std::placeholders::_1), goal_handle}.detach();
    }

    void execute(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
        auto goal = goal_handle->get_goal();
        double tx = goal->target_x, ty = goal->target_y;
        double total_dist = std::sqrt(tx*tx + ty*ty);

        auto feedback = std::make_shared<MoveRobot::Feedback>();
        auto result = std::make_shared<MoveRobot::Result>();

        int steps = std::max(static_cast<int>(total_dist / 0.1), 1);
        for (int i = 0; i < steps; ++i) {
            if (goal_handle->is_canceling()) {
                result->success = false;
                result->message = "Canceled";
                goal_handle->canceled(result);
                return;
            }

            double progress = static_cast<double>(i + 1) / steps;
            feedback->current_x = tx * progress;
            feedback->current_y = ty * progress;
            feedback->distance_remaining = total_dist * (1.0 - progress);
            feedback->progress = progress;
            goal_handle->publish_feedback(feedback);

            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        result->final_x = tx;
        result->final_y = ty;
        result->distance_traveled = total_dist;
        result->success = true;
        result->message = "Arrived!";
        goal_handle->succeed(result);
    }

    rclcpp_action::Server<MoveRobot>::SharedPtr action_server_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MoveRobotActionServer>());
    rclcpp::shutdown();
    return 0;
}

🖥️ Action命令行工具

命令功能
ros2 action list列出所有Action
ros2 action info /navigate_to_pose查看Action信息
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: ...}"发送目标
ros2 action send_goal --feedback /navigate_to_pose ...发送目标并显示反馈
ros2 action type /navigate_to_pose查看Action类型

🎯 练习题

📝 练习1：倒计时Action

创建一个倒计时Action：输入秒数，每秒发布剩余时间反馈，到0时返回结果。

📝 练习2：可取消的巡逻Action

创建巡逻Action：机器人按给定路径点循环巡逻，支持中途取消。

📝 练习3：多目标队列

客户端依次发送多个导航目标，每个完成后再发送下一个。统计总时间和距离。

🏆 成就解锁

🏅 Action通信专家

经验值：+200 XP

← 上一课：服务(Service) 📖 目录 下一课：参数(Parameter) →