ROS Action动作通讯编程C++

自定义Action消息

Action的消息规范主要是为了描述Client端和Server端交互的数据格式，具体来说，就是为了去描述Goal，Feedback，Result的数据格式。

通常我们会新建一个package来管理并且定义Action的消息格式。以当前案例为例，我们会去新建好一个叫做demo_action_msgs的package，新建过程中，我们添加必要的依赖roscpp，rospy，rosmsg, actionlib_msgs。然后删除不必要的文件夹src，include。

 

1. 新建action消息描述文件

在package下创建名称为action的文件夹，接着在Action文件夹中创建CountNumber.action文件，内容如下:

int64 max
float64 duration
---
int64 count
---
float64 percent

 

action消息描述文件，必须放到action目录下，后缀必须是.action，这个是编码规范。

action文件中，被---分隔为三个部分。

第一个部分：描述的是Goal

第二个部分：描述的是Result

第三个部分：描述的是Feedback

 

2. 配置package.xml文件

在package.xml文件中添加运行时消息生成

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

 

3.配置CMakeLists.txt文件

find_package配置

配置find_package,添加message_generation，效果如下:

find_package(catkin REQUIRED COMPONENTS
        actionlib_msgs
        roscpp
        rosmsg
        rospy
        message_generation
        )

 

generation_message配置

配置generation_message，添加actionlib_msgs，效果如下:

catkin_package(
        #  INCLUDE_DIRS include
        #  LIBRARIES demo_actions
        CATKIN_DEPENDS actionlib_msgs roscpp rosmsg rospy
        #  DEPENDS system_lib
)

 

add_action_files配置

配置add_action_files，添加action文件，效果如下：

add_action_files(
        FILES
        CountNumber.action
)

 

4. 编译action消息

来到workspace下面，进行代码编译

catkin_make

 

编译完成后，会在devel/include/demo_action_msgs目录下产出多个头文件。格式如下：

action文件名Action.h，action文件名Goal.h，action文件名Result.h，action文件名Feedback.h，action文件名ActionGoal.h，action文件名ActionResult.h，action文件名ActionFeedback.h

 

SimpleActionServer端(C++)

SimpleActionServer端创建流程

1. 初始化ROS，创建节点

ros::init(argc, argv, nodeName, ros::init_options::AnonymousName);
ros::NodeHandle node;

 2. 创建Action的Server端

actionlib::SimpleActionServer<demo_actions::CountNumberAction> server(node,actionName,boost::bind(&execute_callback, _1, &server),false);
server.start();

 

我们需要引入actionlib这个依赖库，添加头文件依赖

#include "actionlib/server/simple_action_server.h"

actionlib::SimpleActionServer: Action的Server端对应的类。

第一个参数Node: 表示当前的server端是基于这个node节点构建的

第二个参数：当前action的名称

第三个参数：是client端访问server端的回调

第四个参数：是否自动开启server

 

3. Server端的业务逻辑

server端的业务逻辑，主要是用于处理client请求的回调。用于结果反馈和过程反馈，提供简单示例:

void execute_callback1(const demo_actions::CountNumberGoalConstPtr &goal,
                       actionlib::SimpleActionServer<demo_actions::CountNumberAction> *server) {
    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);

    //进度操作
    ros::Rate rate(1.0 / goal->duration);
    int count = 0;
    while (count < goal->max) {
        // 处理进度，反馈进度
        demo_actions::CountNumberFeedback feedback;
        feedback.percent = count * 100.0 / goal->max;
        server->publishFeedback(feedback);

        rate.sleep();
        count++;
    }
    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    server->setSucceeded(result);
}

完整代码

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include <iostream>
#include "demo_actions/CountNumberAction.h"

using namespace std;

void execute_callback(const demo_actions::CountNumberGoalConstPtr &goal,
                       actionlib::SimpleActionServer<demo_actions::CountNumberAction> *server) {
    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);
    //进度操作
    ros::Rate rate(1.0 / goal->duration);
    int count = 0;
    while (count < goal->max) {
        // 处理进度，反馈进度
        demo_actions::CountNumberFeedback feedback;
        feedback.percent = count * 100.0 / goal->max;
        server->publishFeedback(feedback);
        rate.sleep();
        count++;
    }

    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    server->setSucceeded(result);
}

int main(int argc, char **argv) {
    string nodeName = "CountNumberServer";
    string actionName = "/count_number";
    //创建节点
    ros::init(argc, argv, nodeName, ros::init_options::AnonymousName);
    ros::NodeHandle node;

    //创建server
    actionlib::SimpleActionServer<demo_actions::CountNumberAction> server(node, actionName,boost::bind(&execute_callback, _1,&server),false);
    server.start();

    //阻塞
    ros::spin();
    return 0;
}

Action通讯机制，底层是通过Topic通讯机制去实现的。

Action通讯过程中，创建了5个Topic主题，通过5个topic进行数据的通讯，达到异步处理事件的结果。

