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

经测试，在节点运行过程中，我在文件夹突然换一张图片，程序仍可以正常读入新的图片。