ROS2: 手把手教你动作通信案例实现（服务端篇C++)

背影疾风

已于 2025-05-22 10:24:23 修改

阅读量1k

点赞数 13

CC 4.0 BY-SA版权

分类专栏： ROS学习文章标签： c++ 开发语言

于 2025-05-20 18:27:39 首次发布

本文链接：https://blog.youkuaiyun.com/weixin_54358182/article/details/147798924

ROS学习专栏收录该内容

8 篇文章

订阅专栏

ROS动作通信概念

前面两节介绍了话题通信和服务通信，而动作通信则更像二者的融合体。根据官方wiki的介绍来看，Action通信方式的出现是为了解决服务通信在执行长时间耗时任务时不会向客户端反馈进度这一问题的。
简而言之，动作通信分为三部分：目标请求，结果响应，过程反馈。模型如下图：
在这里插入图片描述

运作原理为：

客户端发送目标请求，服务端反馈目标的接收结果:ACCPET or REJECT（服务通信1）
客户端接收到请求被接受的响应后，向服务端再发送一次结果请求。（服务通信2）
服务端接收结果请求后开始执行任务，期间向客户端反馈执行的数据。（话题通信）
任务执行完毕后，服务端向客户端响应执行结果。（服务通信2）

以上过程心中有数即可，具体实现代码ROS2已经封装好，会使用API即可。

动作通信实现

一、接口文件创建与编译

1. 首先创建工作空间

mkdir -p ws02_action_test/src

2. 创建接口文件功能包（可以不依赖任何包）

ros2 pkg create based_interfaces --build-type ament_cmake

3. 在功能包文件夹下创建，action文件夹，并创建`.action`文件，格式如下

#请求
int32 tar
---
#响应
int32 result
---
#反馈
int32 temp
string state

此时工作目录层次结构如下：
在这里插入图片描述

4. 再更改package.xml文件，添加需要的依赖：

  <buildtool_depend>rosidl_defualt_generators</buildtool_depend><!-- 构建依赖 -->

  <exec_depend>rosidl_default_runtime</exec_depend><!-- 运行依赖 -->

  <member_of_group>rosidl_interface_packages</member_of_group><!-- 组依赖 -->

5. 最后更改CmakeList.txt文件，添加编译依赖：

find_package(rosidl_default_generators REQUIRED)

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

6. 在工作空间目录下（即ws02_action_test/.）进行编译

colcon build --packages-select based_interfaces

编译完成后，可以使用命令行工具查看接口是否成功生成：

ros2 interface show based_interfaces/action/Myaction

应该会输出：

#请求
int32 tar
---
#响应
int32 result
---
#反馈
int32 temp
string state

二、动作服务端的实现

从前面服务端的任务有以下几个：

响应客户端的目标请求。
目标接受后执行任务（在执行过程中反馈信息，执行完成后返回最终结果）。
响应客户端的取消请求。

以上每个任务都会对应一个服务回调函数。

1. 创建服务端功能包（依赖于rclcpp、rclcpp_action、接口包）

ros2 pkg create demo01_action_test --build-type ament_cmake --dependencies based_interfaces rclcpp rclcpp_action

2. 修改CmakeList.txt文件，使之能成功编译。（三步：add_executable,ament_target_dependencies,install）

add_executable(cpp01_server src/cpp01_server.cpp)

ament_target_dependencies(
  cpp01_server
  "rclcpp"
  "rclcpp_action"
  "based_interfaces"
)

install(TARGETS
  cpp01_server
  DESTINATION lib/${PROJECT_NAME})

3. 编写核心业务实现（节点类）

先写个简单的节点demo，验证一下编译文件的编写和配置是正确的。

#include "rclcpp/rclcpp.hpp"
class My_Action : public rclcpp::Node{
private:

public:
    My_Action():Node("action_server_node"){
        RCLCPP_INFO(this->get_logger(),"动作服务端节点创建完成！");
    }
};


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

在工作空间下编译：

colcon build --packages-select demo01_action_test

执行一下：

ros2 run demo01_action_test cpp01_server