• Goal Topic: 这个主题传递的数据是Goal指令。client端此时扮演的是Publisher发布者，他主要的功能就是发布需求到这个主题。server端此时扮演的是subscriber订阅者，他主要的功能就是到这个主题中去获取需求。
• Cancel Toic：这个主题传递的是取消指令。client端此时扮演的是Publisher发布者，他主要的功能就是发布取消命令到这个主题。server端此时扮演的是subscriber订阅者，他主要的功能就是到这个主题中去获取取消指令。
• Status Topic：这个主题传递的是当前任务执行的状态。server端此时扮演的是Publisher发布者，他主要的功能就是发布当前任务执行的实时状态到这个主题。client端此时扮演的是subscriber订阅者，他主要的功能就是到这个主题中去获取执行的实时状态。
• Feedback Topic： 这个主题传递的是当前执行过程中的数据。server端此时扮演的是Publisher发布者，他主要的功能就是发布当前任务执行过程中的数据到这个主题。client端此时扮演的是subscriber订阅者，他主要的功能就是到这个主题中去获取执行过程中的数据。
• Result Topic：这个主题传递的是当前执行结果数据。server端此时扮演的是Publisher发布者，他主要的功能就是发布当前任务执行结果数据到这个主题。client端此时扮演的是subscriber订阅者，他主要的功能就是到这个主题中去获取执行结果数据。

调试Server端

调试server端主要是查看server端是否能接收到请求，并根据请求数据处理相应的业务逻辑，然后返回处理好的结果。

在这里，我们只需要模拟client端发送请求就可以了。

在这里，我们需要模拟以下情况:

• client端发送Goal
• client端接收到的Feedback反馈
• client端接收到的Result反馈

ROS提供了命令行工具和图形化工具供我们调试开发。

rosttopic echo /count_number/feedback

client端接收到的Result反馈

rostopic echo /count_number/result

 client端发送Goal

rostopic pub /count_number/goal demo_actions/CountNumberActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  max: 1000
  duration: 0.1"

SimpleActionClient端(C++)

SimpleActionClient创建流程

1. 创建Node节点

ros::init(argc, argv, nodeName, ros::init_options::AnonymousName);
ros::NodeHandle node;

2. 创建SimpleActionClient

actionlib::SimpleActionClient<demo_actions::CountNumberAction> &&client = actionlib::SimpleActionClient<demo_actions::CountNumberAction>(
            node, actionName);
client.waitForServer();

 我们需要引入actionlib这个依赖库，添加头文件依赖

#include "actionlib/client/simple_action_client.h"

actionlib::SimpleActionClient： Action的Client端对应的类。

第一个参数Node: 表示当前的client端是基于这个node节点构建的

第二个参数：当前action的名称

 3. 发送Goal指令给Server端

demo_actions::CountNumberGoal goal;
goal.max = 100;
goal.duration = 0.5;
client.sendGoal(goal,
                boost::bind(&done_cb, _1, _2, &client),
                boost::bind(&active_cb),
                boost::bind(&feedback_cb, _1));

sendGoal(const Goal & goal, SimpleDoneCallback done_cb, SimpleActiveCallback active_cb, SimpleFeedbackCallback feedback_cb)

sendGoal函数，对应了几个参数。

第一个参数goal： 对应的是发送的指令数据，server端会根据客户端的数据进行对应的逻辑。

第二个参数done_cb：server端完成耗时操作时，告知客户端的回调。

第三个参数active_cb: server端在执行操作过程中，告知客户端的开始执行任务的回调。

第四个参数feedback_cb: server端在执行操作过程中，告知客户端的进度回调。

 

4.完成回调

结果回调:

void done_cb(const actionlib::SimpleClientGoalState &state,
             const demo_actions::CountNumberResultConstPtr &result,
             const actionlib::SimpleActionClient<demo_actions::CountNumberAction> *client) {

    if (state == state.ABORTED) {
        ROS_INFO("server working error, don't finish my job.");
    } else if (state == state.PREEMPTED) {
        ROS_INFO("client cancel job.");
    } else if (state == state.SUCCEEDED) {
        ROS_INFO("server finish job.");
        ROS_INFO_STREAM("result: " << result->count);
    }
}

开始回调:

void active_cb() {
    ROS_INFO_STREAM("active callback");
}

进度回调:

void feedback_cb(const demo_actions::CountNumberFeedbackConstPtr &feedback) {
    ROS_INFO_STREAM("percent: " << feedback->percent);
}

完整代码

#include "ros/ros.h"
#include <iostream>
#include "actionlib/client/simple_action_client.h"
#include "demo_actions/CountNumberAction.h"

using namespace std;

