ROS Action通信解读

一、Action通信概念

在 ROS2 中，Action 是一种特殊的通信方式，适用于长时间运行的任务，例如导航、运动控制等。Action 允许客户端和服务器端进行异步通信，并提供反馈和结果。

1.Action的通信结构：

ROS2 Action 使用的是 客户端-服务器 模型，类似于 ROS 服务（Service），但提供更复杂的功能。通信涉及多个话题和服务。

2.Action的组成部分：

目标：即Action客户端告诉服务端要做什么，服务端针对该目标要有响应。解决了不能确认服务端接收并处理目标问题

反馈：即Action服务端告诉客户端此时做的进度如何（类似与工作汇报）。解决执行过程中没有反馈问题

结果：即Action服务端最终告诉客户端其执行结果，结果最后返回，用于表示任务最终执行情况。

同时还要包括取消（客户端取消任务）和状态更新（客户端查询目标的当前状态）。

通过这张图可以看到，左右两边是Action的客户端和服务端节点，完成目标、反馈和结果的功能。这里目标采用的服务通信方式，action的客户端中的目标客户端发送目标请求给action服务端中的目标服务端，action服务端中的目标服务端再把回应发送给action的客户端中的目标客户端。

反馈采用的是话题通信的方式，action服务端的反馈发布者发布feedback话题，由action客户端中的反馈订阅者去订阅该话题获得反馈信息。

结果采用的是服务通信方式，action客户端中的结果客户端向action服务端中的结果服务端发送请求获取结果，由action服务端的结果服务端发送回应给action客户端中的结果客户端获得结果。

二、Action通信自定义接口

这里我们要完成的是机器人的移动控制，目标为移动的距离，结果是当前机器人的位置，反馈是机器人的位置和状态。

1.接口文件

接口文件为.action文件，放在接口功能包的action文件夹下方（src存放服务接口，msg存放话题接口）。

# Goal: 要移动的距离
float32 distance
---
# Result: 最终的位置
float32 pose
---
# Feedback: 中间反馈的位置和状态
float32 pose
uint32 status
uint32 STATUS_MOVEING = 3
uint32 STATUS_STOP = 4

一共三个部分，第一个部分是目标，第二个部分是结果，第三个部分是反馈，中间使用"---"隔开用以区分，这个---不能缺少。这里使用的是基础的数据类型来定义接口文件。

同样，我们需要ROS2的IDL转换器将接口文件转为python或者C++的头文件，这样我们在节点中就能使用接口文件。

2.代码转换

因此在cmakelists.txt文件中要添加内容

find_package(rosidl_default_generators REQUIRED)


rosidl_generate_interfaces(${PROJECT_NAME}
  "action/MoveRobot.action"
)

用find_package找到rosidl_default_generators包，这个包里面提供了 ROS 接口定义语言（IDL）生成工具。

接着我们使用rosidl_generate_interfaces来告诉 CMake 要生成哪些接口。

${PROJECT_NAME}：当前的 ROS 2 文件名，这里是.action文件。

之后在package.xml文件中，添加内容

  <depend>rosidl_default_generators</depend>
  <member_of_group>rosidl_interface_packages</member_of_group>

依赖rosidl_default_generators包，并且将当前包声明为 rosidl_interface_packages 组的成员，用于标记当前这个功能包主要是一个 **接口定义包，**通过这种方式，工具链可以针对接口包进行特殊处理，比如优化接口生成过程。

这样我们就定义好了我们的接口MoveRobot。

三、Action通信cpp实现

我们要建立两个节点，机器人节点（action服务端）和控制节点（action客户端），对应action_robot_01.cpp文件和action_control_01.cpp。由于机器人节点需要控制机器人移动和报告位置状态等功能，我们需要定义一个机器人类来封装功能的实现函数。将机器人类定义在头文件robot.h中，函数具体实现放在robot.cpp中。

因此功能包的目录框架为：

.
├── CMakeLists.txt
├── include
│   └── example_action_rclcpp
│       └── robot.h 
├── package.xml
└── src
    ├── action_control_01.cpp
    ├── action_robot_01.cpp
    └── robot.cpp

3 directories, 6 files

1.机器人类

1.头文件

头文件定义机器人类

cpp 复制代码

#ifndef EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#define EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_
#include "rclcpp/rclcpp.hpp"
#include "robot_control_interfaces/action/move_robot.hpp"

