ROS2服务实现（python & C++）

1. 什么是ROS2服务？

1.1 服务的基本概念

在ROS2中，服务（Service）是一种重要的通信机制，它实现了节点之间的同步请求-响应模式。想象一下你在餐厅点餐的场景：你（客户端）向服务员（服务端）提出点餐请求，服务员处理你的请求并给你回复。这就是服务通信的基本模式。

服务通信涉及两个角色：

• 服务端（Server）：提供具体服务功能的节点，等待接收请求，处理后返回响应
• 客户端（Client）：需要使用服务的节点，发送请求并等待响应

1.2 服务与话题的区别

很多初学者容易混淆服务和话题，让我们来看看它们的主要区别：

话题（Topic）通信特点：

• 异步通信：发布者发布消息后不等待响应，继续执行后续任务
• 一对多通信：一个发布者可以向多个订阅者发送消息
• 持续性通信：适合传感器数据、状态信息等需要持续传输的数据
• 无反馈机制：发布者不知道消息是否被成功接收和处理

服务（Service）通信特点：

• 同步通信：客户端发送请求后会阻塞等待，直到收到响应
• 一对一通信：一个请求只能有一个对应的响应
• 瞬时性通信：适合需要立即反馈的操作，如计算、查询、控制命令
• 有反馈机制：客户端能够确认请求是否被成功处理

1.3 服务的适用场景

服务特别适合以下应用场景：

1. 计算服务：需要复杂计算并返回结果，如路径规划、图像处理
2. 查询服务：获取系统状态信息，如传感器标定参数、配置信息
3. 控制服务：执行特定动作并返回执行结果，如机器人移动到指定位置
4. 配置服务：动态修改节点参数或行为模式
5. 文件操作：保存/加载配置文件，数据备份等

2. 服务的工作原理

2.1 服务通信流程

服务通信遵循以下步骤：

1. 服务注册：服务端启动时向ROS2系统注册自己提供的服务
2. 服务发现：客户端查找所需的服务是否可用
3. 建立连接：客户端与服务端建立通信连接
4. 发送请求：客户端创建请求消息并发送给服务端
5. 处理请求：服务端接收请求，执行相应的处理逻辑
6. 返回响应：服务端将处理结果封装成响应消息返回给客户端
7. 接收响应：客户端接收响应消息，获取处理结果

2.2 服务接口定义

每个服务都需要定义其接口，这就像制定API规范一样。服务接口通过.srv文件定义，包含两部分：

# 请求部分（Request）
# 定义客户端向服务端发送的数据格式
请求字段1 类型
请求字段2 类型
---
# 响应部分（Response）  
# 定义服务端向客户端返回的数据格式
响应字段1 类型
响应字段2 类型

示例 - 两数相加服务 AddTwoInts.srv：

# 请求：客户端发送两个整数
int64 a
int64 b
---
# 响应：服务端返回计算结果
int64 sum

这个定义表示：客户端发送两个64位整数a和b，服务端计算它们的和并返回结果sum。

2.3 常用的内置服务类型

ROS2提供了一些预定义的服务类型，新手可以直接使用：

// 基础服务类型
std_srvs/srv/Empty          // 空服务：无请求参数，无响应数据
std_srvs/srv/SetBool        // 布尔设置：发送布尔值，返回成功状态和消息
std_srvs/srv/Trigger        // 触发服务：无参数触发，返回成功状态和消息

// 示例服务类型
example_interfaces/srv/AddTwoInts    // 两数相加
example_interfaces/srv/SetBool       // 设置布尔参数

3. C++服务实现详解

3.1 服务端实现步骤

创建服务端需要以下几个关键步骤：

步骤1：包含必要的头文件

#include "rclcpp/rclcpp.hpp"                    // ROS2 C++基础库
#include "example_interfaces/srv/add_two_ints.hpp"  // 服务接口定义
#include                                // 智能指针支持

步骤2：创建服务端节点类

