ROS2-service通信及闪退的问题

1. 简介

service通信和topic通信的主要区别在于service是有应答的,使用起来很简单,在网上一搜就有例程,之所以整理这篇笔记是因为本人在使用中遇到一个闪退的问题,排查了好一阵子才解决,为了后人少采坑整理本笔记

本笔记内容如下:

• 简单介绍service在ros2使用
  • 客户端
  • 服务端
• spin的闪退问题
• 常用命令

2. service在ros2使用

2.1 服务端

不多说直接copy例程

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

关键点:

• create_service创建server
  • 需要注明服务端的名称,客户端是通过该名字匹配
  • 提供回调处理函数

2.2 客户端

#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::executor::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;
}

关键点:

• create_client创建客户端对象,需要指明服务端的名字;
• wait_for_service检查服务端是否存在;
• std::make_shared<SRV_MSG::Request>创建请求消息;
• async_send_request异步发送请求:这个API的返回是一个std::shared_future对象,简单来说这类对象是C++11的std库提供用于异步操作,该对象用于等待"未发生"的事件;
• spin_until_future_complete用于检查刚才返回的"未发生"对象,输入参数可以增加超时时间

3. 闪退问题

3.1 现象描述

在应用上面的客户端时发现,程序执行到spin_until_future_complete这个api时出现闪退.

3.2 分析

刚好留意到这个api带有spin字样,以及输入node节点,这个和我们使用的rclcpp::spin(pNode),是不是同一个节点不能调两个spin呢?

刚好找到以下的博客遇到同样的问题
https://fishros.org.cn/forum/topic/421/ros2自定义客户端中spin_until_future_complete的问题/2

我想将client封装为一个类，类中定义一个定时器以及一个客户端，定时器回调函数中会发送客户端请求，同时阻塞直到接受server的response（使用spin_until_future_complete方法）。这样有一个问题，rclcpp::spin_until_future_complete( )相当于执行了一次spin，而我定义的类，最终是要添加到executors中，executors再调用spin（）方法，本质上就是spin中又调用了spin，在运行时会出现冲突。

3.3 目的和意义

这个用法其实是常见的:

• 一种就是和博客上面的情况,用ros的定时器设了回调,回调里面再执行轮询服务端,定时器的回调肯定是通过另一个spin来拉起的(关于ros的spin工作原理请翻看另一篇笔记);
• 另一种就是结点里面注册了一些话题的回调,同样必然需要放入spin,然后在主线程中执行和服务端的通讯,然后阻塞等待结果返回

所以这种问题还是很容易遇到的.

3.4 解决方法

方法:阻塞等待时不使用ros的spin_until_future_complete,而使用std::shared_future自身的wait_for,如下:

auto response = camera_switchers_->async_send_request(request);

auto status = response.wait_for(100ms);

// Wait for the result.
if(status == std::future_status::timeout) {
    return response.get()->result;
} else {
    RCLCPP_ERROR(p_node_->get_logger(), "Fail to call server.");
    return false;
}

3.5 补充

另外上面提到的博客他还遇到另一个问题,通过他的描述我估计是这样的:
因为它的阻塞等待放在回调中,而wait_for本身又是要等待服务应答后触发另一种回调,这时候如果不设置可重入回调组会导致wait_for返回的永远是超时,这又涉及到ros的spin机制,详细看另一篇笔记.

4. 常用指令

1. ros2 service call

ros2 service call [service name] [msg type] [content]

# 例子
ros2 service call /camera_control_service \
keystar_msgs/srv/SwitchCamera \ 
"{node_name: 'keystar',arm_id: 0,camera_id: 0,enable: true}"

2. ros2 interface show

和topic一样用于查看服务消息接口的定义

3. ros2 service list \[-t]

-t 选项指标记消息类型