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(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;
}

 另一個客戶端的實現代碼基本相同，只是節點的名字有所不同，這里略去，詳細實現請查看附件的源碼。主要用於測試搶占中斷功能。

 

2.3CMakeLists編寫 

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++ 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 :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: 0
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: 0
Yay! The dishes are now cleanCurrent State: SUCCEEDED

 

3.源碼

下載地址：the_first_actionlib_sample