ROS2动作通信（C++）

1.动作服务端实现

功能包cpp03_action的src目录下，新建C++文件demo01_action_server.cpp，并编辑文件，输入如下内容：

/*  
  需求：编写动作服务端实习，可以提取客户端请求提交的整型数据，并累加从1到该数据之间的所有整数以求和，
       每累加一次都计算当前运算进度并连续反馈回客户端，最后，在将求和结果返回给客户端。
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建动作服务端；
      3-2.处理请求数据；
      3-3.处理取消任务请求；
      3-4.生成连续反馈。
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。

*/
// 1.包含头文件；
#include"rclcpp/rclcpp.hpp"   //引入ROS2的rclcpp类
#include"rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/progress.hpp"    // 引入自定义动作消息头文件

using namespace std::placeholders;
using base_interfaces_demo::action::Progress;  // 使用base_interfaces_demo包中的Progress动作消息
using GoalHandleProgress = rclcpp_action::ServerGoalHandle<Progress>;  // 使用别名简化模板实例化

//3、定义节点类
class MinimalActionServer :public rclcpp::Node{
  public:
  //创建服务端
    MinimalActionServer():Node("wqh")
    {
      this->action_server_ = rclcpp_action::create_server<Progress>(
        this,
        "get_sum",
        /*
          rclcpp_action::Server<ActionT>::GoalCallback handle_goal, 
          rclcpp_action::Server<ActionT>::CancelCallback handle_cancel, 
          rclcpp_action::Server<ActionT>::AcceptedCallback handle_accepted, const
        */
       std::bind(&MinimalActionServer::handle_goal,this,_1,_2),
       std::bind(&MinimalActionServer::handle_cancel,this,_1),
       std::bind(&MinimalActionServer::handle_accepted,this,_1));
      RCLCPP_INFO(this->get_logger(),"动作服务端创建中，等待请求......");
    }

    private:
      rclcpp_action::Server<Progress>::SharedPtr action_server_;  //服务端的指针

      //3-2 处理请求数据                                                 
      rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const Progress::Goal> goal)
      {
        (void)uuid;  //不用的参数防止警告
        RCLCPP_INFO(this->get_logger(),"接收到动作客户端请求，请求数字为 %ld",goal->num);   //输出接收到的请求信息
        if(goal->num < 1)
        {
          return rclcpp_action::GoalResponse::REJECT;  //如果请求数字小于1，拒绝请求
        }
          return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;   //接受并执行请求
      }


      //3-3 处理取消任务请求
       rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleProgress> goal_handle)
      {
        (void)goal_handle;  //防止未使用的参数警告
        RCLCPP_INFO(this->get_logger(),"接收到任务取消请求");  //输出任务取消请求信息
        return rclcpp_action::CancelResponse::ACCEPT;
        
      } 

      //3-5 执行任务
      void execute(const std::shared_ptr<GoalHandleProgress> goal_handle)
      {
        RCLCPP_INFO(this->get_logger(),"开始执行任务");  //输出任务开始执行任务
        rclcpp::Rate loop_rate(10.0);  //设置循环频率为10HZ
        const auto goal = goal_handle->get_goal();  //获取目标信息
        auto feedback = std::make_shared<Progress::Feedback>();   //创建反馈信息的共享指针
        auto result = std::make_shared<Progress::Result>(); //创建结果消息的共享指针
        int64_t sum = 0;  //初始化变量


        for(int i = 1;(i <= goal->num) && rclcpp::ok();i++)
        {
          sum += i; //累加求和
          //检查是否有效取消请求
          if(goal_handle->is_canceling())
          {
            result->sum = sum; //设置结果的求和值
            goal_handle->canceled(result); //发布取消结果
            RCLCPP_INFO(this->get_logger(),"任务取消");  //输出任务取消的信息
            return;   //退出任务执行
          }

          feedback->progress = (double_t)i / goal->num;   //计算反馈的进度
          goal_handle->publish_feedback(feedback);  //发布反馈消息
          RCLCPP_INFO(this->get_logger(),"连续反馈中，进度 %.2f",feedback->progress);   //输出反馈进度信息

          loop_rate.sleep();   //循环休眠，控制频率
        }

        if(rclcpp::ok())
        {
          result -> sum = sum; //设置结果和求和值
          goal_handle->succeed(result);   //发布任务成功的结果
          RCLCPP_INFO(this->get_logger(),"任务完成！");   //输出任务完成的信息
        }

      }

      //3-4 处理目标接受
      void handle_accepted(const std::shared_ptr<GoalHandleProgress> goal_handle)
      {
        std::thread{std::bind(&MinimalActionServer::execute,this,_1),goal_handle}.detach();   //创建新线程执行任务
      }
};


int main(int argc,char *argv[])
{

  //初始化ROS节点
  rclcpp::init(argc,argv);

  //创建动作服务节点对象，并进入事件循环
  auto ss = std::make_shared<MinimalActionServer>();
  rclcpp::spin(ss);

  //释放
  rclcpp::shutdown();

}

2.动作客户端实现

功能包cpp03_action的src目录下，新建C++文件demo02_action_client.cpp，并编辑文件，输入如下内容：

