1. 创建功能包
在ROS工作空间ROS_ws的src文件夹目录下创建一个功能包，命名为action_task。
2. 节点编程与动作消息定义
2.1 案例说明
客户端发送一个运动目标，模拟机器人运动到目标位置的过程，包含服务端和客户端的代码实现，要求带有实时位置反馈。
2.2 动作消息的定义
在功能包目录下创建一个新的文件夹，命名为action，并在此文件夹中创建一个空文件Motion.action。
在Motion.action文件中输入以下代码，定义动作消息内容。
//定义机器人运动终点坐标endx，endy
uint32 endx
uint32 endy
---
//定义机器人动作完成标志位
uint32 Flag_Finished
---
//定义机器人当前位置坐标coordinate_x，coordinate_y
uint32 coordinate_x
uint32 coordinate_y
说明：
- 动作(Action)通信接口提供了五种消息定义，分别为goal、cancel、status、feedback和result，而.action文件用来定义其中三种消息，按顺序分别为goal、result和feedback，与.srv文件中的服务消息定义方式一样，使用“—”作为分隔符。
Motion.action文件经过编译后生成MotionAction.h、MotionActionFeedback.h和MotionGoal.h等多个头文件，文件位置在ROS_ws工作空间下的devel/include/action_task文件夹内，如下图。
相关资料：http://wiki.ros.org/actionlib#Tutorials
2.3 创建.cpp文件
在功能包下面的action文件夹目录下创建一个空文件robotclient.cpp。
2.4 动作客户端编程
打开上面所创建的文件robotclient.cpp，输入以下代码。
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include "action_task/MotionAction.h"
typedef actionlib::SimpleActionClient<action_task::MotionAction> Client;
//当动作完成后，调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
const action_task::MotionResultConstPtr& result)
{
ROS_INFO("The robot has arrived at the destination.");
ros::shutdown();
}
//当动作被激活，调用该函数一次
void activeCb()
{
ROS_INFO("Goal just went active");
}
//接收到feedback后，调用该回调函数
void feedbackCb(const action_task::MotionFeedbackConstPtr& feedback)
{
ROS_INFO(" The place of robot : (%d , %d) ", feedback->coordinate_x, feedback->coordinate_y);
}
int main(int argc, char** argv)
{
//初始化ROS节点
ros::init(argc, argv, "robot_client");
//创建一个action客户端
Client client("robot_motion", true);
//等待action服务器响应
ROS_INFO("Waiting for action server to start.");
client.waitForServer();
ROS_INFO("Action server started, sending goal.");
//创建一个action目标
action_task::MotionGoal goal;
goal.endx = 5;
goal.endy = 4;
//发送action目标给服务器，并设置回调函数
client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
ros::spin();
return 0;
}
说明：
- 头文件actionlib/client/simple_action_client.h用于实现简单的动作客户端。其中actionlib软件包提供了创建执行长期目标的服务器，以及一个客户端接口，以便将请求发送到服务器。
- 头文件action_task/MotionAction.h是由Motion.action文件编译生成，定义了动作消息内容，该文件存放在ROS_ws工作空间下的devel/include/action_task文件夹中。
- typedef actionlib::SimpleActionClient<action_task::MotionAction> Client;的作用是使用Client声明定义action_task::MotionAction类型的actionlib::SimpleActionClient类。
- doneCb(const actionlib::SimpleClientGoalState& state, const action_task::MotionResultConstPtr& result)该函数在服务器完成任务后，通知客户端调用一次，调用该回调函数后ROS系统在终端界面输出字符串”The robot has arrived at the destination.”表明动作完成，同时ros::shutdown()关闭客户端节点，终止所有开放的订阅，发布，服务及调用。
- void activeCb()该函数是在动作服务器被激活后，通知客户端开始执行任务的回调函数，调用该回调函数后ROS系统在终端界面输出字符串”Goal just went active”表明action被激活。
- void feedbackCb(const action_task::MotionFeedbackConstPtr& feedback)该函数是在动作服务器执行任务过程中，通知客户端机器人当前坐标的回调函数。const action_task::MotionFeedbackConstPtr& feedback这里存放的是由主调函数放进来的实参变量feedback的地址，通过引用传递给回调函数。调用该回调函数后ROS系统在终端界面输出当前机器人位置的横纵坐标。
- main函数中首先使用ros::init()初始化ROS节点，将该节点命名为robot_client。
- Client client(“robot_motion”, true);的作用是创建一个action_task::MotionAction类型的action客户端。Client是之前已经使用typedef重新声明定义的actionlib::SimpleActionClient<action_task::MotionAction>，client是一个action_task::MotionAction消息类型的对象，第一个参数表示action客户端与服务器之间通信的消息名称，第二个参数true表示action客户端以单线程运行。
- client.waitForServer()函数的作用是等待客户端连接到动作服务器，否则客户端将停在这里一直处于等待，不过也可以使用ros::duration()函数作为参数设置等待时间。
- action_task::MotionGoal goal是创建一个action_task::MotionGoal类的对象goal，并为goal成员endx、endy即终点坐标赋值。
- client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);的作用是发送一个目标值到action服务器，并设置多个回调函数检测goal的执行情况（action服务器和客户端分别在接收和发送goal之后会通过状态机追踪goal的状态，并采取相应的处理）。
- ros::spin();的作用是让程序进入自循环的挂起状态，从而让程序以最好的效率接收客户端的请求并调用回调函数。
2.5 action服务器编程
在src文件夹下再创建一个空文件robotserver.cpp，输入以下代码。
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "action_task/MotionAction.h"
#define wid 5
#define hig 5
typedef actionlib::SimpleActionServer<action_task::MotionAction> Server;
//定义结构体
struct note
{
int x;//横坐标
int y;//纵坐标
int f;//父节点在队列中的编号
int s;//步数
};
//接收到action的goal之后，调用该回调函数一次
void execute(const action_task::MotionGoalConstPtr& goal, Server* as)
{
struct note que[40];//定义一个note结构体的队列
//定义地图大小及形式，1为障碍物，0为正常道路
int map[6][6]={{0, 0, 1, 0, 0, 1},
{1, 0, 1, 0, 0, 0},
{1, 0 ,0, 1, 0, 1},
{0, 1, 0, 0, 0, 0},
{0, 0, 0, 1, 1, 0},
{1, 0, 0, 1, 0, 0}};
//记录哪些点已经在队列中，防止一个点被重复扩展
int book[6][6]={0};
//定义一个表示走的方向的数组
int next[4][2]={{0,1}, //向右
{1,0}, //向下
{0,-1},//向左
{-1,0}};//向上
int head, tail;
int j, k, l;
int startx = 0, starty = 0;
int p,q,tx,ty,flag;
ros::Rate r(1); //设置ROS系统延时频率
action_task::MotionFeedback feedback; //创建一个feedback对象
//初始化队列
head=0;//head point to the point needed to expand
tail=0;//tail point to the point expanded
//定义起点坐标
que[tail].x=startx;
que[tail].y=starty;
que[tail].f=0;
que[tail].s=0;
tail++;
book[startx][starty]=1;
flag=0;//用来标记是否到达目标点，0表示还没有到达，1表示已到达
//当队列不为空时循环
while(head<tail)
{
//枚举四个方向
for(k=0;k<=3;k++)
{
//计算下一个点的坐标
tx=que[head].x+next[k][0];
ty=que[head].y+next[k][1];
//判断是否越界
if(tx<0 || tx>wid || ty<0 || ty>hig)
continue;
//判断是否为障碍物或已经在路径中
if(map[tx][ty]==0 && book[tx][ty]==0)
{
//标记这个点已经被走过
book[tx][ty]=1;
//插入新的坐标到队列中
que[tail].x=tx;
que[tail].y=ty;
que[tail].f=head;
que[tail].s=que[head].s+1;
tail++;
}
feedback.coordinate_x = tx;
feedback.coordinate_y = ty;
as->publishFeedback(feedback);//按照1Hz的频率发送机器人当前坐标位置
r.sleep();//延时至1s
if(tx==goal->endx && ty==goal->endy)
{
flag=1;//到达目标点后标志位flag置1
break;
}
}
if(flag==1)
break;
head++;
}
//表示已经发送成功
as->setSucceeded();
}
int main(int argc, char** argv)
{
//初始化ROS节点
ros::init(argc, argv, "robot_server");
//创建节点句柄
ros::NodeHandle n;
//创建一个action服务器
Server server(n, "robot_motion", boost::bind(&execute, _1, &server), false);
//启动action服务器
server.start();
ros::spin();
return 0;
}
说明：
- 头文件ros/ros.h包含了标准ROS类的声明，在每一个ROS程序中都需要包含它。
- 头文件actionlib/server/simple_action_server.h用于实现简单的动作服务端。
- 头文件action_task/MotionAction.h 是由Motion.action文件编译生成，定义了动作消息内容，该文件存放在ROS_ws工作空间下的devel/include/action_task文件夹中。
- typedef actionlib::SimpleActionServer<action_task::MotionAction> Server;的作用是使用Server声明定义action_task::MotionAction类型的actionlib::SimpleActionServer类。
- void execute(const action_task::MotionGoalConstPtr& goal, Server_ as)当action服务器接收到客户端的goal时调用该回调函数。
①第一个参数是通过引用向回调函数传递的指向goal的指针，goal的消息类型为action_task::MotionGoal；
②第二个参数是actionlib::SimpleActionServer类的指针as，消息类型为action_task::MotionAction。 - 在execute()回调函数中通过建立一个6×6的数组模拟机器人运行的地图，根据action客户端发来的goal目标点坐标，使用广度优先搜索算法寻找可运行的路径，服务器按照1Hz频率发布feedback。
- ros::Rate r(1);的作用是设置feedback发布频率为1Hz。
- action_task::MotionFeedback feedback;的作用是创建一个action_task::MotionFeedback类的对象feedback。
- as->publishFeedback(feedback);调用指针as指向的actionlib::SimpleActionServer类所包含的publishFeedback()函数，publishFeedback (const Feedback &feedback)函数用于发布feedback。
- r.sleep();该函数并不是必要的，这里的作用是为了延时，让服务器保证以1Hz的频率发布feedback。
- as->setSucceeded();调用指针as指向的actionlib::SimpleActionServer类所包含的setSucceeded ()函数，setSucceeded (const Result &result=Result(), const std::string &text=std::string(“”))函数中第一个参数表示动作执行任务的结果result，默认为0，第二个参数是有关状态更改的文本消息，两个参数均可省略。
- main函数中一开始都是类似的，初始化ROS节点，创建节点句柄，从而启动ROS节点。
- Server server(n, “robot_motion”, boost::bind(&execute, _1, &server), false);的作用是创建一个action服务器。
- 第一个参数表示所创建节点句柄名；
- 第二个参数表示服务器与客户端之间通信的消息名；
- 第三个参数表示所要调用的回调函数，当接收到新的goal后，就会在单独的线程中调用该回调函数。
其中boost::bind()函数用来向一个函数(或函数对象)绑定某些参数，返回值是一个函数对象。
- 第一个参数是需要绑定的原始函数的地址，此处为&execute；
- 第二个参数是需要绑定到原始函数excute()的第一个参数值，而此处_1表示execute(const action_task::MotionGoalConstPtr& goal, Server_ as)函数的第一个参数不需要绑定，因此使用_1占位；
- 第三个参数是需要绑定到原始函数excute()的第二个参数值，此处为&server表示将server对象的地址绑定到execute()函数中的Server* as指针。
- 第四个参数是一个bool值，它的作用是通知action服务器是否在被创建后立即开始发布，此参数应当始终设置为false，并且在服务器被创建后调用start()。
- server.start();的作用是启动action服务器，使用时需保证创建action服务器时第四个参数auto_start设置为false。
- ros::spin();的作用是让程序进入自循环的挂起状态，从而让程序以最好的效率接收客户端的请求并调用回调函数。
附相关资料：
(1) Writing a Simple Action Server using the Execute Callback
(2) actionlib::SimpleActionServer< ActionSpec > Class Template Reference
(3) actionlib::ServerGoalHandle< ActionSpec > Class Template Reference
3. 配置与编译
3.1 在CMaKeLists.txt中添加编译选项
打开功能包中的CMaKeLists.txt文件，在如下位置的find_package中添加功能包actionlib和actionlib_msgs，以便于（节点）调用它们生成消息。
在如下位置添加相关的.action文件，确保了CMake在重新配置时知道这些新添加的.action文件，同时添加.action文件在生成消息时的所有依赖项（功能包）。
add_action_files(DIRECTORY action FILES Motion.action)
generate_messages(DEPENDENCIES actionlib_msgs std_msgs)
将如下位置中CATLIN_DEPENDS前面的“#”去掉，使能相关的依赖项。
在如下位置进行配置，add_executable(robotclient src/robotclient.cpp)的作用是将src文件夹下的robotclient.cpp文件编译成名为robotclient的可执行文件。target_link_libraries(robotclient ${catkin_LIBRARIES})的作用是将robotclient可执行文件与ROS相关的库链接。add_dependencies(robotclient KaTeX parse error: Expected ‘}’, got ‘EOF’ at end of input: {{PROJECT_NAME}_EXPORTED_TARGETS})的作用是创建一个显式的依赖，以便相关文件能以正确的顺序编译。
add_executable(robotclient src/robotclient.cpp)
target_link_libraries(robotclient ${catkin_LIBRARIES})
add_dependencies(robotclient ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_executable(robotserver src/robotserver.cpp)
target_link_libraries(robotserver ${catkin_LIBRARIES})
add_dependencies(robotserver ${${PROJECT_NAME}_EXPORTED_TARGETS})
3.2 在package.xml中添加功能包依赖
打开功能包中的package.xml文件，在如下位置添加功能包依赖。<build_depend>actionlib</build_depend>表示在编译时会依赖actionlib功能包。<exec_depend>actionlib</exec_depend>表示在运行时会依赖actionlib功能包。
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
3.3 编译文件
在/ROS_ws文件夹路径下打开一个新的终端，输入以下代码进行编译。
$ catkin_make
编译完成后，输入以下代码运行主节点。
$ roscore
打开一个新的终端，配置环境变量后，输入以下代码运行客户端。
$ rosrun action_task robotclient
打开一个新的终端，配置环境变量后，输入以下代码运行服务器。
$ rosrun action_task robotserver
若想停止运行，关闭终端，使用快捷键Ctrl+c即可。
4. 话题可视化
打开一个新的终端，在action服务器及客户端运行时输入以下代码。
$ rqt_graph
由此可以得到如下的基于Qt的GUI界面，直观地看到动作通信的节点和消息。
评论（0）
您还未登录，请登录后发表或查看评论