Skip to content

Commit 0750dc4

Browse files
ivanpaunoivanpauno
authored
Support to defer to send a response in services (#1709)
Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
1 parent 0d6d9e6 commit 0750dc4

File tree

3 files changed

+141
-65
lines changed

3 files changed

+141
-65
lines changed

‎rclcpp/include/rclcpp/any_service_callback.hpp‎

Lines changed: 100 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -15,10 +15,12 @@
1515
#ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_
1616
#define RCLCPP__ANY_SERVICE_CALLBACK_HPP_
1717

18+
#include <variant>
1819
#include <functional>
1920
#include <memory>
2021
#include <stdexcept>
2122
#include <type_traits>
23+
#include <utility>
2224

2325
#include "rclcpp/function_traits.hpp"
2426
#include "rclcpp/visibility_control.hpp"
@@ -29,93 +31,133 @@
2931
namespace rclcpp
3032
{
3133

32-
template<typename ServiceT>
33-
class AnyServiceCallback
34+
namespace detail
3435
{
35-
private:
36-
using SharedPtrCallback = std::function<
37-
void (
38-
const std::shared_ptr<typename ServiceT::Request>,
39-
std::shared_ptr<typename ServiceT::Response>
40-
)>;
41-
using SharedPtrWithRequestHeaderCallback = std::function<
42-
void (
43-
const std::shared_ptr<rmw_request_id_t>,
44-
const std::shared_ptr<typename ServiceT::Request>,
45-
std::shared_ptr<typename ServiceT::Response>
46-
)>;
36+
template<typename T, typename = void>
37+
struct can_be_nullptr : std::false_type {};
38+
39+
// Some lambdas define a comparison with nullptr,
40+
// but we see a warning that they can never be null when using it.
41+
// We also test if `T &` can be assigned to `nullptr` to avoid the issue.
42+
template<typename T>
43+
struct can_be_nullptr<T, std::void_t<
44+
decltype(std::declval<T>() == nullptr), decltype(std::declval<T &>() = nullptr)>>
45+
: std::true_type {};
46+
} // namespace detail
4747

48-
SharedPtrCallback shared_ptr_callback_;
49-
SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
48+
// Forward declare
49+
template<typename ServiceT>
50+
class Service;
5051

52+
template<typename ServiceT>
53+
class AnyServiceCallback
54+
{
5155
public:
5256
AnyServiceCallback()
53-
: shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr)
57+
: callback_(std::monostate{})
5458
{}
5559

56-
AnyServiceCallback(const AnyServiceCallback &) = default;
57-
5860
template<
5961
typename CallbackT,
60-
typename std::enable_if<
61-
rclcpp::function_traits::same_arguments<
62-
CallbackT,
63-
SharedPtrCallback
64-
>::value
65-
>::type * = nullptr
66-
>
67-
void set(CallbackT callback)
62+
typename std::enable_if_t<!detail::can_be_nullptr<CallbackT>::value, int> = 0>
63+
void
64+
set(CallbackT && callback)
6865
{
69-
shared_ptr_callback_ = callback;
66+
callback_ = std::forward<CallbackT>(callback);
7067
}
7168

7269
template<
7370
typename CallbackT,
74-
typename std::enable_if<
75-
rclcpp::function_traits::same_arguments<
76-
CallbackT,
77-
SharedPtrWithRequestHeaderCallback
78-
>::value
79-
>::type * = nullptr
80-
>
81-
void set(CallbackT callback)
71+
typename std::enable_if_t<detail::can_be_nullptr<CallbackT>::value, int> = 0>
72+
void
73+
set(CallbackT && callback)
8274
{
83-
shared_ptr_with_request_header_callback_ = callback;
75+
if (!callback) {
76+
throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr");
77+
}
78+
callback_ = std::forward<CallbackT>(callback);
8479
}
8580

86-
void dispatch(
87-
std::shared_ptr<rmw_request_id_t> request_header,
88-
std::shared_ptr<typename ServiceT::Request> request,
89-
std::shared_ptr<typename ServiceT::Response> response)
81+
// template<typename Allocator = std::allocator<typename ServiceT::Response>>
82+
std::shared_ptr<typename ServiceT::Response>
83+
dispatch(
84+
const std::shared_ptr<rclcpp::Service<ServiceT>> & service_handle,
85+
const std::shared_ptr<rmw_request_id_t> & request_header,
86+
std::shared_ptr<typename ServiceT::Request> request)
9087
{
9188
TRACEPOINT(callback_start, static_cast<const void *>(this), false);
92-
if (shared_ptr_callback_ != nullptr) {
89+
if (std::holds_alternative<std::monostate>(callback_)) {
90+
// TODO(ivanpauno): Remove the set method, and force the users of this class
91+
// to pass a callback at construnciton.
92+
throw std::runtime_error{"unexpected request without any callback set"};
93+
}
94+
if (std::holds_alternative<SharedPtrDeferResponseCallback>(callback_)) {
95+
const auto & cb = std::get<SharedPtrDeferResponseCallback>(callback_);
96+
cb(request_header, std::move(request));
97+
return nullptr;
98+
}
99+
if (std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_)) {
100+
const auto & cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_);
101+
cb(service_handle, request_header, std::move(request));
102+
return nullptr;
103+
}
104+
// auto response = allocate_shared<typename ServiceT::Response, Allocator>();
105+
auto response = std::make_shared<typename ServiceT::Response>();
106+
if (std::holds_alternative<SharedPtrCallback>(callback_)) {
93107
(void)request_header;
94-
shared_ptr_callback_(request, response);
95-
} else if (shared_ptr_with_request_header_callback_ != nullptr) {
96-
shared_ptr_with_request_header_callback_(request_header, request, response);
97-
} else {
98-
throw std::runtime_error("unexpected request without any callback set");
108+
const auto & cb = std::get<SharedPtrCallback>(callback_);
109+
cb(std::move(request), response);
110+
} else if (std::holds_alternative<SharedPtrWithRequestHeaderCallback>(callback_)) {
111+
const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_);
112+
cb(request_header, std::move(request), response);
99113
}
100114
TRACEPOINT(callback_end, static_cast<const void *>(this));
115+
return response;
101116
}
102117