class AddTwoIntsServer : public rclcpp::Node
{
public:
    AddTwoIntsServer() : Node("add_two_ints_server")
    {
        // 在构造函数中创建服务
        service_ = this->create_service<example_interfaces::srv::AddTwoInts>(
            "add_two_ints",  // 服务名称
            std::bind(&AddTwoIntsServer::handle_service, this, 
                     std::placeholders::_1, std::placeholders::_2)  // 回调函数
        );
        
        RCLCPP_INFO(this->get_logger(), "加法服务端已启动，等待客户端请求...");
    }

private:
    // 处理服务请求的回调函数
    void handle_service(
        const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
        std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response)
    {
        // 执行加法运算
        response->sum = request->a + request->b;
        
        // 输出日志信息，便于调试
        RCLCPP_INFO(this->get_logger(), 
                   "收到请求: %ld + %ld，计算结果: %ld", 
                   request->a, request->b, response->sum);
    }

    // 服务对象
    rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service_;
};

步骤3：编写主函数

int main(int argc, char** argv)
{
    // 初始化ROS2
    rclcpp::init(argc, argv);
    
    // 创建服务端节点
    auto node = std::make_shared<AddTwoIntsServer>();
    
    // 进入事件循环，等待和处理请求
    rclcpp::spin(node);
    
    // 清理资源
    rclcpp::shutdown();
    return 0;
}

3.2 客户端实现步骤

客户端的实现稍微复杂一些，因为需要处理异步通信：

步骤1：创建客户端节点类

class AddTwoIntsClient : public rclcpp::Node
{
public:
    AddTwoIntsClient() : Node("add_two_ints_client")
    {
        // 创建服务客户端
        client_ = this->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
    }

    void send_request(int64_t a, int64_t b)
    {
        // 步骤1：等待服务变为可用状态
        // 这很重要！如果服务端还没启动，客户端会一直等待
        while (!client_->wait_for_service(std::chrono::seconds(1))) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(), "程序被中断退出");
                return;
            }
            RCLCPP_INFO(this->get_logger(), "等待服务 'add_two_ints' 可用...");
        }

        // 步骤2：创建并填充请求消息
        auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
        request->a = a;
        request->b = b;

        RCLCPP_INFO(this->get_logger(), "发送请求: %ld + %ld", a, b);

        // 步骤3：异步发送请求
        auto future = client_->async_send_request(request);
        
        // 步骤4：等待响应
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) ==
            rclcpp::FutureReturnCode::SUCCESS) {
            // 请求成功，获取响应
            auto response = future.get();
            RCLCPP_INFO(this->get_logger(), "收到响应: %ld + %ld = %ld", 
                       a, b, response->sum);
        } else {
            // 请求失败
            RCLCPP_ERROR(this->get_logger(), "服务调用失败或超时");
        }
    }

private:
    rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;
};

步骤2：主函数实现

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    
    // 检查命令行参数
    if (argc != 3) {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), 
                   "用法: ros2 run package_name client X Y");
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), 
                   "例如: ros2 run package_name client 10 20");
        return 1;
    }

    // 创建客户端并发送请求
    auto node = std::make_shared<AddTwoIntsClient>();
    node->send_request(atoll(argv[1]), atoll(argv[2]));
    
    rclcpp::shutdown();
    return 0;
}

3.3 同步vs异步调用的理解

上面的例子使用了异步调用。让我解释一下同步和异步的区别：

异步调用（推荐）：

• 发送请求后不会立即阻塞程序
• 可以在等待响应期间处理其他任务
• 适合复杂的ROS2应用

同步调用（简单但不推荐）：

void sync_call_example(int64_t a, int64_t b)
{
    auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
    request->a = a;
    request->b = b;
    
    // 同步调用 - 程序会在这里阻塞等待
    auto future_result = client_->async_send_request(request);
    auto status = future_result.wait_for(std::chrono::seconds(5));  // 最多等待5秒
    
    if (status == std::future_status::ready) {
        auto response = future_result.get();
        RCLCPP_INFO(this->get_logger(), "结果: %ld", response->sum);
    } else {
        RCLCPP_ERROR(this->get_logger(), "服务调用超时");
    }
}