class Robot {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  Robot() = default;
  ~Robot() = default;
  float move_step(); /*移动一小步，请间隔500ms调用一次*/
  bool set_goal(float distance); /*移动一段距离*/
  float get_current_pose();
  int get_status();
  bool close_goal(); /*是否接近目标*/
  void stop_move();  /*停止移动*/

 private:
  float current_pose_ = 0.0;             /*声明当前位置*/
  float target_pose_ = 0.0;              /*目标距离*/
  float move_distance_ = 0.0;            /*目标距离*/
  std::atomic<bool> cancel_flag_{false}; /*取消标志*/
  int status_ = MoveRobot::Feedback::STATUS_STOP;
};

#endif  // EXAMPLE_ACTIONI_RCLCPP_ROBOT_H_

该类的框架为：

cpp 复制代码

成员变量：
    float current_pose_    记录当前位置
    float target_pose_    记录目标位置
    float move_distance_    记录当前位置到目标位置还需移动的距离
    std::atomic<bool> cancel_flag_    取消标识，用来检查任务是否被中止
    int status_    记录当前的状态，处于停止还是移动中
成员函数：
    构造与析构函数
    float move_step();    用于完成移动距离的操作
    bool set_goal(float distance);    用于设置目标的位置，和move_step()配合使用，设置并移动
    float get_current_pose();    用于获取当前的位置
    int get_status();    用于获取当前的状态
    bool close_goal();    判断是否接近目标，判断当前位置与目标位置的距离是否在一个范围内
    void stop_move();    设置状态为停止移动

2.函数的实现

robot.cpp文件中，实现函数功能

cpp 复制代码

#include "example_action_rclcpp/robot.h"

float Robot::move_step() {
  int direct = move_distance_ / fabs(move_distance_);
  float step = direct * fabs(target_pose_ - current_pose_) *
               0.1; /* 每一步移动当前到目标距离的1/10*/
  current_pose_ += step;
  std::cout << "移动了：" << step << "当前位置：" << current_pose_ << std::endl;
  return current_pose_;
}


bool Robot::set_goal(float distance) {
  move_distance_ = distance;
  target_pose_ += move_distance_;

  /* 当目标距离和当前距离大于0.01同意向目标移动 */
  if (close_goal()) {
    status_ = MoveRobot::Feedback::STATUS_STOP;
    return false;
  }
  status_ = MoveRobot::Feedback::STATUS_MOVEING;
  return true;
}

float Robot::get_current_pose() { return current_pose_; }
int Robot::get_status() { return status_; }
bool Robot::close_goal() { return fabs(target_pose_ - current_pose_) < 0.01; }
void Robot::stop_move() {
  status_ = MoveRobot::Feedback::STATUS_STOP;
}

1.move_step()函数：

首先move_distance_ / fabs(move_distance_)来确定移动方向，分为正负方向。

float step = direct * fabs(target_pose_ - current_pose_) * 0.1;每次的步长为到目标距离的长度的1/10。

完成移动后更新当前位置current_pose_，并输出内容到终端，最后返回current_pose_当前位置的值。

2.set_goal(float distance)函数：

首先将成员变量中的需要移动的距离move_distance_设置为传入的distance值，再将目标位置target_pose_的值加上distance用于更新target_pose_的值。

接着使用 close_goal() 检查目标位置与当前位置的差值是否小于 0.01，小于0.01时，将机器人的状态设置为STATUS_STOP停止状态，返回一个bool值false，否则将机器人的状态设置为STATUS_MOVEING移动，返回一个bool值true这样可以设置目标位置，并判断是否可以开始移动。

剩下的函数简单的返回和设置成员变量的值。

2.机器人节点（action服务端）

机器人节点的代码为：

cpp 复制代码

class ActionRobot01 : public rclcpp::Node {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ServerGoalHandle<MoveRobot>;

  explicit ActionRobot01(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    using namespace std::placeholders;  // NOLINT

    this->action_server_ = rclcpp_action::create_server<MoveRobot>(
        this, "move_robot",
        std::bind(&ActionRobot01::handle_goal, this, _1, _2),
        std::bind(&ActionRobot01::handle_cancel, this, _1),
        std::bind(&ActionRobot01::handle_accepted, this, _1));
  }

 private:
  Robot robot;
  rclcpp_action::Server<MoveRobot>::SharedPtr action_server_;