103118
void register_callback_for_tracing()
104119
{
105120
#ifndef TRACETOOLS_DISABLED
106-
if (shared_ptr_callback_) {
107-
TRACEPOINT(
108-
rclcpp_callback_register,
109-
static_cast<const void *>(this),
110-
tracetools::get_symbol(shared_ptr_callback_));
111-
} else if (shared_ptr_with_request_header_callback_) {
112-
TRACEPOINT(
113-
rclcpp_callback_register,
114-
static_cast<const void *>(this),
115-
tracetools::get_symbol(shared_ptr_with_request_header_callback_));
116-
}
121+
std::visit(
122+
[this](auto && arg) {
123+
TRACEPOINT(
124+
rclcpp_callback_register,
125+
static_cast<const void *>(this),
126+
tracetools::get_symbol(arg));
127+
}, callback_);
117128
#endif // TRACETOOLS_DISABLED
118129
}
130+
131+
private:
132+
using SharedPtrCallback = std::function<
133+
void (
134+
std::shared_ptr<typename ServiceT::Request>,
135+
std::shared_ptr<typename ServiceT::Response>
136+
)>;
137+
using SharedPtrWithRequestHeaderCallback = std::function<
138+
void (
139+
std::shared_ptr<rmw_request_id_t>,
140+
std::shared_ptr<typename ServiceT::Request>,
141+
std::shared_ptr<typename ServiceT::Response>
142+
)>;
143+
using SharedPtrDeferResponseCallback = std::function<
144+
void (
145+
std::shared_ptr<rmw_request_id_t>,
146+
std::shared_ptr<typename ServiceT::Request>
147+
)>;
148+
using SharedPtrDeferResponseCallbackWithServiceHandle = std::function<
149+
void (
150+
std::shared_ptr<rclcpp::Service<ServiceT>>,
151+
std::shared_ptr<rmw_request_id_t>,
152+
std::shared_ptr<typename ServiceT::Request>
153+
)>;
154+
155+
std::variant<
156+
std::monostate,
157+
SharedPtrCallback,
158+
SharedPtrWithRequestHeaderCallback,
159+
SharedPtrDeferResponseCallback,
160+
SharedPtrDeferResponseCallbackWithServiceHandle> callback_;
119161
};
120162

