From 548b91ddd49fc42da3cb5be7ee23d56c614086b8 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Fri, 18 Aug 2023 19:52:38 -0400 Subject: [PATCH 1/8] Adding service type adapters Signed-off-by: CursedRock17 --- .../get_service_type_support_handle.hpp | 154 +++++++++++ rclcpp/include/rclcpp/service.hpp | 135 +++++++++- rclcpp/test/msg/Bool.msg | 1 + rclcpp/test/rclcpp/CMakeLists.txt | 20 +- .../rclcpp/test_service_with_type_adapter.cpp | 245 ++++++++++++++++++ rclcpp/test/srv/SetBool.srv | 4 + 6 files changed, 551 insertions(+), 8 deletions(-) create mode 100644 rclcpp/include/rclcpp/get_service_type_support_handle.hpp create mode 100644 rclcpp/test/msg/Bool.msg create mode 100644 rclcpp/test/rclcpp/test_service_with_type_adapter.cpp create mode 100644 rclcpp/test/srv/SetBool.srv 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..0b113f2f0c --- /dev/null +++ b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp @@ -0,0 +1,154 @@ +// 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" + +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; +} + +/// The following checks will fix for the 4 different ways you could assemble +/// 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 &&, + rclcpp::TypeAdapter::is_specialized::value, + const rosidl_service_type_support_t & +> +get_service_type_support_handle() +{ + struct AdaptedTypeStructTemp + { + using Request = typename TypeAdapter::ros_message_type; + using Response = 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; +} + +template +constexpr +std::enable_if_t< + !rosidl_generator_traits::is_service::value && + !rclcpp::TypeAdapter::is_specialized::value &&, + rclcpp::TypeAdapter::is_specialized::value, + const rosidl_service_type_support_t & +> +get_service_type_support_handle() +{ + struct AdaptedTypeStructTemp + { + using Request = typename TypeAdapter::custom_type; + using Response = 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; +} + +template +constexpr +std::enable_if_t< + !rosidl_generator_traits::is_service::value && + rclcpp::TypeAdapter::is_specialized::value &&, + !rclcpp::TypeAdapter::is_specialized::value, + const rosidl_service_type_support_t & +> +get_service_type_support_handle() +{ + struct AdaptedTypeStructTemp + { + using Request = typename TypeAdapter::ros_message_type; + using Response = typename TypeAdapter::custom_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< + typename AdaptedTypeStruct +> +constexpr +typename std::enable_if_t< + !rosidl_generator_traits::is_service::value && + !TypeAdapter::is_specialized::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/service.hpp b/rclcpp/include/rclcpp/service.hpp index 2ae79a6637..08ca8cc5a1 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_message_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,12 +284,54 @@ 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: + static_assert( + rclcpp::is_ros_compatible_type::value, + "Service Request type is not compatible with ROS 2 and cannot be used with a Service"); + static_assert( + rclcpp::is_ros_compatible_type::value, + "Service Response type is not compatible with ROS 2 and cannot be used with a Service"); + + /// ServiceT::Request::custom_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ServiceRequestType = typename rclcpp::TypeAdapter::custom_type; + /// ServiceT::Request::ros_message_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ROSServiceRequestType = + typename rclcpp::TypeAdapter::ros_message_type; + /// ServiceT::Response::custom_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ServiceResponseType = typename rclcpp::TypeAdapter::custom_type; + /// ServiceT::Response::ros_message_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ROSServiceResponseType = + typename rclcpp::TypeAdapter::ros_message_type; + using CallbackType = std::function< void ( const std::shared_ptr, @@ -315,7 +361,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 +422,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 +457,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 +489,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,12 +500,44 @@ 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 + > + take_request(const 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 + > + take_request(const T & request_out, rmw_request_id_t & request_id_out) + { + ROSServiceRequestType ros_service_request_out; + rclcpp::TypeAdapter::convert_to_ros_message( + 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 { @@ -481,8 +562,21 @@ class Service } } - 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, const T & response) { rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response); @@ -491,6 +585,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, const T & response) + { + ROSServiceResponseType ros_service_response; + rclcpp::TypeAdapter::convert_to_ros_message( + 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/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/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 8c31a95415..ebd4209e22 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -8,12 +8,20 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs ../msg/Header.msg ../msg/MessageWithHeader.msg ../msg/String.msg + ../msg/Bool.msg + DEPENDENCIES builtin_interfaces + LIBRARY_NAME ${PROJECT_NAME} + SKIP_INSTALL +) + +rosidl_generate_interfaces(${PROJECT_NAME}_test_srvs + ../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") +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "${PROJECT_NAME}_test_srvs" "rosidl_typesupport_cpp") ament_add_gtest( test_allocator_common @@ -530,6 +538,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}) +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_service_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp new file mode 100644 index 0000000000..bd9f274232 --- /dev/null +++ b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp @@ -0,0 +1,245 @@ +// 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 "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/string.hpp" +#include "rclcpp/msg/bool.hpp" + +#include "rclcpp/srv/set_bool.hpp" + +using namespace std::chrono_literals; + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + + +class TestService: public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = bool; + using ros_message_type = rclcpp::msg::Bool; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +// Throws in conversion +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = int; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + } +}; + +} // namespace rclcpp + +/* + * Testing the basic creation of services with a TypeAdapter for both Request and Response + */ +TEST_F(TestService, total_type_adaption_service_creation) +{ + using AdaptedRequestType = rclcpp::TypeAdapter>; + using AdaptedResponseType = rclcpp::TypeAdapter>; + + struct AdaptedTypeStruct { + using Request = AdaptedRequestType; + using Response = AdaptedResponseType; + }; + + auto service = rclcpp::create_service( + "service", + [](const std::string & req, const bool & res) {}); + + /// Now try to adapt the type with the `as` metafunction + (void)service; + + using AdaptedRequestType = rclcpp::adapt_type::as; + using AdaptedResponseType = rclcpp::adapt_type::as; + + struct AdaptedTypeStruct { + using Request = AdaptedRequestType; + using Response = AdaptedResponseType; + }; + + auto service = rclcpp::create_service( + "service", + [](const std::string & req, const bool & res) {}); + (void)service; +} + +TEST_F(TestService, request_type_adaption_service_creation) +{ + using AdaptedRequestType = rclcpp::TypeAdapter>; + + struct AdaptedTypeStruct { + using Request = AdaptedRequestType; + using Response = rclcpp::srv::SetBool::Response; + }; + + auto service = rclcpp::create_service( + "service", + [](const std::string & req, rclcpp::srv::SetBool::Response & res) {}); + + /// Now try to adapt the type with the `as` metafunction + (void)service; + + using AdaptedRequestType = rclcpp::adapt_type::as; + + struct AdaptedTypeStruct { + using Request = AdaptedRequestType; + using Response = rclcpp::srv::SetBool::Response; + }; + + auto service = rclcpp::create_service( + "service", + [](const std::string & req, rclcpp::srv::SetBool::Response & res) {}); + (void)service; +} + +TEST_F(TestService, response_type_adaption_service_creation) +{ + using AdaptedResponseType = rclcpp::TypeAdapter>; + + struct AdaptedTypeStruct { + using Request = rclcpp::msg::String; + using Response = AdaptedResponseType; + }; + + auto service = rclcpp::create_service( + "service", + [](const rclcpp::msg::String & req, const bool & res) {}); + + /// Now try to adapt the type with the `as` metafunction + (void)service; + + using AdaptedResponseType = rclcpp::adapt_type::as; + + struct AdaptedTypeStruct { + using Request = rclcpp::msg::String; + using Response = AdaptedResponseType; + }; + + auto service = rclcpp::create_service( + "service", + [](const rclcpp::msg::String & req, const bool & res) {}); + (void)service; +} + +/// Testing that conversion errors are passed up +TEST_F(TestService, conversion_exception_is_passed_up) +{ + using BadAdaptedResponseType = rclcpp::TypeAdapter>; + + struct AdaptedTypeStruct { + using Request = rclcpp::msg::String; + using Response = BadAdaptedResponseType; + }; + + auto service = rclcpp::create_service( + "service", + [](const rclcpp::msg::String & req) {}); +} 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 From 94dd5769982338fd3f2736d6cd58111d5e91d6ca Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Fri, 18 Aug 2023 23:00:22 -0400 Subject: [PATCH 2/8] Adding Client Side Signed-off-by: CursedRock17 --- rclcpp/include/rclcpp/client.hpp | 100 +++++++- rclcpp/include/rclcpp/service.hpp | 2 +- rclcpp/test/rclcpp/CMakeLists.txt | 8 + .../rclcpp/test_client_with_type_adapter.cpp | 239 ++++++++++++++++++ .../rclcpp/test_service_with_type_adapter.cpp | 8 + 5 files changed, 351 insertions(+), 6 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_client_with_type_adapter.cpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 85b0a2d5f9..431c9d67de 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -39,6 +39,7 @@ #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/logging.hpp" #include "rclcpp/macros.hpp" @@ -382,6 +383,28 @@ class Client : public ClientBase using Request = typename ServiceT::Request; using Response = typename ServiceT::Response; + static_assert( + rclcpp::is_ros_compatible_type::value, + "Service Request type is not compatible with ROS 2 and cannot be used with a Client"); + static_assert( + rclcpp::is_ros_compatible_type::value, + "Service Response type is not compatible with ROS 2 and cannot be used with a Client"); + + /// ServiceT::Request::custom_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ServiceRequestType = typename rclcpp::TypeAdapter::custom_type; + /// ServiceT::Request::ros_message_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Request + using ROSServiceRequestType = + typename rclcpp::TypeAdapter::ros_message_type; + /// ServiceT::Response::custom_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ServiceResponseType = typename rclcpp::TypeAdapter::custom_type; + /// ServiceT::Response::ros_message_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::Response + using ROSServiceResponseType = + typename rclcpp::TypeAdapter::ros_message_type; + using SharedRequest = typename ServiceT::Request::SharedPtr; using SharedResponse = typename ServiceT::Response::SharedPtr; @@ -475,7 +498,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 +529,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 +541,44 @@ 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 + > + take_response(const 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 + > + take_response(const T & response_out, rmw_request_id_t & request_header_out) + { + ROSServiceResponseType ros_service_response_out; + rclcpp::TypeAdapter::convert_to_ros_message( + request_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 /** @@ -628,7 +687,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. * @@ -823,8 +882,13 @@ class Client : public ClientBase CallbackTypeValueVariant, CallbackWithRequestTypeValueVariant>; + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value + > int64_t - async_send_request_impl(const Request & request, CallbackInfoVariant value) + async_send_request_impl(const T & request, CallbackInfoVariant value) { int64_t sequence_number; std::lock_guard lock(pending_requests_mutex_); @@ -838,6 +902,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_message( + 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/service.hpp b/rclcpp/include/rclcpp/service.hpp index 08ca8cc5a1..adf1f187b3 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -39,7 +39,7 @@ #include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" -#include "rclcpp/get_message_type_support_handle.hpp" +#include "rclcpp/get_service_type_support_handle.hpp" #include "rclcpp/is_ros_compatible_type.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index ebd4209e22..5db1455d08 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -89,6 +89,14 @@ 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}) +endif() ament_add_gtest(test_create_timer test_create_timer.cpp) if(TARGET test_create_timer) ament_target_dependencies(test_create_timer 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..201d45c40e --- /dev/null +++ b/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp @@ -0,0 +1,239 @@ +// 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 "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/string.hpp" +#include "rclcpp/msg/bool.hpp" + +#include "rclcpp/srv/set_bool.hpp" + +using namespace std::chrono_literals; + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + + +class TestClient: public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = bool; + using ros_message_type = rclcpp::msg::Bool; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +// Throws in conversion +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = int; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + } +}; + +} // namespace rclcpp + +/* + * Testing the basic creation of clients with a TypeAdapter for both Request and Response + */ +TEST_F(TestClient, total_type_adaption_client_creation) +{ + using AdaptedRequestType = rclcpp::TypeAdapter>; + using AdaptedResponseType = rclcpp::TypeAdapter>; + + std::shared_ptr node = std::make_shared("my_node"); + + struct AdaptedTypeStruct { + using Request = AdaptedRequestType; + using Response = AdaptedResponseType; + }; + + auto client = node->create_client("client"); + + /// Now try to adapt the type with the `as` metafunction + (void)client; + + using AdaptedRequestType = rclcpp::adapt_type::as; + using AdaptedResponseType = rclcpp::adapt_type::as; + + struct AdaptedTypeStruct { + using Request = AdaptedRequestType; + using Response = AdaptedResponseType; + }; + + auto client = node->create_client("client"); + (void)client; +} + +TEST_F(TestClient, request_type_adaption_client_creation) +{ + std::shared_ptr node = std::make_shared("my_node"); + + using AdaptedRequestType = rclcpp::TypeAdapter>; + + struct AdaptedTypeStruct { + using Request = AdaptedRequestType; + using Response = rclcpp::srv::SetBool::Response; + }; + + auto client = node->create_client("client"); + + /// Now try to adapt the type with the `as` metafunction + (void)client; + + using AdaptedRequestType = rclcpp::adapt_type::as; + + struct AdaptedTypeStruct { + using Request = AdaptedRequestType; + using Response = rclcpp::srv::SetBool::Response; + }; + + auto client = node->create_client("client"); + (void)client; +} + +TEST_F(TestClient, response_type_adaption_client_creation) +{ + std::shared_ptr node = std::make_shared("my_node"); + + using AdaptedResponseType = rclcpp::TypeAdapter>; + + struct AdaptedTypeStruct { + using Request = rclcpp::msg::String; + using Response = AdaptedResponseType; + }; + + auto client = node->create_client("client"); + + /// Now try to adapt the type with the `as` metafunction + (void)client; + + using AdaptedResponseType = rclcpp::adapt_type::as; + + struct AdaptedTypeStruct { + using Request = rclcpp::msg::String; + using Response = AdaptedResponseType; + }; + + auto client = node->create_client("client"); + (void)client; +} + +/// Testing that conversion errors are passed up +TEST_F(TestClient, conversion_exception_is_passed_up) +{ + std::shared_ptr node = std::make_shared("my_node"); + + using BadAdaptedResponseType = rclcpp::TypeAdapter>; + + struct AdaptedTypeStruct { + using Request = rclcpp::msg::String; + using Response = BadAdaptedResponseType; + }; + + auto client = node->create_client("client"); +} diff --git a/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp index bd9f274232..1afd2d2175 100644 --- a/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp @@ -142,6 +142,8 @@ struct TypeAdapter */ TEST_F(TestService, total_type_adaption_service_creation) { + std::shared_ptr node = std::make_shared("my_node"); + using AdaptedRequestType = rclcpp::TypeAdapter>; using AdaptedResponseType = rclcpp::TypeAdapter>; @@ -173,6 +175,8 @@ TEST_F(TestService, total_type_adaption_service_creation) TEST_F(TestService, request_type_adaption_service_creation) { + std::shared_ptr node = std::make_shared("my_node"); + using AdaptedRequestType = rclcpp::TypeAdapter>; struct AdaptedTypeStruct { @@ -202,6 +206,8 @@ TEST_F(TestService, request_type_adaption_service_creation) TEST_F(TestService, response_type_adaption_service_creation) { + std::shared_ptr node = std::make_shared("my_node"); + using AdaptedResponseType = rclcpp::TypeAdapter>; struct AdaptedTypeStruct { @@ -232,6 +238,8 @@ TEST_F(TestService, response_type_adaption_service_creation) /// 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 BadAdaptedResponseType = rclcpp::TypeAdapter>; struct AdaptedTypeStruct { From 1caf1ed25636c13ace3bf57c9031004f23403e39 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Mon, 21 Aug 2023 14:30:30 -0400 Subject: [PATCH 3/8] Current Changes Signed-off-by: CursedRock17 --- rclcpp/include/rclcpp/client.hpp | 23 +++++---- .../get_service_type_support_handle.hpp | 38 ++++++++------- .../include/rclcpp/is_ros_compatible_type.hpp | 9 ++++ rclcpp/include/rclcpp/service.hpp | 47 +++++++++++-------- rclcpp/test/rclcpp/CMakeLists.txt | 9 +--- 5 files changed, 70 insertions(+), 56 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 431c9d67de..2cb1a6b44b 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -41,6 +41,7 @@ #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" @@ -380,15 +381,13 @@ template class Client : public ClientBase { public: + static_assert( + rclcpp::is_ros_compatible_service_type::value, + "Service Request type is not compatible with ROS 2 and cannot be used with a Client"); + using Request = typename ServiceT::Request; using Response = typename ServiceT::Response; - static_assert( - rclcpp::is_ros_compatible_type::value, - "Service Request type is not compatible with ROS 2 and cannot be used with a Client"); - static_assert( - rclcpp::is_ros_compatible_type::value, - "Service Response type is not compatible with ROS 2 and cannot be used with a Client"); /// ServiceT::Request::custom_type if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Request @@ -498,7 +497,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_(rclcpp::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(), @@ -576,7 +575,7 @@ class Client : public ClientBase { ROSServiceResponseType ros_service_response_out; rclcpp::TypeAdapter::convert_to_ros_message( - request_out, ros_service_response_out); + response_out, ros_service_response_out); return this->take_type_erased_response(&ros_service_response_out, request_header_out); } @@ -885,9 +884,9 @@ class Client : public ClientBase template typename std::enable_if_t< rosidl_generator_traits::is_message::value && - std::is_same::value + std::is_same::value, + int64_t > - int64_t async_send_request_impl(const T & request, CallbackInfoVariant value) { int64_t sequence_number; @@ -905,9 +904,9 @@ class Client : public ClientBase template typename std::enable_if_t< rclcpp::TypeAdapter::is_specialized::value && - std::is_same::value + std::is_same::value, + int64_t > - int64_t async_send_request_impl(const T & request, CallbackInfoVariant value) { int64_t sequence_number; diff --git a/rclcpp/include/rclcpp/get_service_type_support_handle.hpp b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp index 0b113f2f0c..f1ced99b94 100644 --- a/rclcpp/include/rclcpp/get_service_type_support_handle.hpp +++ b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp @@ -59,16 +59,18 @@ template constexpr std::enable_if_t< !rosidl_generator_traits::is_service::value && - rclcpp::TypeAdapter::is_specialized::value &&, - rclcpp::TypeAdapter::is_specialized::value, + rclcpp::TypeAdapter::is_specialized::value && + rclcpp::TypeAdapter::is_specialized::value, const rosidl_service_type_support_t & > get_service_type_support_handle() { struct AdaptedTypeStructTemp { - using Request = typename TypeAdapter::ros_message_type; - using Response = typename TypeAdapter::ros_message_type; + using Request = + typename TypeAdapter::ros_message_type; + using Response = + typename TypeAdapter::ros_message_type; }; auto handle = rosidl_typesupport_cpp::get_service_type_support_handle constexpr std::enable_if_t< !rosidl_generator_traits::is_service::value && - !rclcpp::TypeAdapter::is_specialized::value &&, - rclcpp::TypeAdapter::is_specialized::value, + !rclcpp::TypeAdapter::is_specialized::value && + rclcpp::TypeAdapter::is_specialized::value, const rosidl_service_type_support_t & > get_service_type_support_handle() { struct AdaptedTypeStructTemp { - using Request = typename TypeAdapter::custom_type; - using Response = typename TypeAdapter::ros_message_type; + using Request = + typename TypeAdapter::custom_type; + using Response = + typename TypeAdapter::ros_message_type; }; auto handle = rosidl_typesupport_cpp::get_service_type_support_handle constexpr std::enable_if_t< !rosidl_generator_traits::is_service::value && - rclcpp::TypeAdapter::is_specialized::value &&, - !rclcpp::TypeAdapter::is_specialized::value, + rclcpp::TypeAdapter::is_specialized::value && + !rclcpp::TypeAdapter::is_specialized::value, const rosidl_service_type_support_t & > get_service_type_support_handle() { struct AdaptedTypeStructTemp { - using Request = typename TypeAdapter::ros_message_type; - using Response = typename TypeAdapter::custom_type; + using Request = + typename TypeAdapter::ros_message_type; + using Response = + typename TypeAdapter::custom_type; }; auto handle = rosidl_typesupport_cpp::get_service_type_support_handle +template constexpr typename std::enable_if_t< !rosidl_generator_traits::is_service::value && - !TypeAdapter::is_specialized::value, - !TypeAdapter::is_specialized::value, + !TypeAdapter::is_specialized::value && + !TypeAdapter::is_specialized::value, const rosidl_service_type_support_t & > get_service_type_support_handle() diff --git a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp index 97c6ad6a27..b3f14477c5 100644 --- a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp +++ b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp @@ -30,6 +30,15 @@ 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 || + 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 adf1f187b3..f47dd01ea9 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -311,26 +311,25 @@ class Service { public: static_assert( - rclcpp::is_ros_compatible_type::value, + rclcpp::is_ros_compatible_service_type::value, "Service Request type is not compatible with ROS 2 and cannot be used with a Service"); - static_assert( - rclcpp::is_ros_compatible_type::value, - "Service Response type is not compatible with ROS 2 and cannot be used with a Service"); /// ServiceT::Request::custom_type if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Request - using ServiceRequestType = typename rclcpp::TypeAdapter::custom_type; + using ServiceRequestType = + typename rclcpp::TypeAdapter::custom_type; /// ServiceT::Request::ros_message_type if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Request using ROSServiceRequestType = - typename rclcpp::TypeAdapter::ros_message_type; + typename rclcpp::TypeAdapter::ros_message_type; /// ServiceT::Response::custom_type if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Response - using ServiceResponseType = typename rclcpp::TypeAdapter::custom_type; + using ServiceResponseType = + typename rclcpp::TypeAdapter::custom_type; /// ServiceT::Response::ros_message_type if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Response using ROSServiceResponseType = - typename rclcpp::TypeAdapter::ros_message_type; + typename rclcpp::TypeAdapter::ros_message_type; using CallbackType = std::function< void ( @@ -361,7 +360,7 @@ class Service AnyServiceCallback any_callback, rcl_service_options_t & service_options) : ServiceBase(node_handle), any_callback_(any_callback), - srv_type_support_handle_(rclcpp::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( @@ -422,7 +421,7 @@ class Service std::shared_ptr service_handle, AnyServiceCallback any_callback) : ServiceBase(node_handle), any_callback_(any_callback), - srv_type_support_handle_(rclcpp::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())) { @@ -457,7 +456,7 @@ class Service rcl_service_t * service_handle, AnyServiceCallback any_callback) : ServiceBase(node_handle), any_callback_(any_callback), - srv_type_support_handle_(rclcpp::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)) { @@ -505,7 +504,7 @@ class Service rosidl_generator_traits::is_message::value && std::is_same::value > - take_request(const T & request_out, rmw_request_id_t & request_id_out) + take_request(T & request_out, rmw_request_id_t & request_id_out) { return this->take_type_erased_request(&request_out, request_id_out); } @@ -527,13 +526,13 @@ class Service */ template typename std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && + rclcpp::TypeAdapter::is_specialized::value && std::is_same::value > - take_request(const T & request_out, rmw_request_id_t & request_id_out) + take_request(T & request_out, rmw_request_id_t & request_id_out) { ROSServiceRequestType ros_service_request_out; - rclcpp::TypeAdapter::convert_to_ros_message( + rclcpp::TypeAdapter::convert_to_ros_message( request_out, ros_service_request_out); return this->take_type_erased_request(&ros_service_request_out, request_id_out); } @@ -573,13 +572,21 @@ class Service */ template std::enable_if_t< - rosidl_generator_traits::is_message::value && + rosidl_generator_traits::is_message::value && std::is_same::value > - send_response(rmw_request_id_t & req_id, const T & response) + send_response(rmw_request_id_t & req_id, T & response) { rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response); + 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"); } @@ -596,13 +603,13 @@ class Service */ template std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && + rclcpp::TypeAdapter::is_specialized::value && std::is_same::value > - send_response(rmw_request_id_t & req_id, const T & response) + send_response(rmw_request_id_t & req_id, T & response) { ROSServiceResponseType ros_service_response; - rclcpp::TypeAdapter::convert_to_ros_message( + rclcpp::TypeAdapter::convert_to_ros_message( response, ros_service_response); rcl_ret_t ret = rcl_send_response( get_service_handle().get(), &req_id, &ros_service_response); diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 5db1455d08..c735dc9bf4 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -9,19 +9,14 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs ../msg/MessageWithHeader.msg ../msg/String.msg ../msg/Bool.msg - DEPENDENCIES builtin_interfaces - LIBRARY_NAME ${PROJECT_NAME} - SKIP_INSTALL -) - -rosidl_generate_interfaces(${PROJECT_NAME}_test_srvs ../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" "${PROJECT_NAME}_test_srvs" "rosidl_typesupport_cpp") +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "rosidl_typesupport_cpp") ament_add_gtest( test_allocator_common From 99809782250558cba29abd9161a95724476a2c5f Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Mon, 21 Aug 2023 23:35:33 -0400 Subject: [PATCH 4/8] more stuff added Signed-off-by: CursedRock17 --- rclcpp/include/rclcpp/client.hpp | 29 ++++--- .../include/rclcpp/is_ros_compatible_type.hpp | 2 +- rclcpp/include/rclcpp/service.hpp | 6 +- .../node_type_descriptions.cpp | 82 ++++++++++++++++++- rclcpp/test/msg/Empty.msg | 0 rclcpp/test/rclcpp/CMakeLists.txt | 1 + .../rclcpp/test_client_with_type_adapter.cpp | 48 +++++------ .../rclcpp/test_service_with_type_adapter.cpp | 16 +--- 8 files changed, 129 insertions(+), 55 deletions(-) create mode 100644 rclcpp/test/msg/Empty.msg diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 2cb1a6b44b..29e14e3175 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -381,13 +381,16 @@ template class Client : public ClientBase { public: - static_assert( - rclcpp::is_ros_compatible_service_type::value, - "Service Request type is not compatible with ROS 2 and cannot be used with a Client"); - using Request = typename ServiceT::Request; using Response = typename ServiceT::Response; + static_assert( + rclcpp::is_ros_compatible_type::value, + "Service Request type is not compatible with ROS 2 and cannot be used with a Client"); + + static_assert( + rclcpp::is_ros_compatible_type::value, + "Service Response type is not compatible with ROS 2 and cannot be used with a Client"); /// ServiceT::Request::custom_type if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Request @@ -404,8 +407,8 @@ class Client : public ClientBase using ROSServiceResponseType = typename rclcpp::TypeAdapter::ros_message_type; - using SharedRequest = typename ServiceT::Request::SharedPtr; - using SharedResponse = typename ServiceT::Response::SharedPtr; + using SharedRequest = typename ROSServiceRequestType::SharedPtr; + using SharedResponse = typename ROSServiceResponseType::SharedPtr; using Promise = std::promise; using PromiseWithRequest = std::promise>; @@ -544,9 +547,10 @@ class Client : public ClientBase template typename std::enable_if_t< rosidl_generator_traits::is_message::value && - std::is_same::value + std::is_same::value, + bool > - take_response(const T & response_out, rmw_request_id_t & request_header_out) + take_response(T & response_out, rmw_request_id_t & request_header_out) { return this->take_type_erased_response(&response_out, request_header_out); } @@ -569,9 +573,10 @@ class Client : public ClientBase template typename std::enable_if_t< rclcpp::TypeAdapter::is_specialized::value && - std::is_same::value + std::is_same::value, + bool > - take_response(const T & response_out, rmw_request_id_t & request_header_out) + take_response(T & response_out, rmw_request_id_t & request_header_out) { ROSServiceResponseType ros_service_response_out; rclcpp::TypeAdapter::convert_to_ros_message( @@ -586,7 +591,7 @@ class Client : public ClientBase std::shared_ptr create_response() override { - return std::shared_ptr(new typename ServiceT::Response()); + return std::shared_ptr(new Response()); } /// Create a shared pointer with a rmw_request_id_t @@ -617,7 +622,7 @@ class Client : public ClientBase return; } auto & value = *optional_pending_request; - auto typed_response = std::static_pointer_cast( + auto typed_response = std::static_pointer_cast( std::move(response)); if (std::holds_alternative(value)) { auto & promise = std::get(value); diff --git a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp index b3f14477c5..db58bbeb81 100644 --- a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp +++ b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp @@ -36,7 +36,7 @@ struct is_ros_compatible_service_type static constexpr bool value = rosidl_generator_traits::is_service::value || rclcpp::TypeAdapter::is_specialized::value || - rclcpp::TypeAdapter::is_specialized::value; + rclcpp::TypeAdapter::is_specialized::value; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index f47dd01ea9..9e468f7983 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -311,9 +311,13 @@ class Service { public: static_assert( - rclcpp::is_ros_compatible_service_type::value, + rclcpp::is_ros_compatible_type::value, "Service Request type is not compatible with ROS 2 and cannot be used with a Service"); + static_assert( + rclcpp::is_ros_compatible_type::value, + "Service Response type is not compatible with ROS 2 and cannot be used with a Service"); + /// ServiceT::Request::custom_type if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Request using ServiceRequestType = diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp index f4b5e20d30..65a5d40f65 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -18,8 +18,10 @@ #include "rclcpp/node_interfaces/node_type_descriptions.hpp" #include "rclcpp/parameter_client.hpp" +#include "rclcpp/type_adapter.hpp" #include "type_description_interfaces/srv/get_type_description.h" +#include "type_description_interfaces/srv/get_type_description.hpp" namespace { @@ -30,6 +32,79 @@ struct GetTypeDescription__C using Response = type_description_interfaces__srv__GetTypeDescription_Response; using Event = type_description_interfaces__srv__GetTypeDescription_Event; }; + +/* +template<> +struct rclcpp::TypeAdapter< + type_description_interfaces__srv__GetTypeDescription_Request, + type_description_interfaces::srv::GetTypeDescription::Request +> +{ + using is_specialized = std::true_type; + using custom_type = type_description_interfaces__srv__GetTypeDescription_Request; + using ros_message_type = type_description_interfaces::srv::GetTypeDescription::Request; + + static + void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static + void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +template<> +struct rclcpp::TypeAdapter< + type_description_interfaces__srv__GetTypeDescription_Response, + type_description_interfaces::srv::GetTypeDescription::Response +> +{ + using is_specialized = std::true_type; + using custom_type = type_description_interfaces__srv__GetTypeDescription_Request; + using ros_message_type = type_description_interfaces::srv::GetTypeDescription::Request; + + static + void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static + void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + + using TypeRequest = rclcpp::TypeAdapter< + type_description_interfaces__srv__GetTypeDescription_Request, + type_description_interfaces::srv::GetTypeDescription::Request>; + using TypeResponse = rclcpp::TypeAdapter< + type_description_interfaces__srv__GetTypeDescription_Response, + type_description_interfaces::srv::GetTypeDescription::Response>; + +struct CustomDescriptionTypes +{ + using Request = TypeRequest; + using Response = TypeResponse; +}; +*/ } // namespace // Helper function for C typesupport. @@ -50,12 +125,13 @@ namespace node_interfaces class NodeTypeDescriptions::NodeTypeDescriptionsImpl { + public: using ServiceT = GetTypeDescription__C; rclcpp::Logger logger_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::Service::SharedPtr type_description_srv_; + //rclcpp::Service::SharedPtr type_description_srv_; NodeTypeDescriptionsImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -65,6 +141,7 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl : logger_(node_logging->get_logger()), node_base_(node_base) { + /* const std::string enable_param_name = "start_type_description_service"; bool enabled = false; @@ -123,12 +200,13 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl std::dynamic_pointer_cast(type_description_srv_), nullptr); } + */ } ~NodeTypeDescriptionsImpl() { if ( - type_description_srv_ && +// type_description_srv_ && RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle())) { RCLCPP_ERROR( 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 c735dc9bf4..d034fc5bff 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -9,6 +9,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs ../msg/MessageWithHeader.msg ../msg/String.msg ../msg/Bool.msg + ../msg/Empty.msg ../srv/SetBool.srv DEPENDENCIES builtin_interfaces LIBRARY_NAME ${PROJECT_NAME} diff --git a/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp index 201d45c40e..d413eb1525 100644 --- a/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp @@ -26,10 +26,9 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" -#include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" -#include "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/empty.hpp" #include "rclcpp/msg/string.hpp" #include "rclcpp/msg/bool.hpp" @@ -37,10 +36,6 @@ using namespace std::chrono_literals; -static const int g_max_loops = 200; -static const std::chrono::milliseconds g_sleep_per_loop(10); - - class TestClient: public ::testing::Test { public: @@ -57,10 +52,9 @@ class TestClient: public ::testing::Test } }; -namespace rclcpp -{ + template<> -struct TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = bool; @@ -84,7 +78,7 @@ struct TypeAdapter }; template<> -struct TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = std::string; @@ -109,7 +103,7 @@ struct TypeAdapter // Throws in conversion template<> -struct TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = int; @@ -135,15 +129,14 @@ struct TypeAdapter } }; -} // namespace rclcpp /* * Testing the basic creation of clients with a TypeAdapter for both Request and Response */ TEST_F(TestClient, total_type_adaption_client_creation) { - using AdaptedRequestType = rclcpp::TypeAdapter>; - using AdaptedResponseType = rclcpp::TypeAdapter>; + using AdaptedRequestType = rclcpp::TypeAdapter; + using AdaptedResponseType = rclcpp::TypeAdapter; std::shared_ptr node = std::make_shared("my_node"); @@ -160,20 +153,20 @@ TEST_F(TestClient, total_type_adaption_client_creation) using AdaptedRequestType = rclcpp::adapt_type::as; using AdaptedResponseType = rclcpp::adapt_type::as; - struct AdaptedTypeStruct { + struct AdaptedTypeAsStruct { using Request = AdaptedRequestType; using Response = AdaptedResponseType; }; - auto client = node->create_client("client"); - (void)client; + auto Asclient = node->create_client("client"); + (void)Asclient; } TEST_F(TestClient, request_type_adaption_client_creation) { std::shared_ptr node = std::make_shared("my_node"); - using AdaptedRequestType = rclcpp::TypeAdapter>; + using AdaptedRequestType = rclcpp::TypeAdapter; struct AdaptedTypeStruct { using Request = AdaptedRequestType; @@ -187,20 +180,20 @@ TEST_F(TestClient, request_type_adaption_client_creation) using AdaptedRequestType = rclcpp::adapt_type::as; - struct AdaptedTypeStruct { + struct AdaptedTypeAsStruct { using Request = AdaptedRequestType; using Response = rclcpp::srv::SetBool::Response; }; - auto client = node->create_client("client"); - (void)client; + auto Asclient = node->create_client("client"); + (void)Asclient; } TEST_F(TestClient, response_type_adaption_client_creation) { std::shared_ptr node = std::make_shared("my_node"); - using AdaptedResponseType = rclcpp::TypeAdapter>; + using AdaptedResponseType = rclcpp::TypeAdapter; struct AdaptedTypeStruct { using Request = rclcpp::msg::String; @@ -214,13 +207,13 @@ TEST_F(TestClient, response_type_adaption_client_creation) using AdaptedResponseType = rclcpp::adapt_type::as; - struct AdaptedTypeStruct { + struct AdaptedTypeAsStruct { using Request = rclcpp::msg::String; using Response = AdaptedResponseType; }; - auto client = node->create_client("client"); - (void)client; + auto Asclient = node->create_client("client"); + (void)Asclient; } /// Testing that conversion errors are passed up @@ -228,12 +221,13 @@ TEST_F(TestClient, conversion_exception_is_passed_up) { std::shared_ptr node = std::make_shared("my_node"); - using BadAdaptedResponseType = rclcpp::TypeAdapter>; + using BadAdaptedResponseType = rclcpp::TypeAdapter; - struct AdaptedTypeStruct { + struct BadAdaptedTypeStruct { using Request = rclcpp::msg::String; using Response = BadAdaptedResponseType; }; auto client = node->create_client("client"); + (void)client; } diff --git a/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp index 1afd2d2175..08b9d04aee 100644 --- a/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp @@ -37,10 +37,6 @@ using namespace std::chrono_literals; -static const int g_max_loops = 200; -static const std::chrono::milliseconds g_sleep_per_loop(10); - - class TestService: public ::testing::Test { public: @@ -57,10 +53,8 @@ class TestService: public ::testing::Test } }; -namespace rclcpp -{ template<> -struct TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = bool; @@ -79,12 +73,12 @@ struct TypeAdapter const ros_message_type & source, custom_type & destination) { - destination = source.data; + destination = source; } }; template<> -struct TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = std::string; @@ -109,7 +103,7 @@ struct TypeAdapter // Throws in conversion template<> -struct TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; using custom_type = int; @@ -135,8 +129,6 @@ struct TypeAdapter } }; -} // namespace rclcpp - /* * Testing the basic creation of services with a TypeAdapter for both Request and Response */ From c312b89e3cd2dc7d7b1939e44c237a23386650db Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Wed, 23 Aug 2023 17:16:45 -0400 Subject: [PATCH 5/8] Reverting to Failures Signed-off-by: CursedRock17 --- .../get_service_type_support_handle.hpp | 12 +-- .../node_type_descriptions.cpp | 82 +------------------ .../rclcpp/test_client_with_type_adapter.cpp | 12 +-- .../rclcpp/test_service_with_type_adapter.cpp | 5 +- 4 files changed, 17 insertions(+), 94 deletions(-) diff --git a/rclcpp/include/rclcpp/get_service_type_support_handle.hpp b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp index f1ced99b94..3b219edd0e 100644 --- a/rclcpp/include/rclcpp/get_service_type_support_handle.hpp +++ b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp @@ -68,9 +68,9 @@ get_service_type_support_handle() struct AdaptedTypeStructTemp { using Request = - typename TypeAdapter::ros_message_type; + typename rclcpp::TypeAdapter::ros_message_type; using Response = - typename TypeAdapter::ros_message_type; + typename rclcpp::TypeAdapter::ros_message_type; }; auto handle = rosidl_typesupport_cpp::get_service_type_support_handle::custom_type; + typename rclcpp::TypeAdapter::custom_type; using Response = - typename TypeAdapter::ros_message_type; + typename rclcpp::TypeAdapter::ros_message_type; }; auto handle = rosidl_typesupport_cpp::get_service_type_support_handle::ros_message_type; + typename rclcpp::TypeAdapter::ros_message_type; using Response = - typename TypeAdapter::custom_type; + typename rclcpp::TypeAdapter::custom_type; }; auto handle = rosidl_typesupport_cpp::get_service_type_support_handle -struct rclcpp::TypeAdapter< - type_description_interfaces__srv__GetTypeDescription_Request, - type_description_interfaces::srv::GetTypeDescription::Request -> -{ - using is_specialized = std::true_type; - using custom_type = type_description_interfaces__srv__GetTypeDescription_Request; - using ros_message_type = type_description_interfaces::srv::GetTypeDescription::Request; - - static - void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) - { - destination.data = source; - } - - static - void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) - { - destination = source.data; - } -}; - -template<> -struct rclcpp::TypeAdapter< - type_description_interfaces__srv__GetTypeDescription_Response, - type_description_interfaces::srv::GetTypeDescription::Response -> -{ - using is_specialized = std::true_type; - using custom_type = type_description_interfaces__srv__GetTypeDescription_Request; - using ros_message_type = type_description_interfaces::srv::GetTypeDescription::Request; - - static - void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) - { - destination.data = source; - } - - static - void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) - { - destination = source.data; - } -}; - - using TypeRequest = rclcpp::TypeAdapter< - type_description_interfaces__srv__GetTypeDescription_Request, - type_description_interfaces::srv::GetTypeDescription::Request>; - using TypeResponse = rclcpp::TypeAdapter< - type_description_interfaces__srv__GetTypeDescription_Response, - type_description_interfaces::srv::GetTypeDescription::Response>; - -struct CustomDescriptionTypes -{ - using Request = TypeRequest; - using Response = TypeResponse; -}; -*/ } // namespace // Helper function for C typesupport. @@ -125,13 +50,12 @@ namespace node_interfaces class NodeTypeDescriptions::NodeTypeDescriptionsImpl { - public: using ServiceT = GetTypeDescription__C; rclcpp::Logger logger_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - //rclcpp::Service::SharedPtr type_description_srv_; + rclcpp::Service::SharedPtr type_description_srv_; NodeTypeDescriptionsImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -141,7 +65,6 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl : logger_(node_logging->get_logger()), node_base_(node_base) { - /* const std::string enable_param_name = "start_type_description_service"; bool enabled = false; @@ -200,13 +123,12 @@ class NodeTypeDescriptions::NodeTypeDescriptionsImpl std::dynamic_pointer_cast(type_description_srv_), nullptr); } - */ } ~NodeTypeDescriptionsImpl() { if ( -// type_description_srv_ && + type_description_srv_ && RCL_RET_OK != rcl_node_type_description_service_fini(node_base_->get_rcl_node_handle())) { RCLCPP_ERROR( diff --git a/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp index d413eb1525..748517f445 100644 --- a/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp @@ -129,17 +129,18 @@ struct rclcpp::TypeAdapter } }; - /* * Testing the basic creation of clients with a TypeAdapter for both Request and Response */ TEST_F(TestClient, total_type_adaption_client_creation) { - using AdaptedRequestType = rclcpp::TypeAdapter; - using AdaptedResponseType = rclcpp::TypeAdapter; std::shared_ptr node = std::make_shared("my_node"); + { + using AdaptedRequestType = rclcpp::TypeAdapter; + using AdaptedResponseType = rclcpp::TypeAdapter; + struct AdaptedTypeStruct { using Request = AdaptedRequestType; using Response = AdaptedResponseType; @@ -148,8 +149,8 @@ TEST_F(TestClient, total_type_adaption_client_creation) auto client = node->create_client("client"); /// Now try to adapt the type with the `as` metafunction - (void)client; - + } + { using AdaptedRequestType = rclcpp::adapt_type::as; using AdaptedResponseType = rclcpp::adapt_type::as; @@ -160,6 +161,7 @@ TEST_F(TestClient, total_type_adaption_client_creation) auto Asclient = node->create_client("client"); (void)Asclient; + } } TEST_F(TestClient, request_type_adaption_client_creation) diff --git a/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp index 08b9d04aee..c8ac92a5df 100644 --- a/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp @@ -26,10 +26,9 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" -#include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" -#include "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/empty.hpp" #include "rclcpp/msg/string.hpp" #include "rclcpp/msg/bool.hpp" @@ -73,7 +72,7 @@ struct rclcpp::TypeAdapter const ros_message_type & source, custom_type & destination) { - destination = source; + destination = source.data; } }; From 7b7dc9ef07babd39e55295674011c1fa1eee0a78 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Fri, 1 Sep 2023 13:27:10 -0400 Subject: [PATCH 6/8] Changing Style Signed-off-by: CursedRock17 finished_service_side Signed-off-by: CursedRock17 Small Reversion Signed-off-by: CursedRock17 More specialization Signed-off-by: CursedRock17 --- .../include/rclcpp/any_service_callback.hpp | 381 ++++++++++-- rclcpp/include/rclcpp/client.hpp | 569 ++++++++++++++---- .../get_service_type_support_handle.hpp | 73 +-- .../include/rclcpp/is_ros_compatible_type.hpp | 3 +- rclcpp/include/rclcpp/service.hpp | 109 +++- .../node_type_descriptions.cpp | 17 +- rclcpp/test/rclcpp/CMakeLists.txt | 4 +- rclcpp/test/rclcpp/test_client.cpp | 6 +- .../rclcpp/test_client_with_type_adapter.cpp | 344 +++++++---- .../rclcpp/test_service_with_type_adapter.cpp | 300 +++++---- 10 files changed, 1288 insertions(+), 518 deletions(-) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 918d8e5a29..25d74e7fd9 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 29e14e3175..0c1cb98240 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__CLIENT_HPP_ #define RCLCPP__CLIENT_HPP_ +#include + #include #include #include @@ -24,11 +26,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" @@ -46,6 +50,7 @@ #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" @@ -54,6 +59,8 @@ #include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" +#include + namespace rclcpp { @@ -381,47 +388,113 @@ template class Client : public ClientBase { public: - using Request = typename ServiceT::Request; - using Response = typename ServiceT::Response; - - static_assert( - rclcpp::is_ros_compatible_type::value, - "Service Request type is not compatible with ROS 2 and cannot be used with a Client"); - static_assert( - rclcpp::is_ros_compatible_type::value, - "Service Response type is not compatible with ROS 2 and cannot be used with a Client"); + rclcpp::is_ros_compatible_service_type::value, + "Service type is not compatible with ROS 2 and cannot be used with a Client"); - /// ServiceT::Request::custom_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::custom_type::Request if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Request - using ServiceRequestType = typename rclcpp::TypeAdapter::custom_type; - /// ServiceT::Request::ros_message_type if ServiceT is a TypeAdapter, otherwise just the + 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; - /// ServiceT::Response::custom_type if ServiceT is a TypeAdapter, otherwise just the + 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; - /// ServiceT::Response::ros_message_type if ServiceT is a TypeAdapter, otherwise just the + 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; + 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 SharedRequest = typename ROSServiceRequestType::SharedPtr; - using SharedResponse = typename ROSServiceResponseType::SharedPtr; + using SharedFuture [[deprecated("Use ROSSharedFuture instead of SharedFuture")]] = + ROSSharedFuture; - using Promise = std::promise; - using PromiseWithRequest = std::promise>; + using CustomTotalSharedFutureWithRequest = + std::shared_future>; + using ROSTotalSharedFutureWithRequest = + std::shared_future>; + using CustomROSSharedFutureWithRequest = + std::shared_future>; + using ROSCustomSharedFutureWithRequest = + std::shared_future>; - using SharedPromise = std::shared_ptr; - using SharedPromiseWithRequest = std::shared_ptr; + using SharedFutureWithRequest + [[deprecated("Use ROSTotalSharedFutureWithRequest instead of SharedFutureWithRequest")]] = + ROSTotalSharedFutureWithRequest; - using Future = std::future; - using SharedFuture = std::shared_future; - using SharedFutureWithRequest = std::shared_future>; + using CustomCallbackType = std::function; + using ROSCallbackType = std::function; - using CallbackType = std::function; - using CallbackWithRequestType = 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) @@ -429,53 +502,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; + // delegate future like methods in the std::future impl_ + + /// See std::future::share(). + ROSSharedFuture share() noexcept {return this->future.share();} + }; + + struct ROSFutureAndRequestId + : 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 CustomFutureAndRequestId + : 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();} + 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; + }; + + struct ROSSharedFutureAndRequestId + : detail::FutureAndRequestId> { - using detail::FutureAndRequestId>::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< @@ -483,6 +585,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. @@ -572,15 +709,15 @@ class Client : public ClientBase */ template typename std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && + 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_message( - response_out, 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); } @@ -591,7 +728,7 @@ class Client : public ClientBase std::shared_ptr create_response() override { - return std::shared_ptr(new Response()); + return std::shared_ptr(new ROSServiceResponseType()); } /// Create a shared pointer with a rmw_request_id_t @@ -622,26 +759,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)); + } } } @@ -673,15 +850,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. @@ -699,27 +895,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) { - Promise promise; + ROSPromise promise; auto shared_future = promise.get_future().share(); auto req_id = async_send_request_impl( *request, std::make_tuple( - CallbackType{std::forward(cb)}, + ROSCallbackType{std::forward(cb)}, shared_future, std::move(promise))); - return SharedFutureAndRequestId{std::move(shared_future), req_id}; + 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) + { + CustomPromise promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + *request, + std::make_tuple( + CustomCallbackType{std::forward(cb)}, + shared_future, + std::move(promise))); + return CustomSharedFutureAndRequestId{std::move(shared_future), req_id}; } /// Send a request to the service server and schedule a callback in the executor. @@ -730,28 +946,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) + { + ROSTotalPromiseWithRequest promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + *request, + std::make_tuple( + ROSTotalCallbackWithRequestType{std::forward(cb)}, + request, + shared_future, + std::move(promise))); + 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) { - PromiseWithRequest promise; + CustomTotalPromiseWithRequest promise; auto shared_future = promise.get_future().share(); auto req_id = async_send_request_impl( *request, std::make_tuple( - CallbackWithRequestType{std::forward(cb)}, + CustomTotalCallbackWithRequestType{std::forward(cb)}, request, shared_future, std::move(promise))); - return SharedFutureWithRequestAndRequestId{std::move(shared_future), req_id}; + return CustomTotalSharedFutureWithRequestAndRequestId{std::move(shared_future), req_id}; } /// Cleanup a pending request. @@ -765,8 +1002,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; @@ -778,8 +1016,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); } @@ -790,8 +1037,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); } @@ -802,8 +1058,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); } @@ -877,14 +1160,74 @@ 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 {}; + + 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>; + }; + + template<> + struct CallbackInfoVariantHelper + { + using CallbackInfoVariant = std::variant< + ROSPromise, + CustomPromise, + CallbackTypeValueVariant, + CallbackWithRequestTypeValueVariant>; + }; - using CallbackInfoVariant = std::variant< - std::promise, - CallbackTypeValueVariant, - CallbackWithRequestTypeValueVariant>; + using CallbackInfoVariant = typename CallbackInfoVariantHelper<>::CallbackInfoVariant; template typename std::enable_if_t< @@ -908,7 +1251,7 @@ class Client : public ClientBase template typename std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && + rclcpp::TypeAdapter::is_specialized::value && std::is_same::value, int64_t > @@ -918,11 +1261,11 @@ class Client : public ClientBase std::lock_guard lock(pending_requests_mutex_); ROSServiceRequestType ros_service_request; - rclcpp::TypeAdapter::convert_to_ros_message( - request, 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); + 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"); } diff --git a/rclcpp/include/rclcpp/get_service_type_support_handle.hpp b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp index 3b219edd0e..c40a72fc9a 100644 --- a/rclcpp/include/rclcpp/get_service_type_support_handle.hpp +++ b/rclcpp/include/rclcpp/get_service_type_support_handle.hpp @@ -23,6 +23,7 @@ #include "rosidl_typesupport_cpp/service_type_support.hpp" #include "rclcpp/type_adapter.hpp" +#include "type_adapter.hpp" namespace rclcpp { @@ -53,80 +54,19 @@ get_service_type_support_handle() return *handle; } -/// The following checks will fix for the 4 different ways you could assemble -/// the TypeAdapter struct when using custom or ROS types. +/// 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 && - rclcpp::TypeAdapter::is_specialized::value, + rclcpp::TypeAdapter::is_specialized::value, const rosidl_service_type_support_t & > get_service_type_support_handle() { - struct AdaptedTypeStructTemp - { - using Request = - typename rclcpp::TypeAdapter::ros_message_type; - using Response = - typename rclcpp::TypeAdapter::ros_message_type; - }; + using CustomType = typename TypeAdapter::ros_message_type; + auto handle = rosidl_typesupport_cpp::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; -} - -template -constexpr -std::enable_if_t< - !rosidl_generator_traits::is_service::value && - !rclcpp::TypeAdapter::is_specialized::value && - rclcpp::TypeAdapter::is_specialized::value, - const rosidl_service_type_support_t & -> -get_service_type_support_handle() -{ - struct AdaptedTypeStructTemp - { - using Request = - typename rclcpp::TypeAdapter::custom_type; - using Response = - typename rclcpp::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; -} - -template -constexpr -std::enable_if_t< - !rosidl_generator_traits::is_service::value && - rclcpp::TypeAdapter::is_specialized::value && - !rclcpp::TypeAdapter::is_specialized::value, - const rosidl_service_type_support_t & -> -get_service_type_support_handle() -{ - struct AdaptedTypeStructTemp - { - using Request = - typename rclcpp::TypeAdapter::ros_message_type; - using Response = - typename rclcpp::TypeAdapter::custom_type; - }; - - auto handle = rosidl_typesupport_cpp::get_service_type_support_handle(); if (!handle) { throw std::runtime_error("Type support handle unexpectedly nullptr"); } @@ -140,8 +80,7 @@ template constexpr typename std::enable_if_t< !rosidl_generator_traits::is_service::value && - !TypeAdapter::is_specialized::value && - !TypeAdapter::is_specialized::value, + !TypeAdapter::is_specialized::value, const rosidl_service_type_support_t & > get_service_type_support_handle() diff --git a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp index db58bbeb81..ff6b1588c0 100644 --- a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp +++ b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp @@ -35,8 +35,7 @@ struct is_ros_compatible_service_type { static constexpr bool value = rosidl_generator_traits::is_service::value || - rclcpp::TypeAdapter::is_specialized::value || - rclcpp::TypeAdapter::is_specialized::value; + rclcpp::TypeAdapter::is_specialized::value; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 9e468f7983..0313e7857d 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -311,40 +311,83 @@ class Service { public: static_assert( - rclcpp::is_ros_compatible_type::value, - "Service Request type is not compatible with ROS 2 and cannot be used with a Service"); + rclcpp::is_ros_compatible_service_type::value, + "Service type is not compatible with ROS 2 and cannot be used with a Service"); - static_assert( - rclcpp::is_ros_compatible_type::value, - "Service Response type is not compatible with ROS 2 and cannot be used with a Service"); - - /// ServiceT::Request::custom_type if ServiceT is a TypeAdapter, otherwise just the + /// ServiceT::custom_type::Request if ServiceT is a TypeAdapter, otherwise just the /// ServiceT::Request using ServiceRequestType = - typename rclcpp::TypeAdapter::custom_type; - /// ServiceT::Request::ros_message_type if ServiceT is a TypeAdapter, otherwise just the + 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; - /// ServiceT::Response::custom_type if ServiceT is a TypeAdapter, otherwise just the + 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; - /// ServiceT::Response::ros_message_type if ServiceT is a TypeAdapter, otherwise just the + 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; + 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 CallbackType = std::function< + 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. @@ -506,7 +549,8 @@ class Service template typename std::enable_if_t< rosidl_generator_traits::is_message::value && - std::is_same::value + std::is_same::value, + bool > take_request(T & request_out, rmw_request_id_t & request_id_out) { @@ -530,21 +574,22 @@ class Service */ template typename std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && - std::is_same::value + 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_message( - request_out, 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(); + return std::make_shared(); } std::shared_ptr @@ -558,7 +603,7 @@ class Service std::shared_ptr request_header, std::shared_ptr request) override { - auto typed_request = std::static_pointer_cast(request); + 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); @@ -576,7 +621,7 @@ class Service */ template std::enable_if_t< - rosidl_generator_traits::is_message::value && + rosidl_generator_traits::is_message::value && std::is_same::value > send_response(rmw_request_id_t & req_id, T & response) @@ -607,16 +652,16 @@ class Service */ template std::enable_if_t< - rclcpp::TypeAdapter::is_specialized::value && + 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_message( - response, 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); + 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"); 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/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d034fc5bff..9a3c33ba65 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -91,7 +91,7 @@ ament_add_gtest(test_client_with_type_adapter test_client_with_type_adapter.cpp if(TARGET test_client_with_type_adapter) target_link_libraries(test_client_with_type_adapter ${PROJECT_NAME} - ${cpp_typesupport_target}) + ${cpp_typesupport_target} mimick) endif() ament_add_gtest(test_create_timer test_create_timer.cpp) if(TARGET test_create_timer) @@ -549,7 +549,7 @@ ament_add_gtest(test_service_with_type_adapter test_service_with_type_adapter.cp if(TARGET test_service_with_type_adapter) target_link_libraries(test_service_with_type_adapter ${PROJECT_NAME} - ${cpp_typesupport_target}) + ${cpp_typesupport_target} mimick) endif() # Creating and destroying nodes is slow with Connext, so this needs larger timeout. 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 index 748517f445..2034f33f94 100644 --- a/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_client_with_type_adapter.cpp @@ -26,6 +26,7 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" #include "rclcpp/msg/empty.hpp" @@ -36,83 +37,84 @@ using namespace std::chrono_literals; -class TestClient: public ::testing::Test +struct CustomBool { -public: - static void SetUpTestCase() + struct SetBoolResponse { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } + bool success; + std::string message; + }; - static void TearDownTestCase() - { - rclcpp::shutdown(); - } + 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 +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; - using custom_type = bool; - using ros_message_type = rclcpp::msg::Bool; + using custom_type = CustomBool; + using ros_message_type = rclcpp::srv::SetBool; static void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) + convert_to_ros_service_request( + const custom_type::Request & source, + ros_message_type::Request & destination) { destination.data = source; } static void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) + convert_to_custom_service_request( + const ros_message_type::Request & source, + custom_type::Request & destination) { destination = source.data; } -}; - -template<> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = std::string; - using ros_message_type = rclcpp::msg::String; static void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) + convert_to_ros_service_response( + const custom_type::Response & source, + ros_message_type::Response & destination) { - destination.data = source; + destination.success = source.success; + destination.message = source.message; } static void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) + convert_to_custom_service_response( + const ros_message_type::Response & source, + custom_type::Response & destination) { - destination = source.data; + destination.success = source.success; + destination.message = source.message; } }; // Throws in conversion template<> -struct rclcpp::TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; - using custom_type = int; - using ros_message_type = rclcpp::msg::String; + using custom_type = CustomIncorrectBool; + using ros_message_type = rclcpp::srv::SetBool; static void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) + convert_to_ros_service_request( + const custom_type::Request & source, + ros_message_type::Request & destination) { (void) source; (void) destination; @@ -120,116 +122,228 @@ struct rclcpp::TypeAdapter } static void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) + 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; } }; -/* - * Testing the basic creation of clients with a TypeAdapter for both Request and Response - */ -TEST_F(TestClient, total_type_adaption_client_creation) +class TestClient : public ::testing::Test { +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } - std::shared_ptr node = std::make_shared("my_node"); + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + void SetUp() { - using AdaptedRequestType = rclcpp::TypeAdapter; - using AdaptedResponseType = rclcpp::TypeAdapter; + node = std::make_shared("my_node", "/ns"); + } - struct AdaptedTypeStruct { - using Request = AdaptedRequestType; - using Response = AdaptedResponseType; - }; + void TearDown() + { + node.reset(); + } - auto client = node->create_client("client"); + rclcpp::Node::SharedPtr node; +}; - /// Now try to adapt the type with the `as` metafunction - } +class TestClientWithServer : public ::testing::Test +{ +protected: + static void SetUpTestCase() { - using AdaptedRequestType = rclcpp::adapt_type::as; - using AdaptedResponseType = rclcpp::adapt_type::as; - - struct AdaptedTypeAsStruct { - using Request = AdaptedRequestType; - using Response = AdaptedResponseType; - }; + rclcpp::init(0, nullptr); + } - auto Asclient = node->create_client("client"); - (void)Asclient; + static void TearDownTestCase() + { + rclcpp::shutdown(); } -} -TEST_F(TestClient, request_type_adaption_client_creation) -{ - std::shared_ptr node = std::make_shared("my_node"); + void SetUp() + { + node = std::make_shared("node", "ns"); - using AdaptedRequestType = rclcpp::TypeAdapter; + auto callback = []( + const std::shared_ptr, + const std::shared_ptr) {}; - struct AdaptedTypeStruct { - using Request = AdaptedRequestType; - using Response = rclcpp::srv::SetBool::Response; - }; + service = node->create_service(service_name, std::move(callback)); + } - auto client = node->create_client("client"); + ::testing::AssertionResult SendBoolRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + using SharedFuture = rclcpp::Client::CustomSharedFuture; - /// Now try to adapt the type with the `as` metafunction - (void)client; + auto client = node->create_client(service_name); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Waiting for service failed"; + } - using AdaptedRequestType = rclcpp::adapt_type::as; + 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); + } - struct AdaptedTypeAsStruct { - using Request = AdaptedRequestType; - using Response = rclcpp::srv::SetBool::Response; - }; + 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"; + } - auto Asclient = node->create_client("client"); - (void)Asclient; -} + return request_result; + } + + using AdaptedTypeStruct = rclcpp::TypeAdapter; + std::shared_ptr node; + std::shared_ptr> service; + const std::string service_name{"empty_service"}; +}; -TEST_F(TestClient, response_type_adaption_client_creation) +/* + * Testing the basic creation of clients with a TypeAdapter for both Request and Response + */ +TEST_F(TestClient, various_creation_signatures) { - std::shared_ptr node = std::make_shared("my_node"); + { + using AdaptedTypeStruct = rclcpp::TypeAdapter; + auto client = node->create_client("client"); - using AdaptedResponseType = rclcpp::TypeAdapter; + (void)client; + } + { + /// Now try to adapt the type with the `as` metafunction + using AdaptedTypeStruct = rclcpp::adapt_type::as; - struct AdaptedTypeStruct { - using Request = rclcpp::msg::String; - using Response = AdaptedResponseType; - }; + auto client = node->create_client("client"); + (void)client; + } +} - auto client = node->create_client("client"); +/// Testing that conversion errors are passed up +TEST_F(TestClient, conversion_exception_is_passed_up) +{ + using BadAdaptedTypeStruct = rclcpp::TypeAdapter; - /// Now try to adapt the type with the `as` metafunction - (void)client; + auto client = node->create_client("client"); +} - using AdaptedResponseType = rclcpp::adapt_type::as; +TEST_F(TestClientWithServer, test_adapted_client_remove_pending_request) { + auto client = node->create_client("no_service_server_available_here"); - struct AdaptedTypeAsStruct { - using Request = rclcpp::msg::String; - using Response = AdaptedResponseType; - }; + auto request = std::make_shared(); + auto future = client->async_send_request(request); - auto Asclient = node->create_client("client"); - (void)Asclient; + EXPECT_TRUE(client->remove_pending_request(future)); } -/// Testing that conversion errors are passed up -TEST_F(TestClient, conversion_exception_is_passed_up) +TEST_F(TestClientWithServer, take_adapted_response) { - std::shared_ptr node = std::make_shared("my_node"); + 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; - using BadAdaptedResponseType = rclcpp::TypeAdapter; + client->async_send_request(request); + EXPECT_FALSE(client->take_response(response, *request_header.get())); - struct BadAdaptedTypeStruct { - using Request = rclcpp::msg::String; - using Response = BadAdaptedResponseType; - }; + { + // 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); + } +} - auto client = node->create_client("client"); - (void)client; +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 index c8ac92a5df..03c79865bb 100644 --- a/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_service_with_type_adapter.cpp @@ -26,6 +26,7 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" #include "rclcpp/msg/empty.hpp" @@ -36,7 +37,7 @@ using namespace std::chrono_literals; -class TestService: public ::testing::Test +class TestService : public ::testing::Test { public: static void SetUpTestCase() @@ -52,66 +53,84 @@ class TestService: public ::testing::Test } }; +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 +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; - using custom_type = bool; - using ros_message_type = rclcpp::msg::Bool; + using custom_type = CustomBool; + using ros_message_type = rclcpp::srv::SetBool; static void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) + convert_to_ros_service_request( + const custom_type::Request & source, + ros_message_type::Request & destination) { destination.data = source; } static void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) + convert_to_custom_service_request( + const ros_message_type::Request & source, + custom_type::Request & destination) { destination = source.data; } -}; - -template<> -struct rclcpp::TypeAdapter -{ - using is_specialized = std::true_type; - using custom_type = std::string; - using ros_message_type = rclcpp::msg::String; static void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) + convert_to_ros_service_response( + const custom_type::Response & source, + ros_message_type::Response & destination) { - destination.data = source; + destination.success = source.success; + destination.message = source.message; } static void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) + convert_to_custom_service_response( + const ros_message_type::Response & source, + custom_type::Response & destination) { - destination = source.data; + destination.success = source.success; + destination.message = source.message; } }; // Throws in conversion template<> -struct rclcpp::TypeAdapter +struct rclcpp::TypeAdapter { using is_specialized = std::true_type; - using custom_type = int; - using ros_message_type = rclcpp::msg::String; + using custom_type = CustomIncorrectBool; + using ros_message_type = rclcpp::srv::SetBool; static void - convert_to_ros_message( - const custom_type & source, - ros_message_type & destination) + convert_to_ros_service_request( + const custom_type::Request & source, + ros_message_type::Request & destination) { (void) source; (void) destination; @@ -119,126 +138,157 @@ struct rclcpp::TypeAdapter } static void - convert_to_custom( - const ros_message_type & source, - custom_type & destination) + convert_to_custom_service_request( + const ros_message_type::Request & source, + custom_type::Request & destination) { (void) source; (void) destination; } -}; -/* - * Testing the basic creation of services with a TypeAdapter for both Request and Response - */ -TEST_F(TestService, total_type_adaption_service_creation) -{ - std::shared_ptr node = std::make_shared("my_node"); - - using AdaptedRequestType = rclcpp::TypeAdapter>; - using AdaptedResponseType = rclcpp::TypeAdapter>; - - struct AdaptedTypeStruct { - using Request = AdaptedRequestType; - using Response = AdaptedResponseType; - }; - - auto service = rclcpp::create_service( - "service", - [](const std::string & req, const bool & res) {}); - - /// Now try to adapt the type with the `as` metafunction - (void)service; - - using AdaptedRequestType = rclcpp::adapt_type::as; - using AdaptedResponseType = rclcpp::adapt_type::as; + 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"); + } - struct AdaptedTypeStruct { - using Request = AdaptedRequestType; - using Response = AdaptedResponseType; - }; + static void + convert_to_custom_service_response( + const ros_message_type::Response & source, + custom_type::Response & destination) + { + (void) source; + (void) destination; + } +}; - auto service = rclcpp::create_service( - "service", - [](const std::string & req, const bool & res) {}); - (void)service; +void custom_total_callback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + (void)res; } -TEST_F(TestService, request_type_adaption_service_creation) +void custom_ros_callback( + const std::shared_ptr req, + const std::shared_ptr res) { - std::shared_ptr node = std::make_shared("my_node"); + (void)req; + (void)res; +} - using AdaptedRequestType = rclcpp::TypeAdapter>; +void ros_custom_callback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + (void)res; +} - struct AdaptedTypeStruct { - using Request = AdaptedRequestType; - using Response = rclcpp::srv::SetBool::Response; - }; +void incorrect_callback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + (void)res; +} - auto service = rclcpp::create_service( - "service", - [](const std::string & req, rclcpp::srv::SetBool::Response & 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"); - /// Now try to adapt the type with the `as` metafunction - (void)service; + 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 AdaptedRequestType = rclcpp::adapt_type::as; + 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"); - struct AdaptedTypeStruct { - using Request = AdaptedRequestType; - using Response = rclcpp::srv::SetBool::Response; - }; + using AdaptedTypeStruct = rclcpp::TypeAdapter; + auto service = node->create_service("service", &custom_ros_callback); + (void)service; + } + { + std::shared_ptr node = std::make_shared("my_node"); - auto service = rclcpp::create_service( - "service", - [](const std::string & req, rclcpp::srv::SetBool::Response & res) {}); - (void)service; + using AdaptedTypeStruct = rclcpp::TypeAdapter; + auto service = node->create_service("service", &ros_custom_callback); + (void)service; + } } -TEST_F(TestService, response_type_adaption_service_creation) +/// 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 AdaptedResponseType = rclcpp::TypeAdapter>; + using BadAdaptedTypeStruct = rclcpp::TypeAdapter; - struct AdaptedTypeStruct { - using Request = rclcpp::msg::String; - using Response = AdaptedResponseType; - }; - - auto service = rclcpp::create_service( - "service", - [](const rclcpp::msg::String & req, const bool & res) {}); - - /// Now try to adapt the type with the `as` metafunction - (void)service; - - using AdaptedResponseType = rclcpp::adapt_type::as; - - struct AdaptedTypeStruct { - using Request = rclcpp::msg::String; - using Response = AdaptedResponseType; - }; - - auto service = rclcpp::create_service( - "service", - [](const rclcpp::msg::String & req, const bool & res) {}); - (void)service; + auto service = node->create_service("service", &incorrect_callback); } -/// Testing that conversion errors are passed up -TEST_F(TestService, conversion_exception_is_passed_up) -{ +TEST_F(TestService, send_adapted_response) { std::shared_ptr node = std::make_shared("my_node"); + using AdaptedTypeStruct = rclcpp::TypeAdapter; - using BadAdaptedResponseType = 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); + } +} - struct AdaptedTypeStruct { - using Request = rclcpp::msg::String; - using Response = BadAdaptedResponseType; - }; +TEST_F(TestService, take_adapted_request) { + std::shared_ptr node = std::make_shared("my_node"); + using AdaptedTypeStruct = rclcpp::TypeAdapter; - auto service = rclcpp::create_service( - "service", - [](const rclcpp::msg::String & req) {}); + 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); + } } From 34d8989822f8db84eada03b1a74a7ba14b8ab118 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 24 Sep 2023 21:06:07 -0400 Subject: [PATCH 7/8] removing a header Signed-off-by: CursedRock17 --- rclcpp/include/rclcpp/client.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 0c1cb98240..0f309bac67 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -15,8 +15,6 @@ #ifndef RCLCPP__CLIENT_HPP_ #define RCLCPP__CLIENT_HPP_ -#include - #include #include #include From 50ec0073620fb9b8cc3493425c7f2cb8a46e0e14 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 12 Nov 2023 02:43:50 -0500 Subject: [PATCH 8/8] Fixing Callbacks Signed-off-by: CursedRock17 --- .../include/rclcpp/any_service_callback.hpp | 6 +- rclcpp/include/rclcpp/service.hpp | 99 ++++++++++++++++++- 2 files changed, 98 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 25d74e7fd9..7ad7af0694 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -457,13 +457,13 @@ class AnyServiceCallback return nullptr; } // auto response = allocate_shared(); - auto response = std::make_shared(); + 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_); + const auto & cb = std::get(callback_); cb(request_header, std::move(request), response); } TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 0313e7857d..1912437b6f 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -589,6 +589,38 @@ class Service std::shared_ptr create_request() override { + 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(); } @@ -603,10 +635,69 @@ 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); + } } }