ROS知识（5）----消息与服务的示例

ROS中已经定义了较多的标准类型的消息，你可以用在这些标准类型的消息上再自定义自己的消息类型。这个在复杂数据传输很有用，例如节点和服务器进行交互时，就可能用到传输多个参数到服务器，并返回相应的结果。为了保证例子的完整，将详述每一步。

基本思路和创建talker和listener的例子类似，步骤如下：

• 建立工作空间workspace（类似于vs下的解决方案，用来管理很多的项目）；

• 建立包package（类似于vs下的项目）；
• 创建msg和srv文件；
• 编写服务节点和客户节点代码；
• 利用rosmake进行编译（catkin_make也可以，但稍有不同，请参考另一篇博文的ROS知识(3)）；
• 利用rosrun运行；

1.1、创建工作空间

在开始具体工作之前，首先创建工作空间，并且为工作空间设置环境变量到~/.bashrc中，如果要查看已有的空间路径，可以用查询命令

$ echo $ROS_PACKAGE_PATH

你将会看到如下的信息：

/home/horsetail/dev/rosbook:/home/horsetail/catkin_ws/src:/opt/ros/jade/share:/opt/ros/jade/stacks

这里的创建空间实际上就是先建立一个文件夹，然后把文件夹的路径设置到环境变量~/.bashrc中。例如我们这里创建目录~/dev/rosbook作为工作空间。

首先执行命令：

$ cd ~
$ mkdir -p dev/rosbook

然后将创建的路径加入到环境变量中，执行如下命令：

$ echo "export ROS_PACKAGE_PATH=~/dev/rosbook:${ROS_PACKAGE_PATH}" >> ~/.bashrc
$ . ~/.bashrc

这样，我们就完成了工作空间的配置，注意：ROS安装的时候，一定要把ROS的环境变量也加到~/.bashrc中。这里还需要把ROS。接下来就是在这个空间下创建包了。

1.2、创建包

 可以手动创建包，但是非常的繁琐，为了方便，最好使用roscreate-pkg命令行工具，该命令行的格式如下：

roscreate-pkg [package_name] [depend1] [depend2] [depend3] ...

命令行包含了要创建包的名字，依赖包。

我们的例子中，创建一个叫mypacakge1的 新包，命令如下：

$ cd ~/dev/rosbook
$ roscreate-pkg mypackage1 std_msgs roscpp rospy

过一会弹出如下的信息，表示创建成功：

Created package directory /home/horsetail/dev/rosbook/mypackage1
Created include directory /home/horsetail/dev/rosbook/mypackage1/include/mypackage1
Created cpp source directory /home/horsetail/dev/rosbook/mypackage1/src
Created package file /home/horsetail/dev/rosbook/mypackage1/Makefile
Created package file /home/horsetail/dev/rosbook/mypackage1/manifest.xml
Created package file /home/horsetail/dev/rosbook/mypackage1/CMakeLists.txt
Created package file /home/horsetail/dev/rosbook/mypackage1/mainpage.dox

Please edit mypackage1/manifest.xml and mainpage.dox to finish creating your package

好了这样就完成了包的创建，我们发现在mypackage1的目录下有一个src文件夹，我们接下来就是网这里添加源程序了。

1.3、创建msg和srv文件

 首先，在mypackage1功能包下，创建msg文件夹，并在其中创建一个新的文件mypackage_msg1.msg，将在这个文件里自定义消息的类型，在文件中添加以下代码：

int32 A
int32 B
int32 C

现在编辑CMakeList.txt，从#rosbuild_genmsg()这一行中删除#，然后使用rosmake命令编译功能包：

$ rosmake mypackage1

为了检查正确性，使用rosmsg命令:

$ rosmsg show mypackage1/mypackage1_msg1

如果看到的内容和文件一样，说明编译正确。

现在新建一个srv文件，在mypackage1文件夹下建立srv文件夹，并在srv文件夹下新建一个文件mypackage1_srv1.srv，并添加以下代码：

int32 A
int32 B
int32 C
---
int32 sum

编辑CMakeList.txt，从#rosbuild_genmsg()这一行中删除#，然后使用rosmake命令编译功能包，可以通过一下命令验证正确性：

$ rossrv show mypackage1/mypackage1_srv1

如果看到的内容和文件一样，说明编译正确。

1.4、编写服务节点和客户节点代码

接下来建立用于验证服务中请求响应的代码，在mypackage1/src下新建文件example_srv_request.cpp，添加如下代码：

#include "ros/ros.h"
#include "mypackage1/mypackage_srv1.h"

