ros c++

9、ROS使用C++编写一个简单的Service和Client

Posted by pzque on 2016-04-19     

在开始本篇文章之前，默认我们在前面的几篇文章中已经进行了如下的工作：

  1. 创建了beginner_tutorial包
  2. 在beginner_tutorial包中创建了AddTwoInts.srv文件

如果没有满足以上条件的话，请重新阅读本系列的第7、8篇文章。

一、Service节点

在包内的src下创建add_two_ints_server.cpp，粘贴以下内容：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
# include "ros/ros.h"
# include "beginner_tutorials/AddTwoInts.h"

bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_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;
}

现在逐一对以上代码进行解释。

首先"beginner_tutorials/AddTwoInts.h"是由我们之前创建的srv文件自动生成的头文件，此文件内定义了要使用的服务的类型。 

1
2
bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::AddTwoInts::Response &res)

上面函数的两个参数分别为srv文件内定义的AddTwoInts的request类型和response类型。

1
2
3
4
5
6
{
  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;
}

由代码可以看到此函数简单的把req.a和req.b相加，赋值给res.sum。然后打印两条简单的信息。

然后ros::init()和ros::NodeHandle前面已经提过，这里不再讲解。

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

上面则创建了一个名为add_two_ints的服务，服务的具体功能就是add函数实现的功能，接受请求，返回响应。

二、Client节点

在包内的src下创建add_two_ints_client.cpp，粘贴以下内容：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
# include "ros/ros.h"
# include "beginner_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<beginner_tutorials::AddTwoInts>("add_two_ints");
  beginner_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;
}

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

上面的代码创建了一个可以请求"add_two_ints"服务的ros::ServiceClient对象。

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

首先声明了一个AddTwoInts类型的对象srv，然后把命令行的两个参数转为长整型分别赋值给srv.request.a和srv.request.b。其中AddTwoInts是srv文件自动生成的服务类，每个这样的类都包含request和response两个成员。

1
if (client.call(srv))

client.call()会请求一次服务，当请求成功时会返回true并且响应会赋值给srv的response成员。如果失败的话则返回false，这时候srv的response是无效的。

三、构建节点

编辑~/catkin_ws/src/beginner_tutorials/CMakeLists.txt文件，在其最后添加以下内容：

1
2
3
4
5
6
7
add_executable(add_two_ints_server src/add_two_ints_server.cpp)
target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
add_dependencies(add_two_ints_server beginner_tutorials_gencpp)

add_executable(add_two_ints_client src/add_two_ints_client.cpp)
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
add_dependencies(add_two_ints_client beginner_tutorials_gencpp)

之前已经讲解过这几个配置的含义，这里不再细说。

然后进入工作空间顶层 目录，执行catkin_make：

1
2
cd ~/catkin_ws
catkin_make

默认情况下会在~/catkin_ws/devel/lib/<package name>目录下生成可执行文件，我们可以直接执行它们，也可以使用rosrun启动它们。

四、启动节点

启动服务：

1
rosrun beginner_tutorials add_two_ints_server

得到：

1
[ INFO] [1461055492.802877894]: Ready to add two ints.

启动客户端：

1
rosrun beginner_tutorials add_two_ints_client 1 3

得到：

1
[ INFO] [1461055704.698358074]: Sum: 4
FEATURED TAGS
ros c++
FRIENDS