// 结果回调
void done_cb(const actionlib::SimpleClientGoalState &state,
             const demo_actions::CountNumberResultConstPtr &result,
             const actionlib::SimpleActionClient<demo_actions::CountNumberAction> *client) {
    if (state == state.ABORTED) {
        ROS_INFO("server working error, don't finish my job.");
    } else if (state == state.PREEMPTED) {
        ROS_INFO("client cancel job.");
    } else if (state == state.SUCCEEDED) {
        ROS_INFO("server finish job.");
        ROS_INFO_STREAM("result: " << result->count);
    }
}

// 激活回调
void active_cb() {
    ROS_INFO_STREAM("active callback");
}

//过程反馈
void feedback_cb(const demo_actions::CountNumberFeedbackConstPtr &feedback) {
    ROS_INFO_STREAM("percent: " << feedback->percent);
}

int main(int argc, char **argv) {
    string nodeName = "CountNumberClient";
    string actionName = "/count_number";

    // 创建节点
    ros::init(argc, argv, nodeName, ros::init_options::AnonymousName);
    ros::NodeHandle node;

    // 创建client
    actionlib::SimpleActionClient<demo_actions::CountNumberAction> &&client = actionlib::SimpleActionClient<demo_actions::CountNumberAction>(
            node, actionName);
    client.waitForServer();

    // 发送
    demo_actions::CountNumberGoal goal;
    goal.max = 100;
    goal.duration = 0.5;
    client.sendGoal(goal,
                    boost::bind(&done_cb, _1, _2, &client),
                    boost::bind(&active_cb),
                    boost::bind(&feedback_cb, _1));
    // 阻塞线程
    ros::spin();

    return 0;
}

调试Client端

通过现有的server端进行调试。

 

SimpleAction响应结果(C++)

响应的结果状态

 

server端在处理完成client的需求时，分为三种状态给client进行答复：

• Succeed：server端成功完成client的需求，将成功结果返回。
• Preempted：server端在在执行过程中，client端取消了执行指令，server端将取消的结果返回。
• Aborted：server端在执行过程中，自身出现了状况，没有完成任务，将结果反馈给client端。

 

Succeed结果反馈实现

Succeed状态是由 server端 反馈给 client端 的，因此我们只需要保证server端正常运行即可。server端代码如下：

void execute_callback(const demo_actions::CountNumberGoalConstPtr &goal,
                       actionlib::SimpleActionServer<demo_actions::CountNumberAction> *server) {
    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);

    //进度操作
    ros::Rate rate(1.0 / goal->duration);
    int count = 0;
    while (count < goal->max) {
        // 处理进度，反馈进度
        demo_actions::CountNumberFeedback feedback;
        feedback.percent = count * 100.0 / goal->max;
        server->publishFeedback(feedback);

        rate.sleep();
        count++;
    }
    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    server->setSucceeded(result);
}

 

Preempted结果反馈实现

Preempted状态是由 server端 反馈给 client端 的，但是前提是client端发出了取消的指令。

client端

client端发送取消指令操作代码如下:

client.cancelGoal();

server端

server端处理取消判断代码如下:

void execute_callback(const demo_actions::CountNumberGoalConstPtr &goal,
                       actionlib::SimpleActionServer<demo_actions::CountNumberAction> *server) {
    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);

    //进度操作
    ros::Rate rate(1.0 / goal->duration);
    int count = 0;
    bool isPreempt = false;
    while (count < goal->max) {
        if (server->isPreemptRequested()) {
            isPreempt = true;
            break;
        }
        // 处理进度，反馈进度
        demo_actions::CountNumberFeedback feedback;
        feedback.percent = count * 100.0 / goal->max;
        server->publishFeedback(feedback);
        rate.sleep();
        count++;
    }

    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    if (isPreempt) {
        server->setPreempted(result);
    } else {
        server->setSucceeded(result);
    }
}

 

Aborted结果响应

Aborted状态说由 server端 反馈给 client端 的，但是前提是server端因为自身业务逻辑或者是出现了不可完成的异常导致的结果。

server端代码如下:

 

void execute_callback(const demo_actions::CountNumberGoalConstPtr &goal,
                      actionlib::SimpleActionServer<demo_actions::CountNumberAction> *server) {
    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);

    //进度操作
    ros::Rate rate(1.0 / goal->duration);
    int count = 0;
    bool isPreempt = false;
    bool isAbort = false;

    while (count < goal->max) {
        if (server->isPreemptRequested()) {
            isPreempt = true;
            break;
        }
        if (count > 10) {
            isAbort = true;
            break;
        }
        // 处理进度，反馈进度
        demo_actions::CountNumberFeedback feedback;
        feedback.percent = count * 100.0 / goal->max;
        server->publishFeedback(feedback);
        rate.sleep();
        count++;
    }
    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    if (isPreempt) {
        ROS_INFO("Preempted");
        server->setPreempted(result, "text Preempted");
    } else if (isAbort) {
        ROS_INFO("Aborted");
        server->setAborted(result, "text Aborted");
    } else {
        ROS_INFO("Succeeded");
        server->setSucceeded(result, "text Succeeded");
    }
}