ROS 服务的请求与相应（基于 C++）

1、 创建一个 srv 

1.1 在 dashgo_tutorials package 中查看一个服务：

$ roscd dashgo_tutorials/srv
$ cat AddTwoInts.srv

显示： 

Int64 a
Int64 b
---
Int64 sum

1.2 使用 rossrv 

通过 rosmsg show 命令，检查 ROS 是否能够识该服务。

$ rossrv show dashgo_tutorials /AddTwoInts

显示：

int64 a
 int64 b
 ---
 int64 sum

也可以不指定具体的 package 名来查找服务文件：

$ rossrv show AddTwoInts

显示：

[dashgo_tutorials /AddTwoInts]:
int64 a
int64 b
---
int64 sum
[rospy_tutorials/AddTwoInts]:
int64 a
int64 b
---
int64 sum

2、编写 Service 节点 

2.1 进入 dashgo_tutorials 包所在的目录并查看 add_two_ints_server.cpp 文件： 

$ roscd dashgo_tutorials/src
$ cat add_two_ints_server.cpp

显示：

#include "ros/ros.h"
#include "dashgo_tutorials /AddTwoInts.h"
bool add(dashgo_tutorials ::AddTwoInts::Request &req, dashgo_tutorials ::AddTwoInts::Response &res)
{
 	res.sum = req.a + req.b;
 	ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
 	ROS_INFO("sending back response: [%ld]", (long int)res.sum);
 	return true;
}
int main(int argc, char **argv)
{
 	ros::init(argc, argv, "add_two_ints_server");
 	ros::NodeHandle n;
 	ros::ServiceServer service = n.advertiseService("add_two_ints", add);
 	ROS_INFO("Ready to add two ints.");
 	ros::spin();
 	return 0;
}

2.2 代码解释

#include "ros/ros.h"
#include "dashgo_tutorials /AddTwoInts.h"

dashgo_tutorials /AddTwoInts.h 是由编译系统自动根据我们先前创建的 srv 文件生成的对应 该 srv 文件的头文件。

bool add(dashgo_tutorials ::AddTwoInts::Request &req, dashgo_tutorials ::AddTwoInts::Response &res)

这个函数提供两个 int 值求和的服务，int 值从 request 里面获取，而返回数据装入 response 内，这些数据类型都定义在 srv 文件内部，函数返回一个 boolean 值。 

{
 	res.sum = req.a + req.b;
 	ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
 	ROS_INFO("sending back response: [%ld]", (long int)res.sum);
 	return true;
}

现在，两个 int 值已经相加，并存入了 response。然后一些关于 request 和 response 的信息 被记录下来。最后，service 完成计算后返回 true 值。

 ros::ServiceServer service = n.advertiseService("add_two_ints", add);

这里，service 已经建立起来，并在 ROS 内发布出来。

3 编写 Client 节点 

3.1 进入 dashgo_tutorials/src 包所在的目录并查看 add_two_ints_client.cpp 文件：

#include "ros/ros.h"
#include "dashgo_tutorials /AddTwoInts.h"
#include <cstdlib>
int main(int argc, char **argv)
{
 	ros::init(argc, argv, "add_two_ints_client");
 	if (argc != 3)
 	{
 		ROS_INFO("usage: add_two_ints_client X Y");
 		return 1;
 	}
 	ros::NodeHandle n;
 	ros::ServiceClient client = 
	n.serviceClient<dashgo_tutorials ::AddTwoInts>("add_two_ints");
 	dashgo_tutorials ::AddTwoInts srv;
 	srv.request.a = atoll(argv[1]);
 	srv.request.b = atoll(argv[2]);
 	if (client.call(srv))
 	{
 		ROS_INFO("Sum: %ld", (long int)srv.response.sum);
 	}
 	else
 	{
 		ROS_ERROR("Failed to call service add_two_ints");
 		return 1;
 	}
 	return 0;
}

3.2 代码解释

ros::ServiceClient client = n.serviceClient<dashgo_tutorials ::AddTwoInts>("add_two_ints");

这段代码为 add_two_ints service 创建一个 client。ros::ServiceClient 对象待会用来调用 service。

 dashgo_tutorials ::AddTwoInts srv;
 srv.request.a = atoll(argv[1]);
 srv.request.b = atoll(argv[2]);

这里，我们实例化一个由 ROS 编译系统自动生成的 service 类，并给其 request 成员赋值。 一个 service 类包含两个成员 request 和 response。同时也包括两个类定义 Request 和 Response。 

 if (client.call(srv))

这段代码是在调用 service。由于 service 的调用是模态过程（调用的时候占用进程阻止其他 代码的执行），一旦调用完成，将返回调用结果。如果 service 调用成功，call()函数将返回 true，srv.response 里面的值将是合法的值。如果调用失败，call()函数将返回 false， srv.response 里面的值将是非法的。

4、测试 Service 和 Client 

4.1 运行：

$ roscore

4.2 运行 Service

$ rosrun dashgo_tutorials add_two_ints_server

显示： 

Ready to add two ints.

4.3 运行 Client

$ rosrun dashgo_tutorials add_two_ints_client 1 3

显示： 

request: x=1, y=3
sending back response: [4]