自定义ROS服务，编写C++ server+client样例
1 资料
关于ROS服务的原理，参考本人ROS高效入门博客第二章的2.6节: ROS高效入门第二章 – 基本概念和常用命令学习，基于小乌龟样例。
本文主要是把下面两个资料的样例重写一遍，并组织成自己的目录，难度从易到难。建议读者按照顺序，耐心敲一遍，感受调试过程中的细节。
（1）《机器人操作系统（ROS）浅析》[美] Jason M. O’Kane 著 肖军浩 译，第8章
（2）ros Tutorials 初级教程的10，14~16节： ros Tutorials
2 正文
2.1 基于turtlesim，写一个客户端，申请繁殖多个小乌龟
（1）第一个样例基于turtlesim实现，只编写一个client，向turtlesim的/spawn服务发送请求，申请繁殖多个小乌龟。多数情况下，/spawn服务是初学者接触的第一个服务，读者可以先参考如下资料：ROS高效入门博客第二章的2.6节: ROS高效入门第二章 – 基本概念和常用命令学习，基于小乌龟样例，使用命令行操作/spawn服务，增加感性认知。
（2）创建spawn_turtle软件包和相关文件
cd ~/catkin_ws/src/cpp
catkin_create_pkg spawn_turtle turtlesim roscpp rospy
mkdir launch
touch launch/start.launch src/spawn_turtle.cpp
（3）编写spawn_turtle.cpp
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc, char ** argv) {
ros::init(argc, argv, "spawn_turtle");
ros::NodeHandle nh;
// 创建服务client句柄，服务名为/spawn，类型为turtlesim::Spawn
ros::ServiceClient spawnClient = nh.serviceClient<turtlesim::Spawn>("spawn");
// 由于/spawn是turtlesim_node节点提供的，因此必须等节点和服务起来，才能发送服务申请
ros::service::waitForService("spawn");
for (int i = 0; i < 10; i ++) {
turtlesim::Spawn::Request req;
turtlesim::Spawn::Response resp;
req.x = 1 + i;
req.y = 1 + i;
req.theta = M_PI/2;
req.name = "Leo" + std::to_string(i);
// 调用服务，req是申请，resp是返回值，调用成功会返回true
bool ok = spawnClient.call(req, resp);
if (ok) {
ROS_INFO("spawned a turtle named %s", resp.name.c_str());
} else {
ROS_ERROR("Failed to spawn");
}
}
return 0;
}
（4）编写CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(spawn_turtle)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
turtlesim
)
catkin_package()
include_directories(${
catkin_INCLUDE_DIRS})
add_executable(${
PROJECT_NAME}_node src/spawn_turtle.cpp)
target_link_libraries(${
PROJECT_NAME}_node
${
catkin_LIBRARIES}
)
（5）编写start.launch
<launch>
<node
pkg="turtlesim"
type="turtlesim_node"
name="turtlesim"
required="true"
/>
<node
pkg="spawn_turtle"
type="spawn_turtle_node"
name="spawn_turtle_node"
output="screen"
/>
</launch>
（6）编译并运行
cd ~/catkin_ws
catkin_make --source src/cpp/spawn_turtle/
source devel/setup.bash
roslaunch spawn_turtle start.launch
2.2 自定义服务，写server+client，实现两个整数求和
（1）此例主要是演示如何自定义服务，实现的功能很简单：client向server发送两个整数，server返回整数和。
创建srv_self软件包和文件：
cd ~/catkin_ws/src/cpp
catkin_create_pkg srv_self message_generation rospy roscpp std_msgs message_runtime
mkdir launch srv
touch launch/start.launch srv/AddTwoInts.srv src/server.cpp src/client.cpp
（2）编写AddTwoInts.srv，“- - -”上面的是请求，下面的是返回值
int64 a
int64 b
---
int64 sum
（3）编写server.cpp和client.cpp
server.cpp
#include <ros/ros.h>
// 引用AddTwoInts头文件
#include <srv_self/AddTwoInts.h>
// server的回调函数，成功返回true
bool add(srv_self::AddTwoInts::Request &req, srv_self::AddTwoInts::Response &resp) {
resp.sum = req.a + req.b;
ROS_INFO("server: receive a = %ld, b = %ld, return sum = %ld", req.a, req.b, resp.sum);
return true;
}
int main(int argc, char ** argv) {
ros::init(argc, argv, "add_server");
ros::NodeHandle nh;
// 创建service的server句柄，服务名为/add_ints，注册回调函数
ros::ServiceServer srv = nh.advertiseService("add_ints", &add);
ROS_INFO("ready to add two ints..");
// 服务的server程序，类似消息的sub程序，主程序必须调用spin()，检查服务回调
ros::spin();
return 0;
}
client.cpp
#include <ros/ros.h>
#include <srv_self/AddTwoInts.h>
#include <cstdlib>
int main(int argc, char ** argv) {
ros::init(argc, argv, "add_client");
ros::NodeHandle nh;
// 创建服务的client句柄，服务名为/add_ints
ros::ServiceClient client = nh.serviceClient<srv_self::AddTwoInts>("add_ints");
while (1) {
srv_self::AddTwoInts::Request req;
srv_self::AddTwoInts::Response resp;
req.a = rand();
req.b = rand();
// 申请服务，传入req，获取resp
bool ok = client.call(req, resp);
if (ok) {
ROS_INFO("client: send a = %ld, b = %ld, receive sum = %ld", req.a, req.b, resp.sum);
} else {
ROS_ERROR("failed to send add_ints service");
}
sleep(2);
}
return 0;
}
（4）编写CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(srv_self)
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
rospy
std_msgs
)
add_service_files(
FILES
AddTwoInts.srv
)
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(CATKIN_DEPENDS message_runtime roscpp rospy std_msgs)
include_directories(${
catkin_INCLUDE_DIRS})
add_executable(${
PROJECT_NAME}_server src/server.cpp)
add_executable(${
PROJECT_NAME}_client src/client.cpp)
// 由于自定义了srv，因此这两句不能少，不然编译的时候找不到AddTwoInts
add_dependencies(${
PROJECT_NAME}_server ${
PROJECT_NAME}_generate_messages_cpp)
add_dependencies(${
PROJECT_NAME}_client ${
PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${
PROJECT_NAME}_server
${
catkin_LIBRARIES}
)
target_link_libraries(${
PROJECT_NAME}_client
${
catkin_LIBRARIES}
)
（5）编写start.launch
<launch>
<node
pkg="srv_self"
type="srv_self_server"
name="srv_self_server"
respawn="true"
output="screen"
/>
<node
pkg="srv_self"
type="srv_self_client"
name="srv_self_client"
required="true"
output="screen"
/>
</launch>
（6）编译并运行
cd ~/catkin_ws
catkin_make --source src/cpp/srv_self/
source devel/setup.bash
roslaunch srv_self start.launch
2.3 基于turtlesim，写server+client，控制小乌龟转向
（1）本样例基于turtlesim，client向server申请修改全局控制量forward，server收到后，根据修改后的forward，向turtlesim_node发送新的运动命令，最终实现控制小乌龟转向。
创建toggle_forward软件包和文件：
cd ~/catkin_ws/src/cpp
catkin_create_pkg toggle_forward turtlesim geometry_msgs std_srvs roscpp rospy
mkdir launch
touch launch/start.launch src/toggle_client.cpp src/toggle_server.cpp
（2）编写toggle_server.cpp和toggle_client.cpp
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/Twist.h>
#include <thread>
// 全局变量forward
bool forward = true;
// /toggle_forward服务类型为std_srvs::Empty，服务功能就是把forward取反
bool toggleForward(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) {
forward = !forward;
ROS_INFO("now receiving : %s", forward ? "forward" : "rotate");
return true;
}
int main(int argc, char ** argv) {
ros::init(argc, argv, "toggle_server");
ros::NodeHandle nh;
//创建/toggle_forward服务
ros::ServiceServer server = nh.advertiseService("toggle_forward", &toggleForward);
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
auto thread_pub = [=]() {
ros::Rate rate(2);
while(ros::ok()) {
geometry_msgs::Twist msg;
// forward控制/turtle1/cmd_vel消息内容
msg.linear.x = forward? 1.0 : 0.0;
msg.angular.z = forward ? 0.0 : 1.0;
pub.publish(msg);
rate.sleep();
}
};
// 创建单独的线程，给turtlesim_node发送消息
std::thread th1 = std::thread(thread_pub);
ros::spin();
if (th1.joinable()) {
th1.join();
}
return 0;
}
toggle_client.cpp
#include <ros/ros.h>
#include <std_srvs/Empty.h>
int main(int argc, char ** argv) {
ros::init(argc, argv, "toggle_client");
ros::NodeHandle nh;
ros::ServiceClient client = nh.serviceClient<std_srvs::Empty>("toggle_forward");
// 等待/toggle_forward服务
ros::service::waitForService("toggle_forward");
while (1) {
std_srvs::Empty::Request req;
std_srvs::Empty::Response resp;
bool ok = client.call(req, resp);
if (ok) {
ROS_INFO("send toggle cmd");
} else {
ROS_ERROR("failed to send cmd");
}
sleep(2);
}
return 0;
}
（3）编写CMakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(toggle_forward)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
std_srvs
turtlesim
)
catkin_package()
include_directories(
${
catkin_INCLUDE_DIRS}
)
add_executable(${
PROJECT_NAME}_server src/toggle_server.cpp)
add_executable(${
PROJECT_NAME}_client src/toggle_client.cpp)
target_link_libraries(${
PROJECT_NAME}_server
${
catkin_LIBRARIES}
pthread
)
target_link_libraries(${
PROJECT_NAME}_client
${
catkin_LIBRARIES}
)
（4）编写start.launch
<launch>
<node
pkg="turtlesim"
type="turtlesim_node"
name="turtlesim"
required="true"
/>
<node
pkg="toggle_forward"
type="toggle_forward_server"
name="toggle_forward_server"
output="screen"
/>
<node
pkg="toggle_forward"
type="toggle_forward_client"
name="toggle_forward_client"
output="screen"
/>
</launch>
（5）编译并运行
cd ~/catkin_ws
catkin_make --source src/cpp/toggle_forward/
source devel/setup.bash
roslaunch toggle_forward start.launch
3 总结
本文中的例子放在了本人的github上： ros_src