bool add(chapter2_tutorials::chapter2_srv1::Request  &req,
         chapter2_tutorials::chapter2_srv1::Response &res)
{
  res.sum = req.A + req.B + req.C;
  ROS_INFO("request: A=%ld, B=%ld C=%ld", (int)req.A, (int)req.B, (int)req.C);
  ROS_INFO("sending back response: [%ld]", (int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_3_ints_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("add_3_ints", add);
  ROS_INFO("Ready to add 3 ints.");
  ros::spin();

  return 0;
}

在mypackage1/src下新建文件example_srv_respone.cpp，添加如下代码：

#include "ros/ros.h"
#include "mypackage1/mypackage_srv1.h"
#include <cstdlib>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_3_ints_client");
  if (argc != 4)
  {
    ROS_INFO("usage: add_3_ints_client A B C ");
    return 1;
  }

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<chapter2_tutorials::chapter2_srv1>("add_3_ints");
  chapter2_tutorials::chapter2_srv1 srv;
  srv.request.A = atoll(argv[1]);
  srv.request.B = atoll(argv[2]);
  srv.request.C = atoll(argv[3]);
  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;
}

接下来建立用于验证消息传递的代码，在mypackage1/src下新建文件example_talker_msg.cpp，添加如下代码：

#include "ros/ros.h"
#include "mypackage1/mypackage1_msg1.h"
#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "example_talker_msg");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<chapter2_tutorials::chapter2_msg1>("message", 1000);
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    chapter2_tutorials::chapter2_msg1 msg;
    msg.A = 1;
    msg.B = 2;
    msg.C = 3;
    pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

在mypackage1/src下新建文件example_listener_msg.cpp，添加如下代码：

#include "ros/ros.h"
#include "mypackage1/mypackage1_msg1.h"

void messageCallback(const chapter2_tutorials::chapter2_msg1::ConstPtr& msg)
{
  ROS_INFO("I heard: [%d] [%d] [%d]", msg->A, msg->B, msg->C);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "example_listener_msg");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("message", 1000, messageCallback);
  ros::spin();
  return 0;
}

好了，至此完成了服务和消息的测试代码编写。

1.5、利用rosmake进行编译

 接下来，要告诉编译器如何去找到这两个文件。你需要打开mypackage1/CMakeLists.txt，在文件的末尾添加两行命令：

rosbuild_add_executable(example_srv_request src/example_srv_request.cpp)
rosbuild_add_executable(example_srv_respone src/example_srv_respone.cpp)
rosbuild_add_executable(example_talker_msg src/example_talker_msg.cpp)
rosbuild_add_executable(example_listener_msg src/example_listener_msg.cpp)

 添加后的文件结构是这样的：

cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

# Set the build type.  Options are:
#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
#  Debug          : w/ debug symbols, w/o optimization
#  Release        : w/o debug symbols, w/ optimization
#  RelWithDebInfo : w/ debug symbols, w/ optimization
#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)

rosbuild_init()

#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

#uncomment if you have defined messages
rosbuild_genmsg()
#uncomment if you have defined services
rosbuild_gensrv()

#common commands for building c++ executables and libraries

rosbuild_add_executable(example_srv_request src/example_srv_request.cpp)
rosbuild_add_executable(example_srv_respone src/example_srv_respone.cpp)
rosbuild_add_executable(example_talker_msg src/example_talker_msg.cpp)
rosbuild_add_executable(example_listener_msg src/example_listener_msg.cpp)

 这样用rosmake命令来编译这个mypackage1包了。执行下面的命令：

$ rosmake mypackage1

输出下面的信息：

horsetail@horsetail-book:~$ roscore
... logging to /home/horsetail/.ros/log/6eae5b9c-628d-11e5-8bd7-3859f9722953/roslaunch-horsetail-book-6447.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://horsetail-book:44362/
ros_comm version 1.11.13


SUMMARY
========

PARAMETERS
 * /rosdistro: jade
 * /rosversion: 1.11.13

NODES

auto-starting new master
process[master]: started with pid [6459]
ROS_MASTER_URI=http://horsetail-book:11311/

...(内容太长了，省去)
             
[ rosmake ] Results:                                                            
[ rosmake ] Built 26 packages with 0 failures.                                  
[ rosmake ] Summary output to directory                                         
[ rosmake ] /home/horsetail/.ros/rosmake/rosmake_output-20150924-164014  

哇，编译通过，大家注意到实际上也是用catkin进行编译的，额。我们来运行一下吧。

1.4、运行

首先打开一个新的终端，启动初始化ROS，执行命令：

$ roscore

先验证一下服务请求和相应的功能，需要在不同窗口分别执行以下命令：

rosrun mypackage1 example_talker_msg
rosrun mypackage1 example_listener_msg

可以看到请求listener的窗口，显示如下信息：

[ INFO] [1443154332.742277621]: I heard:[1][2][3]
[ INFO] [1443164722.557755739]: I heard:[1][2][3]
[ INFO] [1443164744.557858055]: I heard:[1][2][3]

好了至此完成了服务srv和消息msg的验证。
 

1.5、源码

最后，附上源码：mypackage1.tar.gz（这个是ROS机器人程序设计中第二章的例子，里面包含了消息和服务等例子）

参考资料

[1]. Aaron Martinez Enrique Fern andez, ROS机器人程序设计[B], P14-42, 2014.