  rclcpp_action::GoalResponse handle_goal(
      const rclcpp_action::GoalUUID& uuid,
      std::shared_ptr<const MoveRobot::Goal> goal) {
    RCLCPP_INFO(this->get_logger(), "Received goal request with distance %f",
                goal->distance);
    (void)uuid;
    if (fabs(goal->distance > 100)) {
      RCLCPP_WARN(this->get_logger(), "目标距离太远了，本机器人表示拒绝！");
      return rclcpp_action::GoalResponse::REJECT;
    }
    RCLCPP_INFO(this->get_logger(),
                "目标距离%f我可以走到，本机器人接受，准备出发！",
                goal->distance);
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(
      const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
    (void)goal_handle;
    robot.stop_move(); /*认可取消执行，让机器人停下来*/
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void execute_move(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    const auto goal = goal_handle->get_goal();
    RCLCPP_INFO(this->get_logger(), "开始执行移动 %f 。。。", goal->distance);

    auto result = std::make_shared<MoveRobot::Result>();
    rclcpp::Rate rate = rclcpp::Rate(2);
    robot.set_goal(goal->distance);
    while (rclcpp::ok() && !robot.close_goal()) {
      robot.move_step();
      auto feedback = std::make_shared<MoveRobot::Feedback>();
      feedback->pose = robot.get_current_pose();
      feedback->status = robot.get_status();
      goal_handle->publish_feedback(feedback);
      /*检测任务是否被取消*/
      if (goal_handle->is_canceling()) {
        result->pose = robot.get_current_pose();
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal Canceled");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Publish Feedback"); /*Publish feedback*/
      rate.sleep();
    }

    result->pose = robot.get_current_pose();
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "Goal Succeeded");
  }

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

该节点的主要功能是接受目标距离，控制虚拟机器人逐步移动到目标位置，并提供实时反馈和结果。

节点ActionRobot01类的框架

cpp 复制代码

成员变量：
     Robot robot    Robot 类的实例，用于实际执行移动操作
     action_server_    Action服务器对象，处理客户端的请求、取消和反馈操作
成员函数：
    构造函数，创建一个MoveRobot类型的Action服务端，并绑定回调函数
    handle_goal，接收客户端的目标请求，并做系列的判断来决定是否执行
    handle_cancel，取消客户端的请求，停止机器人的移动
    execute_move，执行目标任务并进行反馈
    handle_accepted，启动一个新的线程来异步执行目标任务，调用 execute_move() 执行移动操作

1.构造函数：

cpp 复制代码

explicit ActionRobot01(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    using namespace std::placeholders;  // NOLINT

    this->action_server_ = rclcpp_action::create_server<MoveRobot>(
        this, "move_robot",
        std::bind(&ActionRobot01::handle_goal, this, _1, _2),
        std::bind(&ActionRobot01::handle_cancel, this, _1),
        std::bind(&ActionRobot01::handle_accepted, this, _1));
  }

传入参数为节点的名称name，进入构造函数，首先使用 RCLCPP_INFO 打印日志，表示节点已成功启动。

创建服务端

cpp 复制代码

this->action_server_ = rclcpp_action::create_server<MoveRobot>(...)

调用 rclcpp_action::create_server方法创建服务端，<MoveRobot>是任务类型接口，即我们之前创建的MoveRobot.action文件。

通过API文档可以看到传入参数的信息

第一个参数需要传入我们的节点，服务端将绑定为这个节点，这里我们传入this即我们创建的这个ActionRobot01节点指针。

第二个参数要传入我们的action名称，这里为"move_robot"

第三个参数要传入handle_goal的回调函数，用于处理客户端的目标请求是否被接受，这里传入为：

cpp 复制代码

std::bind(&ActionRobot01::handle_goal, this, _1, _2)

传入成员函数handle_goal，并将 handle_goal 成员函数绑定到 Action 服务器的 "接收目标请求" 回调函数上，后面的_1, _2 是占位符，表示传入回调函数的参数。

第四个参数要传入handle_cancel的回调函数，用于收到取消运行请求，可以取消则返回ACCEPT，不可以返回REJECT。这里传入：

cpp 复制代码

std::bind(&ActionRobot01::handle_cancel, this, _1)

传入成员函数handle_cancel，并将 handle_cancel 成员函数绑定到 Action 服务器的 "接收取消请求" 回调函数，后面_1是占位符，表示传入回调函数的参数。

第五个参数要传入handle_accepted，处理接受请求，当handle_goal中对移动请求ACCEPT后则进入到这里进行执行。这里传入：

cpp 复制代码

std::bind(&ActionRobot01::handle_accepted, this, _1))

传入成员函数handle_accepted，并将 handle_accepted 成员函数绑定到 Action 服务器的"目标请求被接受"回调函数，后面_1是占位符，表示传入回调函数的参数。

2.handle_goal函数：

cpp 复制代码

rclcpp_action::GoalResponse handle_goal(
      const rclcpp_action::GoalUUID& uuid,
      std::shared_ptr<const MoveRobot::Goal> goal) {
    RCLCPP_INFO(this->get_logger(), "Received goal request with distance %f",
                goal->distance);
    (void)uuid;
    if (fabs(goal->distance > 100)) {
      RCLCPP_WARN(this->get_logger(), "目标距离太远了，本机器人表示拒绝！");
      return rclcpp_action::GoalResponse::REJECT;
    }
    RCLCPP_INFO(this->get_logger(),
                "目标距离%f我可以走到，本机器人接受，准备出发！",
                goal->distance);
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

函数原型为：

cpp 复制代码

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

返回类型是rclcpp_action::GoalResponse，这是一个枚举类型，表示对目标请求的响应，常见的响应值包括：

ACCEPT_AND_EXECUTE：接受目标请求，并开始执行。

REJECT：拒绝目标请求，通常用于目标请求不符合要求的情况（如目标超出范围）。

ACCEPT：接受目标请求，但不立即执行，一般情况下不会单独使用，通常用在需要进一步处理的情况下。

传入的第一个参数：

cpp 复制代码

const rclcpp_action::GoalUUID& uuid

uuid 是一个 Goal UUID，即目标请求的唯一标识符（UUID）。在 ROS 2 中，每个目标请求都有一个唯一的标识符，uuid 使得服务器能够区分和管理不同的目标请求。rclcpp_action::GoalUUID 是一个别名，通常是一个 std::array<uint8_t, 16> 类。使用const &的常量引用，表示该参数不会被修改，同时避免了不必要的拷贝操作。

传入的第二个参数：

cpp 复制代码

std::shared_ptr<const MoveRobot::Goal> goal

goal 是一个智能指针类型，指向客户端发送的目标请求的数据。它包含了实际的目标信息，这里是一个指向 MoveRobot::Goal 类型的指针。MoveRobot::Goal是我们之前定义的类型，包含了distance数据。

进入函数中，首先打印日志信息，内容为接收到的目标请求。

(void)uuid；由于 uuid 在此函数中没有被使用，通过 (void)uuid; 忽略了它，这是一种防止编译器警告的做法。

cpp 复制代码

if (fabs(goal->distance > 100)) {
    RCLCPP_WARN(this->get_logger(), "目标距离太远了，本机器人表示拒绝！");
    return rclcpp_action::GoalResponse::REJECT;
}

判断当前距离是否过大，如果是则返回REJECT。

cpp 复制代码

RCLCPP_INFO(this->get_logger(), "目标距离%f我可以走到，本机器人接受，准备出发！", goal->distance);
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;

否则打印当前的目标信息，并返回ACCEPT_AND_EXECUTE，即接受并开始执行。

3.handle_cancel函数：

函数原型为：

cpp 复制代码

rclcpp_action::CancelResponse handle_cancel(
      const std::shared_ptr<GoalHandleMoveRobot> goal_handle)

返回类型是rclcpp_action::CancelResponse，这是一个枚举类型，用于告诉action服务端是否成功处理了取消请求，有两个枚举值：

CancelResponse::ACCEPT: 接受取消请求并执行取消操作。

CancelResponse::REJECT: 拒绝取消请求，不执行任何操作。

参数类型是 std::shared_ptr<GoalHandleMoveRobot>，这是一个 GoalHandle 的智能指针，指针类型为GoalHandleMoveRobot，GoalHandleMoveRobot 是 ROS 2 动作（Action）框架中的一个模板类，专门用于管理与动作相关的目标（Goal）。通过goal_handle指针可以来调用GoalHandleMoveRobot类的成员函数，如：

get_goal(): 获取目标对象。

is_canceling(): 检查目标是否正在取消。

publish_feedback(): 发布目标的反馈。

succeed(): 标记目标成功完成。

canceled(): 标记目标被取消。

这样就可以通过 goal_handle 来访问当前正在执行的目标信息。

进入函数首先输出日志信息，表示接收到取消请求。

cpp 复制代码

(void)goal_handle;

这行代码将 goal_handle 转换为 (void)，表示故意不使用它。这是为了避免编译器发出未使用变量的警告。

调用 robot.stop_move() 来让机器人停止其当前的动作，stop_move()会让机器人的状态变为STATUS_STOP。

最后返回 rclcpp_action::CancelResponse::ACCEPT，表示取消请求已被接受并执行了取消操作。

4.handle_accepted函数：

cpp 复制代码

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

函数原型：

cpp 复制代码

void handle_accepted(const std::shared_ptr<GoalHandleMoveRobot> goal_handle)

该函数接受一个 std::shared_ptr<GoalHandleMoveRobot> 类型的参数 goal_handle。

cpp 复制代码

std::thread{ ... }.detach();

用于创建一个新线程来执行传入的可调用对象。detach() 是 std::thread 类中的一个成员函数，它会将线程从主线程分离，意味着主线程不会等待这个新线程完成。

cpp 复制代码

std::bind(&ActionRobot01::execute_move, this, _1)

创建了一个新的可调用对象，指向 ActionRobot01::execute_move 方法，并绑定了当前对象 this 和占位符 _1，表示该函数需要一个参数（即 goal_handle）。

这句代码创建了一个新线程来异步执行 execute_move 函数，并将 goal_handle 作为参数传递给它。

5.execute_move函数：

execute_move是在 ActionRobot01 类中处理实际执行机器人移动操作的函数。它在新线程中执行，并通过 goal_handle 来与客户端通信，报告执行状态、反馈信息以及任务结果。

cpp 复制代码

void execute_move(const std::shared_ptr<GoalHandleMoveRobot> goal_handle) {
    const auto goal = goal_handle->get_goal();
    RCLCPP_INFO(this->get_logger(), "开始执行移动 %f 。。。", goal->distance);

    auto result = std::make_shared<MoveRobot::Result>();
    rclcpp::Rate rate = rclcpp::Rate(2);
    robot.set_goal(goal->distance);
    while (rclcpp::ok() && !robot.close_goal()) {
      robot.move_step();
      auto feedback = std::make_shared<MoveRobot::Feedback>();
      feedback->pose = robot.get_current_pose();
      feedback->status = robot.get_status();
      goal_handle->publish_feedback(feedback);
      /*检测任务是否被取消*/
      if (goal_handle->is_canceling()) {
        result->pose = robot.get_current_pose();
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal Canceled");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Publish Feedback"); /*Publish feedback*/
      rate.sleep();
    }

    result->pose = robot.get_current_pose();
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "Goal Succeeded");
  }

首先使用goal_handle的成员函数get_goal()来获取当前目标任务的 goal 对象，goal 包含了客户端请求的目标信息，这里主要是目标的距离（distance）。之后输出日志信息开始执行和距离信息distance。

cpp 复制代码

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

这里创建一个共享指针，指向 MoveRobot::Result 类型的结果对象。这个对象将保存任务执行完成后的结果（如当前位置等信息），并在任务完成后返回给客户端。

然后设置执行频率为2，即每秒2次。

使用robot的成员函数来设置目标robot.set_goal(goal->distance)，这里传入的是目标的距离 goal->distance。

之后进入循环，来循环移动机器人

cpp 复制代码

while (rclcpp::ok() && !robot.close_goal()) {
      robot.move_step();
      auto feedback = std::make_shared<MoveRobot::Feedback>();
      feedback->pose = robot.get_current_pose();
      feedback->status = robot.get_status();
      goal_handle->publish_feedback(feedback);
      /*检测任务是否被取消*/
      if (goal_handle->is_canceling()) {
        result->pose = robot.get_current_pose();
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal Canceled");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Publish Feedback"); /*Publish feedback*/
      rate.sleep();
    }

判断条件为节点状态良好，并且还没有接近目标。

进入循环，首先使用成员函数robot.move_step()，机器人根据设定的目标移动一步。该方法会同时会更新机器人的当前位置current_pose_。

std::make_shared<MoveRobot::Feedback>(): 创建一个共享指针，指向 MoveRobot::Feedback 类型的反馈对象。这个对象将包含机器人的当前位置和状态，用来在每次循环中发送反馈。

cpp 复制代码

feedback->pose = robot.get_current_pose();
feedback->status = robot.get_status();

将反馈数据的pose和status进行设置。

cpp 复制代码

goal_handle->publish_feedback(feedback);

调用成员方法publish_feedback来发布反馈信息，客户端可以通过这个反馈信息了解任务执行的进度（例如当前机器人的位置和状态）。

cpp 复制代码

if (goal_handle->is_canceling()) {
    result->pose = robot.get_current_pose();
    goal_handle->canceled(result);
    RCLCPP_INFO(this->get_logger(), "Goal Canceled");
    return;
}

如果当前任务被取消了，则设置结果，设置 result 对象的 pose 字段为机器人当前的位置，以便在任务取消时返回。

cpp 复制代码

goal_handle->canceled(result);

向客户端发送任务取消的反馈，并传递结果对象（包含当前位置等信息）。并输出日志信息。

最后rate.sleep(): 使得当前线程暂停于用以保持循环的频率为 2 Hz。

退出循环后，即任务完成，已经接近目标。

cpp 复制代码

result->pose = robot.get_current_pose();

更新 result 对象的 pose 字段为机器人的最终位置。

goal_handle->succeed(result)用于向客户端发送任务成功的反馈，并传递结果对象。客户端将接收到任务完成的状态和结果，并输出日志信息。

6.整体的流程

cpp 复制代码

+--------------------------------+
|         主程序启动             |
+--------------------------------+
              |
              v
+--------------------------------+
| 初始化 ActionRobot01           |
| 创建 Action 服务器 "move_robot" |
| 绑定回调函数:                  |
| - handle_goal()                |
| - handle_cancel()              |
| - handle_accepted()            |
+--------------------------------+
              |
              v
+--------------------------------+
| 等待客户端发送目标请求         |
+--------------------------------+
              |
        [收到目标请求]
              |
              v
+--------------------------------+
| handle_goal():                 |
| - 检查目标距离是否合法          |
|   (fabs(goal->distance) > 100) |
| - 合法: ACCEPT_AND_EXECUTE     |
| - 不合法: REJECT               |
+--------------------------------+
              |
        [目标被接受?]
         /       \
       是          否
       |           |
       v           v
+--------------------------------+   +------------------------------+
| handle_accepted():             |   | 拒绝目标，返回错误信息       |
| - 启动 execute_move() 执行任务 |   +------------------------------+
| - 在新线程中执行               |
+--------------------------------+
              |
              v
+--------------------------------+
| execute_move():                |
| - 控制机器人移动               |
| - 发布反馈 (当前位置/状态)      |
+--------------------------------+
              |
              v
+--------------------------------+
| [检测取消请求]                 |
| goal_handle->is_canceling() ?  |
| - 是: 取消任务，停止机器人      |
| - 否: 继续执行任务             |
+--------------------------------+
              |
       [目标是否达到?]
         /          \
       是            否
       |             |
       v             v
+--------------------------------+   +------------------------------+
| 任务成功:                      |   | 继续移动机器人               |
| - 返回结果 (最终位置 pose)       |   | 发布反馈                     |
| - goal_handle->succeed(result) |   +------------------------------+
+--------------------------------+
              |
              v
+--------------------------------+
| 任务完成，等待下一个请求        |
+--------------------------------+

3.控制节点

控制节点的代码为：

cpp 复制代码

class ActionControl01 : public rclcpp::Node {
 public:
  using MoveRobot = robot_control_interfaces::action::MoveRobot;
  using GoalHandleMoveRobot = rclcpp_action::ClientGoalHandle<MoveRobot>;

