大纲
- Service模式的服务端
- 请求响应函数
- 启动Service
- 停止Service
- 完整代码
 
- Service模式的客户端
- 异步模式的客户端
- 完整代码
 
- 同步模式的客户端
- 完整代码
 
 
- 测试
- 长期运行的服务
- 发送请求
- 响应一次的服务
- 发送请求
 
 
- 参考资料
在ROS 2中,除了 《Robot Operating System——topic的发布和订阅》介绍的Topic,还有其他通信机制。Service就是其中一种。
Service是一种同步的请求/响应通信机制。一个节点可以提供一个Service,其他节点可以请求这个Service,并等待响应。Service由Service类型定义,包括请求和响应消息。
Service模式的服务端
我们参考demo_nodes_cpp/src/services/add_two_ints_server.cpp的例子来讲Service的实现。
class ServerNode final : public rclcpp::Node
{
public:
  DEMO_NODES_CPP_PUBLIC
  explicit ServerNode(const rclcpp::NodeOptions & options)
  : Node("add_two_ints_server", options)
  {
……
  }
  
private:
  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool saw_request_{false};
};
ServerNode继承于rclcpp::Node,它包含了一个rclcpp::Service模板类的智能指针成员变量srv_。模板参数是一个通过IDL文件生成的结构体example_interfaces::srv::AddTwoInts。这个IDL文件可以参考https://github.com/ros2/example_interfaces/blob/rolling/srv/AddTwoInts.srv。其内容也很简单:int64类型的a和b是这个Service的Request部分,int64类型的sum是这个Service的Response部分。
int64 a
int64 b
---
int64 sum
上述srv文件会通过rosidl_generator_cpp编译成C++版本对应的代码文件。这个文件会比较长,其部分内容如下
template<class ContainerAllocator>
struct AddTwoInts_Request_
{
  using Type = AddTwoInts_Request_<ContainerAllocator>;
  explicit AddTwoInts_Request_(rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL)
  {
    if (rosidl_runtime_cpp::MessageInitialization::ALL == _init ||
      rosidl_runtime_cpp::MessageInitialization::ZERO == _init)
    {
      this->a = 0ll;
      this->b = 0ll;
    }
  }
  explicit AddTwoInts_Request_(const ContainerAllocator & _alloc, rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL)
  {
    (void)_alloc;
    if (rosidl_runtime_cpp::MessageInitialization::ALL == _init ||
      rosidl_runtime_cpp::MessageInitialization::ZERO == _init)
    {
      this->a = 0ll;
      this->b = 0ll;
    }
  }
  // field types and members
  using _a_type =
    int64_t;
  _a_type a;
  using _b_type =
    int64_t;
  _b_type b;
……
template<class ContainerAllocator>
struct AddTwoInts_Response_
{
  using Type = AddTwoInts_Response_<ContainerAllocator>;
  explicit AddTwoInts_Response_(rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL)
  {
    if (rosidl_runtime_cpp::MessageInitialization::ALL == _init ||
      rosidl_runtime_cpp::MessageInitialization::ZERO == _init)
    {
      this->sum = 0ll;
    }
  }
  explicit AddTwoInts_Response_(const ContainerAllocator & _alloc, rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL)
  {
    (void)_alloc;
    if (rosidl_runtime_cpp::MessageInitialization::ALL == _init ||
      rosidl_runtime_cpp::MessageInitialization::ZERO == _init)
    {
      this->sum = 0ll;
    }
  }
  // field types and members
  using _sum_type =
    int64_t;
  _sum_type sum;
……
struct AddTwoInts
{
  using Request = example_interfaces::srv::AddTwoInts_Request;
  using Response = example_interfaces::srv::AddTwoInts_Response;
  using Event = example_interfaces::srv::AddTwoInts_Event;
};
请求响应函数
回到Demo中。下一步我们需要设置Service收到请求时的响应函数。
    auto handle_add_two_ints = [this](
      const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
      std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) -> void
      {
        (void)request_header;
        RCLCPP_INFO(
          this->get_logger(), "Incoming request\na: %" PRId64 " b: %" PRId64,
          request->a, request->b);
        response->sum = request->a + request->b;
        saw_request_ = true;
      };
这个函数接受三个参数:
- 请求头。它是rmw_request_id_t类型,和我们之前定义的srv文件无关。
/// An rmw service request identifier
typedef struct RMW_PUBLIC_TYPE rmw_request_id_s
{
  /// The guid of the writer associated with this request
  uint8_t writer_guid[RMW_GID_STORAGE_SIZE];
  /// Sequence number of this service
  int64_t sequence_number;
} rmw_request_id_t;
- 请求体。它是srv文件上半部分的内容生成的结构体example_interfaces::srv::AddTwoInts::Request。
- 返回体。他是srv文件下半部分的内容生成的结构体example_interfaces::srv::AddTwoInts::response。
请求体和返回体中的成员变量都是public类型,可以很方便的直接访问。
如函数名handle_add_two_ints 所表达的,这个lambda函数就是将请求中的参数a和b相加,设置到返回体的sum中。
启动Service
我们可以通过Node基类的create_service方法创建并启动Service。传递的第一个参数是Service的名称,第二个参数是其请求处理函数,即上面见到的lambda函数。
    // Create a service that will use the callback function to handle requests.
    srv_ = create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", handle_add_two_ints);
停止Service
下面这段代码,会根据程序启动时所携带的参数one_shot决定这个服务是一个长期运行的服务,还是响应了一些请求后自动退出的服务。
如果one_shot 为true,则会启动一个每隔100毫秒轮询一次的定时器,检查是否已经响应过请求。如果相应过了,则调用rclcpp::shutdown()来停止Node,进而停止服务;如果没有响应过,则继续轮询。
    bool one_shot = this->declare_parameter("one_shot", false);
    if (one_shot) {
      timer_ = this->create_wall_timer(
        std::chrono::milliseconds(100),
        [this]() {
          if (saw_request_) {
            rclcpp::shutdown();
          }
        });
    }
完整代码
// Copyright 2014 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <cinttypes>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
#include "demo_nodes_cpp/visibility_control.h"
namespace demo_nodes_cpp
{
class ServerNode final : public rclcpp::Node
{
public:
  DEMO_NODES_CPP_PUBLIC
  explicit ServerNode(const rclcpp::NodeOptions & options)
  : Node("add_two_ints_server", options)
  {
    auto handle_add_two_ints = [this](
      const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
      std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) -> void
      {
        (void)request_header;
        RCLCPP_INFO(
          this->get_logger(), "Incoming request\na: %" PRId64 " b: %" PRId64,
          request->a, request->b);
        response->sum = request->a + request->b;
        saw_request_ = true;
      };
    // Create a service that will use the callback function to handle requests.
    srv_ = create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", handle_add_two_ints);
    bool one_shot = this->declare_parameter("one_shot", false);
    if (one_shot) {
      timer_ = this->create_wall_timer(
        std::chrono::milliseconds(100),
        [this]() {
          if (saw_request_) {
            rclcpp::shutdown();
          }
        });
    }
  }
private:
  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool saw_request_{false};
};
}  // namespace demo_nodes_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ServerNode)
Service模式的客户端
ROS2的Client基类只提供了异步请求客户端的方案。但是对于异步结果,我们可以选择等待返回,从而实现同步请求的功能。我们先看下异步模式。
异步模式的客户端
我们参考demo_nodes_cpp/src/services/add_two_ints_client_async.cpp的实现。
class ClientNode : public rclcpp::Node
{
public:
  DEMO_NODES_CPP_PUBLIC
  explicit ClientNode(const rclcpp::NodeOptions & options)
  : Node("add_two_ints_client", options)
  {
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    client_ = create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
    queue_async_request();
  }
……
private:
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;
};
我们需要使用之前srv文件生成的example_interfaces::srv::AddTwoInts作为模板参数,构建一个rclcpp::Client模板类。然后通过queue_async_request()来发送请求。
  DEMO_NODES_CPP_PUBLIC
  void queue_async_request()
  {
    while (!client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
    }
    auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
    request->a = 2;
    request->b = 3;
    // We give the async_send_request() method a callback that will get executed once the response
    // is received.
    // This way we can return immediately from this method and allow other work to be done by the
    // executor in `spin` while waiting for the response.
    using ServiceResponseFuture =
      rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture;
    auto response_received_callback = [this](ServiceResponseFuture future) {
        auto result = future.get();
        RCLCPP_INFO(this->get_logger(), "Result of add_two_ints: %" PRId64, result->sum);
        rclcpp::shutdown();
      };
    auto future_result = client_->async_send_request(request, response_received_callback);
  }
我们先要通过client_->wait_for_service(1s)来检测服务端是否处于Ready状态。
如果是Ready状态,则构建Request结构体并设置参数。
然后创建一个用于接收异步请求返回结果的回调函数,它的参数是rclcpp::Client模板类的SharedFuture。
最后通过async_send_request发送请求。
需要注意的是,由于我们使用的是Node来发送请求,所以在async_send_request调用结束后,程序还在运行,只是处于idle状态。当服务端返回时,response_received_callback 回调函数会被执行,我们可以通过其入参拿到结果,然后调用rclcpp::shutdown()来终止这个Node的运行。
完整代码
// Copyright 2014 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <cinttypes>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
#include "demo_nodes_cpp/visibility_control.h"
using namespace std::chrono_literals;
namespace demo_nodes_cpp
{
class ClientNode : public rclcpp::Node
{
public:
  DEMO_NODES_CPP_PUBLIC
  explicit ClientNode(const rclcpp::NodeOptions & options)
  : Node("add_two_ints_client", options)
  {
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    client_ = create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
    queue_async_request();
  }
  DEMO_NODES_CPP_PUBLIC
  void queue_async_request()
  {
    while (!client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
    }
    auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
    request->a = 2;
    request->b = 3;
    // We give the async_send_request() method a callback that will get executed once the response
    // is received.
    // This way we can return immediately from this method and allow other work to be done by the
    // executor in `spin` while waiting for the response.
    using ServiceResponseFuture =
      rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture;
    auto response_received_callback = [this](ServiceResponseFuture future) {
        auto result = future.get();
        RCLCPP_INFO(this->get_logger(), "Result of add_two_ints: %" PRId64, result->sum);
        rclcpp::shutdown();
      };
    auto future_result = client_->async_send_request(request, response_received_callback);
  }
private:
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_;
};
}  // namespace demo_nodes_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ClientNode)
同步模式的客户端
同步发送的模式,就是等待异步执行完毕。我们只要等待上述SharedFuture即可。
这次我们参考demo_nodes_cpp/src/services/add_two_ints_client.cpp。它不再使用Node模式,而是直接编译成一个可执行文件。
int main(int argc, char ** argv)
{
  // Force flush of the stdout buffer.
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("add_two_ints_client");
  auto client = node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
  auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
  request->a = 2;
  request->b = 3;
  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(node->get_logger(), "service not available, waiting again...");
  }
  // TODO(wjwwood): make it like `client->send_request(node, request)->sum`
  // TODO(wjwwood): consider error condition
  auto result = send_request(node, client, request);
  if (result) {
    RCLCPP_INFO_STREAM(node->get_logger(), "Result of add_two_ints: " << result->sum);
  } else {
    RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for response. Exiting.");
  }
  rclcpp::shutdown();
  return 0;
}
其过程也和异步模式很像,我们主要关注下实现了同步等待功能的send_request是怎么实现的。
// TODO(wjwwood): make this into a method of rclcpp::Client.
example_interfaces::srv::AddTwoInts::Response::SharedPtr send_request(
  rclcpp::Node::SharedPtr node,
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client,
  example_interfaces::srv::AddTwoInts::Request::SharedPtr request)
{
  auto result = client->async_send_request(request);
  // Wait for the result.
  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::FutureReturnCode::SUCCESS)
  {
    return result.get();
  } else {
    return NULL;
  }
}
可以看到,这次对于异步请求,我们没有再设置回调函数,而是直接通过rclcpp::spin_until_future_complete来等待SharedFuture即可;如果等到了就返回对应的值。
完整代码
// Copyright 2014 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
using namespace std::chrono_literals;
// TODO(wjwwood): make this into a method of rclcpp::Client.
example_interfaces::srv::AddTwoInts::Response::SharedPtr send_request(
  rclcpp::Node::SharedPtr node,
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client,
  example_interfaces::srv::AddTwoInts::Request::SharedPtr request)
{
  auto result = client->async_send_request(request);
  // Wait for the result.
  if (rclcpp::spin_until_future_complete(node, result) ==
    rclcpp::FutureReturnCode::SUCCESS)
  {
    return result.get();
  } else {
    return NULL;
  }
}
int main(int argc, char ** argv)
{
  // Force flush of the stdout buffer.
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("add_two_ints_client");
  auto client = node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
  auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
  request->a = 2;
  request->b = 3;
  while (!client->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting.");
      return 0;
    }
    RCLCPP_INFO(node->get_logger(), "service not available, waiting again...");
  }
  // TODO(wjwwood): make it like `client->send_request(node, request)->sum`
  // TODO(wjwwood): consider error condition
  auto result = send_request(node, client, request);
  if (result) {
    RCLCPP_INFO_STREAM(node->get_logger(), "Result of add_two_ints: " << result->sum);
  } else {
    RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for response. Exiting.");
  }
  rclcpp::shutdown();
  return 0;
}
测试
长期运行的服务
./build/demo_nodes_cpp/add_two_ints_server 
发送请求
./build/demo_nodes_cpp/add_two_ints_client_async

 服务端相应
 
 同步模式类似,不再演示。
响应一次的服务
我们将one_shot设置为true,让其响应一次后,通过rclcpp::shutdown()来关闭Node,进而关闭服务。
./build/demo_nodes_cpp/add_two_ints_server --ros-args -p one_shot:=true
发送请求
./build/demo_nodes_cpp/add_two_ints_client

 服务端相应
 
 可以看到服务端响应后就退出了。
参考资料
- https://github.com/ros2/example_interfaces/blob/rolling/srv/AddTwoInts.srv



















