SimpleActionClient端(C++)

SimpleActionClient创建流程

1. 创建Node节点

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

2. 创建SimpleActionClient

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

Tip

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

#include "actionlib/client/simple_action_client.h"

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

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

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

3. 发送Goal指令给Server端

1
2
3
4
5
6
7
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));

Tip

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.完成回调

结果回调:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
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);
    }
}

开始回调:

1
2
3
void active_cb() {
    ROS_INFO_STREAM("active callback");
}

进度回调:

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

完整代码

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
#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端进行调试。