ROS action

Dear 丶 2022-05-25 13:13 193阅读 0赞

原文链接：[点击打开链接][Link 1]

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

1.主要作用

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

![这里写图片描述][SouthEast]  
为了能够通信，需要定义少量的messages，也就是action specification。定义包括Goal, FEedback, Result.

2.定义ros的action服务

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

1)创建工程

cd ~/catkin_ws/src
    catkin_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++ 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

友情链接:

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

[actionlib-编写简单的action客户端][actionlib-_action]

[actionlib-运行action服务器和客户端][actionlib-_action 1]

[Link 1]: http://blog.csdn.net/xu1129005165/article/details/70048237
[SouthEast]: /images/20220525/82adcb4a164b4e37aa15e14a818f7b09.png
[actionlib-_action_Execute Callback]: http://www.ncnynl.com/archives/201702/1315.html
[actionlib-_action]: http://www.ncnynl.com/archives/201702/1316.html
[actionlib-_action 1]: http://www.ncnynl.com/archives/201702/1317.html