ROS2学习笔记，案例实现记录——12.服务server
前言
在学习了ROS 2官方教程之后，我决定以自导自演的方式，给自己出题来实践所学知识，以加深对ROS 2的各个概念和使用方式的理解。我将把我的学习过程记录在博客中，并将代码放在我的GitHub仓库中https://github.com/YuAndWang/ROS2_Case（当然博客中也会贴上代码），后面有新的案例将会放到对应的章节之后，供大家参考和学习。
欢迎━(｀∀´)ノ亻! 大家一起出题，一起探讨！希望我的实践过程对你理解ROS 2有所帮助。如果你有任何问题或建议，欢迎提出！谢谢~~~
12.服务server
12.1案例1：compare_two_ints
这一次想实现一个服务客户端节点，用于向服务端发送服务请求，比较输入的两个整数的大小，并获取最大值和最小值。
实现过程如下：
建立功能包
打开一个新的终端， cd 到我的工作空间目录。并且应该在 src 目录中创建包，而不是在工作空间的根目录中创建包。因此，cd 到 my_ws/src，并运行包创建命令：
cd my_ws/src
ros2 pkg create --build-type ament_python compare_srvcli_py
创建自定义服务文件
这里需要创建服务，我在mine_interface功能包下建立了srv文件夹，在文件夹中创建了一个名为CompareTwoInts.srv的服务文件，并在其中定义了请求和响应的消息格式。以下是文件内容：
int64 a
int64 b
---
int64 max
int64 min
这个服务文件定义了一个名为CompareTwoInts的服务，它具有一个请求和一个响应部分。请求部分包含两个int64类型的字段a和b，用于传递要比较的两个整数。响应部分包含两个int64类型的字段max和min，用于返回最大值和最小值。
接着需要更新CMakeLists.txt文件，将新的srv文件引用到CMakeLists.txt文件中：
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/CompareTwoInts.srv"
)
客户端 compare_two_ints_cli.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 王仰旭
@说明: 比较输入的两个整数大小——客户端
"""
import sys
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from mine_interface.srv import CompareTwoInts # 自定义的服务接口
class compareClient(Node):
def __init__(self):
super().__init__('compare_two_ints_client') # ROS2节点父类初始化
self.client = self.create_client(CompareTwoInts, 'compare_two_ints') # 创建服务客户端对象（服务接口类型，服务名）
while not self.client.wait_for_service(timeout_sec=1.0): # 循环等待服务器端成功启动
self.get_logger().info('service not available, waiting again...')
self.request = CompareTwoInts.Request() # 创建服务请求的数据对象
def send_request(self): # 创建一个发送服务请求的函数
self.request.a = int(sys.argv[1])
self.request.b = int(sys.argv[2])
self.future = self.client.call_async(self.request) # 异步方式发送服务请求
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = compareClient() # 创建ROS2节点对象并进行初始化
node.send_request() # 发送服务请求
while rclpy.ok(): # ROS2系统正常运行
rclpy.spin_once(node) # 循环执行一次节点
if node.future.done(): # 数据是否处理完成
try:
response = node.future.result() # 接收服务器端的反馈数据
except Exception as e:
node.get_logger().info(
'Service call failed %r' % (e,))
else:
node.get_logger().info( # 将收到的反馈信息打印输出
'Result of compare_two_ints: %d and %d,\nmax is: %d, min is: %d'
% (node.request.a, node.request.b, response.max, response.min))
break
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
服务端 compare_two_ints_srv.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 王仰旭
@说明: 比较输入的两个整数大小——服务端
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from mine_interface.srv import CompareTwoInts # 自定义的服务接口
class compareServer(Node):
def __init__(self):
super().__init__('compare_two_ints_server') # ROS2节点父类初始化
self.srv = self.create_service(CompareTwoInts, 'compare_two_ints', self.compare_callback) # 创建服务器对象（接口类型、服务名、服务器回调函数）
def compare_callback(self, request, response): # 创建回调函数，执行收到请求后对数据的处理
response.max = max(request.a, request.b) # 完成求最大值操作，将结果放到反馈的数据中
response.min = min(request.a, request.b)
self.get_logger().info('Incoming request: %d and %d\nI will response: min: %d, max: %d'
% (request.a, request.b, response.max, response.min)) # 输出日志信息，提示已经完成
return response # 反馈应答信息
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = compareServer() # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
配置package.xml文件
记得这里要加入【mine_interface】，声明你的包依赖于自定义的消息包 mine_interface。
<package format="3">
<name>compare_srvcli_pyname>
<version>0.0.0version>
<description>compare ints client and server using rclpydescription>
<maintainer email="[email protected]">wangmaintainer>
<license>BSDlicense>
<test_depend>ament_copyrighttest_depend>
<test_depend>ament_flake8test_depend>
<test_depend>ament_pep257test_depend>
<test_depend>python3-pytesttest_depend>
<depend>mine_interfacedepend>
<export>
<build_type>ament_pythonbuild_type>
export>
package>
配置setup.py文件
记得这里需要在setup.py文件中添加入口点（entry points），以便能够通过命令行运行你的节点。
from setuptools import setup
package_name = 'compare_srvcli_py'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='wang',
maintainer_email='[email protected]',
description='compare ints client and server using rclpy',
license='BSD',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'compare_two_ints_cli = compare_srvcli_py.compare_two_ints_cli:main',
'compare_two_ints_srv = compare_srvcli_py.compare_two_ints_srv:main',
],
},
)
配置setup.cfg文件
setup.cfg文件用于配置ROS 2的包安装和开发时的相关设置。检查一下是否正确配置了安装和开发时的脚本目录。这里会告诉setuptools把编译后的可执行文件放在lib中，因为运行时ros2 run命令将会在那里寻找它们。
[develop]
script_dir=$base/lib/compare_srvcli_py
[install]
install_scripts=$base/lib/compare_srvcli_py
编译
cd my_ws/
colcon build
运行
打开新终端，刷新环境
source ~/my_ws/install/setup.bash
ros2 run compare_srvcli_py compare_two_ints_srv
ros2 run compare_srvcli_py compare_two_ints_cli 22 53
效果gif
12.2案例2：compare_three_ints
该案例我想实现比较三个整数大小并返回最大、最小和平均值的服务代码。
在案例1的功能包基础上，继续实现该案例2。
进入自定义服务功能包 mine_interface/srv目录中，新建一个服务文件CompareThreeInts.srv，写入如下内容：
int64 a
int64 b
int64 c
---
int64 max
int64 min
float64 ave
接着需要更新CMakeLists.txt文件，将新的srv文件引用到CMakeLists.txt文件中：
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/CompareThreeInts.srv"
)
客户端 compare_three_ints_cli.py
这里相对案例1，主要的改变是需要加入一个request参数，self.request.c = int(sys.argv[3])，以及打印输出的语句加参数。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 王仰旭
@说明: 比较输入的三个整数大小，返回最大、最小、平均值——客户端
"""
import sys
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from mine_interface.srv import CompareThreeInts # 自定义的服务接口
class compareClient(Node):
def __init__(self):
super().__init__('compare_three_ints_client') # ROS2节点父类初始化
self.client = self.create_client(CompareThreeInts, 'compare_three_ints') # 创建服务客户端对象（服务接口类型，服务名）
while not self.client.wait_for_service(timeout_sec=1.0): # 循环等待服务器端成功启动
self.get_logger().info('service not available, waiting again...')
self.request = CompareThreeInts.Request() # 创建服务请求的数据对象
def send_request(self): # 创建一个发送服务请求的函数
self.request.a = int(sys.argv[1])
self.request.b = int(sys.argv[2])
self.request.c = int(sys.argv[3])
self.future = self.client.call_async(self.request) # 异步方式发送服务请求
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = compareClient() # 创建ROS2节点对象并进行初始化
node.send_request() # 发送服务请求
while rclpy.ok(): # ROS2系统正常运行
rclpy.spin_once(node) # 循环执行一次节点
if node.future.done(): # 数据是否处理完成
try:
response = node.future.result() # 接收服务器端的反馈数据
except Exception as e:
node.get_logger().info(
'Service call failed %r' % (e,))
else:
node.get_logger().info( # 将收到的反馈信息打印输出
'Result of compare_three_ints: %d and %d and %d,\nmax is: %d, min is: %d, average is: %f'
% (node.request.a, node.request.b, node.request.c, response.max, response.min, response.ave))
break
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
服务端 compare_three_ints_srv.py
这里相对案例1，主要的改变是改变计算方法，将结果放到更多需要反馈的数据中，以及打印输出的语句加参数。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 王仰旭
@说明: 比较输入的三个整数大小，返回最大、最小、平均值——服务端
"""
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from mine_interface.srv import CompareThreeInts # 自定义的服务接口
class compareServer(Node):
def __init__(self):
super().__init__('compare_three_ints_server') # ROS2节点父类初始化
self.srv = self.create_service(CompareThreeInts, 'compare_three_ints', self.compare_callback) # 创建服务器对象（接口类型、服务名、服务器回调函数）
def get_ave(self, a, b, c):
sum = a + b + c
ave = sum / 3
return ave
def compare_callback(self, request, response): # 创建回调函数，执行收到请求后对数据的处理
response.max = max(request.a, request.b, request.c) # 完成求最大值操作，将结果放到反馈的数据中
response.min = min(request.a, request.b, request.c)
response.ave = self.get_ave(request.a, request.b, request.c)
self.get_logger().info('Incoming request: %d and %d and %d\nI will response: min: %d, max: %d, average: %f'
% (request.a, request.b, request.c, response.max, response.min, response.ave)) # 输出日志信息，提示已经完成
return response # 反馈应答信息
def main(args=None): # ROS2节点主入口main函数
rclpy.init(args=args) # ROS2 Python接口初始化
node = compareServer() # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
配置package.xml文件
这里相对案例1，无需改变。
<package format="3">
<name>compare_srvcli_pyname>
<version>0.0.0version>
<description>compare ints client and server using rclpydescription>
<maintainer email="[email protected]">wangmaintainer>
<license>BSDlicense>
<test_depend>ament_copyrighttest_depend>
<test_depend>ament_flake8test_depend>
<test_depend>ament_pep257test_depend>
<test_depend>python3-pytesttest_depend>
<depend>mine_interfacedepend>
<export>
<build_type>ament_pythonbuild_type>
export>
package>
配置setup.py文件
记得这里需要在setup.py文件中添加入口点（entry points），加入新文件的入口点。
from setuptools import setup
package_name = 'compare_srvcli_py'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='wang',
maintainer_email='[email protected]',
description='compare ints client and server using rclpy',
license='BSD',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'compare_two_ints_cli = compare_srvcli_py.compare_two_ints_cli:main',
'compare_two_ints_srv = compare_srvcli_py.compare_two_ints_srv:main',
'compare_three_ints_cli = compare_srvcli_py.compare_three_ints_cli:main',
'compare_three_ints_srv = compare_srvcli_py.compare_three_ints_srv:main',
],
},
)
配置setup.cfg文件
这里相对案例1，无需改变。
[develop]
script_dir=$base/lib/compare_srvcli_py
[install]
install_scripts=$base/lib/compare_srvcli_py
编译
cd my_ws/
colcon build
运行
打开新终端，刷新环境
source ~/my_ws/install/setup.bash
ros2 run compare_srvcli_py compare_three_ints_srv
ros2 run compare_srvcli_py compare_three_ints_cli 51 12 18
效果gif
12.3案例3：camera_object_py
该案例我想实现cv2进行图像识别后显示结果, 自定义发布x,y位置消息。
创建自定义消息文件
这里需要创建消息，我在mine_interface功能包下建立了msg文件夹，在文件夹中创建了一个名为ObjectPosition.msg的消息文件，写入如下内容：
int32 x # 目标的X坐标
int32 y # 目标的Y坐标
这个消息文件定义了一个自定义消息ObjectPosition，它包含两个字段，x和y，分别表示目标的X坐标和Y坐标。
接着需要更新CMakeLists.txt文件，将新的srv文件引用到CMakeLists.txt文件中：
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/ObjectPosition.msg"
)
建立功能包
打开一个新的终端， cd 到我的工作空间目录。并且应该在 src 目录中创建包，而不是在工作空间的根目录中创建包。因此，cd 到 my_ws/src，并运行包创建命令：
cd my_ws/src
ros2 pkg create --build-type ament_python camera_object_py
发布者 camera_object_pub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 王仰旭
@说明: cv2进行图像识别后显示结果, 自定义发布x,y位置消息版 (无法开启摄像头, 将读取camera_image_publisher节点的消息, 模拟视频流物体识别)
@先运行: ros2 run camera_object_py camera_image_publisher
@打开摄像头: ros2 run usb_cam usb_cam_node_exe
"""
import rclpy
import cv2
import numpy as np
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from mine_interface.msg import ObjectPosition
lower_red = np.array([0, 90, 128])
upper_red = np.array([180, 255, 255])
class ImageSubscriber(Node):
def __init__(self):
super().__init__('camera_object_node')
self.sub = self.create_subscription(Image, 'image_raw', self.listener_callback, 10) # 订阅
self.pub = self.create_publisher(ObjectPosition, "object_position", 10) # 发布
self.cv_bridge = CvBridge()
self.X = 0
self.Y = 0
def object_detect(self, image):
hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
mask_red = cv2.inRange(hsv_img, lower_red, upper_red)
#cv2.imshow("mask_red", mask_red)
contours, hierarchy = cv2.findContours(mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 轮廓检测
for cnt in contours:
if cnt.shape[0] < 150:
continue
(x, y, w, h) = cv2.boundingRect(cnt)
cv2.drawContours(image, [cnt], -1, (0, 0, 255), 2)
cv2.circle(image, (int(x+w/2), int(y+h/2)), 5, (0, 255, 0), -1)
self.X = int(x+w/2)
self.Y = int(y+h/2)
cv2.imshow("object", image)
cv2.waitKey(50)
#cv2.destroyAllWindows()
def listener_callback(self, data):
self.get_logger().info("Receiving video frame")
#image = cv2.imread("/home/wang/my_ws/src/camera_object_py/camera_object_py/apple.jpg")
image = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8')
position = ObjectPosition()
self.object_detect(image)
position.x, position.y = int(self.X), int(self.Y)
self.pub.publish(position)
def main(args=None):
rclpy.init(args=args)
node = ImageSubscriber()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
这是一个基于OpenCV进行图像识别并显示结果的节点代码，同时发布识别到的物体位置信息。以下是对代码的解释和说明：
该节点创建了一个名为camera_object_node的节点类，继承自rclpy.node.Node，用于接收图像消息并进行物体识别。在节点的构造函数中，它创建了一个订阅者对象，用于订阅名为image_raw的图像消息。同时，它创建了一个发布者对象，用于发布名为object_position的自定义消息ObjectPosition，该消息包含了物体的位置信息。
在object_detect函数中，它首先将图像从BGR颜色模型转换为HSV模型，并对图像进行二值化处理。然后，通过轮廓检测找到图像中的物体，并将物体位置标记在图像上。最后，它更新了物体的位置坐标。
在listener_callback函数中，它首先将接收到的图像消息转换为OpenCV格式的图像。然后，调用object_detect函数对图像进行物体识别，并获取物体的位置坐标。接着，创建一个ObjectPosition类型的消息实例，并将物体的位置信息赋值给消息的x和y字段。最后，通过发布者对象将消息发布出去。
在main函数中，我们初始化ROS 2 Python接口，并创建并运行ImageSubscriber节点。通过rclpy.spin函数，我们循环等待ROS 2退出，并在退出前销毁节点对象。
订阅者 camera_object_sub.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 王仰旭
@说明: 订阅camera_object_interface_pub.py发送的目标位置
"""
import rclpy
from rclpy.node import Node
from mine_interface.msg import ObjectPosition
class SubscriberNode(Node):
def __init__(self):
super().__init__('camera_object_node')
self.sub = self.create_subscription(ObjectPosition, 'object_position', self.listener_callback, 10) # 订阅
def listener_callback(self, msg):
self.get_logger().info('I Target Position: "(%d, %d)"' % (msg.x, msg.y))
def main(args=None):
rclpy.init(args=args)
node = SubscriberNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
该节点将订阅名为object_position的ObjectPosition消息，并在收到消息时调用listener_callback回调函数。在回调函数中，它打印出接收到的目标位置信息(x, y)。
（图像发布者 camera_image_publisher.py）
由于我的虚拟机无法使用摄像头，尝试一直读一张照片，一直发布ROS图像信息，使用摄像头同理，改变imread的内容即可。
如果能正常打开ros2 run usb_cam usb_cam_node_exe节点，发布实际摄像头的图像，即无需此步骤。
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@作者: 王仰旭
@说明: 一直将图像读入后, 发布为ROS图像消息
(因无法开启摄像头, 模拟视频流物体识别, 做后面的自定义消息发布苹果的坐标)
"""
import os
import cv2
import numpy as np
import rclpy
from PIL import Image
from rclpy.node import Node
from std_msgs.msg import Header
from sensor_msgs.msg import Image as ImageMsg
from cv_bridge import CvBridge
class ImagePublisher(Node):
def __init__(self):
super().__init__('image_publisher')
self.publisher = self.create_publisher(ImageMsg, 'image_raw', 10)
timer_period = 0.5
self.timer = self.create_timer(timer_period, self.publish_image)
self.cv_bridge = CvBridge()
def publish_image(self):
image_path = '/home/wang/my_ws/src/camera_object_py/camera_object_py/apple.jpg' # 读取图像文件
if os.path.exists(image_path):
img = Image.open(image_path)
cv_img = cv2.cvtColor(np.array(img), cv2.COLOR_RGB2BGR) # 将PIL图像转换为OpenCV图像
ros_img = self.cv_bridge.cv2_to_imgmsg(cv_img, 'bgr8') # 将OpenCV图像转换为ROS2图像消息
ros_img.header = Header()
ros_img.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(ros_img)
self.get_logger().info('Image published')
else:
self.get_logger().warning('Image file not found')
def main(args=None):
rclpy.init(args=args)
node = ImagePublisher()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
配置package.xml文件
<package format="3">
<name>camera_object_pyname>
<version>0.0.0version>
<description>camera object using rclpydescription>
<maintainer email="[email protected]">wangmaintainer>
<license>BSDlicense>
<test_depend>ament_copyrighttest_depend>
<test_depend>ament_flake8test_depend>
<test_depend>ament_pep257test_depend>
<test_depend>python3-pytesttest_depend>
<export>
<build_type>ament_pythonbuild_type>
export>
package>
配置setup.py文件
记得这里需要在setup.py文件中添加入口点（entry points），加入新文件的入口点。
from setuptools import setup
package_name = 'camera_object_py'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='wang',
maintainer_email='[email protected]',
description='camera object using rclpy',
license='BSD',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'camera_image_publisher = camera_object_py.camera_image_publisher:main', #视情况是否读取图片模拟摄像头
'camera_object_pub = camera_object_py.camera_object_pub:main',
'camera_object_sub = camera_object_py.camera_object_sub:main',
]
},
)
配置setup.cfg文件
这里相对案例1，无需改变。
[develop]
script_dir=$base/lib/camera_object_py
[install]
install_scripts=$base/lib/camera_object_py
编译
cd my_ws/
colcon build
运行
打开新终端，刷新环境
source ~/my_ws/install/setup.bash
ros2 run usb_cam usb_cam_node_exe # 打开摄像头以提供图像消息的源
ros2 run camera_object_py camera_image_publisher # 视情况是否读取图片模拟摄像头
ros2 run camera_object_py camera_object_pub
ros2 run camera_object_py camera_object_sub
效果gif
经测试，在节点运行过程中，我在文件夹突然换一张图片，程序仍可以正常读入新的图片。