4. Python服务实现详解

Python的实现相对更简洁，但概念是相同的。

4.1 服务端实现步骤

步骤1：导入必要的模块

#!/usr/bin/env python3
import rclpy                           # ROS2 Python基础库
from rclpy.node import Node           # 节点基类
from example_interfaces.srv import AddTwoInts  # 服务接口

步骤2：创建服务端类

class AddTwoIntsServer(Node):
    def __init__(self):
        # 调用父类构造函数，设置节点名称
        super().__init__('add_two_ints_server')
        
        # 创建服务
        # 参数说明：
        # - AddTwoInts: 服务类型
        # - 'add_two_ints': 服务名称  
        # - self.handle_service: 处理请求的回调函数
        self.srv = self.create_service(
            AddTwoInts, 
            'add_two_ints', 
            self.handle_service
        )
        
        self.get_logger().info('加法服务端已启动，正在等待客户端请求...')

    def handle_service(self, request, response):
        """
        处理服务请求的回调函数
        
        参数:
            request: 客户端发送的请求数据
            response: 需要填充并返回给客户端的响应数据
            
        返回:
            response: 填充完毕的响应数据
        """
        # 执行加法计算
        response.sum = request.a + request.b
        
        # 记录日志，便于调试和监控
        self.get_logger().info(
            f'收到请求: {request.a} + {request.b}，计算结果: {response.sum}'
        )
        
        # 必须返回response对象
        return response

步骤3：主函数

def main(args=None):
    # 初始化ROS2 Python客户端库
    rclpy.init(args=args)
    
    # 创建服务端节点
    server = AddTwoIntsServer()
    
    try:
        # 进入事件循环，持续监听和处理请求
        rclpy.spin(server)
    except KeyboardInterrupt:
        # 用户按Ctrl+C时优雅退出
        print('\n程序被用户中断')
    finally:
        # 清理资源
        server.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

4.2 客户端实现步骤

步骤1：创建客户端类

class AddTwoIntsClient(Node):
    def __init__(self):
        super().__init__('add_two_ints_client')
        
        # 创建服务客户端
        self.client = self.create_client(AddTwoInts, 'add_two_ints')
        
        # 等待服务变为可用
        # 这是一个重要步骤！确保服务端已经启动
        timeout_counter = 0
        while not self.client.wait_for_service(timeout_sec=1.0):
            timeout_counter += 1
            self.get_logger().info(f'等待服务可用... ({timeout_counter}秒)')
            
            # 避免无限等待
            if timeout_counter > 10:
                self.get_logger().error('服务长时间不可用，请检查服务端是否启动')
                return

        self.get_logger().info('服务已连接，可以发送请求')

    def send_request(self, a, b):
        """
        发送服务请求
        
        参数:
            a, b: 要相加的两个数字
            
        返回:
            Future对象，用于异步获取响应
        """
        # 创建请求对象
        request = AddTwoInts.Request()
        request.a = a
        request.b = b
        
        self.get_logger().info(f'发送请求: {a} + {b}')
        
        # 异步发送请求
        future = self.client.call_async(request)
        return future

步骤2：主函数实现

import sys  # 用于处理命令行参数

def main(args=None):
    rclpy.init(args=args)
    
    # 检查命令行参数
    if len(sys.argv) != 3:
        print('错误：参数数量不正确')
        print('用法: ros2 run package_name client X Y')
        print('例如: ros2 run package_name client 10 20')
        return
    
    try:
        # 将命令行参数转换为整数
        num1 = int(sys.argv[1])
        num2 = int(sys.argv[2])
    except ValueError:
        print('错误：参数必须是整数')
        return
    
    # 创建客户端
    client = AddTwoIntsClient()
    
    try:
        # 发送请求
        future = client.send_request(num1, num2)
        
        # 等待响应
        rclpy.spin_until_future_complete(client, future)
        
        # 检查响应结果
        if future.result() is not None:
            response = future.result()
            client.get_logger().info(
                f'收到响应: {num1} + {num2} = {response.sum}'
            )
        else:
            client.get_logger().error('服务调用失败，未收到响应')
            
    except KeyboardInterrupt:
        print('\n程序被用户中断')
    finally:
        client.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

