ROS2 服务通信——自定义服务接口（Python）

实验目的：

         自定义服务接口，要求客户端发送两个整型数据给服务端，服务端进行求和并且反馈结果给客户端。

1、创建工作空间和自定义接口功能包

        请参考：https://blog.csdn.net/qq_58457030/article/details/141336239

2、自定义服务接口

        在interface_pkg功能包中新建srv文件夹，用于存放服务接口文件，在srv目录中创建Addints.srv文件

mkdir -p ./text_ws/src/interface_pkg/srv
touch ./text_ws/src/interface_pkg/srv/Addints.srv

       在Addints.srv文件定义以下数据格式 

int32 num1
int32 num2
---
int32 sum

        修改interface_pkg中的CMakeLists.txt文件

        保存然后编译

cd text_ws/
colcon build --packages-select interface_pkg 

        在install目录下面生成了我们所需要的文件，即编译成功

        创建服务功能包service_pkg，新建服务端和客户端节点

cd ./text_ws/src/
ros2 pkg create service_pkg --build-type ament_python --dependencies rclpy --node-name server
touch ./service_pkg/service_pkg/client.py

        编写服务端节点

import rclpy                                        #ROS2 Python接口库
from rclpy.node import Node                         #ROS2 节点类
from interface_pkg.srv import Addints               #导入自定义接口包


class adderServer(Node):
    def __init__(self,name):
        super().__init__(name)                                  #ROS2节点父类初始化
        self.get_logger().info("服务端创建成功！")
        self.srv = self.create_service(Addints,'add_ints',self.add_cllback)#创建服务器对象（接口类型、服务名、服务器回调函数）

    def add_cllback(self,request,response):#创建回调函数，执行收到请求后对数据的处理
        response.sum = request.num1 + request.num2 #完成加法求和，将结果放到反馈的数据中
        self.get_logger().info('num1: %d num2:%d sum = %d'% (request.num1,request.num2,response.sum)) #输出日志信息
        return response                                                                  #反馈应答信息

def main(args=None):                                #ROS2节点主入口main函数
    rclpy.init(args=args)                           #ROS2 Python接口初始化
    node = adderServer("service_add_server")      #创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                #循环等待ROS2退出
    node.destroy_node()                             #销毁节点对象
    rclpy.shutdown()                                #关闭ROS2 Python接口

        编写客户端节点

import sys

import rclpy                                        #ROS2 Python接口库
from rclpy.node import Node                         #ROS2 节点类
from interface_pkg.srv import Addints               #导入自定义接口包
from rclpy.logging import get_logger


class adderClient(Node):
    def __init__(self,name):
        super().__init__(name)                                  #ROS2节点父类初始化
        self.client = self.create_client(Addints,'add_ints')    #创建客户端对象（服务接口类型，服务名）

        while not self.client.wait_for_service(timeout_sec=1.0): #循环等待服务器端成功启动
            self.get_logger().info('服务器连接中...')    #输出日志信息

        self.request = Addints.Request()                      #创建服务请求的数据对象

    def send_request(self):                               #创建一个发送服务请求的函数
        self.request.num1 = int(sys.argv[1])
        self.request.num2 = int(sys.argv[2])
        self.future = self.client.call_async(self.request)#异步方式发送服务请求

def main (args=None):
    #校验操作
    if len(sys.argv) !=3:
        get_logger("rclpy").error("请提交两个整型数据！")
        return
    rclpy.init(args=args)                           #ROS2 Python接口初始化
    node = adderClient("service_add_client")        #创建ROS2节点对象并进行初始化
    node.send_request()                             #发送服务请求

    while rclpy.ok():
        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 add_two_ints: for %d + %d = %d' % 
                (node.request.num1,node.request.num2,response.sum))
            break
            

    node.destroy_node()                             #销毁节点对象
    rclpy.shutdown()                                #关闭ROS2 Python接口

        修改setup.py

        分别开两个终端先编译一下，然后运行服务端和客户端节点

colcon build --packages-select service_pkg
source install/setup.bash 
ros2 run service_pkg server


source install/setup.bash 
ros2 run service_pkg client 10 20

        可以看到客户端提交两个整形数据10和20，然后服务端处理求和并且反馈给客户端结果为30，测试成功。