/*  
  需求：编写动作客户端实现，可以提交一个整型数据到服务端，并处理服务端的连续反馈以及最终返回结果。
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.创建动作客户端；
      3-2.发送请求；
      3-3.处理目标发送后的反馈；
      3-4.处理连续反馈；
      3-5.处理最终响应。
    4.调用spin函数，并传入节点对象指针；
    5.释放资源。
*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"                              // 包含 ROS 2 C++ 客户端库的头文件
#include "rclcpp_action/rclcpp_action.hpp"                // 包含 ROS 2 动作库的头文件
#include "base_interfaces_demo/action/progress.hpp"       // 包含自定义的动作消息头文件

using base_interfaces_demo::action::Progress;            // 使用自定义动作消息 Progress
using GoalHandleProgress = rclcpp_action::ClientGoalHandle<Progress>;  // 定义使用 Progress 动作的目标句柄类型
using namespace std::placeholders;                      // 使用 C++ 标准库的占位符命名空间

// 3.定义节点类；
class MinimalActionClient : public rclcpp::Node           // 定义一个继承自 rclcpp::Node 的节点类
{
public:
  explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
  : Node("minimal_action_client", node_options)           // 节点构造函数，指定节点名称
  {
    // 3-1.创建动作客户端；
    this->client_ptr_ = rclcpp_action::create_client<Progress>(this, "get_sum");  // 创建 Progress 动作的客户端
  }

  // 3-2.发送请求；
  void send_goal(int64_t num)
  {
    if (!this->client_ptr_) {
      RCLCPP_ERROR(this->get_logger(), "动作客户端未被初始化。");  // 如果客户端未初始化，输出错误信息
    }

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(), "服务连接失败！");  // 等待服务端连接超时，输出错误信息
      return;
    }

    auto goal_msg = Progress::Goal();  // 创建一个 Progress 动作的目标消息对象
    goal_msg.num = num;                // 设置目标消息中的数值字段

    RCLCPP_INFO(this->get_logger(), "发送请求数据！");  // 输出信息，表示正在发送请求

    auto send_goal_options = rclcpp_action::Client<Progress>::SendGoalOptions();  // 创建发送目标的选项对象
    send_goal_options.goal_response_callback = std::bind(&MinimalActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback = std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback = std::bind(&MinimalActionClient::result_callback, this, _1);

    auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);  // 异步发送目标
  }

private:
  rclcpp_action::Client<Progress>::SharedPtr client_ptr_;  // 定义 Progress 动作的客户端指针

  // 3-3.处理目标发送后的反馈；
  void goal_response_callback(GoalHandleProgress::SharedPtr goal_handle)
  {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "目标请求被服务器拒绝！");  // 输出错误信息，表示目标请求被拒绝
    } else {
      RCLCPP_INFO(this->get_logger(), "目标被接收，等待结果中");  // 输出信息，表示目标已经被接收
    }
  }

  // 3-4.处理连续反馈；
  void feedback_callback(GoalHandleProgress::SharedPtr, const std::shared_ptr<const Progress::Feedback> feedback)
  {
    int32_t progress = (int32_t)(feedback->progress * 100);  // 计算并获取反馈中的进度百分比
    RCLCPP_INFO(this->get_logger(), "当前进度: %d%%", progress);  // 输出当前进度信息
  }

  // 3-5.处理最终响应。
  void result_callback(const GoalHandleProgress::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "任务被中止");  // 输出错误信息，表示任务被中止
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "任务被取消");  // 输出错误信息，表示任务被取消
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "未知异常");  // 输出错误信息，表示发生未知异常
        return;
    }

    RCLCPP_INFO(this->get_logger(), "任务执行完毕，最终结果: %ld", result.result->sum);  // 输出任务执行完毕的最终结果
  }
}; 

// 主函数
int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc, argv);  // 初始化 ROS 2 客户端

  // 4.调用 spin 函数，并传入节点对象指针；
  auto action_client = std::make_shared<MinimalActionClient>();  // 创建 MinimalActionClient 节点对象
  action_client->send_goal(10);  // 向服务端发送目标请求
  rclcpp::spin(action_client);   // 进入 ROS 2 的事件循环，处理消息和事件

  // 5.释放资源。
  rclcpp::shutdown();  // 关闭 ROS 2 客户端，释放资源
  return 0;  // 返回主函数退出状态
}

3.编辑配置文件

1.packages.xml

在创建功能包时，所依赖的功能包已经自动配置了，配置内容如下：

<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>base_interfaces_demo</depend>

2.CMakeLists.txt

CMakeLists.txt中服务端和客户端程序核心配置如下：

find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(base_interfaces_demo REQUIRED)

add_executable(demo01_action_server src/demo01_action_server.cpp)
ament_target_dependencies(
  demo01_action_server
  "rclcpp"
  "rclcpp_action"
  "base_interfaces_demo"
)

add_executable(demo02_action_client src/demo02_action_client.cpp)
ament_target_dependencies(
  demo02_action_client
  "rclcpp"
  "rclcpp_action"
  "base_interfaces_demo"
)

install(TARGETS 
  demo01_action_server
  demo02_action_client
  DESTINATION lib/${PROJECT_NAME})

4.编译

终端中进入当前工作空间，编译功能包：

colcon build --packages-select cpp03_action

5.执行

当前工作空间下，启动两个终端，终端1执行动作服务端程序，终端2执行动作客户端程序。

终端1输入如下指令：

. install/setup.bash
ros2 run cpp03_action demo01_action_server

终端2输入如下指令：

. install/setup.bash
ros2 run cpp03_action demo02_action_client

最终运行结果与案例类似。