  explicit ActionControl01(
      std::string name,
      const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions())
      : Node(name, node_options) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    this->client_ptr_ =
        rclcpp_action::create_client<MoveRobot>(this, "move_robot");

    this->timer_ =
        this->create_wall_timer(std::chrono::milliseconds(500),
                                std::bind(&ActionControl01::send_goal, this));
  }

  void send_goal() {
    using namespace std::placeholders;

    this->timer_->cancel();

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(),
                   "Action server not available after waiting");
      rclcpp::shutdown();
      return;
    }

    auto goal_msg = MoveRobot::Goal();
    goal_msg.distance = 10;

    RCLCPP_INFO(this->get_logger(), "Sending goal");

    auto send_goal_options =
        rclcpp_action::Client<MoveRobot>::SendGoalOptions();
    send_goal_options.goal_response_callback =
        std::bind(&ActionControl01::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
        std::bind(&ActionControl01::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
        std::bind(&ActionControl01::result_callback, this, _1);
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

 private:
  rclcpp_action::Client<MoveRobot>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;

  void goal_response_callback(GoalHandleMoveRobot::SharedPtr goal_handle) {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(),
                  "Goal accepted by server, waiting for result");
    }
  }

  void feedback_callback(
      GoalHandleMoveRobot::SharedPtr,
      const std::shared_ptr<const MoveRobot::Feedback> feedback) {
    RCLCPP_INFO(this->get_logger(), "Feedback current pose:%f", feedback->pose);
  }

  void result_callback(const GoalHandleMoveRobot::WrappedResult& result) {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
        return;
    }

    RCLCPP_INFO(this->get_logger(), "Result received: %f", result.result->pose);
    // rclcpp::shutdown();
  }
};  // class ActionControl01

