ROS动作通信的编程实现

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 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，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.5action服务器编程

在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 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指针。server(n, “robot_motion”, boost::bind(&execute, _1, &server), false)中的第四个参数是一个bool值，它的作用是通知action服务器是否在被创建后立即开始发布，此参数应当始终设置为false，并且在服务器被创建后调用start()。
◆server.start();的作用是启动action服务器，使用时需保证创建action服务器时第四个参数auto_start设置为false。
◆ros::spin();的作用是让程序进入自循环的挂起状态，从而让程序以最好的效率接收客户端的请求并调用回调函数。
附相关资料：
1.http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionServer%28ExecuteCallbackMethod%29
2.https://docs.ros.org/diamondback/api/actionlib/html/classactionlib_1_1SimpleActionServer.html
3.http://docs.ros.org/melodic/api/actionlib/html/classactionlib_1_1ServerGoalHandle.html#ac297923512ac62c9f10a801571b29738

3.配置与编译

3.1在CMaKeLists.txt中添加编译选项

打开功能包中的CMaKeLists.txt文件，在如下位置的find_package中添加功能包actionlib和actionlib_msgs，以便于（节点）调用它们生成消息。

在如下位置添加相关的.action文件，确保了CMake在重新配置时知道这些新添加的.action文件，同时添加.action文件在生成消息时的所有依赖项（功能包）。

1
2
3

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 ${${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>