本節(jié)介紹使用C++編寫ROS消息服務(wù)器節(jié)點(diǎn)和客戶端節(jié)點(diǎn)的方法和步驟，以及如何構(gòu)建和測試這兩個(gè)節(jié)點(diǎn)。
1．編寫服務(wù)器節(jié)點(diǎn)代碼
使用kdevelop在ros_tutorials/src目錄下創(chuàng)建一個(gè)名為add_two_ints_server的C++文件：
$ roscd ros_tutorials
$ cd src
$ kdevelopadd_two_ints_server.cpp
輸入如下代碼：
#include"ros/ros.h"
#include "ros_tutorials/AddTwoInts.h"
bool add(ros_tutorials::AddTwoInts::Request &req,
ros_tutorials::AddTwoInts::Response&res)
{
res.sum = req.a + req.b;1
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;
}
部分代碼解釋如下：
#include "ros_tutorials/AddTwoInts.h"
ros_tutorials/AddTwoInts.h是由編譯系統(tǒng)自動根據(jù)先前創(chuàng)建的srv文件生成的對應(yīng)該srv文件的頭文件。
bool add(ros_tutorials::AddTwoInts::Request &req,
ros_tutorials::AddTwoInts::Response&res)
該函數(shù)提供兩個(gè)int值求和的服務(wù)，int值從Request里面獲取，而返回?cái)?shù)據(jù)裝入Response內(nèi)，這些數(shù)據(jù)類型都定義在srv文件內(nèi)部，函數(shù)返回一個(gè)bool值。
{
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;
}
進(jìn)行兩個(gè)整數(shù)相加運(yùn)算，結(jié)果存儲在Response中。把Request和Response的一些信息記錄日志，最后當(dāng)運(yùn)算完成時(shí)服務(wù)返回True。
ros::ServiceServerservice = n.advertiseService("add_two_ints", add);
創(chuàng)建服務(wù)service并在ROS網(wǎng)絡(luò)上進(jìn)行廣播。
2．編寫客戶端節(jié)點(diǎn)代碼
使用kdevelop在ros_tutorials/src目錄下創(chuàng)建一個(gè)名為add_two_ints_client的C++文件：
$ roscd ros_tutorials
$ cd src
$ kdevelopadd_two_ints_client.cpp
輸入如下代碼：
#include"ros/ros.h"
#include "ros_tutorials/AddTwoInts.h"
#include
int main(int argc, char**argv)
{
ros::init(argc, argv,"add_two_ints_client");
if (argc != 3)
{
ROS_INFO("usage: add_two_ints_client XY");
return 1;
}
ros::NodeHandle n;
ros::ServiceClient client =n.serviceClient ("add_two_ints");
ros_tutorials::AddTwoInts srv;
srv.request.a = atoll(argv[1]);
srv.request.b = atoll(argv[2]);
if (client.call(srv))
{
ROS_INFO("Sum: %ld", (longint)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call serviceadd_two_ints");
return 1;
}
return 0;
}
部分代碼解釋如下：
ros::ServiceClientclient = n.serviceClient ("add_two_ints");
為add_two_ints服務(wù)創(chuàng)建一個(gè)客戶端。ros::ServiceClient對象用來隨后調(diào)用服務(wù)。
ros_tutorials::AddTwoIntssrv;
srv.request.a= atoll(argv[1]);
srv.request.b= atoll(argv[2]);
實(shí)例化一個(gè)自動生成的服務(wù)類，并為其request成員賦值。一個(gè)服務(wù)類包含兩個(gè)成員，request和response，也包括兩個(gè)類的定義：Request和Response。
if(client.call(srv))
調(diào)用服務(wù)，調(diào)用完成后立即返回。如果服務(wù)調(diào)用成功，call()將返回true，此時(shí)srv.response中的值有效。如果服務(wù)調(diào)用失敗，call()將返回false，srv.response中的值無效。
3．生成節(jié)點(diǎn)
打開~/catkin_ws/src/ros_tutorials/CMakeLists.txt，將以下代碼添加到最后：
add_executable(add_two_ints_serversrc/add_two_ints_server.cpp)
target_link_libraries(add_two_ints_server${catkin_LIBRARIES})
add_dependencies(add_two_ints_serverros_tutorials_gencpp)
add_executable(add_two_ints_clientsrc/add_two_ints_client.cpp)
target_link_libraries(add_two_ints_client${catkin_LIBRARIES})
add_dependencies(add_two_ints_clientros_tutorials_gencpp)
這將會創(chuàng)建兩個(gè)可執(zhí)行程序，add_two_ints_server和add_two_ints_client。默認(rèn)情況下，它們將會位于你的開發(fā)空間里面的ros_tutorials功能包目錄下（~/catkin_ws/devel/lib/ros_tutorials），你可以直接調(diào)用它們或者使用rosrun運(yùn)行它們。
運(yùn)行 catkin_make:
# In your catkinworkspace
$ cd ~/catkin_ws
$ catkin_make
4．測試服務(wù)器和客戶端節(jié)點(diǎn)
首先啟動roscore：
$ roscore
在另一個(gè)終端窗口運(yùn)行下面的命令啟動服務(wù)器節(jié)點(diǎn)：
$ rosrun ros_tutorialsadd_two_ints_server
你會看到下面的輸出：
Ready to add two ints.
新建一個(gè)終端，運(yùn)行客戶端節(jié)點(diǎn)：
$ rosrun ros_tutorials add_two_ints_client 2 3
你會看到類似下面的輸出：
Requesting 2+3
2 + 3 = 5
?