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

时间：2025-03-16 13:34:01

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

1.原理

1.1功能

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

1.2框架

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

ROS知识（15）----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

相关文章