4.3 Python中的同步调用

Python也支持同步调用，但需要小心使用：

def sync_call_service(self, a, b):
    """
    同步服务调用示例
    注意：这会阻塞程序直到收到响应或超时
    """
    request = AddTwoInts.Request()
    request.a = a
    request.b = b
    
    try:
        # 同步调用，最多等待5秒
        response = self.client.call(request, timeout_sec=5.0)
        self.get_logger().info(f'同步调用结果: {response.sum}')
        return response
    except Exception as e:
        self.get_logger().error(f'服务调用失败: {e}')
        return None

5. 项目构建配置详解

5.1 C++项目配置要点

CMakeLists.txt 核心修改：

# 1. 在find_package中添加服务依赖
find_package(example_interfaces REQUIRED)

# 2. 编译可执行文件时链接服务接口
add_executable(server src/server.cpp)
ament_target_dependencies(server rclcpp example_interfaces)  # 注意这里添加example_interfaces

add_executable(client src/client.cpp)
ament_target_dependencies(client rclcpp example_interfaces)  # 注意这里添加example_interfaces

package.xml 核心修改：

<depend>example_interfacesdepend>

5.2 Python项目配置要点

setup.py 核心修改：

entry_points={
    'console_scripts': [
        # 这里定义了可以通过ros2 run运行的命令
        'server = service_example_py.server:main',
        'client = service_example_py.client:main',
    ],
},

package.xml 核心修改：

<exec_depend>rclpyexec_depend>
<exec_depend>example_interfacesexec_depend>

5.3 创建和组织项目结构

C++项目结构：

service_example_cpp/
├── CMakeLists.txt
├── package.xml
└── src/
    ├── server.cpp
    └── client.cpp

Python项目结构：

service_example_py/
├── package.xml
├── setup.py
├── setup.cfg
├── resource/
│   └── service_example_py
└── service_example_py/
    ├── __init__.py
    ├── server.py
    └── client.py

6. 编译和运行实战

6.1 完整的工作流程

第一步：创建工作空间

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

第二步：创建包

# C++包
ros2 pkg create --build-type ament_cmake service_example_cpp --dependencies rclcpp example_interfaces

# Python包  
ros2 pkg create --build-type ament_python service_example_py --dependencies rclpy example_interfaces

第三步：编写代码
将前面的服务端和客户端代码分别放入对应的文件中。

第四步：编译

cd ~/ros2_ws
colcon build --packages-select service_example_cpp service_example_py

第五步：测试运行

# 激活环境
source install/setup.bash

# 第一个终端运行服务端
ros2 run service_example_cpp server

# 第二个终端运行客户端
ros2 run service_example_cpp client 10 20

6.2 常用调试命令

查看服务状态：

# 列出所有可用服务
ros2 service list

# 查看服务类型
ros2 service type /add_two_ints

# 查看服务接口定义
ros2 interface show example_interfaces/srv/AddTwoInts

# 直接调用服务进行测试
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 15, b: 25}"

查看节点信息：

# 列出运行中的节点
ros2 node list

# 查看特定节点信息
ros2 node info /add_two_ints_server

# 查看计算图
rqt_graph

7. 常见问题和解决方案

7.1 编译相关问题

问题1：找不到服务接口

fatal error: example_interfaces/srv/add_two_ints.hpp: No such file or directory

解决方案：

• 确保CMakeLists.txt中包含了find_package(example_interfaces REQUIRED)
• 确保在ament_target_dependencies中添加了example_interfaces
• 运行rosdep install --from-paths src --ignore-src -r -y安装依赖

问题2：Python模块找不到

ModuleNotFoundError: No module named 'example_interfaces'

解决方案：

• 确保package.xml中有example_interfaces
• 重新编译并source环境：colcon build && source install/setup.bash

7.2 运行时问题

问题1：服务不可用

[INFO]: 等待服务 'add_two_ints' 可用...

解决方案：