控制节点类ActionControl01的框架为：

cpp 复制代码

成员变量：
    client_ptr_    action客户端，用于与服务端通信
    timer_    定时器，定时触发send_goal()
成员函数：
    构造函数，创建了action客户端和定时器
    send_goal，向服务端发送目标
    goal_response_callback，处理目标发送后接受到服务端的响应
    feedback_callback，接受并处理实时的反馈信息
    result_callback，处理服务端的结果

1.构造函数：

cpp 复制代码

explicit ActionControl01(
      std::string name,
      const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions())
      : Node(name, node_options) {
    RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());

    this->client_ptr_ =
        rclcpp_action::create_client<MoveRobot>(this, "move_robot");

    this->timer_ =
        this->create_wall_timer(std::chrono::milliseconds(500),
                                std::bind(&ActionControl01::send_goal, this));
  }

传入参数为name节点名称，node_options 可选的配置项（如设置命名空间等）。

进入构造函数，首先打印日志信息，节点已经启动。

调用action的方法create_client创建客户端。

该函数API要求传入三个参数，<MoveRobot>是我们的action接口类型，即我们之前定义的MoveRobot.action文件。

第一个参数是节点，客户端将绑定为这个节点，这里我们传入this即我们创建的这个ActionControl01节点指针。

第二个参数是action的类型名称，这里传入和之前服务端一样的action名称"move robot"。

