ros知识（12）----actionlib的使用

//这是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;
}

//这是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;
}

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}
 )

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

首先，启动服务器
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

[ 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

[ 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

[ 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