ros2 客户端服务编写流程 C++

在ROS 2中，使用C++编写客户端服务（Client Service）主要包括以下步骤：

• 添加必要的头文件：根据你的需求，添加所需的ROS 2头文件。例如，如果你想使用example_interfaces/srv/AddTwoInts服务接口，需要添加example_interfaces/srv/add_two_ints.hpp头文件。

• 初始化ROS 2节点：使用rclcpp::init()初始化ROS 2节点。

• 创建一个执行器（Executor）：创建rclcpp::executors::SingleThreadedExecutor对象，用于执行ROS 2节点。

• 创建一个客户端服务对象：使用服务接口类型和节点名字创建一个客户端服务对象。例如，对于AddTwoInts服务接口，可以使用rclcpp::create_client<example_interfaces::srv::AddTwoInts>(“add_two_ints”, rmw_qos_profile_default)创建一个客户端服务对象。

• 等待服务可用：使用wait_for_service()函数等待服务变为可用。例如，使用client->wait_for_service()等待add_two_ints服务可用。

• 创建请求并发送服务请求：创建一个请求消息，并使用async_send_request()或invoke_async()函数发送服务请求。例如，使用auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>()创建一个请求，并使用client->async_send_request(request)发送服务请求。

• 处理服务响应：使用回调函数来处理服务响应。可以使用add_response_callback()函数添加一个回调函数来处理响应。例如，使用client->add_response_callback([](rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture future) { /* 响应处理代码 */ })来添加一个回调函数。

• 执行ROS 2节点：使用执行器对象的spin()函数来执行ROS 2节点，并处理服务请求和响应。

• 下面是一个简单的示例代码，展示了如何编写一个ROS 2客户端服务：

#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"

void handle_response(const rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture future)
{
try {
 // 获取服务响应
 auto response = future.get();
 // 处理响应
 RCLCPP_INFO(rclcpp::get_logger("client"), "Sum: %ld", response->sum);
} catch (const std::exception& e) {
 // 处理异常
 RCLCPP_ERROR(rclcpp::get_logger("client"), "Failed to get response: %s", e.what());
}
}

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("client_node");

// 创建客户端服务对象
auto client = node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");

// 等待服务可用
if (!client->wait_for_service(std::chrono::seconds(1))) {
 RCLCPP_ERROR(node->get_logger(), "Service not available");
 return 1;
}

// 创建请求
auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
request->a = 5;
request->b = 10;

// 发送服务请求并处理响应
auto future = client->async_send_request(request);
client->add_response_callback(handle_response);

// 创建执行器并执行节点
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
executor.spin();

rclcpp::shutdown();
return 0;
}