ROS Action动作通讯编程C++

145
0
2020年9月23日 11时58分

自定义Action消息

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

 

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

 

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.haction文件名Goal.haction文件名Result.haction文件名Feedback.haction文件名ActionGoal.haction文件名ActionResult.haction文件名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");
    }
}

 

发表评论

后才能评论