第三个参数有默认值，不用传入。

之后使用create_wall_timer方法，创建定时器，传入两个参数，

第一个参数是定时器的周期，这里传入500ms。

第二个传入的是回调函数，这里将定时器触发时的回调函数与类的成员函数send_goal 绑定。

2.send_goal函数：

cpp 复制代码

void send_goal() {
    using namespace std::placeholders;

    this->timer_->cancel();

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(),
                   "Action server not available after waiting");
      rclcpp::shutdown();
      return;
    }

    auto goal_msg = MoveRobot::Goal();
    goal_msg.distance = 10;

    RCLCPP_INFO(this->get_logger(), "Sending goal");

    auto send_goal_options =
        rclcpp_action::Client<MoveRobot>::SendGoalOptions();
    send_goal_options.goal_response_callback =
        std::bind(&ActionControl01::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =
        std::bind(&ActionControl01::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
        std::bind(&ActionControl01::result_callback, this, _1);
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

进入函数，首先

cpp 复制代码

this->timer_->cancel();

取消了 500 毫秒触发的定时器，保证 send_goal 只执行一次。

然后等待服务端的准备

cpp 复制代码

this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))

wait_for_action_server 会阻塞程序，直到动作服务器就绪或者超时（这里设为最大 10 秒）。

当服务器没有准备就绪时，会打印错误信息并关闭程序。