121163
} // namespace rclcpp

‎rclcpp/include/rclcpp/service.hpp‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -141,7 +141,9 @@ class ServiceBase
141141
};
142142

143143
template<typename ServiceT>
144-
class Service : public ServiceBase
144+
class Service
145+
: public ServiceBase,
146+
public std::enable_shared_from_this<Service<ServiceT>>
145147
{
146148
public:
147149
using CallbackType = std::function<
@@ -335,9 +337,10 @@ class Service : public ServiceBase
335337
std::shared_ptr<void> request) override
336338
{
337339
auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
338-
auto response = std::make_shared<typename ServiceT::Response>();
339-
any_callback_.dispatch(request_header, typed_request, response);
340-
send_response(*request_header, *response);
340+
auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request);
341+
if (response) {
342+
send_response(*request_header, *response);
343+
}
341344
}
342345

343346
void

‎rclcpp/test/rclcpp/test_any_service_callback.cpp‎

Lines changed: 34 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include <utility>
2323

2424
#include "rclcpp/any_service_callback.hpp"
25+
#include "rclcpp/service.hpp"
2526
#include "test_msgs/srv/empty.hpp"
2627
#include "test_msgs/srv/empty.h"
2728

@@ -44,7 +45,7 @@ class TestAnyServiceCallback : public ::testing::Test
4445

4546
TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) {
4647
EXPECT_THROW(
47-
any_service_callback_.dispatch(request_header_, request_, response_),
48+
any_service_callback_.dispatch(nullptr, request_header_, request_),
4849
std::runtime_error);
4950
}
5051

@@ -57,7 +58,7 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) {
5758

5859
any_service_callback_.set(callback);
5960
EXPECT_NO_THROW(
60-
any_service_callback_.dispatch(request_header_, request_, response_));
61+
EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
6162
EXPECT_EQ(callback_calls, 1);
6263
}
6364

@@ -73,6 +74,36 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_header) {
7374

7475
any_service_callback_.set(callback_with_header);
7576
EXPECT_NO_THROW(
76-
any_service_callback_.dispatch(request_header_, request_, response_));
77+
EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
78+
EXPECT_EQ(callback_with_header_calls, 1);
79+
}
80+
81+
TEST_F(TestAnyServiceCallback, set_and_dispatch_defered) {
82+
int callback_with_header_calls = 0;
83+
auto callback_with_header =
84+
[&callback_with_header_calls](const std::shared_ptr<rmw_request_id_t>,
85+
const std::shared_ptr<test_msgs::srv::Empty::Request>) {
86+
callback_with_header_calls++;
87+
};
88+
89+
any_service_callback_.set(callback_with_header);
90+
EXPECT_NO_THROW(
91+
EXPECT_EQ(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
92+
EXPECT_EQ(callback_with_header_calls, 1);
93+
}
94+
95+
TEST_F(TestAnyServiceCallback, set_and_dispatch_defered_with_service_handle) {
96+
int callback_with_header_calls = 0;
97+
auto callback_with_header =
98+
[&callback_with_header_calls](std::shared_ptr<rclcpp::Service<test_msgs::srv::Empty>>,
99+
const std::shared_ptr<rmw_request_id_t>,
100+
const std::shared_ptr<test_msgs::srv::Empty::Request>)
101+
{
102+
callback_with_header_calls++;
103+
};
104+
105+
any_service_callback_.set(callback_with_header);
106+
EXPECT_NO_THROW(
107+
EXPECT_EQ(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
77108
EXPECT_EQ(callback_with_header_calls, 1);
78109
}

0 commit comments

Comments
 (0)