ROS 2边学边练（15）-- 写一个简单的服务（C++）

前言

        此篇我们即将编写一个简单的服务（service）通信例子，客户端节点向服务端节点发出请求（.srv文件中规定了通信的数据结构格式），服务端节点收到请求后将结果回复给客户端节点，一问一答，简单明了。

        例子中，我们实现求和的服务内容，客户端发出请求数据（两个int数），服务端将这俩数相加后将求和的结果回复给客户端。

动动手

创建一个功能包

        老步骤，打开一个终端，激活ROS 2安装环境，再进入工作空间根路径下的src目录，执行下面命令生成我们今天的服务功能包cpp_srvcli：

$ros2 pkg create --build-type ament_cmake --license Apache-2.0 cpp_srvcli --dependencies rclcpp example_interfaces

上面的命令与上一篇主题（topic）的有些不一样，多了--dependencies rclcpp example_interfaces，--dependencies会将依赖项rclcpp和example_interfaces自动补充到package.xml和CMakeLists.txt中去，我们可以打开这俩文件看看：

package.xml（手动更新了description、maintainer字段）

CMakeLists.txt

 example_interfaces也是一个功能包（ROS 2提供的），它包含了我们在服务通信过程中需要用到的.srv文件（定义了通信的数据结构格式），.srv文件的内容可能像下面这样：

int64 a
int64 b
---
int64 sum

 三道杠的上面为客户端的请求数据（通信时会填充具体的两个值），三道杠下面是服务端利用请求数据运算得到的结果值，会将其回复给客户端。

编写服务端节点

        进入ros2_ws/src/cpp_srvcli/src路径，新建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();
}

分析代码

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);
}

        add函数有两个参数，其一为请求数据的指针request（由客户端发过来），其二为回复数据指针response，会发送给客户端。函数体内，会将请求数据相加的结果赋值给回复数据，另外向控制台（终端）打印出相关的一些信息。

rclcpp::init(argc, argv);

初始化ROS 2 C++库，

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

创建一个名为add_two_ints_server的节点，用来作服务端，

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

创建一个名为add_two_ints的服务，当有服务请求的时候自动调用add函数处理服务回复。

添加可执行目标文件信息到CMakeLists.txt

        将下面内容添加到CMakeLists.txt中，add_two_ints_server.cpp编译完成后生成的目标文件名称我们定为“server”，另外install宏会使ROS 2能在lib路径下找到目标文件：

add_executable(server src/add_two_ints_server.cpp)
ament_target_dependencies(server rclcpp example_interfaces)

install(TARGETS
    server
  DESTINATION lib/${PROJECT_NAME})

编写客户端节点

        同样的，我们在ros2_ws/src/cpp_srvcli/src下，新建一个add_two_ints_client.cpp文件，将下面源代码复制进去：

#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");

  auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
  request->a = atoll(argv[1]);
  request->b = atoll(argv[2]);

  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. 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 add_two_ints");
  }

  rclcpp::shutdown();
  return 0;
}

分析代码

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");

        创建服务add_two_ints的客户端节点add_two_ints_client，

auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
request->a = atoll(argv[1]);
request->b = atoll(argv[2]);

创建一个请求request,并且将启动客户端节点时传入的两个int数（字符串转长整型）赋值给上面提到的.srv文件里面定义的请求数据变量a和b，

while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
  }

进入while循环寻找网络中的服务节点，如果超过1秒还没有找到，打印出“service not available,waiting again...”，并继续寻找，直到找到服务节点才退出while循环语句，如果你主动通过ctrl + c关闭了客户端节点，打印出“Interrupted while waiting for the service. Exiting.”

auto result = client->async_send_request(request);

找到服务节点后发送数据请求（request）给对方，

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 add_two_ints");
  }

进入spin_until_future_complete函数，等待服务节点的结果回复或者返回失败 。

添加可执行目标文件信息到CMakeLists.txt

        同服务端节点的类似，但是这次我们可以只使用一个install宏（上一篇我们lisener和talker各用了一个install宏语句），简练一点，最后结果如下：

构建运行 

检查依赖

        先回到我们的工作空间根路径下，检查下依赖情况：

$rosdep install -i --from-path src --rosdistro iron -y

构建功能包 

        依然是在工作空间根路径下，我们来构建cpp_srvcli功能包：

$colcon build --packages-select cpp_srvcli

构建成功之后你可以看看ros2_ws/install/cpp_srvcli/lib/cpp_srvcli下生成了我们的目标文件client和server。

运行server和client 

        新开两个终端并进入工作空间根路径，分别执行下述命令激活环境变量（underlay和overlay的一起，上一篇有解释不用再分别执行underlay和overlay的）：

$source install/setup.bash

我们现在一个终端启动服务端节点：

$ros2 run cpp_srvcli server

我们在另外一个终端启动客户端节点，但是注意，我们要传入2个参数作为请求数据：

$ros2 run cpp_srvcli client 5 6

我们传入了5和6，很快服务端就返回了结果：

服务端的打印如下：

此时客户端收到回复后立即退出运行了，而我们的服务端节点还在等待新的请求，我们再启动一个客户端节点，传入5和5，服务端返回了10，并继续等待新的请求，如果不需要再做什么了我们可以主动关掉服务节点。

 本篇完。