当服务器准备就绪后，构建目标，

cpp 复制代码

auto goal_msg = MoveRobot::Goal();
goal_msg.distance = 10;

创建一个我们定义的接口文件中goal类型的结构体goal_msg，将distance设为10。

然后进行回调函数的配置：

cpp 复制代码

auto send_goal_options =
        rclcpp_action::Client<MoveRobot>::SendGoalOptions();
send_goal_options.goal_response_callback =
        std::bind(&ActionControl01::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
        std::bind(&ActionControl01::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
        std::bind(&ActionControl01::result_callback, this, _1);

首先创建了一个结构体send_goal_options，类型为rclcpp_action::Client<MoveRobot>::SendGoalOptions()，用于配置发送动作目标时的选项和回调函数。

配置了三个回调函数，

goal_response_callback：当服务器接收或拒绝目标时调用。

feedback_callback：服务器执行目标时，周期性返回执行过程中的反馈。

result_callback：服务器完成目标后返回最终的执行结果。

三个回调函数都使用std::bind 将类的成员函数绑定到回调接口上，this 指针用于调用当前类的成员函数。

最后调用客户端的async_send_goal(goal_msg, send_goal_options)方法，异步地将目标发送到动作服务器。传入参数为goal结构体goal_msg和发送目标的配置send_goal_options。

3.goal_response_callback函数：

cpp 复制代码

void goal_response_callback(GoalHandleMoveRobot::SharedPtr goal_handle) {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(),
                  "Goal accepted by server, waiting for result");
    }
  }