此时终端应该会输出"动作服务端节点创建完成！",至此一个简单的节点就创建完成了！

核心：三大回调函数的编写

下面这段代码是从ROS2源码中摘抄出来的，代表着最官方的回调函数格式：

/// Signature of a callback that accepts or rejects goal requests.
  using GoalCallback = std::function<GoalResponse(
        const GoalUUID &,
        std::shared_ptr<const typename ActionT::Goal>)>;
  /// Signature of a callback that accepts or rejects requests to cancel a goal.
  using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
  /// Signature of a callback that is used to notify when the goal has been accepted.
  using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;

首先GoalCallback，这个函数在服务端接受到目标请求时触发，传入值一为uuid代表请求的唯一id号，传入值二为客户端请求的目标值，返回值为rclcpp_action::GoalResponse类型的数据（REJECT\ACCEPT_AND_EXECUTE）代表客户端的两种动作。客户端需要在这个回调函数中判断请求值是否合理，返回客户端判断的结果
AcceptedCallback，这个函数在服务端确认接受目标后执行，主要进行任务执行（执行过程中可以反馈信息，执行完成后可以响应结果），传入值为ServerGoalHandle类型的指针，可以通过这个指针发送反馈值以及最终结果，无返回值。
CancelCallback ，这个函数当客户端向服务端发送取消任务的请求时触发，传入值同上，为ServerGoalHandle类型的指针，返回值为rclcpp_action::CancelResponse类型的数据（ACCPET/REJECT）代表服务端是否允许结束任务。

在理解上述的三个回调函数后，便可以根据逻辑写出下面action服务端的demo代码了：

动作服务端demo

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "based_interfaces/action/myaction.hpp"
using based_interfaces::action::Myaction;
using namespace std::placeholders;

class My_Action : public rclcpp::Node{
private:
    //声明服务端变量
    rclcpp_action::Server<Myaction>::SharedPtr server_;
    //声明三个回调函数
    /*
  using GoalCallback = std::function<GoalResponse(
        const GoalUUID &,
        std::shared_ptr<const typename ActionT::Goal>)>;

  using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;

  using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;
    */

    //收到目标请求时触发，返回接收或者拒绝目标请求
   rclcpp_action::GoalResponse goal_handle( const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const Myaction::Goal> goal)
    {
        return rclcpp_action::GoalResponse::REJECT; //拒绝执行
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; //接收且执行
    }

    //收到取消请求时触发，返回接收或者拒绝取消请求
    rclcpp_action::CancelResponse cancel_handle(std::shared_ptr<rclcpp_action::ServerGoalHandle<Myaction>> goal_handle)
    {
        return rclcpp_action::CancelResponse::REJECT; //拒绝取消请求
        return rclcpp_action::CancelResponse::ACCEPT; //接收取消请求

    }

    //当目标请求被接收后触发，执行任务
    void accepted_handle(std::shared_ptr<rclcpp_action::ServerGoalHandle<Myaction>> goal_handle)
    {
        /*
            执行任务代码
        */
    }

public:
    My_Action():Node("action_server_node"){
        RCLCPP_INFO(this->get_logger(),"动作服务端节点创建完成！");
        //创建服务端对象,需要绑定三个回调函数：
        /*
        收到目标请求时触发rclcpp_action::Server<ActionT>::GoalCallback handle_goal, 
        收到取消请求时触发rclcpp_action::Server<ActionT>::CancelCallback handle_cancel, 
        确认执行后触发：rclcpp_action::Server<ActionT>::AcceptedCallback handle_accepted
        */
        server_ = rclcpp_action::create_server<Myaction>(this,"add_sum",
            std::bind(&My_Action::goal_handle,this,_1,_2),
            std::bind(&My_Action::cancel_handle,this,_1),
            std::bind(&My_Action::accepted_handle,this,_1)
        );
    }
};


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

确认模板可以编译通过

colcon build --packages-select demo01_action_test

