526互联

Beginner：Client libraries-5-实现一个简单的服务和客户端(c++)

发布时间 2023-06-02 14:57:40作者: gary_123

目标：创建和运行服务和客户端节点使用C++.

背景

当一个节点使用服务通信时，客户端节点发送请求数据，服务节点响应请求。请求和响应的结构文件为.srv。

下面的例子是：一个节点请求两个整数求和，另外一个节点响应这个结果。

任务

1、创建一个包

ros2 pkg create --build-type ament_cmake cpp_srvcli --dependencies rclcpp example_interfaces

example_interfaces包包含了文件.srv，里面的内容如下

int64 a
int64 b
---
int64 sum

上两行为请求的内容，下面一行为响应。

1.1 更新package.xml

由于在创建包的时候，使用了--dependencies，所以不需要再package.xml和CMakeLists.txt中再添加。

2、实现服务节点

创建文件：add_two_ints_server.cpp

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

#include <memory>

void add(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(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld",
                request->a, request->b);
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum);
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");

  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
    node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");

  rclcpp::spin(node);
  rclcpp::shutdown();
}

3、实现客户端节点

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

#include <chrono>
#include <cstdlib>
#include <memory>

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    if(argc != 3)
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y");
        return 1;
    }
    //创建一个节点并初始化客户端
    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_client");

    rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client = 
    node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
    //请求被创建，定义在.srv文件中
    auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();

    request->a = atoll(argv[1]);
    request->b = atoll(argv[2]);
    //while的loop给客户端1s的时间在网络上搜索，找不到就等待
    while(!client->wait_for_service(1s))
    {
        //如果客户端被取消就退出
        if(!rclcpp::ok())
        {
            RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the serrvice.Exiting.");
            return 0;
        }
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again ...");
        
    }

    auto result = client->async_send_request(request);
    //wait for the result
    if(rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum);
    }
    else
    {
        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service and_two_ints");
    }

    rclcpp::shutdown();
    return 0;
}

4、运行

ros2 run cpp_srvcli server

ros2 run cpp_srvcli client 5 9

总结

创建了两个节点来请求和响应数据通过service服务。将一些依赖和二进制添加 到了CMakeLists.txt中以便能够编译和运行。