• 检查服务端是否启动：ros2 node list
• 检查服务是否注册：ros2 service list
• 确保两个节点在同一个ROS域：检查ROS_DOMAIN_ID环境变量

问题2：客户端超时

[ERROR]: 服务调用失败或超时

解决方案：

• 增加超时时间
• 检查服务端是否正常处理请求
• 查看服务端日志是否有错误信息

7.3 调试技巧

日志输出技巧：

// C++中设置日志级别
RCLCPP_DEBUG(this->get_logger(), "详细调试信息: a=%ld, b=%ld", a, b);
RCLCPP_INFO(this->get_logger(), "正常信息");
RCLCPP_WARN(this->get_logger(), "警告信息");
RCLCPP_ERROR(this->get_logger(), "错误信息");

# Python中的日志输出
self.get_logger().debug(f'详细调试信息: a={a}, b={b}')
self.get_logger().info('正常信息')
self.get_logger().warn('警告信息')
self.get_logger().error('错误信息')

运行时设置日志级别：

# 设置DEBUG级别查看详细信息
ros2 run service_example_cpp server --ros-args --log-level DEBUG

# 查看所有日志输出
ros2 topic echo /rosout

8. 高级应用技巧

8.1 错误处理最佳实践

服务端错误处理：

void handle_service(const Request::SharedPtr request, Response::SharedPtr response)
{
    try {
        // 输入验证
        if (request->a > 1000000 || request->b > 1000000) {
            RCLCPP_WARN(this->get_logger(), "输入值过大，限制在合理范围内");
            response->sum = 0;  // 或者返回错误码
            return;
        }
        
        // 执行计算
        response->sum = request->a + request->b;
        
        RCLCPP_INFO(this->get_logger(), "成功处理请求: %ld + %ld = %ld", 
                   request->a, request->b, response->sum);
    } catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "处理服务时发生异常: %s", e.what());
        response->sum = 0;  // 设置默认值
    }
}

客户端超时处理：

def call_service_with_timeout(self, a, b, timeout_seconds=5.0):
    """带超时处理的服务调用"""
    request = AddTwoInts.Request()
    request.a = a
    request.b = b
    
    future = self.client.call_async(request)
    
    # 使用自定义超时
    start_time = time.time()
    while rclpy.ok():
        rclpy.spin_once(self, timeout_sec=0.1)
        
        if future.done():
            result = future.result()
            if result is not None:
                return result
            else:
                self.get_logger().error('服务返回空结果')
                return None
        
        if time.time() - start_time > timeout_seconds:
            self.get_logger().error(f'服务调用超时（{timeout_seconds}秒）')
            return None
    
    return None

8.2 性能优化建议

减少内存分配：

class OptimizedServer : public rclcpp::Node
{
private:
    // 预分配响应对象，避免频繁创建
    std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> cached_response_;
    
public:
    OptimizedServer() : Node("optimized_server")
    {
        cached_response_ = std::make_shared<example_interfaces::srv::AddTwoInts::Response>();
        // ... 其他初始化代码
    }
};

并发处理：

// 使用回调组支持并发处理多个请求
class ConcurrentServer : public rclcpp::Node
{
public:
    ConcurrentServer() : Node("concurrent_server")
    {
        // 创建可重入的回调组
        callback_group_ = this->create_callback_group(
            rclcpp::CallbackGroupType::Reentrant);
        
        service_ = this->create_service<example_interfaces::srv::AddTwoInts>(
            "add_two_ints",
            std::bind(&ConcurrentServer::handle_service, this, _1, _2),
            rmw_qos_profile_services_default,
            callback_group_);
    }
    
private:
    rclcpp::CallbackGroup::SharedPtr callback_group_;
};

8.3 实用的服务模式

状态查询服务：

// 查询机器人状态的服务示例
void handle_status_query(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    std_srvs::srv::Trigger::Response::SharedPtr response)
{
    // 获取当前状态
    std::string status = get_robot_status();
    
    response->success = true;
    response->message = "机器人状态: " + status;
    
    RCLCPP_INFO