大纲
- 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