From 1a3dfaf45c5a4304a7ccb32a893e8c488ac3fddb Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 13 Sep 2024 00:04:59 +0800 Subject: [PATCH] Implement generic service (#2617) * Implement generic service Signed-off-by: Barry Xu * Add the required header files Signed-off-by: Barry Xu * Fix compiling errors on Windows Signed-off-by: Barry Xu * Fix compiling errors on Windows Signed-off-by: Barry Xu * Fix compiling errors on Windows Signed-off-by: Barry Xu --------- Signed-off-by: Barry Xu --- rclcpp/CMakeLists.txt | 1 + .../include/rclcpp/create_generic_service.hpp | 102 +++++ .../include/rclcpp/exceptions/exceptions.hpp | 9 + rclcpp/include/rclcpp/generic_service.hpp | 308 +++++++++++++ rclcpp/include/rclcpp/node.hpp | 19 + rclcpp/include/rclcpp/node_impl.hpp | 20 + rclcpp/src/rclcpp/create_generic_service.cpp | 49 ++ rclcpp/src/rclcpp/generic_service.cpp | 172 +++++++ rclcpp/test/rclcpp/CMakeLists.txt | 12 + rclcpp/test/rclcpp/test_generic_service.cpp | 422 ++++++++++++++++++ 10 files changed, 1114 insertions(+) create mode 100644 rclcpp/include/rclcpp/create_generic_service.hpp create mode 100644 rclcpp/include/rclcpp/generic_service.hpp create mode 100644 rclcpp/src/rclcpp/create_generic_service.cpp create mode 100644 rclcpp/src/rclcpp/generic_service.cpp create mode 100644 rclcpp/test/rclcpp/test_generic_service.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index b1dbfec227..8712b48856 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -77,6 +77,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/future_return_code.cpp src/rclcpp/generic_client.cpp src/rclcpp/generic_publisher.cpp + src/rclcpp/generic_service.cpp src/rclcpp/generic_subscription.cpp src/rclcpp/graph_listener.cpp src/rclcpp/guard_condition.cpp diff --git a/rclcpp/include/rclcpp/create_generic_service.hpp b/rclcpp/include/rclcpp/create_generic_service.hpp new file mode 100644 index 0000000000..9cb032ef76 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_service.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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. + +#ifndef RCLCPP__CREATE_GENERIC_SERVICE_HPP_ +#define RCLCPP__CREATE_GENERIC_SERVICE_HPP_ + +#include +#include +#include + +#include "rclcpp/generic_service.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ +/// Create a generic service with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the generic service. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::GenericService::SharedPtr +create_generic_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + rclcpp::GenericServiceCallback any_service_callback; + any_service_callback.set(std::forward(callback)); + + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = qos.get_rmw_qos_profile(); + + auto serv = GenericService::make_shared( + node_base->get_shared_rcl_node_handle(), + service_name, service_type, any_service_callback, service_options); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + node_services->add_service(serv_base_ptr, group); + return serv; +} + +/// Create a generic service with a given type. +/** + * The NodeT type needs to have NodeBaseInterface implementation and NodeServicesInterface + * implementation of the node which to create the generic service. + * + * \param[in] node The node on which to create the generic service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::GenericService::SharedPtr +create_generic_service( + NodeT node, + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return create_generic_service( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_services_interface(node), + service_name, + service_type, + std::forward(callback), qos.get_rmw_qos_profile(), group); +} +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index b3a53373ed..08c6b88250 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -100,6 +100,15 @@ class InvalidServiceNameError : public NameValidationError {} }; +class InvalidServiceTypeError : public std::runtime_error +{ +public: + InvalidServiceTypeError() + : std::runtime_error("Service type is invalid.") {} + explicit InvalidServiceTypeError(const std::string & msg) + : std::runtime_error(msg) {} +}; + class UnimplementedError : public std::runtime_error { public: diff --git a/rclcpp/include/rclcpp/generic_service.hpp b/rclcpp/include/rclcpp/generic_service.hpp new file mode 100644 index 0000000000..b4c8d5d9a7 --- /dev/null +++ b/rclcpp/include/rclcpp/generic_service.hpp @@ -0,0 +1,308 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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. + +#ifndef RCLCPP__GENERIC_SERVICE_HPP_ +#define RCLCPP__GENERIC_SERVICE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/typesupport_helpers.hpp" + +#include "rosidl_runtime_c/service_type_support_struct.h" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +#include "service.hpp" + +namespace rclcpp +{ +class GenericService; + +class GenericServiceCallback +{ +public: + using SharedRequest = std::shared_ptr; + using SharedResponse = std::shared_ptr; + + GenericServiceCallback() + : callback_(std::monostate{}) + {} + + template< + typename CallbackT, + typename std::enable_if_t::value, int> = 0> + void + set(CallbackT && callback) + { + // Workaround Windows issue with std::bind + if constexpr ( + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT, can't satisfy both cpplint and uncrustify + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace(callback); + } else { + // the else clause is not needed, but anyways we should only be doing this instead + // of all the above workaround ... + callback_ = std::forward(callback); + } + } + + template< + typename CallbackT, + typename std::enable_if_t::value, int> = 0> + void + set(CallbackT && callback) + { + if (!callback) { + throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr"); + } + // Workaround Windows issue with std::bind + if constexpr ( + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace(callback); + } else { + // the else clause is not needed, but anyways we should only be doing this instead + // of all the above workaround ... + callback_ = std::forward(callback); + } + } + + SharedResponse + dispatch( + const std::shared_ptr & service_handle, + const std::shared_ptr & request_header, + SharedRequest request, + SharedRequest response) + { + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); + if (std::holds_alternative(callback_)) { + // TODO(ivanpauno): Remove the set method, and force the users of this class + // to pass a callback at construnciton. + throw std::runtime_error{"unexpected request without any callback set"}; + } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request)); + return nullptr; + } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(service_handle, request_header, std::move(request)); + return nullptr; + } + + if (std::holds_alternative(callback_)) { + (void)request_header; + const auto & cb = std::get(callback_); + cb(std::move(request), std::move(response)); + } else if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request), std::move(response)); + } + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + return response; + } + + void register_callback_for_tracing() + { +#ifndef TRACETOOLS_DISABLED + std::visit( + [this](auto && arg) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(arg); + TRACETOOLS_DO_TRACEPOINT( + rclcpp_callback_register, + static_cast(this), + symbol); + std::free(symbol); + } + }, callback_); +#endif // TRACETOOLS_DISABLED + } + +private: + using SharedPtrCallback = std::function; + using SharedPtrWithRequestHeaderCallback = std::function< + void ( + std::shared_ptr, + SharedRequest, + SharedResponse + )>; + using SharedPtrDeferResponseCallback = std::function< + void ( + std::shared_ptr, + SharedRequest + )>; + using SharedPtrDeferResponseCallbackWithServiceHandle = std::function< + void ( + std::shared_ptr, + std::shared_ptr, + SharedRequest + )>; + + std::variant< + std::monostate, + SharedPtrCallback, + SharedPtrWithRequestHeaderCallback, + SharedPtrDeferResponseCallback, + SharedPtrDeferResponseCallbackWithServiceHandle> callback_; +}; + +class GenericService + : public ServiceBase, + public std::enable_shared_from_this +{ +public: + using Request = void *; // Serialized/Deserialized data pointer of request message + using Response = void *; // Serialized/Deserialized data pointer of response message + using SharedRequest = std::shared_ptr; + using SharedResponse = std::shared_ptr; + using CallbackType = std::function; + + using CallbackWithHeaderType = + std::function, + const SharedRequest, + SharedResponse)>; + + RCLCPP_SMART_PTR_DEFINITIONS(GenericService) + + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_name Name of the topic to publish to. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] any_callback User defined callback to call when a client request is received. + * \param[in] service_options options for the service. + */ + RCLCPP_PUBLIC + GenericService( + std::shared_ptr node_handle, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + rcl_service_options_t & service_options); + + GenericService() = delete; + + RCLCPP_PUBLIC + virtual ~GenericService() {} + + /// Take the next request from the service. + /** + * \sa ServiceBase::take_type_erased_request(). + * + * \param[out] request_out The reference to a service deserialized request object + * into which the middleware will copy the taken request. + * \param[out] request_id_out The output id for the request which can be used + * to associate response with this request in the future. + * \returns true if the request was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl calls fail. + */ + RCLCPP_PUBLIC + bool + take_request(SharedRequest request_out, rmw_request_id_t & request_id_out); + + RCLCPP_PUBLIC + std::shared_ptr + create_request() override; + + RCLCPP_PUBLIC + std::shared_ptr + create_response(); + + RCLCPP_PUBLIC + std::shared_ptr + create_request_header() override; + + RCLCPP_PUBLIC + void + handle_request( + std::shared_ptr request_header, + std::shared_ptr request) override; + + RCLCPP_PUBLIC + void + send_response(rmw_request_id_t & req_id, SharedResponse & response); + +private: + RCLCPP_DISABLE_COPY(GenericService) + + GenericServiceCallback any_callback_; + + std::shared_ptr ts_lib_; + const rosidl_typesupport_introspection_cpp::MessageMembers * request_members_; + const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_; +}; + +} // namespace rclcpp +#endif // RCLCPP__GENERIC_SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f395bf8ea3..930bf419f1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -44,6 +44,7 @@ #include "rclcpp/event.hpp" #include "rclcpp/generic_client.hpp" #include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_service.hpp" #include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" @@ -303,6 +304,24 @@ class Node : public std::enable_shared_from_this const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericService. + /** + * \param[in] service_name The topic to service on. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool" + * \param[in] callback User-defined callback function. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created service. + */ + template + typename rclcpp::GenericService::SharedPtr + create_generic_service( + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericPublisher. /** * The returned pointer will never be empty, but this function can throw various exceptions, for diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 9ca2c42c2d..5b81bdcba4 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -40,6 +40,7 @@ #include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" +#include "rclcpp/create_generic_service.hpp" #include "rclcpp/create_subscription.hpp" #include "rclcpp/create_timer.hpp" #include "rclcpp/detail/resolve_enable_topic_statistics.hpp" @@ -171,6 +172,25 @@ Node::create_service( group); } +template +typename rclcpp::GenericService::SharedPtr +Node::create_generic_service( + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_generic_service( + node_base_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + service_type, + std::forward(callback), + qos, + group); +} + template std::shared_ptr Node::create_generic_publisher( diff --git a/rclcpp/src/rclcpp/create_generic_service.cpp b/rclcpp/src/rclcpp/create_generic_service.cpp new file mode 100644 index 0000000000..492635beb2 --- /dev/null +++ b/rclcpp/src/rclcpp/create_generic_service.cpp @@ -0,0 +1,49 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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 +#include + +#include "rclcpp/create_generic_service.hpp" +#include "rclcpp/generic_service.hpp" + +namespace rclcpp +{ +rclcpp::GenericService::SharedPtr +create_generic_service( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + rcl_service_options_t options = rcl_service_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + + auto srv = rclcpp::GenericService::make_shared( + node_base.get(), + node_graph, + service_name, + service_type, + any_callback, + options); + + auto srv_base_ptr = std::dynamic_pointer_cast(srv); + node_services->add_service(srv_base_ptr, group); + return srv; +} +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_service.cpp b/rclcpp/src/rclcpp/generic_service.cpp new file mode 100644 index 0000000000..75b34993bf --- /dev/null +++ b/rclcpp/src/rclcpp/generic_service.cpp @@ -0,0 +1,172 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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 "rclcpp/generic_service.hpp" + +namespace rclcpp +{ +GenericService::GenericService( + std::shared_ptr node_handle, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + rcl_service_options_t & service_options) +: ServiceBase(node_handle), + any_callback_(any_callback) +{ + const rosidl_service_type_support_t * service_ts; + try { + ts_lib_ = get_typesupport_library( + service_type, "rosidl_typesupport_cpp"); + + service_ts = get_service_typesupport_handle( + service_type, "rosidl_typesupport_cpp", *ts_lib_); + + auto request_type_support_intro = get_message_typesupport_handle( + service_ts->request_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + request_members_ = static_cast( + request_type_support_intro->data); + + auto response_type_support_intro = get_message_typesupport_handle( + service_ts->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + response_members_ = static_cast( + response_type_support_intro->data); + } catch (std::runtime_error & err) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), + "Invalid service type: %s", + err.what()); + throw rclcpp::exceptions::InvalidServiceTypeError(err.what()); + } + + // rcl does the static memory allocation here + service_handle_ = std::shared_ptr( + new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service) + { + if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl service handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete service; + }); + *service_handle_.get() = rcl_get_zero_initialized_service(); + + rcl_ret_t ret = rcl_service_init( + service_handle_.get(), + node_handle.get(), + service_ts, + service_name.c_str(), + &service_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); + } + TRACETOOLS_TRACEPOINT( + rclcpp_service_callback_added, + static_cast(get_service_handle().get()), + static_cast(&any_callback_)); +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif +} + +bool +GenericService::take_request( + SharedRequest request_out, + rmw_request_id_t & request_id_out) +{ + request_out = create_request(); + return this->take_type_erased_request(request_out.get(), request_id_out); +} + +std::shared_ptr +GenericService::create_request() +{ + Request request = new uint8_t[request_members_->size_of_]; + request_members_->init_function(request, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + request, + [this](void * p) + { + request_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericService::create_response() +{ + Response response = new uint8_t[response_members_->size_of_]; + response_members_->init_function(response, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + response, + [this](void * p) + { + response_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericService::create_request_header() +{ + return std::make_shared(); +} + +void +GenericService::handle_request( + std::shared_ptr request_header, + std::shared_ptr request) +{ + auto response = any_callback_.dispatch( + this->shared_from_this(), request_header, request, create_response()); + if (response) { + send_response(*request_header, response); + } +} + +void +GenericService::send_response(rmw_request_id_t & req_id, SharedResponse & response) +{ + rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, response.get()); + + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + node_logger_.get_child("rclcpp"), + "failed to send response to %s (timeout): %s", + this->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + } +} + +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 1c6fafe94a..f9e341087f 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -83,6 +83,18 @@ if(TARGET test_generic_client) ${test_msgs_TARGETS} ) endif() +ament_add_gtest(test_generic_service test_generic_service.cpp) +ament_add_test_label(test_generic_service mimick) +if(TARGET test_generic_service) + target_link_libraries(test_generic_service ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() ament_add_gtest(test_client_common test_client_common.cpp) ament_add_test_label(test_client_common mimick) if(TARGET test_client_common) diff --git a/rclcpp/test/rclcpp/test_generic_service.cpp b/rclcpp/test/rclcpp/test_generic_service.cpp new file mode 100644 index 0000000000..554fdf0c0f --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_service.cpp @@ -0,0 +1,422 @@ +// Copyright 2024 Sony Group Corporation. +// +// 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 + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/basic_types.hpp" + +using namespace std::chrono_literals; + +class TestGenericService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestGenericServiceSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestGenericService, construction_and_destruction) { + auto callback = []( + rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {}; + { + auto generic_service = node->create_generic_service( + "test_generic_service", "rcl_interfaces/srv/ListParameters", callback); + EXPECT_NE(nullptr, generic_service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = generic_service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + + { + ASSERT_THROW( + { + auto generic_service = node->create_generic_service( + "invalid_service?", "test_msgs/srv/Empty", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto generic_service = node->create_generic_service( + "test_generic_service", "test_msgs/srv/NotExist", callback); + }, rclcpp::exceptions::InvalidServiceTypeError); + } +} + +/* + Testing service construction and destruction for subnodes. + */ +TEST_F(TestGenericServiceSub, construction_and_destruction) { + auto callback = []( + rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {}; + { + auto generic_service = subnode->create_generic_service( + "test_generic_service", "rcl_interfaces/srv/ListParameters", callback); + EXPECT_STREQ(generic_service->get_service_name(), "/ns/sub_ns/test_generic_service"); + } + + { + ASSERT_THROW( + { + auto generic_service = subnode->create_generic_service( + "invalid_service?", "test_msgs/srv/Empty", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto generic_service = subnode->create_generic_service( + "test_generic_service", "test_msgs/srv/NotExist", callback); + }, rclcpp::exceptions::InvalidServiceTypeError); + } +} + +TEST_F(TestGenericService, construction_and_destruction_rcl_errors) { + auto callback = []( + rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_init, RCL_RET_ERROR); + // reset() isn't necessary for this exception, it just avoids unused return value warning + EXPECT_THROW( + node->create_generic_service("service", "test_msgs/srv/Empty", callback).reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_fini, RCL_RET_ERROR); + EXPECT_NO_THROW( + node->create_generic_service("service", "test_msgs/srv/Empty", callback).reset()); + } +} + +TEST_F(TestGenericService, generic_service_take_request) { + auto callback = []( + rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_OK); + EXPECT_TRUE(generic_service->take_request(request, *request_id.get())); + } + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_SERVICE_TAKE_FAILED); + EXPECT_FALSE(generic_service->take_request(request, *request_id.get())); + } + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_ERROR); + EXPECT_THROW( + generic_service->take_request(request, *request_id.get()), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestGenericService, generic_service_send_response) { + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + + { + auto request_id = generic_service->create_request_header(); + auto response = generic_service->create_response(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_OK); + EXPECT_NO_THROW(generic_service->send_response(*request_id.get(), response)); + } + + { + auto request_id = generic_service->create_request_header(); + auto response = generic_service->create_response(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_ERROR); + EXPECT_THROW( + generic_service->send_response(*request_id.get(), response), + rclcpp::exceptions::RCLError); + } +} + +/* + Testing on_new_request callbacks. + */ +TEST_F(TestGenericService, generic_service_on_new_request_callback) { + auto server_callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {FAIL();}; + rclcpp::ServicesQoS service_qos; + service_qos.keep_last(3); + auto generic_service = node->create_generic_service( + "~/test_service", "test_msgs/srv/Empty", server_callback, service_qos); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + generic_service->set_on_new_request_callback(increase_c1_cb); + + auto client = node->create_client( + "~/test_service", service_qos); + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + generic_service->set_on_new_request_callback(increase_c2_cb); + + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + generic_service->clear_on_new_request_callback(); + + { + auto request = std::make_shared(); + client->async_send_request(request); + client->async_send_request(request); + client->async_send_request(request); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + generic_service->set_on_new_request_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(generic_service->set_on_new_request_callback(invalid_cb), std::invalid_argument); +} + +TEST_F(TestGenericService, rcl_service_response_publisher_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + RCLCPP_EXPECT_THROW_EQ( + generic_service->get_response_publisher_actual_qos(), + std::runtime_error("failed to get service's response publisher qos settings: error not set")); +} + +TEST_F(TestGenericService, rcl_service_request_subscription_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + RCLCPP_EXPECT_THROW_EQ( + generic_service->get_request_subscription_actual_qos(), + std::runtime_error("failed to get service's request subscription qos settings: error not set")); +} + +TEST_F(TestGenericService, generic_service_qos) { + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); + + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback, qos_profile); + + auto rs_qos = generic_service->get_request_subscription_actual_qos(); + auto rp_qos = generic_service->get_response_publisher_actual_qos(); + + EXPECT_EQ(qos_profile, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); +} + +TEST_F(TestGenericService, generic_service_qos_depth) { + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rclcpp::QoS server_qos_profile(2); + + auto generic_service = server_node->create_generic_service( + "test_qos_depth", "test_msgs/srv/Empty", std::move(server_callback), server_qos_profile); + + rclcpp::QoS client_qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + auto client = node->create_client("test_qos_depth", client_qos_profile); + + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto request = std::make_shared(); + + auto client_callback = [&request_result]( + rclcpp::Client::SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < server_qos_profile.depth()) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(1ms); + } + + // Spin an extra time to check if server QoS depth has been ignored, + // so more server responses might be processed than expected. + rclcpp::spin_some(server_node); + + EXPECT_EQ(server_cb_count_, server_qos_profile.depth()); +} + +TEST_F(TestGenericService, generic_service_and_client) { + const std::string service_name = "test_service"; + const std::string service_type = "test_msgs/srv/BasicTypes"; + int64_t expected_change = 87654321; + + auto callback = [&expected_change]( + const rclcpp::GenericService::SharedRequest request, + rclcpp::GenericService::SharedResponse response) { + auto typed_request = static_cast(request.get()); + auto typed_response = static_cast(response.get()); + + typed_response->int64_value = typed_request->int64_value + expected_change; + }; + auto generic_service = node->create_generic_service(service_name, service_type, callback); + + auto client = node->create_client(service_name); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(client->service_is_ready()); + + auto request = std::make_shared(); + request->int64_value = 12345678; + + auto generic_client_callback = [&request, &expected_change]( + std::shared_future future) { + auto response = future.get(); + EXPECT_EQ(response->int64_value, (request->int64_value + expected_change)); + }; + + auto future = + client->async_send_request(request, generic_client_callback); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(5)); +}