【ROS2】客户端与服务端通信，数据类型为std_srvs::srv::Empty

一、前言

最近在消化ROS2版本的NDT（基于lidar的建定位算法，本文不展开）代码工程，原来的代码只有回环检测达到一个标准才会自动保存pcd地图。想根据自己的需求手动执行保存，比较方便的一种方案是检测if (!rclcpp::ok())，中止终端后可以跳到保存pcd的进程。还一种方便的方案是用ros2的服务端客户端通信机制，响应也很快，而且可以手动多保存几份pcd。网上关于ros2客户端与服务端通信，数据类型为数据类型为std_srvs::srv::Empty的案例比较少，所以这里记录一下。
服务的类型为空（empty),这表明在调用这个服务是不需要参数（比如，请求不需要发送数据，响应也没有数据），这种形式不需要写srv文件，我们这里的例程只需要在收到客户端的request后，服务端执行回调函数。

二、客户端

客户端运行后就会向ndt建图的服务端发送保存pcd的请求，大火也可以自行添加一个无阻塞获取键盘输入，来控制客户端请求的发送，只有一个源文件，这里直接上源码。

#include <rclcpp/rclcpp.hpp>
#include <memory>
#include <chrono>
#include <cstdlib>
#include <unistd.h>
#include<typeinfo>
#include <std_srvs/srv/empty.hpp>


using namespace std::chrono_literals;



int main(int argc, char * argv[])
{
  /* 初始化ROS2 */
  rclcpp::init(argc, argv);

	/* 创建请求request */
  //注意std_srvs::srv::Empty::Request类型的请求
  auto request = std::make_shared<std_srvs::srv::Empty::Request>();
  /* 创建服务名为save_map_client的client客户端节点 */
  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("save_map_client");

  //rclcpp::Client < std_srvs::srv::Empty > ::SharedPtr map_save_client_;
  
  //const char* save = "map_save";
  auto map_save_client_ = node->create_client<std_srvs::srv::Empty>("map_save");
  //rclcpp::Client<std_srvs::srv::Empty>::SharedPtr map_save_client_ = create_client<std_srvs::srv::Empty>("map_save");


  int cnt = 0;
  while(rclcpp::ok()) 
  {
    /* 搜索服务节点，间隔1s */
    if(map_save_client_->wait_for_service(1s))
    {
      /* 发送请求，Request数据类型的为空 */
      auto flag = map_save_client_->async_send_request(request);
      //判断数据类型
      if(typeid(flag) == typeid(std::shared_future<std::shared_ptr<std_srvs::srv::Empty_Response_<std::allocator<void>>>>))
      {
      //如果为if为true说明发送成功
        std::cout << "send an request to save the map" << std::endl;
        cnt++;
      }
      if(cnt == 20){return 0;}//发送20次退出
      usleep(5000);
    }
    /* 如果找不到，将会继续等待 */
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again...");
    usleep(5000);
  }


  /* 退出ROS2 */
  RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
  rclcpp::shutdown();
  return 0;
}

客户端要如何检测服务端收到了request呢？当数据类型不为empty时，我们会在srv文件里面定义一个bool类型变量，当服务端收到request时为true，但这里我们没有写srv文件，所以客户端源码里采取这个语句来识别服务端是否收到request：

/* 发送请求，Request数据类型的为空 */
      auto flag = map_save_client_->async_send_request(request);
      //判断数据类型
      if(typeid(flag) == typeid(std::shared_future<std::shared_ptr<std_srvs::srv::Empty_Response_<std::allocator<void>>>>))

如果服务端收到了request，返回到flag的数据类型就会满足if判断。当然，大火也可以在服务端写检测有无request。关于async_send_request(request)我尝试不用flag接收返回值，也能成功发送request。

我这里客户端是单独一个功能包，服务端写在NDT的工程源文件里面，下面看一下客户端功能包的Cmakelists和package.xml文件：

cmake_minimum_required(VERSION 3.5)
project(mapsave_cli)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_srvs REQUIRED)
find_package(example_interfaces REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  
  ament_lint_auto_find_test_dependencies()
endif()


add_executable(map_save_cli src/cli_mapsave.cpp)
ament_target_dependencies(map_save_cli rclcpp std_srvs)

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

ament_package()

<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>mapsave_cli</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="d120@todo.todo">d120</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>example_interfaces</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

大火也可以在创建功能包的时候就把依赖加上，像这样（xxx是自定义的依赖）：

ros2 pkg create --build-type ament_cmake mapsave_cli --dependencies rclcpp xxxx

三、服务端

服务端收到request后，就会保存pcd文件，因为服务端的代码是写在NDT的源码里面，所以这里粘出一部分，大火可以参考一下，这部分也比较简单。

 auto map_save_callback =
    [this](const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<std_srvs::srv::Empty::Request> request,
      const std::shared_ptr<std_srvs::srv::Empty::Response> response) -> void
    {
      std::cout << "Received an request to save the map" << std::endl;
      if (initial_map_array_received_ == false) 
      {
        std::cout << "initial map is not received" << std::endl;
        return;
      }
    
        doPoseAdjustment(map_array_msg_, true);
      //保存pcd的步骤写在这个函数里面
    };
    
  map_save_srv_ = create_service<std_srvs::srv::Empty>("map_save", map_save_callback);
  //服务的类型为空（empty),这表明在调用这个服务是不需要参数（比如，请求不需要发送数据，响应也没有数据）

编译文件和xml的依赖和客户端没太多区别，这里就不列出来了。

四、结语

客户端和服务端这种方式，适合端对端通信，一般用于发送请求、配置等一些能快速作出反应并处理的任务，参数为empty能应用于很多情况，写起来也比较方便。
需要ROS2版本的NDT源码可以私信。