1、新建功能包
ros2 pkg create pkg_service --build-type ament_python --dependencies rclpy --node-name server_demo
2、服务端实现
import rclpy from rclpy.node import Node from example_interfaces.srv import AddTwoInts class Service_Server(Node): def __init__(self, name): super().__init__(name)
# 创建一个服务端,使用的是create_service函数
# 参数分别为:服务数据类型、服务名称、服务回调函数 self.src = self.create_service(AddTwoInts, "/add_two_ints", self.AddTwoInts_callback)
# 定义服务回调函数 def AddTwoInts_callback(self, request, response): response.sum = request.a + request.b print("result.sum = ", response.sum) return response def main(): rclpy.init() server_demo = Service_Server("server_node") rclpy.spin(server_demo) server_demo.destroy_node() rclpy.shutdown()
ros2 interface show example_interfaces/srv/AddTwoInts
3、客户端实现
import rclpy from rclpy.node import Node from example_interfaces.srv import AddTwoInts class Service_Client(Node): def __init__(self, name): super().__init__(name)
# 创建客户端,使用create_client函数
# 传入参数分别是:服务数据类型、服务名称 self.client = self.create_client(AddTwoInts, "/add_two_ints")
# 循环等待服务端启动 while not self.client.wait_for_service(timeout_sec=1.0): print("service not available, waiting...")
# 创建服务请求数据对象 self.request = AddTwoInts.Request() def send_request(self): self.request.a = 10 self.request.b = 20
# 发送服务请求 self.future = self.client.call_async(self.request) def main(): rclpy.init() client_demo = Service_Client("client_node") client_demo.send_request() # 发送服务请求 while rclpy.ok(): rclpy.spin_once(client_demo)
# 判断数据是否处理完成 if client_demo.future.done(): try:
# 获取服务返回结果 response = client_demo.future.result() print("request.a = ", client_demo.request.a) print("request.b = ", client_demo.request.b) print("response.sum = ", response.sum) except Exception as e: print("Service call failed") break client_demo.destroy_node() rclpy.shutdown()
4、编辑配置文件、编译工作空间
cd ~/ros_ws colcon build --package-selects pkg_service source install/setup.bash
5、运行程序
ros2 run pkg_service server_demo ros2 run pkg_service client_demo
服务端打印: