ROS知识（15）----Actionlib的使用（一）

Actionlib是ROS非常重要的库，像执行各种运动的动作，例如控制手臂去抓取一个杯子，这个过程可能复杂而漫长，执行过程中还可能强制中断或反馈信息，这时Actionlib就能大展伸手了。

1.原理

1.1功能

在任何一个比较大的基于ROS的系统，都会有这样的情况，向某个节点发送请求执行某一个任务，并返回相应的执行结果，这种通常用ROS的服务（services）完成。然而，有一些情况服务执行的时间很长，在执行中想要获得任务处理的进度，或可能取消执行任务，Actionlib就能实现这样的功能，它是ROS的一个非常重要的库。可以实现一些简单的状态机功能，算的上是SMACH的一个弱化版。

1.2框架

如下图所示， Actionlib的框架实际是一种特殊的客户-服务的模式。除了服务请求的功能外，还可以实时获取服务器执行任务的进度状态，以及强制中断服务的功能。

2.例子

下面以洗碟子为例子，实现客户端调用服务器执行洗盘子的动作。这个例子是官网的一个改进版本，涵盖actionli的基本功能，例如获取服务器执行任务的进度状态，强制终端服务的功能，服务活动状态提示。

2.1服务服务端实现

服务端实现了服务器执行任务的反馈信息，中断抢占功能。具体实现较为简单，反馈信息通过发布反馈的消息实现，中断抢占通过注册中断毁掉函数实现，代码如下：

//这是actionlib的服务端

#include <first_actionlib_sample/DoDishesAction.h>
#include <actionlib/server/simple_action_server.h>

//这样定义下会用起来简洁许多
typedef actionlib::SimpleActionServer<first_actionlib_sample::DoDishesAction> Server;

class DoDishesActionServer
{
public:
    DoDishesActionServer(ros::NodeHandle n):
            server(n, "do_dishes",
                    boost::bind(&DoDishesActionServer::ExecuteCb, this, _1), false)
    {
        //注册抢占回调函数
        server.registerPreemptCallback(boost::bind(&DoDishesActionServer::preemptCb, this));
    }

    //启动服务
    void Start()
    {
        server.start();
    }

    //回调函数，在此添加代码实现你要的功能
    void ExecuteCb(const first_actionlib_sample::DoDishesGoalConstPtr& goal) {
        // 在次添加你所要实现的功能
        ROS_INFO("Received goal,the dish id is :%d", goal->dishwasher_id);
        //反馈
        first_actionlib_sample::DoDishesFeedback feedback;
        ros::Rate rate();
        int cur_finished_i = ;
        int toal_dish_num = ;
        for(cur_finished_i = ; cur_finished_i <= toal_dish_num; cur_finished_i++)
        {
            if(!server.isActive())break;

            ROS_INFO("Cleanning the dish::%d", cur_finished_i);
            feedback.percent_complete = cur_finished_i/10.0;
            server.publishFeedback(feedback);
            rate.sleep();
        }
        first_actionlib_sample::DoDishesResult result;
        result.toal_dishes_cleaned = cur_finished_i;

        if(server.isActive())server.setSucceeded();

    }

    //中断回调函数
    void preemptCb()
    {
        if(server.isActive()){
            server.setPreempted();//强制中断
        }
    }

    Server server;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "do_dishes_server");
    ros::NodeHandle n;
    //初始化，绑定回调函数

    DoDishesActionServer actionServer(n);
    //启动服务器，等待客户端信息到来
    actionServer.Start();
    ros::spin();
    return ;
}

2.2客户端实现

客户端注册了三个毁掉函数，DoneCb,ActivCb,FeedbackCb，分别地，DoneCb：用于监听服务器任务执行完后的相应消息以及客户端的相应处理，ActivCb：服务器任务被激活时的消息提示以及客户端的相应处理，FeedbackCb：接收服务器的反馈消息以及客户端的相应处理。代码如下：

//这是actionlib的客户端

#include <first_actionlib_sample/DoDishesAction.h>
//#include <actionlib_msgs/GoalStatusArray.h>
#include <actionlib/client/simple_action_client.h>

//这样定义下会用起来简洁许多
typedef actionlib::SimpleActionClient<first_actionlib_sample::DoDishesAction> Client;

class DoDishesActionClient {
private:
    // Called once when the goal completes
    void DoneCb(const actionlib::SimpleClientGoalState& state,
            const first_actionlib_sample::DoDishesResultConstPtr& result) {
        ROS_INFO("Finished in state [%s]", state.toString().c_str());
        ROS_INFO("Toal dish cleaned: %i", result->toal_dishes_cleaned);
        ros::shutdown();
    }

    // 当目标激活的时候，会调用一次
    void ActiveCb() {
        ROS_INFO("Goal just went active");
    }

    // 接收服务器的反馈信息
    void FeedbackCb(
            const first_actionlib_sample::DoDishesFeedbackConstPtr& feedback) {
        ROS_INFO("Got Feedback Complete Rate: %f", feedback->percent_complete);
    }
public:
    DoDishesActionClient(const std::string client_name, bool flag = true) :
            client(client_name, flag) {
    }

