Action响应结果(C++)

Action状态变化

Action通讯的状态按照执行流程分为以上几种。在这几种状态中，有做了一下细分，如下图：

server端在处理状态的过程种，会将所有的状态拆分为常用的两种:Active，Done。

  • Active为激活状态。

  • Done是Succeed，Preempted，Aborted，Rejected几种状态的统称。代表了当前任务已经执行完成。

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

Active和Done状态的分流处理

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
void transition_cb(ClientGoalHandle goalHandle) {
    const actionlib::CommState &state = goalHandle.getCommState();
    if (state == actionlib::CommState::ACTIVE) {
        ROS_INFO_STREAM("ACTIVE");
    } else if (state == actionlib::CommState::DONE) {
        ROS_INFO_STREAM("DONE");
        actionlib::TerminalState terminalState = goalHandle.getTerminalState();
        demo_actions::CountNumberResultConstPtr result = goalHandle.getResult();
        done_cb(terminalState, result);
    } else {
        ROS_INFO_STREAM(state.toString());
    }
}

Tip

server端状态反馈结果都会回调到client端的transition_cb回调中来。通常我们通过以上代码进行状态的判断和分流。

以上第2行中，我们可以通过goalHanle获得状态CommState,CommState可以有效的进行分流判断。

以上第7行中，我们可以通过goalHanle获得状态TerminalState，TerminalState就是Done的四种状态之一。

以上第8行中，我们可以通过goalHanle获得server端处理的结果，根据实际情况，我们可以定义函数done_cb来处理结果。

在Done状态中，done_cb可以继续的进行状态的判断分流，结果如下:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
void done_cb(actionlib::TerminalState &state, ResultConstPtr &result) {
    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);
    } else if(state == state.REJECTED) {
        ROS_INFO("server rejected job.");
    }
}

Tip

以上就可以进行多种状态分流，进行自己的业务逻辑了。

Succeed结果反馈实现

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

server端代码如下： 

 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
void goal_cb(ServerGoalHandle &goalHandle){
    auto goal = goalHandle.getGoal();

    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);
    //改变状态为Active
    goalHandle.setAccepted();

    //进度操作
    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;
        goalHandle.publishFeedback(feedback);

        rate.sleep();
        count++;
    }

    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    ROS_INFO("Succeeded");
    goalHandle.setSucceeded(result, "text Succeeded");
}

Tip

goalHandle可以获得goal数据，这个是客户端传递的goal指令数据。

在执行过程种，默认的状态还是PENDING，我们需要通过goalHandle.setAccepted();将状态切换为ACTIVE。

Preempted结果反馈实现

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

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

1
client.cancelGoal();

server端处理取消判断的逻辑，通常我们会定义一个map<string,bool>，在cancel_cb取消回调中去记录取消的goal，代码如下:

1
2
3
4
5
void cancel_cb(const ServerGoalHandle goalHandle) {
    ROS_INFO("cancel_cb");
    auto goalId = goalHandle.getGoalID().id;
    cancel_ids[goalId] = true;
}

Tip

goalHandle可以获得goal的唯一标识id。

如果取消就记录下来。

在执行逻辑的回调代码中，我们做如下的处理：

 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
void goal_cb(ServerGoalHandle &goalHandle){
    auto goal = goalHandle.getGoal();
    auto goalId = goalHandle.getGoalID().id;
    //标记goalId没有被取消
    cancel_ids[goalId] = false;

    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);

    goalHandle.setAccepted();

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

        rate.sleep();
        count++;
    }

    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    if (isPreempt) {
        ROS_INFO("Preempted");
        goalHandle.setCanceled(result, "text Preempted");
    } else {
        ROS_INFO("Succeeded");
        goalHandle.setSucceeded(result, "text Succeeded");
    }
    //移除取消的记录
    cancel_ids.erase(goalId);
}

Tip

以上高亮代码块中，第一部分和最后一部分，用来记录和删除当前goal是否已经被取消了.

第二部分用来判断是否被取消了，取消了，就走第三部分的高亮逻辑。

Aborted结果响应

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

 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
void goal_cb(ServerGoalHandle &goalHandle){
    auto goal = goalHandle.getGoal();
    auto goalId = goalHandle.getGoalID().id;
    //标记goalId没有被取消
    cancel_ids[goalId] = false;

    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);

    goalHandle.setAccepted();

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

        rate.sleep();
        count++;
    }

    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    if (isPreempt) {
        ROS_INFO("Preempted");
        goalHandle.setCanceled(result, "text Preempted");
    } else if (isAbort) {
        ROS_INFO("Aborted");
        goalHandle.setAborted(result, "text Aborted");
    } else {
        ROS_INFO("Succeeded");
        goalHandle.setSucceeded(result, "text Succeeded");
    }
    cancel_ids.erase(goalId);
}

Tip

代码高亮第一部分，我们模拟server端处理过程中，无法完成任务而退出。

第二部分则是如何将状态响应给client端。

Rejected结果反馈实现

Rejected状态说由server端反馈给client端的，但是前提是server端因为client端发送的goal数据不符合自身要求，拒绝了client请求的结果。server端代码如下:

 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
void goal_cb(ServerGoalHandle &goalHandle){
    auto goal = goalHandle.getGoal();
    auto goalId = goalHandle.getGoalID().id;
    //标记goalId没有被取消
    cancel_ids[goalId] = false;

    ROS_INFO_STREAM("max: " << goal->max);
    ROS_INFO_STREAM("duration: " << goal->duration);

    if (goal->max < 0) {
        goalHandle.setRejected();
        return;
    }
    goalHandle.setAccepted();

    //进度操作
    ros::Rate rate(1.0 / goal->duration);
    int count = 0;
    bool isPreempt = false;
    bool isAbort = false;
    while (count < goal->max) {
        if (cancel_ids[goalId]) {
            isPreempt = true;
            break;
        }

        if (count > 10) {
            isAbort = true;
            break;
        }
        // 处理进度，反馈进度
        demo_actions::CountNumberFeedback feedback;
        feedback.percent = count * 100.0 / goal->max;
        goalHandle.publishFeedback(feedback);

        rate.sleep();
        count++;
    }

    // 响应结果
    demo_actions::CountNumberResult result;
    result.count = count;
    if (isPreempt) {
        ROS_INFO("Preempted");
        goalHandle.setCanceled(result, "text Preempted");
    } else if (isAbort) {
        ROS_INFO("Aborted");
        goalHandle.setAborted(result, "text Aborted");
    } else {
        ROS_INFO("Succeeded");
        goalHandle.setSucceeded(result, "text Succeeded");
    }
    cancel_ids.erase(goalId);
}

Tip

代码高亮块部分，是在PENDING状态改变为ACTIVE状态前，先去校验client传入的goal数据是否符合自己处理的逻辑。

并发处理

通过多线程来处理并发。

server端的处理

在goal_cb的回调种，我们可以将所有逻辑放到子线程种去执行。

1
2
3
void goal_cb(ServerGoalHandle goalHandle) {
    new thread(execute_callback, goalHandle);
}

client端处理

在启动ros节点时，通过AsyncSpinner来进行线程的阻塞处理。

1
2
ros::AsyncSpinner spinner(4);
spinner.start();