ROS action

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

在ros中，如果想要发送一个request给一个节点该节点完成一些任务，并且给出一个回复，这样可以使用ros中的service完成。但是有一些应用中任务执行的时间很长，用户需要查看执行的进度如何，以及或者取消该任务，ros中提供了一个actionlib package来建立服务，执行一个长时间运行的可抢占目标。
使用client server接口，使用ROS action 协议通信。client和server都提供一套简单的API给用户来执行任务。

为了能够通信，需要定义少量的messages，也就是action specification。定义包括Goal, FEedback, Result.

2.定义ros的action服务

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

1)创建工程

cd ~/catkin_ws/srccatkin_create_pkg action std_msgs rospy

2)设计action定义文件，内容有goal ,feedback, result

这个和service的srv文件很像，放置在工程目录的action目录下

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(1);        int cur_finished_i = 1;        int toal_dish_num = 10;        for(cur_finished_i = 1; 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 0;}

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 = 1;        //发送目标至服务器        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 0;}

另一个客户端

//这是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 DoDishesActionClient1 {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:    DoDishesActionClient1(const std::string client_name, bool flag = true) :            client(client_name, flag) {    }    void Start() {        //等待服务器初始化完成        client.waitForServer();        //定义要做的目标        first_actionlib_sample::DoDishesGoal goal;        goal.dishwasher_id = 1;        //发送目标至服务器        client.sendGoal(goal,                boost::bind(&DoDishesActionClient1::DoneCb, this, _1, _2),                boost::bind(&DoDishesActionClient1::ActiveCb, this),                boost::bind(&DoDishesActionClient1::FeedbackCb, this, _1));        //等待结果，时间间隔5秒        client.waitForResult(ros::Duration(11.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());    }    Client client;};int main(int argc, char** argv) {    ros::init(argc, argv, "do_dishes_client1");    DoDishesActionClient1 actionclient("do_dishes", true); // true -> don't need ros::spin()    actionclient.Start();    ros::spin();    return 0;}

2.3 CMakeLists编写

cmake_minimum_required(VERSION 2.8.3)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++ executableadd_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然后，启动客户端1rosrun first_actionlib_sample do_dishes_action_client_node接着，启动客户端2rosrun first_actionlib_sample do_dishes_action_client_node1

2.5.2 结果

服务器结果如下：

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

客户端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: 0Current 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: 0Yay! The dishes are now cleanCurrent State: SUCCEEDED

友情链接:

actionlib-编写简单的action服务器(Execute Callback)

actionlib-编写简单的action客户端

actionlib-运行action服务器和客户端