|
15 | 15 | #ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_ |
16 | 16 | #define RCLCPP__ANY_SERVICE_CALLBACK_HPP_ |
17 | 17 |
|
| 18 | +#include <variant> |
18 | 19 | #include <functional> |
19 | 20 | #include <memory> |
20 | 21 | #include <stdexcept> |
21 | 22 | #include <type_traits> |
| 23 | +#include <utility> |
22 | 24 |
|
23 | 25 | #include "rclcpp/function_traits.hpp" |
24 | 26 | #include "rclcpp/visibility_control.hpp" |
|
29 | 31 | namespace rclcpp |
30 | 32 | { |
31 | 33 |
|
32 | | -template<typename ServiceT> |
33 | | -class AnyServiceCallback |
| 34 | +namespace detail |
34 | 35 | { |
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 {}; |
47 | 38 |
|
48 | | - SharedPtrCallback shared_ptr_callback_; |
49 | | - SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_; |
| 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 | +#ifdef __QNXNTO__ |
| 44 | +struct can_be_nullptr<T, std::void_t< |
| 45 | + decltype(std::declval<T>() == nullptr)>>: std::true_type {}; |
| 46 | +#else |
| 47 | +struct can_be_nullptr<T, std::void_t< |
| 48 | + decltype(std::declval<T>() == nullptr), decltype(std::declval<T &>() = nullptr)>> |
| 49 | + : std::true_type {}; |
| 50 | +#endif |
| 51 | +} // namespace detail |
50 | 52 |
|
| 53 | +// Forward declare |
| 54 | +template<typename ServiceT> |
| 55 | +class Service; |
| 56 | + |
| 57 | +template<typename ServiceT> |
| 58 | +class AnyServiceCallback |
| 59 | +{ |
51 | 60 | public: |
52 | 61 | AnyServiceCallback() |
53 | | - : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr) |
| 62 | + : callback_(std::monostate{}) |
54 | 63 | {} |
55 | 64 |
|
56 | | - AnyServiceCallback(const AnyServiceCallback &) = default; |
57 | | - |
58 | 65 | template< |
59 | 66 | typename CallbackT, |
60 | | - typename std::enable_if< |
| 67 | + typename std::enable_if_t<!detail::can_be_nullptr<CallbackT>::value, int> = 0> |
| 68 | + void |
| 69 | + set(CallbackT && callback) |
| 70 | + { |
| 71 | + // Workaround Windows issue with std::bind |
| 72 | + if constexpr ( |
61 | 73 | rclcpp::function_traits::same_arguments< |
62 | 74 | CallbackT, |
63 | 75 | SharedPtrCallback |
64 | | - >::value |
65 | | - >::type * = nullptr |
66 | | - > |
67 | | - void set(CallbackT callback) |
68 | | - { |
69 | | - shared_ptr_callback_ = callback; |
| 76 | + >::value) |
| 77 | + { |
| 78 | + callback_.template emplace<SharedPtrCallback>(callback); |
| 79 | + } else if constexpr ( // NOLINT, can't satisfy both cpplint and uncrustify |
| 80 | + rclcpp::function_traits::same_arguments< |
| 81 | + CallbackT, |
| 82 | + SharedPtrWithRequestHeaderCallback |
| 83 | + >::value) |
| 84 | + { |
| 85 | + callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback); |
| 86 | + } else if constexpr ( // NOLINT |
| 87 | + rclcpp::function_traits::same_arguments< |
| 88 | + CallbackT, |
| 89 | + SharedPtrDeferResponseCallback |
| 90 | + >::value) |
| 91 | + { |
| 92 | + callback_.template emplace<SharedPtrDeferResponseCallback>(callback); |
| 93 | + } else if constexpr ( // NOLINT |
| 94 | + rclcpp::function_traits::same_arguments< |
| 95 | + CallbackT, |
| 96 | + SharedPtrDeferResponseCallbackWithServiceHandle |
| 97 | + >::value) |
| 98 | + { |
| 99 | + callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback); |
| 100 | + } else { |
| 101 | + // the else clause is not needed, but anyways we should only be doing this instead |
| 102 | + // of all the above workaround ... |
| 103 | + callback_ = std::forward<CallbackT>(callback); |
| 104 | + } |
70 | 105 | } |
71 | 106 |
|
72 | 107 | template< |
73 | 108 | typename CallbackT, |
74 | | - typename std::enable_if< |
| 109 | + typename std::enable_if_t<detail::can_be_nullptr<CallbackT>::value, int> = 0> |
| 110 | + void |
| 111 | + set(CallbackT && callback) |
| 112 | + { |
| 113 | + if (!callback) { |
| 114 | + throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr"); |
| 115 | + } |
| 116 | + // Workaround Windows issue with std::bind |
| 117 | + if constexpr ( |
| 118 | + rclcpp::function_traits::same_arguments< |
| 119 | + CallbackT, |
| 120 | + SharedPtrCallback |
| 121 | + >::value) |
| 122 | + { |
| 123 | + callback_.template emplace<SharedPtrCallback>(callback); |
| 124 | + } else if constexpr ( // NOLINT |
75 | 125 | rclcpp::function_traits::same_arguments< |
76 | 126 | CallbackT, |
77 | 127 | SharedPtrWithRequestHeaderCallback |
78 | | - >::value |
79 | | - >::type * = nullptr |
80 | | - > |
81 | | - void set(CallbackT callback) |
82 | | - { |
83 | | - shared_ptr_with_request_header_callback_ = callback; |
| 128 | + >::value) |
| 129 | + { |
| 130 | + callback_.template emplace<SharedPtrWithRequestHeaderCallback>(callback); |
| 131 | + } else if constexpr ( // NOLINT |
| 132 | + rclcpp::function_traits::same_arguments< |
| 133 | + CallbackT, |
| 134 | + SharedPtrDeferResponseCallback |
| 135 | + >::value) |
| 136 | + { |
| 137 | + callback_.template emplace<SharedPtrDeferResponseCallback>(callback); |
| 138 | + } else if constexpr ( // NOLINT |
| 139 | + rclcpp::function_traits::same_arguments< |
| 140 | + CallbackT, |
| 141 | + SharedPtrDeferResponseCallbackWithServiceHandle |
| 142 | + >::value) |
| 143 | + { |
| 144 | + callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback); |
| 145 | + } else { |
| 146 | + // the else clause is not needed, but anyways we should only be doing this instead |
| 147 | + // of all the above workaround ... |
| 148 | + callback_ = std::forward<CallbackT>(callback); |
| 149 | + } |
84 | 150 | } |
85 | 151 |
|
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) |
| 152 | + // template<typename Allocator = std::allocator<typename ServiceT::Response>> |
| 153 | + std::shared_ptr<typename ServiceT::Response> |
| 154 | + dispatch( |
| 155 | + const std::shared_ptr<rclcpp::Service<ServiceT>> & service_handle, |
| 156 | + const std::shared_ptr<rmw_request_id_t> & request_header, |
| 157 | + std::shared_ptr<typename ServiceT::Request> request) |
90 | 158 | { |
91 | 159 | TRACEPOINT(callback_start, static_cast<const void *>(this), false); |
92 | | - if (shared_ptr_callback_ != nullptr) { |
| 160 | + if (std::holds_alternative<std::monostate>(callback_)) { |
| 161 | + // TODO(ivanpauno): Remove the set method, and force the users of this class |
| 162 | + // to pass a callback at construnciton. |
| 163 | + throw std::runtime_error{"unexpected request without any callback set"}; |
| 164 | + } |
| 165 | + if (std::holds_alternative<SharedPtrDeferResponseCallback>(callback_)) { |
| 166 | + const auto & cb = std::get<SharedPtrDeferResponseCallback>(callback_); |
| 167 | + cb(request_header, std::move(request)); |
| 168 | + return nullptr; |
| 169 | + } |
| 170 | + if (std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_)) { |
| 171 | + const auto & cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>(callback_); |
| 172 | + cb(service_handle, request_header, std::move(request)); |
| 173 | + return nullptr; |
| 174 | + } |
| 175 | + // auto response = allocate_shared<typename ServiceT::Response, Allocator>(); |
| 176 | + auto response = std::make_shared<typename ServiceT::Response>(); |
| 177 | + if (std::holds_alternative<SharedPtrCallback>(callback_)) { |
93 | 178 | (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"); |
| 179 | + const auto & cb = std::get<SharedPtrCallback>(callback_); |
| 180 | + cb(std::move(request), response); |
| 181 | + } else if (std::holds_alternative<SharedPtrWithRequestHeaderCallback>(callback_)) { |
| 182 | + const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_); |
| 183 | + cb(request_header, std::move(request), response); |
99 | 184 | } |
100 | 185 | TRACEPOINT(callback_end, static_cast<const void *>(this)); |
| 186 | + return response; |
101 | 187 | } |
102 | 188 |
|
103 | 189 | void register_callback_for_tracing() |
104 | 190 | { |
105 | 191 | #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 | | - } |
| 192 | + std::visit( |
| 193 | + [this](auto && arg) { |
| 194 | + TRACEPOINT( |
| 195 | + rclcpp_callback_register, |
| 196 | + static_cast<const void *>(this), |
| 197 | + tracetools::get_symbol(arg)); |
| 198 | + }, callback_); |
117 | 199 | #endif // TRACETOOLS_DISABLED |
118 | 200 | } |
| 201 | + |
| 202 | +private: |
| 203 | + using SharedPtrCallback = std::function< |
| 204 | + void ( |
| 205 | + std::shared_ptr<typename ServiceT::Request>, |
| 206 | + std::shared_ptr<typename ServiceT::Response> |
| 207 | + )>; |
| 208 | + using SharedPtrWithRequestHeaderCallback = std::function< |
| 209 | + void ( |
| 210 | + std::shared_ptr<rmw_request_id_t>, |
| 211 | + std::shared_ptr<typename ServiceT::Request>, |
| 212 | + std::shared_ptr<typename ServiceT::Response> |
| 213 | + )>; |
| 214 | + using SharedPtrDeferResponseCallback = std::function< |
| 215 | + void ( |
| 216 | + std::shared_ptr<rmw_request_id_t>, |
| 217 | + std::shared_ptr<typename ServiceT::Request> |
| 218 | + )>; |
| 219 | + using SharedPtrDeferResponseCallbackWithServiceHandle = std::function< |
| 220 | + void ( |
| 221 | + std::shared_ptr<rclcpp::Service<ServiceT>>, |
| 222 | + std::shared_ptr<rmw_request_id_t>, |
| 223 | + std::shared_ptr<typename ServiceT::Request> |
| 224 | + )>; |
| 225 | + |
| 226 | + std::variant< |
| 227 | + std::monostate, |
| 228 | + SharedPtrCallback, |
| 229 | + SharedPtrWithRequestHeaderCallback, |
| 230 | + SharedPtrDeferResponseCallback, |
| 231 | + SharedPtrDeferResponseCallbackWithServiceHandle> callback_; |
119 | 232 | }; |
120 | 233 |
|
121 | 234 | } // namespace rclcpp |
|
0 commit comments