ROS action

其他 2018-06-25 21:25:25 阅读次数: 3

原文链接：点击打开链接

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/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客户端

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

猜你喜欢

转载自blog.csdn.net/weicao1990/article/details/80322110
今日推荐
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
GCC 14.1 发布
面壁智能发布 Eurux-8x22B 开源大模型 —— 堪称「理科状元」
开源日报 | 谷歌扶持鸿蒙上位；开源Rabbit R1；Docker加持的安卓手机；微软的焦虑和野心；海尔电器把开放平台关了
中国码农的“35岁魔咒”
蘭雅 CorelDRAW 插件 2024.5.1 国际劳动节版，免费下载
Arc Browser for Windows 1.0 正式 GA
90后程序员开发视频搬运软件、不到一年获利超 700 万，结局很刑！
周排行
Java自定义时间格式
同步整形电路
在开发中最最最常用的字符串的属性大集合
Linux 查看端口占用并杀掉
Java基础四：ArrayList
多线程之死锁就是这么简单
mysql 基础命令集
awk 命令详解
Centos6.3编译安装nginx+php步骤
OCR （Optical Character Recognition，光学字符识别）
每日归档
更多
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)
2024-05-03(19)
2024-05-02(0)
2024-05-01(4)
2024-04-30(1)
2024-04-29(40)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022