传入的参数goal_handle用于服务端和客户端的通信，之前的handle_goal函数，拒绝时会返回枚举值REJECT，如果服务器返回 REJECT，客户端的 goal_handle 就会是 nullptr。

当被拒绝时，打印日志信息目标被服务器拒绝，否则打印日志信息目标被服务器接受，等待结果。

4.feedback_callback函数：

cpp 复制代码

void feedback_callback(
      GoalHandleMoveRobot::SharedPtr,
      const std::shared_ptr<const MoveRobot::Feedback> feedback) {
    RCLCPP_INFO(this->get_logger(), "Feedback current pose:%f", feedback->pose);
  }

之前服务端使用goal_handle->publish_feedback(feedback);来发送反馈信息到客户端，这里传入参数为：

GoalHandleMoveRobot::SharedPtr，用于确认接收反馈的目标，但在此函数中没有使用，因此没有参数名称。

const std::shared_ptr<const MoveRobot::Feedback> feedback，是服务端发送的反馈消息。

进入函数后，打印日志信息feedback中传来的当前机器人的位置信息。

5.result_callback函数：

cpp 复制代码

void result_callback(const GoalHandleMoveRobot::WrappedResult& result) {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
        return;
    }

    RCLCPP_INFO(this->get_logger(), "Result received: %f", result.result->pose);
    // rclcpp::shutdown();
  }

传入参数为result，action目标最终的执行结果。类型为GoalHandleMoveRobot::WrappedResult，封装了动作目标的最终执行结果，包括：

result.code：表示动作的执行状态（成功、失败、取消等）。

result.result：包含具体的结果数据，是服务器返回的 MoveRobot::Result 结构体。

进入函数后，使用switch来判断执行的状态。

cpp 复制代码

switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
        return;
    }

包含以下几种状态：

SUCCEEDED：目标成功完成。直接退出switch，执行RCLCPP_INFO(this->get_logger(), "Result received: %f", result.result->pose)，打印最后机器人的位置。

ABORTED：目标执行失败，被服务器中止。打印服务器被中止的信息，提前结束函数。

CANCELED：目标被客户端取消。打印客户端被取消的信息，提前结束函数。

其他状态：未知的执行状态。打印未知状态的信息，提前结束函数。

6.整体的流程

cpp 复制代码

+-------------------------+
|   主程序启动            |
+-------------------------+
            |
            v
+-------------------------+
| 初始化 ActionControl01  |
| 创建 Action 客户端       |
| 创建定时器 (500ms)       |
+-------------------------+
            |
            v
+-------------------------+
| 定时器触发 send_goal()  |
| - 取消定时器            |
| - 等待 Action 服务器      |
+-------------------------+
            |
      [服务器可用?]
      /        \
   是            否
   |             |
   v             v
+----------------------+  +-------------------------+
| 构造 goal_msg        |  | 日志：服务器不可用       |
| 设置目标距离 = 10     |  | 关闭节点 rclcpp::shutdown |
+----------------------+  +-------------------------+
            |
            v
+-------------------------------+
| 发送目标 async_send_goal()    |
| - 设置回调函数                |
|   * goal_response_callback()  |
|   * feedback_callback()       |
|   * result_callback()         |
+-------------------------------+
            |
            v
+---------------------------------+
| 回调函数处理：                  |
|                                 |
| goal_response_callback():       |
| - 检查目标是否被接受            |
| - 日志输出                     |
+---------------------------------+
            |
            v
+---------------------------------+
| feedback_callback():            |
| - 接收服务器反馈                |
| - 输出当前位置 feedback->pose    |
+---------------------------------+
            |
            v
+---------------------------------+
| result_callback():              |
| - 检查服务器返回的结果状态       |
| - 日志输出 (成功/失败/取消)      |
| - 输出最终位置 result->pose      |
+---------------------------------+
            |
            v
+--------------------------+
|     任务执行完成         |
+--------------------------+