ROS學習筆記五：創建和使用ROS msg和srv

一、msg和srv簡介

1.1 msg文件

msg文件就是一個簡單的text文件，其中每行有一個類型和名稱，可用的類型如下：

int8, int16, int32, int64 (plus uint*)
float32, float64
string
time, duration
other msg files
variable-length array[] and fixed-length array[C] 
Header(包含一個timestamp和坐標系信息)

1.2 srv文件

srv文件和msg文件很相像，除了它包含兩個部分：請求和回應。

int64 A
int64 B
---
int64 sum

上面一部分為request，下面一部分為response。

二、創建msg和srv文件

2.1 創建msg

首先，在 chapter2_tutorials 功能包下創建 msg 文件夾，並在其中創建一個新的文件 chapter2_msg1.msg。

$ cd ~/dev/catkin_ws/src/chapter2_tutorials
$ mkdir msg
$ vim chapter2_msg1.msg

並在文件中增加如下行：

int32 A
int32 B
int32 C

現在編輯 package.xml，從 <build_depend>message_generation</build_depend> 和 <run_depend>message_runtime</run_depend>行刪除<!-- -->，按照下面加入 message_generation：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  message_generation
)

找到如下行，取消注釋，並加入新消息名稱：

## Generate messages in the 'msg' folder
add_message_files(
  FILES
  chapter2_msg1.msg
)

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)

現在，用下面命令進行編譯：

$ cd ~/dev/catkin_ws/
$ catkin_make

檢查編譯是否成功，使用下面 rosmsg 命令：

$ rosmsg show chapter2_tutorials/chapter2_msg1

如果你在 chapter2_msg1 文件中看到一樣的內容，說明編譯正確。

2.2 創建srv文件

然后創建一個 srv 文件。在 chapter2_tutorials 文件夾下創建一個名為 srv 的文件夾，並新建文件 chapter2_srv1.srv，在文件中增加以下行：

int32 A
int32 B
int32 C
---
int32 sum

為了編譯新的 msg 和 srv 文件，必須取消在 package.xml 和 Cmakefile.txt 中的如下行的注釋。這些包括消息和服務的配置信息，並告訴ROS如何編譯。

首先，按下面方式從 chapter2_tutorials 功能包中打開 CMakefile.txt 文件：

$ rosed chapter2_tutorials CMakefile.txt

找到下面行，取消注釋，並改為正確數據：

catkin_package(
  CATKIN_DEPENDS message_runtime
)

在 add_service_files 如下位置添加服務文件的名字：

add_service_files(
  FILES
  chapter2_srv1.srv
)

現在，用下面命令進行編譯：

$ cd ~/dev/catkin_ws/
$ catkin_make

測試編譯是否成功，使用如下 rossrv 命令：

$ rossrv show chapter2_tutorials/chapter2_srv1

如果你看到跟 chapter2_srv1 文件中相同的內容，說明編譯正確。

三、使用新建的msg和srv文件

3.1 使用新建的srv文件

下面我們將學習如何在ROS中使用新建的服務。該服務將會對三個整數求和。我們需要兩個節點，一個服務器一個客戶端。

在 chapter2_tutorials 功能包中，新建兩個節點並以 example2_a.cpp 和 example2_b.cpp 為名稱。別忘了要在 src 文件夾下創建這兩個文件。

在第一個文件 example2_a.cpp（服務端） 中，添加以下代碼：

#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_srv1.h" //包含先前所創建的srv文件

//對3個變量求和，並將計算結果發生給其他節點，Request為上一部分的請求，而Response為下一部分的響應
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=%d, B=%d C=%d", (int)req.A, (int)req.B, (int)req.C);
  ROS_INFO("sending back response: [%d]", (int)res.sum);
  return true;
}

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

  //創建服務並在ROS中發布廣播
  ros::ServiceServer service = n.advertiseService("add_3_ints", add);
    
  ROS_INFO("Ready to add 3 ints."); //在命令行窗口輸出信息
  ros::spin();

  return 0;
}

在第二個文件 example2_b.cpp（客戶端） 中，添加以下代碼：

#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_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;
  //以add_3_ints為名稱創建一個服務的客戶端
  ros::ServiceClient client = n.serviceClient<chapter2_tutorials::chapter2_srv1>("add_3_ints");
  //下面創建srv文件的一個實例，並且加入需要發生的數據值
  chapter2_tutorials::chapter2_srv1 srv;
  srv.request.A = atoll(argv[1]);
  srv.request.B = atoll(argv[2]);
  srv.request.C = atoll(argv[3]);
  
  //調用服務並發生數據。如果調用成功，call()函數會返回true；如果沒成功，call()函數會返回false
  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;
}

為了編譯節點，在 CMakefile.txt 文件中增加如下行：

add_executable(chap2_example2_a src/example2_a.cpp)
add_executable(chap2_example2_b src/example2_b.cpp)

add_dependencies(chap2_example2_a chapter2_tutorials_generate_messages_cpp)
add_dependencies(chap2_example2_b chapter2_tutorials_generate_messages_cpp)

target_link_libraries(chap2_example2_a ${catkin_LIBRARIES})
target_link_libraries(chap2_example2_b ${catkin_LIBRARIES})

現在執行以下命令：

$ cd ~/dev/catkin_ws
$ catkin_make

為了啟動節點，需要執行以下命令行：

$ rosrun chapter2_tutorials chap2_example2_a
$ rosrun chapter2_tutorials chap2_example2_b 1 2 3

並且你會看到如下顯示：

root@feng-Matrimax-PC:~/dev/catkin_ws# rosrun chapter2_tutorials chap2_example2_a
[ INFO] [1553246488.138400943]: Ready to add 3 ints.
[ INFO] [1553246527.807015869]: request: A=1, B=2 C=3
[ INFO] [1553246527.807034041]: sending back response: [6]

3.2 使用新建的msg文件

現在將要用自定義的 msg 文件來創建節點。在 chapter2_tutorials 功能包中，新建兩個節點並以 example3_a.cpp 和 example3_b.cpp 為名稱。

將下面的代碼放在 example3_a.cpp 文件中：

#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_msg1.h"
#include <sstream>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "example3_a");
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<chapter2_tutorials::chapter2_msg1>("message", 1000);
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    //這里使用了自定義消息類型int32 A，int32 B,int32 C
    chapter2_tutorials::chapter2_msg1 msg;
    msg.A = 1;
    msg.B = 2;
    msg.C = 3;
    pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

將下面的代碼放在 example3_b.cpp 文件中：

#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_msg1.h"

void messageCallback(const chapter2_tutorials::chapter2_msg1::ConstPtr& msg)
{
  //這里使用了自定義消息類型int32 A，int32 B,int32 C
  ROS_INFO("I heard: [%d] [%d] [%d]", msg->A, msg->B, msg->C);
}

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

運行這兩個節點，將會看到如下信息：

...
[ INFO] [1553247022.251160406]: I heard: [1] [2] [3]
[ INFO] [1553247022.350904698]: I heard: [1] [2] [3]
[ INFO] [1553247022.451092989]: I heard: [1] [2] [3]
[ INFO] [1553247022.551088430]: I heard: [1] [2] [3]
[ INFO] [1553247022.651140358]: I heard: [1] [2] [3]
...