自定义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");
    }
}