我们知道ROS有消息机制,有服务机制。一个同步的通讯机制,一个异步的通讯机制,难道这还不够用吗?答案是不够,因为服务机制在要求异步而且响应时间不定的场景下有先天的劣势,如果服务端迟迟没有响应,那么客户端程序将处于阻塞状态,直到超时。这会使操作变得相当耗时。
那么action机制的用武之地就来了。
0.什么叫action?:
action机制从原理上看的话,其实是基于topic实现的,相当于制定了一套由目标话题,反馈话题,结果话题组成的高级协议。action实现了一种类似于service的请求/响应通讯机制,区别在于action带有反馈机制,用来不断向客户端反馈任务的进度,并且还支持在任务中途中止运行。
操作起来就像这样子,客户端给服务端抛出一个目标,然后客户端就可以去干别的事情了,在任务执行期间,客户端会以消息的形式,周期性地接收到来自服务端的进度反馈,如果没有终止任务的话这个过程会一直延续到收到最终的结果。当然也可以随时终止当前的操作,开始一个全新的操作。
1.如何定义action?:
想要创建一个action,首先需要在action定义文件中定义三个消息格式,分别是:目标,结果,反馈。
action的定义文件后缀格式为.action
。action的定义文件看起来和server的定位文件很相似。在编译过程中该文件也会被打包生成为一个消息类型。
tip:
在ROS功能包中:
msg/ :我们需要把自己定义的.msg文件放在这里
srv/:我们需要把自己定义的.srv文件放在这里
action/:我们需要把自己定义的.action文件放在这里
第一步肯定是要创建一个新的ROS包
catkin_create_pkg s_c_demo roscpp actionlib actionlib_msgs
我们需要在与src/
同等目录下新建一个action/
文件夹,用来放我们的.action
定义文件。
cd 到你自己的ROS包目录下
cd catkin_ws/s_c_demo/
mkdir action
接着就新建一个.action
文件,文件格式如下
#part1: the goal, to be sent by the client
---
#part2: the result, to be sent by the server upon completion
---
#part3: the feedback, to be sent by server during execution
作为demo,我们来写一个计数器。功能很简单:client给sever一个目标数字,server由0开始++,直到等于目标数字。
那么我们的.action
文件就该这么写
#part1: the goal message, to be sent by the client
int32 goal_num
---
#part2: the result message, to be sent by the server upon completion
bool finsh
---
#part3: the feedback message, to be sent by server during execution
float32 complete_percent
ok,一个简单的.action
文件就这样完成了
接下来就要将这个文件加入编译。那么查看CMakeLists.txt
文件,find_package应该已经在创建包时已经配置完成,所以我们只需要将剩下的add_action_files和generate_message去掉注释并且修改成下面这样子。
find_package(catkin REQUIRED COMPONENTS
actionlib_msgs
roscpp
actionlib
)
add_action_files(
FILES
demo.action
)
generate_messages(
DEPENDENCIES
actionlib_msgs
)
这是到此为止我生成的package.xml
和CMakeLists.txt
简洁起见,我去掉了一大段注释。
<?xml version="1.0"?>
<package format="2">
<name>s_c_demo</name>
<version>0.0.0</version>
<description>The s_c_demo package</description>
<maintainer email="tloinny@gmail.com">tloinny</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>actionlib</build_export_depend>
<build_export_depend>actionlib_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_depend>message_generation</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
到此,我们就可以开始编译了,编译成功以后会生成一系列的.msg
文件
可以在类似这样的目录下catkin_ws/devel/share/s_c_demo/msg
找到。
而且在类似catkin_ws/devel/include/s_c_demo/
的目录下还可以找到刚刚生成的头文件。
2. 开始coding
前面的准备工作做足了,现在我们可以开始打码了。
这里我用C++为例
首先在src/
目录下新建两个cpp文件
我起名为action_server.cpp
和action_client.cpp
action server 节点源码
/*
*action服务端
*/
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "s_c_demo/demoAction.h" /* 这个头文件每个人写的的名字都可能不同,package name/header file name.h */
typedef actionlib::SimpleActionServer<s_c_demo::demoAction> Server;
/*
*action 接口
*/
void execute(const s_c_demo::demoGoalConstPtr& demo_goal, Server* as)
{
ros::Rate r(1); /* 设置运行频率,这里设置为1hz */
s_c_demo::demoFeedback feedback; /* 创建一个feedback对象 */
ROS_INFO("THE GOAL IS: %d", demo_goal -> goal);
int count = 0;
for (; count < demo_goal -> goal; ++count)
{
feedback.complete_percent = count;
as -> publishFeedback(feedback);
r.sleep();
}
ROS_INFO("COUNT DONE");
as -> setSucceeded(); /* 发送result */
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "action_server_demo");
ros::NodeHandle nh;
/* 创建server对象; */
Server server(nh, "action_demo", boost::bind(&execute, _1, &server), false);
server.start();
ros::spin();
return 0;
}
action client 节点源码
/*
*action客户端
*/
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include "s_c_demo/demoAction.h" /* 这个头文件每个人写的的名字都可能不同,package name/header file name.h */
typedef actionlib::SimpleActionClient<s_c_demo::demoAction> Client;
/*
*action完成时的回调函数,一次性
*/
void doneCd(const actionlib::SimpleClientGoalState& state, const s_c_demo::demoResultConstPtr& result)
{
ROS_INFO("DONE");
ros::shutdown();
}
/*
*action启动时的回调函数,一次性
*/
void activeCd()
{
ROS_INFO("ACTIVE");
}
/*
*action收到反馈时的回调函数
*/
void feedbackCb(const s_c_demo::demoFeedbackConstPtr& feedback)
{
ROS_INFO("THE NUMBER RIGHT NOM IS: %f", feedback -> complete_percent);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "action_client_demo");
/* 定义一个客户端 */
Client client("action_demo", true); /* 这里的第一次参数要特别注意,我这里起名为action_demo,这个名称关系到server和client之间的配对通讯,两边代码对于这个名称必须要一致,否则两个节点无法相互通讯。 */
/* 等待服务端响应 */
ROS_INFO("WAITING FOR ACTION SERVER TO START !");
client.waitForServer();
ROS_INFO("ACTION SERVER START !");
/* 创建一个目标对象 */
s_c_demo::demoGoal demo_goal;
demo_goal.goal = 100; /* 设置目标对象的值 */
/* 发送目标,并且定义回调函数 */
client.sendGoal(demo_goal, &doneCd, &activeCd, &feedbackCb);
ros::spin();
return 0;
}
3.开始编译吧!
常规操作修改完CMakeLists.txt后,就可以开始编译了。这一步没什么好说的。
4.看看运行效果!
客户端运行效果
服务端运行效果
直观上看到的节点关系