服务端代码的核心就是一句代码：
server_ = rclcpp_action::create_server<Myaction>(this,"add_sum", std::bind(&My_Action::goal_handle,this,_1,_2), std::bind(&My_Action::cancel_handle,this,_1), std::bind(&My_Action::accepted_handle,this,_1)
包含两个要素：动作服务名称、动作服务的三个回调。创建了对象后就不要管了，ROS会根据需要自动调用回调函数进行处理。

动作服务端业务逻辑填充

将业务逻辑填充到三个回调函数中

//收到目标请求时触发，返回接收或者拒绝目标请求
   rclcpp_action::GoalResponse goal_handle( const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const Myaction::Goal> goal)
    {
        (void) uuid;
        if(goal->tar < 1) //请求不合法
        {
            return rclcpp_action::GoalResponse::REJECT; //拒绝执行
        }else{            //请求合法

            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; //接收且执行
        }
        

    }

    //收到取消请求时触发，返回接收或者拒绝取消请求
    rclcpp_action::CancelResponse cancel_handle(std::shared_ptr<rclcpp_action::ServerGoalHandle<Myaction>> goal_handle)
    {
        (void) goal_handle;
        //return rclcpp_action::CancelResponse::REJECT; //拒绝取消请求

        return rclcpp_action::CancelResponse::ACCEPT; //接收取消请求

    }

 void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Myaction>> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(),"开始执行任务！");
        Myaction::Result::SharedPtr result_data = std::make_shared<Myaction::Result>();
        Myaction::Feedback::SharedPtr feedback_data = std::make_shared<Myaction::Feedback>();
        rclcpp::Rate rate(1.0);
        int temp = 0;
        for (int i=1;i<=goal_handle->get_goal()->tar;i++){
            temp+=i;
            //检查任务是否取消
            if(goal_handle->is_canceling())
            {
                result_data->result = temp; //装载数据
                goal_handle->canceled(result_data); //发送数据
                return; //终止任务
            }

            feedback_data->state = "累计中"; //装载数据
            feedback_data->temp = temp;
            goal_handle->publish_feedback(feedback_data); //发送数据
            
            rate.sleep(); //延时
        }

        if(rclcpp::ok()){
            result_data->result = temp;//装载数据
            goal_handle->succeed(result_data);//发送数据
            RCLCPP_INFO(this->get_logger(),"任务完成!");
        }

    }


    //当目标请求被接收后触发，执行任务
    void accepted_handle(std::shared_ptr<rclcpp_action::ServerGoalHandle<Myaction>> goal_handle)
    {
        std::thread{std::bind(&My_Action::execute,this,_1),goal_handle}.detach();
    }

补充完业务逻辑后，进行编译处理。

4. 使用命令行工具测试服务

首先把功能包注册到系统环境变量中：

#在工作空间下：
source ./install/setup.bash

然后启动动作服务

ros2 run demo01_action_test cpp01_server

使用命令行工具发生目标请求：

ros2 action send_goal /add_sum based_interfaces/action/Myaction "{tar: 20}" --feedback

预期能得到以下响应：

Waiting for an action server to become available...
Sending goal:
     tar: 20

Goal accepted with ID: a279411f7e9a4eb887406230c654998f

Feedback:
    temp: 1
state: 累计中

Feedback:
    temp: 3
state: 累计中

Feedback:
    temp: 6
state: 累计中

Feedback:
    temp: 10
state: 累计中

Feedback:
    temp: 15
state: 累计中

Feedback:
    temp: 21
state: 累计中

Feedback:
    temp: 28
state: 累计中

Feedback:
    temp: 36
state: 累计中

Feedback:
    temp: 45
state: 累计中

Feedback:
    temp: 55
state: 累计中

Feedback:
    temp: 66
state: 累计中

Feedback:
    temp: 78
state: 累计中

Feedback:
    temp: 91
state: 累计中

Feedback:
    temp: 105
state: 累计中

Feedback:
    temp: 120
state: 累计中

Feedback:
    temp: 136
state: 累计中

Feedback:
    temp: 153
state: 累计中

Feedback:
    temp: 171
state: 累计中

Feedback:
    temp: 190
state: 累计中

Feedback:
    temp: 210
state: 累计中

Result:
    result: 210

Goal finished with status: SUCCEEDED