需求
- 创建服务端与客户端
- 实现客户端提供两个整型数字,服务端计算出两个数字之和并输出,同时客户端可以接受到服务端的处理结果
- 代码优化1: 在服务端未启动之前启动客户端,客户端挂起等待服务端启动
- 代码优化2: 在命令中带入参数,实现启动客户端的同时输入两个整型数字
编写自定义服务数据
创建自定义srv文件
- 在src包文件中新建AddInts.srv:
# 客户端请求时发送的两个数据
int32 num1
int32 num2
# 使用三个“-”进行区分客户端与服务端数据
---
# 服务端响应时发送的数据
int32 sum
修改package.xml文件
- 在当前ros包文件中的package.xml文件内加入两个标签:编译依赖与执行依赖
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
修改CMakeLists.txt文件
- 在find_package中添加message_generation
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
)
- 配置srv源文件
add_service_files(
FILES
AddInts.srv
)
- 声明生成依赖与执行依赖
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
)
创建服务端
- 具体代码如下:
#include "ros/ros.h"
#include "plumbing_server_client/AddInts.h"
// 回调函数,处理请求队列中的数据,处理数据正确返回true,有误返回false
bool getSum(plumbing_server_client::AddInts::Request &request,
plumbing_server_client::AddInts::Response &response)
{
// 请求数据的接收
int num1 = request.num1;
int num2 = request.num2;
ROS_INFO("接收的请求数据: num1 = %d, num2 = %d", num1, num2);
// 请求数据处理,并响应
int sum = num1 + num2;
response.sum = sum;
ROS_INFO("请求和结果: nsum = %d", sum);
return true;
}
int main(int argc, char *argv[])
{
// 设置中文编码
setlocale(LC_ALL, "");
// 初始化 ROS 节点
ros::init(argc, argv, "server");
// 创建节点句柄
ros::NodeHandle nh;
// 创建服务端对象,服务端名称为addInts
ros::ServiceServer server = nh.advertiseService("addInts", getSum);
ROS_INFO("服务端已启动...");
// 不断调用回调函数
ros::spin();
return 0;
}
- 在CMakeLists.txt文件中添加如下
add_executable(server_node src/server.cpp)
add_dependencies(server_node ${PROJECT_NAME}_gencpp)
target_link_libraries(server_node
${catkin_LIBRARIES}
)
其中add_dependencies()用于指出程序依赖的自定义服务数据格式文件,在编译程序之前,已编译完成,避免因编译先后顺序导致编译失败
创建客户端
- 具体代码如下:
#include "ros/ros.h"
#include "plumbing_server_client/AddInts.h"
int main(int argc, char *argv[])
{
// 中文编码设置
setlocale(LC_ALL, "");
// 优化实现:获取命令中的参数
if (argc != 3)
{
ROS_WARN("参数个数有误!!");
return 1;
}
else
{
ROS_INFO("输入的参数有:");
for (int i = 0; i < argc; i++)
{
ROS_INFO("参数 %d : %s ", i, argv[i]);
}
}
// 初始化 ROS 节点
ros::init(argc, argv, "client");
// 初始化句柄
ros::NodeHandle nh;
// 创建客户端对象,addInts为服务端名称
ros::ServiceClient client = nh.serviceClient<plumbing_server_client::AddInts>("addInts");
// 提交请求并处理响应
plumbing_server_client::AddInts client_data; // 创建请求数据
client_data.request.num1 = atoi(argv[1]);
client_data.request.num2 = atoi(argv[2]);
// 优化实现:若服务端未启动,则客户端挂起等待
client.waitForExistence();
// ros::service::waitForService("addInts"); // 同理也是挂起等待,不同点是让代码更易读
bool flag = client.call(client_data); // 处理响应并接受结果正确与否
if (flag)
{
ROS_INFO("处理成功!");
// 获取结果
ROS_INFO("响应结果:%d", client_data.response.sum); // 获取处理结果
}
else
{
ROS_WARN("处理失败!");
}
return 0;
}
- 主函数中的 argc 是整型变量,存储命令中传入的参数个数【需要注意到的是:argc会默认包含一个程序可执行文件地址参数,即实际argc值=命令提供参数值个数+1】;argv 为字符型数组,存储参数数据
- 客户端挂起等待直接调用 ROS 自带的函数实现:一种是 客户端对象.waitForExistence();一种是 ros::service::waitForService(“服务端名称”)
- 在CMakeLists.txt文件中添加如下
add_executable(client_node src/client.cpp)
add_dependencies(client_node ${PROJECT_NAME}_gencpp)
target_link_libraries(client_node
${catkin_LIBRARIES}
)