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.