diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 918d8e5a29..7ad7af0694 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -22,8 +22,11 @@ #include #include +#include "rosidl_runtime_cpp/traits.hpp" + #include "rclcpp/function_traits.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/type_adapter.hpp" #include "rmw/types.h" #include "tracetools/tracetools.h" #include "tracetools/utils.hpp" @@ -57,6 +60,127 @@ class Service; template class AnyServiceCallback { +public: + using ServiceRequestType = + typename TypeAdapter::custom_type::Request; + /// ServiceT::ros_message_type::Request if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ROSServiceRequestType = + typename TypeAdapter::ros_message_type::Request; + /// ServiceT::custom_type::Response if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ServiceResponseType = + typename TypeAdapter::custom_type::Response; + /// ServiceT::ros_message_type::Response if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ROSServiceResponseType = + typename TypeAdapter::ros_message_type::Response; + +private: + using ROSTotalSharedPtrCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr + )>; + using ROSTotalSharedPtrWithRequestHeaderCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr, + std::shared_ptr + )>; + using CustomTotalSharedPtrCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr + )>; + using CustomTotalSharedPtrWithRequestHeaderCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr, + std::shared_ptr + )>; + using ROSCustomSharedPtrCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr + )>; + using ROSCustomSharedPtrWithRequestHeaderCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr, + std::shared_ptr + )>; + using CustomROSSharedPtrCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr + )>; + using CustomROSSharedPtrWithRequestHeaderCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr, + std::shared_ptr + )>; + using ROSSharedPtrDeferResponseCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr + )>; + using ROSSharedPtrDeferResponseCallbackWithServiceHandle = std::function< + void ( + std::shared_ptr>, + std::shared_ptr, + std::shared_ptr + )>; + using CustomSharedPtrDeferResponseCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr + )>; + using CustomSharedPtrDeferResponseCallbackWithServiceHandle = std::function< + void ( + std::shared_ptr>, + std::shared_ptr, + std::shared_ptr + )>; + + template::is_specialized::value> + struct AnyServiceCallbackHelper; + + template<> + struct AnyServiceCallbackHelper + { + using variant_type = std::variant< + std::monostate, + ROSTotalSharedPtrCallback, + ROSTotalSharedPtrWithRequestHeaderCallback, + CustomTotalSharedPtrCallback, + CustomTotalSharedPtrWithRequestHeaderCallback, + ROSCustomSharedPtrCallback, + ROSCustomSharedPtrWithRequestHeaderCallback, + CustomROSSharedPtrCallback, + CustomROSSharedPtrWithRequestHeaderCallback, + ROSSharedPtrDeferResponseCallback, + ROSSharedPtrDeferResponseCallbackWithServiceHandle, + CustomSharedPtrDeferResponseCallback, + CustomSharedPtrDeferResponseCallbackWithServiceHandle>; + }; + + template<> + struct AnyServiceCallbackHelper + { + using variant_type = std::variant< + std::monostate, + ROSTotalSharedPtrCallback, + ROSTotalSharedPtrWithRequestHeaderCallback, + ROSSharedPtrDeferResponseCallback, + ROSSharedPtrDeferResponseCallbackWithServiceHandle>; + }; + + using CallbackHelperT = AnyServiceCallbackHelper<>; + + typename CallbackHelperT::variant_type callback_; + public: AnyServiceCallback() : callback_(std::monostate{}) @@ -72,31 +196,89 @@ class AnyServiceCallback if constexpr ( rclcpp::function_traits::same_arguments< CallbackT, - SharedPtrCallback + ROSTotalSharedPtrCallback >::value) { - callback_.template emplace(callback); + callback_.template emplace(callback); } else if constexpr ( // NOLINT, can't satisfy both cpplint and uncrustify rclcpp::function_traits::same_arguments< CallbackT, - SharedPtrWithRequestHeaderCallback + ROSTotalSharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomTotalSharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomTotalSharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + ROSCustomSharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + ROSCustomSharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomROSSharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomROSSharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + ROSSharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + ROSSharedPtrDeferResponseCallbackWithServiceHandle >::value) { - callback_.template emplace(callback); + callback_.template emplace< + ROSSharedPtrDeferResponseCallbackWithServiceHandle>(callback); } else if constexpr ( // NOLINT rclcpp::function_traits::same_arguments< CallbackT, - SharedPtrDeferResponseCallback + CustomSharedPtrDeferResponseCallback >::value) { - callback_.template emplace(callback); + callback_.template emplace(callback); } else if constexpr ( // NOLINT rclcpp::function_traits::same_arguments< CallbackT, - SharedPtrDeferResponseCallbackWithServiceHandle + CustomSharedPtrDeferResponseCallbackWithServiceHandle >::value) { - callback_.template emplace(callback); + callback_.template emplace< + CustomSharedPtrDeferResponseCallbackWithServiceHandle>(callback); } else { // the else clause is not needed, but anyways we should only be doing this instead // of all the above workaround ... @@ -117,31 +299,89 @@ class AnyServiceCallback if constexpr ( rclcpp::function_traits::same_arguments< CallbackT, - SharedPtrCallback + ROSTotalSharedPtrCallback >::value) { - callback_.template emplace(callback); + callback_.template emplace(callback); + } else if constexpr ( // NOLINT, can't satisfy both cpplint and uncrustify + rclcpp::function_traits::same_arguments< + CallbackT, + ROSTotalSharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomTotalSharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomTotalSharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); } else if constexpr ( // NOLINT rclcpp::function_traits::same_arguments< CallbackT, - SharedPtrWithRequestHeaderCallback + ROSCustomSharedPtrCallback >::value) { - callback_.template emplace(callback); + callback_.template emplace(callback); } else if constexpr ( // NOLINT rclcpp::function_traits::same_arguments< CallbackT, - SharedPtrDeferResponseCallback + ROSCustomSharedPtrWithRequestHeaderCallback >::value) { - callback_.template emplace(callback); + callback_.template emplace(callback); } else if constexpr ( // NOLINT rclcpp::function_traits::same_arguments< CallbackT, - SharedPtrDeferResponseCallbackWithServiceHandle + CustomROSSharedPtrCallback >::value) { - callback_.template emplace(callback); + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomROSSharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + ROSSharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + ROSSharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace< + ROSSharedPtrDeferResponseCallbackWithServiceHandle>(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomSharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + CustomSharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace< + CustomSharedPtrDeferResponseCallbackWithServiceHandle>(callback); } else { // the else clause is not needed, but anyways we should only be doing this instead // of all the above workaround ... @@ -150,11 +390,55 @@ class AnyServiceCallback } // template> - std::shared_ptr + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value, + std::shared_ptr> + dispatch( + const std::shared_ptr> & service_handle, + const std::shared_ptr & request_header, + std::shared_ptr request) + { + 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; + } + // auto response = allocate_shared(); + auto response = std::make_shared(); + if (std::holds_alternative(callback_)) { + (void)request_header; + const auto & cb = std::get(callback_); + cb(std::move(request), response); + } else if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request), response); + } + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + return response; + } + + template + typename std::enable_if_t< + !rosidl_generator_traits::is_message::value && + std::is_same::value, + std::shared_ptr> dispatch( const std::shared_ptr> & service_handle, const std::shared_ptr & request_header, - std::shared_ptr request) + std::shared_ptr request) { TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); if (std::holds_alternative(callback_)) { @@ -162,30 +446,42 @@ class AnyServiceCallback // 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_); + 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_); + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); cb(service_handle, request_header, std::move(request)); return nullptr; } // auto response = allocate_shared(); - auto response = std::make_shared(); - if (std::holds_alternative(callback_)) { + auto response = std::make_shared(); + if (std::holds_alternative(callback_)) { (void)request_header; - const auto & cb = std::get(callback_); + const auto & cb = std::get(callback_); cb(std::move(request), response); - } else if (std::holds_alternative(callback_)) { - const auto & cb = std::get(callback_); + } else if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); cb(request_header, std::move(request), response); } TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); return response; } + typename CallbackHelperT::variant_type & + get_variant() + { + return callback_; + } + + const typename CallbackHelperT::variant_type & + get_variant() const + { + return callback_; + } + void register_callback_for_tracing() { #ifndef TRACETOOLS_DISABLED @@ -202,37 +498,6 @@ class AnyServiceCallback }, callback_); #endif // TRACETOOLS_DISABLED } - -private: - using SharedPtrCallback = std::function< - void ( - std::shared_ptr, - std::shared_ptr - )>; - using SharedPtrWithRequestHeaderCallback = std::function< - void ( - std::shared_ptr, - std::shared_ptr, - std::shared_ptr - )>; - using SharedPtrDeferResponseCallback = std::function< - void ( - std::shared_ptr, - std::shared_ptr - )>; - using SharedPtrDeferResponseCallbackWithServiceHandle = std::function< - void ( - std::shared_ptr>, - std::shared_ptr, - std::shared_ptr - )>; - - std::variant< - std::monostate, - SharedPtrCallback, - SharedPtrWithRequestHeaderCallback, - SharedPtrDeferResponseCallback, - SharedPtrDeferResponseCallbackWithServiceHandle> callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 85b0a2d5f9..0f309bac67 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -24,11 +24,13 @@ #include #include #include +#include #include #include #include #include +#include "is_ros_compatible_type.hpp" #include "rcl/client.h" #include "rcl/error_handling.h" #include "rcl/event_callback.h" @@ -39,11 +41,14 @@ #include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/get_service_type_support_handle.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/is_ros_compatible_type.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp/type_adapter.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -52,6 +57,8 @@ #include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" +#include + namespace rclcpp { @@ -379,24 +386,113 @@ template class Client : public ClientBase { public: - using Request = typename ServiceT::Request; - using Response = typename ServiceT::Response; - - using SharedRequest = typename ServiceT::Request::SharedPtr; - using SharedResponse = typename ServiceT::Response::SharedPtr; - - using Promise = std::promise; - using PromiseWithRequest = std::promise>; - - using SharedPromise = std::shared_ptr; - using SharedPromiseWithRequest = std::shared_ptr; - - using Future = std::future; - using SharedFuture = std::shared_future; - using SharedFutureWithRequest = std::shared_future>; - - using CallbackType = std::function; - using CallbackWithRequestType = std::function; + static_assert( + rclcpp::is_ros_compatible_service_type::value, + "Service type is not compatible with ROS 2 and cannot be used with a Client"); + + /// ServiceT::custom_type::Request if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ServiceRequestType = typename rclcpp::TypeAdapter::custom_type::Request; + /// ServiceT::ros_message_type::Request if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ROSServiceRequestType = + typename rclcpp::TypeAdapter::ros_message_type::Request; + /// ServiceT::custom_type::Response if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ServiceResponseType = typename rclcpp::TypeAdapter::custom_type::Response; + /// ServiceT::ros_message_type::Response if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ROSServiceResponseType = + typename rclcpp::TypeAdapter::ros_message_type::Response; + + using CustomSharedRequest = typename std::shared_ptr; + using CustomSharedResponse = typename std::shared_ptr; + + using ROSSharedRequest = typename ROSServiceRequestType::SharedPtr; + using ROSSharedResponse = typename ROSServiceResponseType::SharedPtr; + + using SharedRequest [[deprecated("Use ROSSharedRequest instead of SharedRequest")]] = + ROSSharedRequest; + + using SharedResponse [[deprecated("Use ROSSharedResponse instead of SharedResponse")]] = + ROSSharedResponse; + + using CustomPromise = std::promise; + using ROSPromise = std::promise; + + using Promise [[deprecated("Use ROSPromise instead of Promise")]] = ROSPromise; + + using CustomTotalPromiseWithRequest = + std::promise>; + using ROSTotalPromiseWithRequest = + std::promise>; + using CustomROSPromiseWithRequest = + std::promise>; + using ROSCustomPromiseWithRequest = + std::promise>; + + using PromiseWithRequest + [[deprecated("Use ROSTotalPromiseWithRequest instead of PromiseWithRequest")]] = + ROSTotalPromiseWithRequest; + + using CustomSharedPromise = std::shared_ptr; + using ROSSharedPromise = std::shared_ptr; + + using SharedPromise [[deprecated("Use ROSSharedPromise instead of SharedPromise")]] = + ROSSharedPromise; + + using CustomTotalSharedPromiseWithRequest = std::shared_ptr; + using ROSTotalSharedPromiseWithRequest = std::shared_ptr; + using CustomROSSharedPromiseWithRequest = std::shared_ptr; + using ROSCustomSharedPromiseWithRequest = std::shared_ptr; + + using SharedPromiseWithRequest + [[deprecated("Use ROSTotalSharedPromiseWithRequest instead fo SharedPromiseWithRequest")]] = + ROSTotalSharedPromiseWithRequest; + + using CustomFuture = std::future; + using ROSFuture = std::future; + + using Future [[deprecated("Use ROSFuture instead of Future")]] = + ROSFuture; + + using CustomSharedFuture = std::shared_future; + using ROSSharedFuture = std::shared_future; + + using SharedFuture [[deprecated("Use ROSSharedFuture instead of SharedFuture")]] = + ROSSharedFuture; + + using CustomTotalSharedFutureWithRequest = + std::shared_future>; + using ROSTotalSharedFutureWithRequest = + std::shared_future>; + using CustomROSSharedFutureWithRequest = + std::shared_future>; + using ROSCustomSharedFutureWithRequest = + std::shared_future>; + + using SharedFutureWithRequest + [[deprecated("Use ROSTotalSharedFutureWithRequest instead of SharedFutureWithRequest")]] = + ROSTotalSharedFutureWithRequest; + + using CustomCallbackType = std::function; + using ROSCallbackType = std::function; + + using CallbackType [[deprecated("Use ROSCallbackType instead of CallbackType")]] = + ROSCallbackType; + + using CustomTotalCallbackWithRequestType = + std::function; + using ROSTotalCallbackWithRequestType = + std::function; + using CustomROSCallbackWithRequestType = + std::function; + using ROSCustomCallbackWithRequestType = + std::function; + + using CallbackWithRequestType + [[deprecated("Use ROSTotalCallbackWithRequestType instead of CallbackWithRequestType")]] = + ROSTotalCallbackWithRequestType; RCLCPP_SMART_PTR_DEFINITIONS(Client) @@ -404,53 +500,82 @@ class Client : public ClientBase /** * Public members: * - future: a std::future. + * where SharedResponse is either ROSSharedResponse or CustomSharedResponse * - request_id: the request id associated with the future. * * All the other methods are equivalent to the ones std::future provides. */ - struct FutureAndRequestId - : detail::FutureAndRequestId> + struct [[deprecated("Use ROSFutureAndRequestId instead of FutureAndRequestId")]] + FutureAndRequestId : detail::FutureAndRequestId> { - using detail::FutureAndRequestId>::FutureAndRequestId; + using detail::FutureAndRequestId>::FutureAndRequestId; + // delegate future like methods in the std::future impl_ - /// Deprecated, use `.future.share()` instead. - /** - * Allow implicit conversions to `std::shared_future` by value. - * \deprecated - */ - [[deprecated( - "FutureAndRequestId: use .future.share() instead of an implicit conversion")]] - operator SharedFuture() {return this->future.share();} + /// See std::future::share(). + ROSSharedFuture share() noexcept {return this->future.share();} + }; + struct ROSFutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; // delegate future like methods in the std::future impl_ /// See std::future::share(). - SharedFuture share() noexcept {return this->future.share();} + ROSSharedFuture share() noexcept {return this->future.share();} + }; + + struct CustomFutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; + // delegate future like methods in the std::future impl_ + + /// See std::future::share(). + CustomSharedFuture share() noexcept {return this->future.share();} }; /// A convenient Client::SharedFuture and request id pair. /** * Public members: * - future: a std::shared_future. + * where SharedResponse is either ROSSharedResponse or CustomSharedResponse * - request_id: the request id associated with the future. * * All the other methods are equivalent to the ones std::shared_future provides. */ - struct SharedFutureAndRequestId - : detail::FutureAndRequestId> + struct [[deprecated("Use ROSSharedFutureAndRequestId instead of SharedFutureAndRequestId")]] + SharedFutureAndRequestId + : detail::FutureAndRequestId> { - using detail::FutureAndRequestId>::FutureAndRequestId; + using detail::FutureAndRequestId>::FutureAndRequestId; + }; + + struct ROSSharedFutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; + }; + + struct CustomSharedFutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; }; /// A convenient Client::SharedFutureWithRequest and request id pair. /** * Public members: - * - future: a std::shared_future. + * - future: a std::shared_future>. + * where SharedRequest, SharedResponse can be a mixture of ROS/Custom Typed * - request_id: the request id associated with the future. * * All the other methods are equivalent to the ones std::shared_future provides. */ - struct SharedFutureWithRequestAndRequestId + struct + [[deprecated( + "Use ROSTotalSharedFutureWithRequest instead of SharedFutureWithRequestAndRequestId")]] + SharedFutureWithRequestAndRequestId : detail::FutureAndRequestId>> { using detail::FutureAndRequestId< @@ -458,6 +583,41 @@ class Client : public ClientBase >::FutureAndRequestId; }; + struct ROSTotalSharedFutureWithRequestAndRequestId + : detail::FutureAndRequestId>> + { + using detail::FutureAndRequestId< + std::shared_future> + >::FutureAndRequestId; + }; + + struct CustomTotalSharedFutureWithRequestAndRequestId + : detail::FutureAndRequestId>> + { + using detail::FutureAndRequestId< + std::shared_future> + >::FutureAndRequestId; + }; + + struct ROSCustomSharedFutureWithRequestAndRequestId + : detail::FutureAndRequestId>> + { + using detail::FutureAndRequestId< + std::shared_future> + >::FutureAndRequestId; + }; + + struct CustomROSSharedFutureWithRequestAndRequestId + : detail::FutureAndRequestId>> + { + using detail::FutureAndRequestId< + std::shared_future> + >::FutureAndRequestId; + }; + /// Default constructor. /** * The constructor for a Client is almost never called directly. @@ -475,7 +635,7 @@ class Client : public ClientBase const std::string & service_name, rcl_client_options_t & client_options) : ClientBase(node_base, node_graph), - srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) + srv_type_support_handle_(&rclcpp::get_service_type_support_handle()) { rcl_ret_t ret = rcl_client_init( this->get_client_handle().get(), @@ -506,6 +666,9 @@ class Client : public ClientBase /** * \sa ClientBase::take_type_erased_response(). * + * This signature is enabled if the service response is a ROSServiceResponseType + * as opposed to the custom type of a respective TypeAdapter + * * \param[out] response_out The reference to a Service Response into * which the middleware will copy the response being taken. * \param[out] request_header_out The request header to be filled by the @@ -515,11 +678,46 @@ class Client : public ClientBase * \throws rclcpp::exceptions::RCLError based exceptions if the underlying * rcl function fail. */ - bool - take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out) + + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value, + bool + > + take_response(T & response_out, rmw_request_id_t & request_header_out) { return this->take_type_erased_response(&response_out, request_header_out); } + /// Take the next response for this client. + /** + * \sa ClientBase::take_type_erased_response(). + * + * This signature is enabled if the service response is a ServiceResponseType + * created with a TypeAdapter, matching its respective custom_type + * + * \param[out] response_out The reference to a Service Response into + * which the middleware will copy the response being taken. + * \param[out] request_header_out The request header to be filled by the + * middleware when taking, and which can be used to associte the response + * to a specific request. + * \returns true if the response was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl function fail. + */ + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value, + bool + > + take_response(T & response_out, rmw_request_id_t & request_header_out) + { + ROSServiceResponseType ros_service_response_out; + rclcpp::TypeAdapter::convert_to_ros_service_response( + response_out, ros_service_response_out); + return this->take_type_erased_response(&ros_service_response_out, request_header_out); + } /// Create a shared pointer with the response type /** @@ -528,7 +726,7 @@ class Client : public ClientBase std::shared_ptr create_response() override { - return std::shared_ptr(new typename ServiceT::Response()); + return std::shared_ptr(new ROSServiceResponseType()); } /// Create a shared pointer with a rmw_request_id_t @@ -559,26 +757,66 @@ class Client : public ClientBase return; } auto & value = *optional_pending_request; - auto typed_response = std::static_pointer_cast( - std::move(response)); - if (std::holds_alternative(value)) { - auto & promise = std::get(value); + if (std::holds_alternative(value)) { + auto typed_response = std::static_pointer_cast( + std::move(response)); + auto & promise = std::get(value); + promise.set_value(std::move(typed_response)); + } else if (std::holds_alternative(value)) { + auto typed_response = std::static_pointer_cast( + std::move(response)); + auto & promise = std::get(value); promise.set_value(std::move(typed_response)); } else if (std::holds_alternative(value)) { auto & inner = std::get(value); - const auto & callback = std::get(inner); - auto & promise = std::get(inner); - auto & future = std::get(inner); - promise.set_value(std::move(typed_response)); - callback(std::move(future)); + if (inner.index() == 0) { + auto typed_response = std::static_pointer_cast( + std::move(response)); + auto & inner_tuple = + std::get>(inner); + const auto & callback = std::get(inner_tuple); + auto & promise = std::get(inner_tuple); + auto & future = std::get(inner_tuple); + promise.set_value(std::move(typed_response)); + callback(std::move(future)); + } else if (inner.index() == 1) { + auto typed_response = std::static_pointer_cast( + std::move(response)); + auto & inner_tuple = + std::get>(inner); + const auto & callback = std::get(inner_tuple); + auto & promise = std::get(inner_tuple); + auto & future = std::get(inner_tuple); + promise.set_value(std::move(typed_response)); + callback(std::move(future)); + } } else if (std::holds_alternative(value)) { auto & inner = std::get(value); - const auto & callback = std::get(inner); - auto & promise = std::get(inner); - auto & future = std::get(inner); - auto & request = std::get(inner); - promise.set_value(std::make_pair(std::move(request), std::move(typed_response))); - callback(std::move(future)); + if (inner.index() == 0) { + auto typed_response = std::static_pointer_cast( + std::move(response)); + auto & inner_tuple = + std::get>(inner); + const auto & callback = std::get(inner_tuple); + auto & promise = std::get(inner_tuple); + auto & future = std::get(inner_tuple); + auto & request = std::get(inner_tuple); + promise.set_value(std::make_pair(std::move(request), std::move(typed_response))); + callback(std::move(future)); + } else if (inner.index() == 1) { + auto typed_response = std::static_pointer_cast( + std::move(response)); + auto & inner_tuple = + std::get>(inner); + const auto & callback = std::get(inner_tuple); + auto & promise = std::get(inner_tuple); + auto & future = std::get(inner_tuple); + auto & request = std::get(inner_tuple); + promise.set_value(std::make_pair(std::move(request), std::move(typed_response))); + callback(std::move(future)); + } } } @@ -610,15 +848,34 @@ class Client : public ClientBase * \param[in] request request to be send. * \return a FutureAndRequestId instance. */ - FutureAndRequestId - async_send_request(SharedRequest request) + template + typename std::enable_if_t< + std::is_same::value, + ROSFutureAndRequestId + > + async_send_request(T request) + { + ROSPromise promise; + auto future = promise.get_future(); + auto req_id = async_send_request_impl( + *request, + std::move(promise)); + return ROSFutureAndRequestId(std::move(future), req_id); + } + + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value, + CustomFutureAndRequestId> + async_send_request(T request) { - Promise promise; + CustomPromise promise; auto future = promise.get_future(); auto req_id = async_send_request_impl( *request, std::move(promise)); - return FutureAndRequestId(std::move(future), req_id); + return CustomFutureAndRequestId(std::move(future), req_id); } /// Send a request to the service server and schedule a callback in the executor. @@ -628,7 +885,7 @@ class Client : public ClientBase * If the callback is never called, because we never got a reply for the service server, remove_pending_request() * has to be called with the returned request id or prune_pending_requests(). * Not doing so will make the `Client` instance use more memory each time a response is not - * received from the service server. + * received from the service server * In this case, it's convenient to setup a timer to cleanup the pending requests. * See for example the `examples_rclcpp_async_client` package in https://github.com/ros2/examples. * @@ -636,27 +893,47 @@ class Client : public ClientBase * \param[in] cb callback that will be called when we get a response for this request. * \return the request id representing the request just sent. */ - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - CallbackType - >::value - >::type * = nullptr + template + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + ROSCallbackType + >::value, + ROSSharedFutureAndRequestId > - SharedFutureAndRequestId - async_send_request(SharedRequest request, CallbackT && cb) + async_send_request(ROSSharedRequest request, CallbackT && cb) + { + ROSPromise promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + *request, + std::make_tuple( + ROSCallbackType{std::forward(cb)}, + shared_future, + std::move(promise))); + return ROSSharedFutureAndRequestId{std::move(shared_future), req_id}; + } + + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + rclcpp::function_traits::same_arguments< + CallbackT, + CustomCallbackType + >::value, + CustomSharedFutureAndRequestId + > + async_send_request(CustomSharedRequest request, CallbackT && cb) { - Promise promise; + CustomPromise promise; auto shared_future = promise.get_future().share(); auto req_id = async_send_request_impl( *request, std::make_tuple( - CallbackType{std::forward(cb)}, + CustomCallbackType{std::forward(cb)}, shared_future, std::move(promise))); - return SharedFutureAndRequestId{std::move(shared_future), req_id}; + return CustomSharedFutureAndRequestId{std::move(shared_future), req_id}; } /// Send a request to the service server and schedule a callback in the executor. @@ -667,28 +944,49 @@ class Client : public ClientBase * \param[in] cb callback that will be called when we get a response for this request. * \return the request id representing the request just sent. */ - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - CallbackWithRequestType - >::value - >::type * = nullptr + template + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + ROSTotalCallbackWithRequestType + >::value, + ROSTotalSharedFutureWithRequestAndRequestId > - SharedFutureWithRequestAndRequestId - async_send_request(SharedRequest request, CallbackT && cb) + async_send_request(ROSSharedRequest request, CallbackT && cb) { - PromiseWithRequest promise; + ROSTotalPromiseWithRequest promise; auto shared_future = promise.get_future().share(); auto req_id = async_send_request_impl( *request, std::make_tuple( - CallbackWithRequestType{std::forward(cb)}, + ROSTotalCallbackWithRequestType{std::forward(cb)}, request, shared_future, std::move(promise))); - return SharedFutureWithRequestAndRequestId{std::move(shared_future), req_id}; + return ROSTotalSharedFutureWithRequestAndRequestId{std::move(shared_future), req_id}; + } + + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + rclcpp::function_traits::same_arguments< + CallbackT, + CustomTotalCallbackWithRequestType + >::value, + CustomTotalSharedFutureWithRequestAndRequestId + > + async_send_request(CustomSharedRequest request, CallbackT && cb) + { + CustomTotalPromiseWithRequest promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + *request, + std::make_tuple( + CustomTotalCallbackWithRequestType{std::forward(cb)}, + request, + shared_future, + std::move(promise))); + return CustomTotalSharedFutureWithRequestAndRequestId{std::move(shared_future), req_id}; } /// Cleanup a pending request. @@ -702,8 +1000,9 @@ class Client : public ClientBase * \param[in] request_id request id returned by async_send_request(). * \return true when a pending request was removed, false if not (e.g. a response was received). */ - bool - remove_pending_request(int64_t request_id) + template + typename std::enable_if_t::value, bool> + remove_pending_request(T request_id) { std::lock_guard guard(pending_requests_mutex_); return pending_requests_.erase(request_id) != 0u; @@ -715,8 +1014,17 @@ class Client : public ClientBase * * `Client::remove_pending_request(this, future.request_id)`. */ - bool - remove_pending_request(const FutureAndRequestId & future) + template + typename std::enable_if_t::value, bool> + remove_pending_request(const T & future) + { + return this->remove_pending_request(future.request_id); + } + + template + typename std::enable_if_t::value && + rclcpp::TypeAdapter::is_specialized::value, bool> + remove_pending_request(const T & future) { return this->remove_pending_request(future.request_id); } @@ -727,8 +1035,17 @@ class Client : public ClientBase * * `Client::remove_pending_request(this, future.request_id)`. */ - bool - remove_pending_request(const SharedFutureAndRequestId & future) + template + typename std::enable_if_t::value, bool> + remove_pending_request(const T & future) + { + return this->remove_pending_request(future.request_id); + } + + template + typename std::enable_if_t::value && + rclcpp::TypeAdapter::is_specialized::value, bool> + remove_pending_request(const T & future) { return this->remove_pending_request(future.request_id); } @@ -739,8 +1056,35 @@ class Client : public ClientBase * * `Client::remove_pending_request(this, future.request_id)`. */ - bool - remove_pending_request(const SharedFutureWithRequestAndRequestId & future) + template + typename std::enable_if_t::value, + bool> + remove_pending_request(const T & future) + { + return this->remove_pending_request(future.request_id); + } + + template + typename std::enable_if_t::value && + rclcpp::TypeAdapter::is_specialized::value, bool> + remove_pending_request(const T & future) + { + return this->remove_pending_request(future.request_id); + } + + template + typename std::enable_if_t::value && + rclcpp::TypeAdapter::is_specialized::value, bool> + remove_pending_request(const T & future) + { + return this->remove_pending_request(future.request_id); + } + + template + typename std::enable_if_t::value && + rclcpp::TypeAdapter::is_specialized::value, bool> + remove_pending_request(const T & future) { return this->remove_pending_request(future.request_id); } @@ -814,17 +1158,82 @@ class Client : public ClientBase } protected: - using CallbackTypeValueVariant = std::tuple; - using CallbackWithRequestTypeValueVariant = std::tuple< - CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>; + template::is_specialized::value> + struct CallbackWithRequestTypeValueVariantHelper {}; + + template<> + struct CallbackWithRequestTypeValueVariantHelper + { + using CallbackWithRequestTypeValueVariant = std::variant< + std::tuple>; + }; + + template<> + struct CallbackWithRequestTypeValueVariantHelper + { + using CallbackWithRequestTypeValueVariant = std::variant< + std::tuple, + std::tuple>; + }; + + using CallbackWithRequestTypeValueVariant = + typename CallbackWithRequestTypeValueVariantHelper<>::CallbackWithRequestTypeValueVariant; + + template::is_specialized::value> + struct CallbackTypeValueVariantHelper {}; - using CallbackInfoVariant = std::variant< - std::promise, - CallbackTypeValueVariant, - CallbackWithRequestTypeValueVariant>; + template<> + struct CallbackTypeValueVariantHelper + { + using CallbackTypeValueVariant = std::variant>; + }; + + template<> + struct CallbackTypeValueVariantHelper + { + using CallbackTypeValueVariant = std::variant< + std::tuple, + std::tuple>; + }; + + using CallbackTypeValueVariant = typename CallbackTypeValueVariantHelper<>:: + CallbackTypeValueVariant; + + template::is_specialized::value> + struct CallbackInfoVariantHelper {}; + + template<> + struct CallbackInfoVariantHelper + { + using CallbackInfoVariant = std::variant< + ROSPromise, + CallbackTypeValueVariant, + CallbackWithRequestTypeValueVariant>; + }; - int64_t - async_send_request_impl(const Request & request, CallbackInfoVariant value) + template<> + struct CallbackInfoVariantHelper + { + using CallbackInfoVariant = std::variant< + ROSPromise, + CustomPromise, + CallbackTypeValueVariant, + CallbackWithRequestTypeValueVariant>; + }; + + using CallbackInfoVariant = typename CallbackInfoVariantHelper<>::CallbackInfoVariant; + + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value, + int64_t + > + async_send_request_impl(const T & request, CallbackInfoVariant value) { int64_t sequence_number; std::lock_guard lock(pending_requests_mutex_); @@ -838,6 +1247,32 @@ class Client : public ClientBase return sequence_number; } + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value, + int64_t + > + async_send_request_impl(const T & request, CallbackInfoVariant value) + { + int64_t sequence_number; + std::lock_guard lock(pending_requests_mutex_); + + ROSServiceRequestType ros_service_request; + rclcpp::TypeAdapter::convert_to_ros_service_request( + request, ros_service_request); + + rcl_ret_t ret = rcl_send_request( + get_client_handle().get(), &ros_service_request, &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); + } + pending_requests_.try_emplace( + sequence_number, + std::make_pair(std::chrono::system_clock::now(), std::move(value))); + return sequence_number; + } + std::optional get_and_erase_pending_request(int64_t request_number) { diff --git a/rclcpp/include/rclcpp/get_service_type_support_handle.hpp b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp new file mode 100644 index 0000000000..c40a72fc9a --- /dev/null +++ b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp @@ -0,0 +1,97 @@ +// Copyright 2023 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. + +#ifndef RCLCPP__GET_SERVICE_TYPE_SUPPORT_HANDLE_HPP_ +#define RCLCPP__GET_SERVICE_TYPE_SUPPORT_HANDLE_HPP_ + +#include +#include + +#include "rosidl_runtime_cpp/traits.hpp" +#include "rosidl_runtime_cpp/service_type_support_decl.hpp" +#include "rosidl_typesupport_cpp/service_type_support.hpp" + +#include "rclcpp/type_adapter.hpp" +#include "type_adapter.hpp" + +namespace rclcpp +{ + +#ifdef DOXYGEN_ONLY + +/// Returns the service type support for the given `ServiceT` type. +/** + * \tparam ServiceT an actual ROS service type or an adapted type using `rclcpp::TypeAdapter` + */ +template +constexpr const rosidl_service_type_support_t & get_service_type_support_handle(); + +#else + +template +constexpr +std::enable_if_t< + rosidl_generator_traits::is_service::value, + const rosidl_service_type_support_t & +> +get_service_type_support_handle() +{ + auto handle = rosidl_typesupport_cpp::get_service_type_support_handle(); + if (!handle) { + throw std::runtime_error("Type support handle unexpectedly nullptr"); + } + return *handle; +} + +/// Checking the TypeAdapter struct when using custom or ROS types. +template +constexpr +std::enable_if_t< + !rosidl_generator_traits::is_service::value && + rclcpp::TypeAdapter::is_specialized::value, + const rosidl_service_type_support_t & +> +get_service_type_support_handle() +{ + using CustomType = typename TypeAdapter::ros_message_type; + auto handle = rosidl_typesupport_cpp::get_service_type_support_handle(); + + if (!handle) { + throw std::runtime_error("Type support handle unexpectedly nullptr"); + } + return *handle; +} + +// This specialization is a pass through runtime check, which allows a better +// static_assert to catch this issue further down the line. +// This should never get to be called in practice, and is purely defensive. +template +constexpr +typename std::enable_if_t< + !rosidl_generator_traits::is_service::value && + !TypeAdapter::is_specialized::value, + const rosidl_service_type_support_t & +> +get_service_type_support_handle() +{ + throw std::runtime_error( + "this specialization of rclcpp::get_service_type_support_handle() " + "should never be called"); +} + +#endif // DOXYGEN_ONLY + +} // namespace rclcpp + +#endif // RCLCPP__GET_SERVICE_TYPE_SUPPORT_HANDLE_HPP_ diff --git a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp index 97c6ad6a27..ff6b1588c0 100644 --- a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp +++ b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp @@ -30,6 +30,14 @@ struct is_ros_compatible_type rclcpp::TypeAdapter::is_specialized::value; }; +template +struct is_ros_compatible_service_type +{ + static constexpr bool value = + rosidl_generator_traits::is_service::value || + rclcpp::TypeAdapter::is_specialized::value; +}; + } // namespace rclcpp #endif // RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9e08dc235d..1912437b6f 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -39,11 +39,15 @@ #include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/get_service_type_support_handle.hpp" +#include "rclcpp/is_ros_compatible_type.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp/type_adapter.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "type_adapter.hpp" namespace rclcpp { @@ -280,22 +284,110 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; }; + +/** + * ServiceT must be either a: + * ROS service type with its own message type support in both the request and response + * (e.g. std_msgs::msgs::String), or a rclcpp::TypeAdapter + * (e.g. struct TypeAdapterStruct { using Request = rclcpp::TypeAdapter; using Response = rclcpp::TypeAdapter; }; + * ) + * + * In the case the ServiceT is a ROS service with ROS message types in both the request and + * response (e.g. std_msgs::msg::Bool, std_msgs::msg::String), all of the custom types + * ServiceRequestType, ROSServiceRequestType, ServiceResponseType, ROSServiceResponseType will + * be their respective types. + * In any case that ServiceT is a struct that uses request and response as a + * TypeAdapter (e.g. struct TypeAdapterStruct { + * using Request = rclcpp::TypeAdapter; + * using Response = rclcpp::TypeAdapter; };) + * ServiceRequestType and ServiceResponseType will be the custom type and + * ROSServiceRequestType and ROSServiceResponseType will be the ROS message type + */ template class Service : public ServiceBase, public std::enable_shared_from_this> { public: - using CallbackType = std::function< + static_assert( + rclcpp::is_ros_compatible_service_type::value, + "Service type is not compatible with ROS 2 and cannot be used with a Service"); + + /// ServiceT::custom_type::Request if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ServiceRequestType = + typename rclcpp::TypeAdapter::custom_type::Request; + /// ServiceT::ros_message_type::Request if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ROSServiceRequestType = + typename rclcpp::TypeAdapter::ros_message_type::Request; + /// ServiceT::custom_type::Response if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ServiceResponseType = + typename rclcpp::TypeAdapter::custom_type::Response; + /// ServiceT::ros_message_type::Response if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ROSServiceResponseType = + typename rclcpp::TypeAdapter::ros_message_type::Response; + + using CallbackType [[deprecated("Use ROSCallbackType instead of CallbackType")]] = + std::function< + void ( + const std::shared_ptr, + std::shared_ptr)>; + + using ROSCallbackType = std::function< + void ( + const std::shared_ptr, + std::shared_ptr)>; + + using CallbackWithHeaderType + [[deprecated("Use ROSCallbackWithHeaderType instead of CallbackWithHeaderType")]] = + std::function< + void ( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr)>; + + using ROSCallbackWithHeaderType = std::function< + void ( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr)>; + + using CustomCallbackType = std::function< + void ( + const std::shared_ptr, + std::shared_ptr)>; + + using CustomCallbackWithHeaderType = std::function< + void ( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr)>; + + using ROSCustomCallbackType = std::function< + void ( + const std::shared_ptr, + std::shared_ptr)>; + + using ROSCustomCallbackWithHeaderType = std::function< + void ( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr)>; + + using CustomROSCallbackType = std::function< void ( - const std::shared_ptr, - std::shared_ptr)>; + const std::shared_ptr, + std::shared_ptr)>; - using CallbackWithHeaderType = std::function< + using CustomROSCallbackWithHeaderType = std::function< void ( const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr)>; + const std::shared_ptr, + std::shared_ptr)>; RCLCPP_SMART_PTR_DEFINITIONS(Service) /// Default constructor. @@ -315,7 +407,7 @@ class Service AnyServiceCallback any_callback, rcl_service_options_t & service_options) : ServiceBase(node_handle), any_callback_(any_callback), - srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) + srv_type_support_handle_(&rclcpp::get_service_type_support_handle()) { // rcl does the static memory allocation here service_handle_ = std::shared_ptr( @@ -376,7 +468,7 @@ class Service std::shared_ptr service_handle, AnyServiceCallback any_callback) : ServiceBase(node_handle), any_callback_(any_callback), - srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) + srv_type_support_handle_(&rclcpp::get_service_type_support_handle()) { // check if service handle was initialized if (!rcl_service_is_valid(service_handle.get())) { @@ -411,7 +503,7 @@ class Service rcl_service_t * service_handle, AnyServiceCallback any_callback) : ServiceBase(node_handle), any_callback_(any_callback), - srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) + srv_type_support_handle_(&rclcpp::get_service_type_support_handle()) { // check if service handle was initialized if (!rcl_service_is_valid(service_handle)) { @@ -443,6 +535,9 @@ class Service /** * \sa ServiceBase::take_type_erased_request(). * + * This signature is enabled if the service request is a ROSServiceRequestType + * as opposed to the custom type of a respective TypeAdapter + * * \param[out] request_out The reference to a service 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 @@ -451,16 +546,82 @@ class Service * \throws rclcpp::exceptions::RCLError based exceptions if the underlying * rcl calls fail. */ - bool - take_request(typename ServiceT::Request & request_out, rmw_request_id_t & request_id_out) + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value, + bool + > + take_request(T & request_out, rmw_request_id_t & request_id_out) { return this->take_type_erased_request(&request_out, request_id_out); } + /// Take the next request from the service. + /** + * \sa ServiceBase::take_type_erased_request(). + * + * This signature is enabled if the service request is a ServiceRequestType + * created with a TypeAdapter, matching its respective custom_type + * + * \param[out] request_out The reference to a service 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. + */ + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value, + bool + > + take_request(T & request_out, rmw_request_id_t & request_id_out) + { + ROSServiceRequestType ros_service_request_out; + rclcpp::TypeAdapter::convert_to_ros_service_request( + request_out, ros_service_request_out); + return this->take_type_erased_request(&ros_service_request_out, request_id_out); + } + std::shared_ptr create_request() override { - return std::make_shared(); + if (std::holds_alternative(any_callback_.get_variant())) + { + return std::make_shared(); + } + if (std::holds_alternative(any_callback_.get_variant())) + { + return std::make_shared(); + } + if (std::holds_alternative(any_callback_.get_variant())) + { + return std::make_shared(); + } + if (std::holds_alternative(any_callback_.get_variant())) + { + return std::make_shared(); + } + if (std::holds_alternative(any_callback_.get_variant())) + { + return std::make_shared(); + } + if (std::holds_alternative(any_callback_.get_variant())) + { + return std::make_shared(); + } + if (std::holds_alternative(any_callback_.get_variant())) + { + return std::make_shared(); + } + if (std::holds_alternative(any_callback_.get_variant())) + { + return std::make_shared(); + } + return std::make_shared(); } std::shared_ptr @@ -474,15 +635,87 @@ class Service std::shared_ptr request_header, std::shared_ptr request) override { - auto typed_request = std::static_pointer_cast(request); - auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); - if (response) { - send_response(*request_header, *response); + if (std::holds_alternative(any_callback_.get_variant())) + { + auto typed_request = std::static_pointer_cast(request); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } + } + if (std::holds_alternative(any_callback_.get_variant())) + { + auto typed_request = std::static_pointer_cast(request); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } + } + if (std::holds_alternative(any_callback_.get_variant())) + { + auto typed_request = std::static_pointer_cast(request); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } + } + if (std::holds_alternative(any_callback_.get_variant())) + { + auto typed_request = std::static_pointer_cast(request); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } + } + if (std::holds_alternative(any_callback_.get_variant())) + { + auto typed_request = std::static_pointer_cast(request); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } + } + if (std::holds_alternative(any_callback_.get_variant())) + { + auto typed_request = std::static_pointer_cast(request); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } + } + if (std::holds_alternative(any_callback_.get_variant())) + { + auto typed_request = std::static_pointer_cast(request); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } + } + if (std::holds_alternative(any_callback_.get_variant())) + { + auto typed_request = std::static_pointer_cast(request); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } } } - void - send_response(rmw_request_id_t & req_id, typename ServiceT::Response & response) + // Send the given response via rcl function + /** + * Enable this response if the given ServiceT::Response is a ROSServiceResponseType, + * a provided ros_message_type opposed to a custom_type from a TypeAdapter + * + * \param[in] req_id The given id assigned to the current response. + * \param[in] response A ServiceT::Response which is meant to be sent via rcl. + * throws rclcpp::exceptions::throw_from_rcl_error if the rcl_ret_t is not alright + */ + template + std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value + > + send_response(rmw_request_id_t & req_id, T & response) { rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response); @@ -499,6 +732,33 @@ class Service } } + // Send the given response via rcl function + /** + * Enable this response if the given ServiceT::Response is a ServiceResponseType, + * a provided custom_type from a TypeAdapter + * + * \param[in] req_id The given id assigned to the current response. + * \param[in] response A ServiceT::Response which is meant to be sent via rcl. + * throws rclcpp::exceptions::throw_from_rcl_error if the rcl_ret_t is not alright + */ + template + std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value + > + send_response(rmw_request_id_t & req_id, T & response) + { + ROSServiceResponseType ros_service_response; + rclcpp::TypeAdapter::convert_to_ros_service_response( + response, ros_service_response); + rcl_ret_t ret = rcl_send_response( + get_service_handle().get(), &req_id, &ros_service_response); + + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + } + } + /// Configure client introspection. /** * \param[in] clock clock to use to generate introspection timestamps diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index f4b5e20d30..ed0a20b5c6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/parameter_client.hpp" @@ -43,11 +44,25 @@ get_service_type_support_handle() } } // namespace rosidl_typesupport_cpp +namespace rosidl_generator_traits +{ +template<> +struct is_service: std::true_type {}; + +template<> +struct is_message: std::true_type {}; + +template<> +struct is_message: std::true_type {}; + +template<> +struct is_message: std::true_type {}; +} // namespace rosidl_generator_traits + namespace rclcpp { namespace node_interfaces { - class NodeTypeDescriptions::NodeTypeDescriptionsImpl { public: diff --git a/rclcpp/test/msg/Bool.msg b/rclcpp/test/msg/Bool.msg new file mode 100644 index 0000000000..e70908b074 --- /dev/null +++ b/rclcpp/test/msg/Bool.msg @@ -0,0 +1 @@ +bool data diff --git a/rclcpp/test/msg/Empty.msg b/rclcpp/test/msg/Empty.msg new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index afa12cd24d..8dff612f22 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -8,10 +8,14 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs ../msg/Header.msg ../msg/MessageWithHeader.msg ../msg/String.msg + ../msg/Bool.msg + ../msg/Empty.msg + ../srv/SetBool.srv DEPENDENCIES builtin_interfaces LIBRARY_NAME ${PROJECT_NAME} SKIP_INSTALL ) + # Need the target name to depend on generated interface libraries rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "rosidl_typesupport_cpp") @@ -75,12 +79,23 @@ if(TARGET test_client) ) target_link_libraries(test_client ${PROJECT_NAME} mimick) endif() + +ament_add_gtest(test_client_with_type_adapter test_client_with_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_client_with_type_adapter) + target_link_libraries(test_client_with_type_adapter + ${PROJECT_NAME} + ${cpp_typesupport_target} mimick) + endif() + ament_add_gtest(test_copy_all_parameter_values test_copy_all_parameter_values.cpp) if(TARGET test_copy_all_parameter_values) ament_target_dependencies(test_copy_all_parameter_values "rcl_interfaces" ) target_link_libraries(test_copy_all_parameter_values ${PROJECT_NAME}) + endif() ament_add_gtest(test_create_timer test_create_timer.cpp) if(TARGET test_create_timer) @@ -531,6 +546,16 @@ if(TARGET test_service) ) target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick) endif() + +ament_add_gtest(test_service_with_type_adapter test_service_with_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_service_with_type_adapter) + target_link_libraries(test_service_with_type_adapter + ${PROJECT_NAME} + ${cpp_typesupport_target} mimick) +endif() + # Creating and destroying nodes is slow with Connext, so this needs larger timeout. ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) if(TARGET test_subscription) diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 9070e1caa9..6cc77d49d5 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -247,7 +247,7 @@ class TestClientWithServer : public ::testing::Test ::testing::AssertionResult SendEmptyRequestAndWait( std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) { - using SharedFuture = rclcpp::Client::SharedFuture; + using SharedFuture = rclcpp::Client::ROSSharedFuture; auto client = node->create_client(service_name); if (!client->wait_for_service()) { @@ -294,7 +294,7 @@ TEST_F(TestClientWithServer, async_send_request) { TEST_F(TestClientWithServer, async_send_request_callback_with_request) { using SharedFutureWithRequest = - rclcpp::Client::SharedFutureWithRequest; + rclcpp::Client::ROSTotalSharedFutureWithRequest; auto client = node->create_client(service_name); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); @@ -563,7 +563,7 @@ TEST_F(TestClient, client_qos_depth) { auto request = std::make_shared(); ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); - using SharedFuture = rclcpp::Client::SharedFuture; + using SharedFuture = rclcpp::Client::ROSSharedFuture; uint64_t client_cb_count_ = 0; auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) { if (nullptr == future_response.get()) { diff --git a/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp new file mode 100644 index 0000000000..2034f33f94 --- /dev/null +++ b/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp @@ -0,0 +1,349 @@ +// Copyright 2023 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 + +#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 "rclcpp/msg/empty.hpp" +#include "rclcpp/msg/string.hpp" +#include "rclcpp/msg/bool.hpp" + +#include "rclcpp/srv/set_bool.hpp" + +using namespace std::chrono_literals; + +struct CustomBool +{ + struct SetBoolResponse + { + bool success; + std::string message; + }; + + using Request = bool; + using Response = SetBoolResponse; +}; + +struct CustomIncorrectBool +{ + struct SetBoolResponse + { + int success; + std::string message; + }; + + using Request = bool; + using Response = SetBoolResponse; +}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = CustomBool; + using ros_message_type = rclcpp::srv::SetBool; + + static void + convert_to_ros_service_request( + const custom_type::Request & source, + ros_message_type::Request & destination) + { + destination.data = source; + } + + static void + convert_to_custom_service_request( + const ros_message_type::Request & source, + custom_type::Request & destination) + { + destination = source.data; + } + + static void + convert_to_ros_service_response( + const custom_type::Response & source, + ros_message_type::Response & destination) + { + destination.success = source.success; + destination.message = source.message; + } + + static void + convert_to_custom_service_response( + const ros_message_type::Response & source, + custom_type::Response & destination) + { + destination.success = source.success; + destination.message = source.message; + } +}; + +// Throws in conversion +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = CustomIncorrectBool; + using ros_message_type = rclcpp::srv::SetBool; + + static void + convert_to_ros_service_request( + const custom_type::Request & source, + ros_message_type::Request & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom_service_request( + const ros_message_type::Request & source, + custom_type::Request & destination) + { + (void) source; + (void) destination; + } + + static void + convert_to_ros_service_response( + const custom_type::Response & source, + ros_message_type::Response & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom_service_response( + const ros_message_type::Response & source, + custom_type::Response & destination) + { + (void) source; + (void) destination; + } +}; + +class TestClient : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestClientWithServer : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + + auto callback = []( + const std::shared_ptr, + const std::shared_ptr) {}; + + service = node->create_service(service_name, std::move(callback)); + } + + ::testing::AssertionResult SendBoolRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + using SharedFuture = rclcpp::Client::CustomSharedFuture; + + auto client = node->create_client(service_name); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Waiting for service failed"; + } + + auto request = std::make_shared(); + bool received_response = false; + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto callback = [&received_response, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + received_response = true; + }; + + auto req_id = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < timeout) + { + rclcpp::spin_some(node); + } + + if (!received_response) { + return ::testing::AssertionFailure() << "Waiting for response timed out"; + } + if (client->remove_pending_request(req_id)) { + return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; + } + + return request_result; + } + + using AdaptedTypeStruct = rclcpp::TypeAdapter; + std::shared_ptr node; + std::shared_ptr> service; + const std::string service_name{"empty_service"}; +}; + +/* + * Testing the basic creation of clients with a TypeAdapter for both Request and Response + */ +TEST_F(TestClient, various_creation_signatures) +{ + { + using AdaptedTypeStruct = rclcpp::TypeAdapter; + auto client = node->create_client("client"); + + (void)client; + } + { + /// Now try to adapt the type with the `as` metafunction + using AdaptedTypeStruct = rclcpp::adapt_type::as; + + auto client = node->create_client("client"); + (void)client; + } +} + +/// Testing that conversion errors are passed up +TEST_F(TestClient, conversion_exception_is_passed_up) +{ + using BadAdaptedTypeStruct = rclcpp::TypeAdapter; + + auto client = node->create_client("client"); +} + +TEST_F(TestClientWithServer, test_adapted_client_remove_pending_request) { + auto client = node->create_client("no_service_server_available_here"); + + auto request = std::make_shared(); + auto future = client->async_send_request(request); + + EXPECT_TRUE(client->remove_pending_request(future)); +} + +TEST_F(TestClientWithServer, take_adapted_response) +{ + auto client = node->create_client(service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto request = std::make_shared(); + auto request_header = client->create_request_header(); + CustomBool::Response response; + + client->async_send_request(request); + EXPECT_FALSE(client->take_response(response, *request_header.get())); + + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_OK); + EXPECT_TRUE(client->take_response(response, *request_header.get())); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED); + EXPECT_FALSE(client->take_response(response, *request_header.get())); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_ERROR); + EXPECT_THROW( + client->take_response(response, *request_header.get()), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestClientWithServer, async_send_request) { + EXPECT_TRUE(SendBoolRequestAndWait()); +} + +TEST_F(TestClientWithServer, async_send_request_callback_with_request) { + using SharedFutureWithRequest = + rclcpp::Client::CustomTotalSharedFutureWithRequest; + + auto client = node->create_client(service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + + auto request = std::make_shared(); + bool received_response = false; + auto callback = [&request, &received_response](SharedFutureWithRequest future) { + auto request_response_pair = future.get(); + EXPECT_EQ(request, request_response_pair.first); + EXPECT_NE(nullptr, request_response_pair.second); + received_response = true; + }; + auto req_id = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) + { + rclcpp::spin_some(node); + } + EXPECT_TRUE(received_response); + EXPECT_FALSE(client->remove_pending_request(req_id)); +} diff --git a/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp new file mode 100644 index 0000000000..03c79865bb --- /dev/null +++ b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp @@ -0,0 +1,294 @@ +// Copyright 2023 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 + +#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 "rclcpp/msg/empty.hpp" +#include "rclcpp/msg/string.hpp" +#include "rclcpp/msg/bool.hpp" + +#include "rclcpp/srv/set_bool.hpp" + +using namespace std::chrono_literals; + +class TestService : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +struct CustomBool +{ + struct SetBoolResponse + { + bool success; + std::string message; + }; + + using Request = bool; + using Response = SetBoolResponse; +}; + +struct CustomIncorrectBool +{ + struct SetBoolResponse + { + int success; + std::string message; + }; + + using Request = bool; + using Response = SetBoolResponse; +}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = CustomBool; + using ros_message_type = rclcpp::srv::SetBool; + + static void + convert_to_ros_service_request( + const custom_type::Request & source, + ros_message_type::Request & destination) + { + destination.data = source; + } + + static void + convert_to_custom_service_request( + const ros_message_type::Request & source, + custom_type::Request & destination) + { + destination = source.data; + } + + static void + convert_to_ros_service_response( + const custom_type::Response & source, + ros_message_type::Response & destination) + { + destination.success = source.success; + destination.message = source.message; + } + + static void + convert_to_custom_service_response( + const ros_message_type::Response & source, + custom_type::Response & destination) + { + destination.success = source.success; + destination.message = source.message; + } +}; + +// Throws in conversion +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = CustomIncorrectBool; + using ros_message_type = rclcpp::srv::SetBool; + + static void + convert_to_ros_service_request( + const custom_type::Request & source, + ros_message_type::Request & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom_service_request( + const ros_message_type::Request & source, + custom_type::Request & destination) + { + (void) source; + (void) destination; + } + + static void + convert_to_ros_service_response( + const custom_type::Response & source, + ros_message_type::Response & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom_service_response( + const ros_message_type::Response & source, + custom_type::Response & destination) + { + (void) source; + (void) destination; + } +}; + +void custom_total_callback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + (void)res; +} + +void custom_ros_callback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + (void)res; +} + +void ros_custom_callback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + (void)res; +} + +void incorrect_callback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + (void)res; +} + +/* + * Testing the basic creation of services with a TypeAdapter for both Request and Response + */ +TEST_F(TestService, various_creation_signatures) +{ + { + std::shared_ptr node = std::make_shared("my_node"); + + using AdaptedTypeStruct = rclcpp::TypeAdapter; + auto service = node->create_service("service", &custom_total_callback); + (void)service; + } + { + /// Now try to adapt the type with the `as` metafunction + std::shared_ptr node = std::make_shared("my_node"); + + using AdaptedTypeStruct = rclcpp::adapt_type::as; + auto service = node->create_service("service", &custom_total_callback); + (void)service; + } + { + std::shared_ptr node = std::make_shared("my_node"); + + using AdaptedTypeStruct = rclcpp::TypeAdapter; + auto service = node->create_service("service", &custom_ros_callback); + (void)service; + } + { + std::shared_ptr node = std::make_shared("my_node"); + + using AdaptedTypeStruct = rclcpp::TypeAdapter; + auto service = node->create_service("service", &ros_custom_callback); + (void)service; + } +} + +/// Testing that conversion errors are passed up +TEST_F(TestService, conversion_exception_is_passed_up) +{ + std::shared_ptr node = std::make_shared("my_node"); + + using BadAdaptedTypeStruct = rclcpp::TypeAdapter; + + auto service = node->create_service("service", &incorrect_callback); +} + +TEST_F(TestService, send_adapted_response) { + std::shared_ptr node = std::make_shared("my_node"); + using AdaptedTypeStruct = rclcpp::TypeAdapter; + + auto server = node->create_service("service", &custom_total_callback); + { + auto request_id = server->create_request_header(); + CustomBool::Response response; + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_OK); + EXPECT_NO_THROW(server->send_response(*request_id.get(), response)); + } + { + auto request_id = server->create_request_header(); + CustomBool::Response response; + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_ERROR); + EXPECT_THROW( + server->send_response(*request_id.get(), response), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestService, take_adapted_request) { + std::shared_ptr node = std::make_shared("my_node"); + using AdaptedTypeStruct = rclcpp::TypeAdapter; + + auto server = node->create_service("service", &custom_total_callback); + { + auto request_id = server->create_request_header(); + CustomBool::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_OK); + EXPECT_TRUE(server->take_request(request, *request_id.get())); + } + { + auto request_id = server->create_request_header(); + CustomBool::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_SERVICE_TAKE_FAILED); + EXPECT_FALSE(server->take_request(request, *request_id.get())); + } + { + auto request_id = server->create_request_header(); + CustomBool::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_ERROR); + EXPECT_THROW(server->take_request(request, *request_id.get()), rclcpp::exceptions::RCLError); + } +} diff --git a/rclcpp/test/srv/SetBool.srv b/rclcpp/test/srv/SetBool.srv new file mode 100644 index 0000000000..6763bdbcfe --- /dev/null +++ b/rclcpp/test/srv/SetBool.srv @@ -0,0 +1,4 @@ +bool data +--- +bool success +string message