    //客户端开始
    void Start() {
        //等待服务器初始化完成
        client.waitForServer();
        //定义要做的目标
        first_actionlib_sample::DoDishesGoal goal;
        goal.dishwasher_id = ;
        //发送目标至服务器
        client.sendGoal(goal,
                boost::bind(&DoDishesActionClient::DoneCb, this, _1, _2),
                boost::bind(&DoDishesActionClient::ActiveCb, this),
                boost::bind(&DoDishesActionClient::FeedbackCb, this, _1));
        //等待结果，时间间隔5秒
        client.waitForResult(ros::Duration(10.0));

        //根据返回结果，做相应的处理
        if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            printf("Yay! The dishes are now clean");
        else {
            ROS_INFO("Cancel Goal!");
            client.cancelAllGoals();
        }

        printf("Current State: %s\n", client.getState().toString().c_str());
    }
private:
    Client client;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "do_dishes_client");
    DoDishesActionClient actionclient("do_dishes", true);
    //启动客户端
    actionclient.Start();
    ros::spin();
    return ;
}

另一个客户端的实现代码基本相同，只是节点的名字有所不同，这里略去，详细实现请查看附件的源码。主要用于测试抢占中断功能。

2.3CMakeLists编写 

cmake_minimum_required(VERSION ..)
project(first_actionlib_sample)

find_package(catkin REQUIRED COMPONENTS
  actionlib
  actionlib_msgs
  roscpp
  rospy
  std_msgs
)

## Generate actions in the 'action' folder
 add_action_files(
   DIRECTORY action
   FILES
   DoDishes.action
 )

## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   actionlib_msgs#   std_msgs
 )

catkin_package()

###########
## Build ##
###########
include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ executable
add_executable(do_dishes_action_client_node src/DoDishesActionClient.cpp)
add_executable(do_dishes_action_client_node1 src/DoDishesActionClient1.cpp)
add_executable(do_dishes_action_server_node src/DoDishesActionServer.cpp)

## Add cmake target dependencies of the executable
## same as for the library above
 add_dependencies(do_dishes_action_client_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(do_dishes_action_client_node1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(do_dishes_action_server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
 target_link_libraries(do_dishes_action_client_node
   ${catkin_LIBRARIES}
 )
  target_link_libraries(do_dishes_action_client_node1
   ${catkin_LIBRARIES}
 )
 target_link_libraries(do_dishes_action_server_node
   ${catkin_LIBRARIES}
 )

2.4package.xml编写

添加actionlib的编译和执行依赖，如下

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<run_depend>actionlib</run_depend>
<run_depend>actionlib_msgs</run_depend>

2.5测试

2.5.1 启动

消息反馈和中断抢占的演示，首先启动服务器，然后分别启动两个客户端，执行命令如下：

首先，启动服务器
rosrun first_actionlib_sample do_dishes_action_server_node

然后，启动客户端1
rosrun first_actionlib_sample do_dishes_action_client_node

接着，启动客户端2

rosrun first_actionlib_sample do_dishes_action_client_node1

2.5.2 结果

服务器结果如下：

[ INFO] [1477561873.888082387]: Received goal,the dish id is :
[ INFO] [1477561876.795322950]: PreemptCb!
[ INFO] [1477561876.795370142]:
[ INFO] [1477561876.795428811]: Set Preempted!
[ INFO] [1477561876.888346882]: Received goal,the dish id is :
[ INFO] [1477561886.888599364]: 

客户端1被被客户端2中断结果如下：

[ INFO] [1477561873.888199741]: Goal just went active
[ INFO] [1477561873.888272439]: Got Feedback Complete Rate: 0.100000
[ INFO] [1477561874.888483533]: Got Feedback Complete Rate: 0.200000
[ INFO] [1477561875.888816424]: Got Feedback Complete Rate: 0.300000
[ INFO] [1477561876.795693526]: Finished in state [PREEMPTED]
[ INFO] [1477561876.795754009]: Toal dish cleaned:
Current State: PREEMPTED

客户端2结果显示如下：

[ INFO] [1477561876.888648770]: Goal just went active
[ INFO] [1477561876.888753797]: Got Feedback Complete Rate: 0.100000
[ INFO] [1477561877.888792417]: Got Feedback Complete Rate: 0.200000
[ INFO] [1477561878.888906824]: Got Feedback Complete Rate: 0.300000
[ INFO] [1477561879.888699775]: Got Feedback Complete Rate: 0.400000
[ INFO] [1477561880.888896675]: Got Feedback Complete Rate: 0.500000
[ INFO] [1477561881.889008655]: Got Feedback Complete Rate: 0.600000
[ INFO] [1477561882.888785829]: Got Feedback Complete Rate: 0.700000
[ INFO] [1477561883.888789072]: Got Feedback Complete Rate: 0.800000
[ INFO] [1477561884.888667138]: Got Feedback Complete Rate: 0.900000
[ INFO] [1477561885.888919402]: Got Feedback Complete Rate: 1.000000
[ INFO] [1477561886.889587174]: Finished in state [SUCCEEDED]
[ INFO] [1477561886.889736499]: Toal dish cleaned:
Yay! The dishes are now cleanCurrent State: SUCCEEDED

3.源码

下载地址：the_first_actionlib_sample