diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 701609843a..746ad5c7b2 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,23 @@ class Executor; namespace node { +// TODO: add support for functors, std::function, lambdas and object members +template +struct function_traits; + +template +struct function_traits +{ + static constexpr std::size_t arity = sizeof ... (Args); + + template + using argument_type = typename std::tuple_element>::type; +}; + +template +struct function_traits: public function_traits +{}; + /* ROS Node Interface. * * This is the single point of entry for creating publishers and subscribers. @@ -109,19 +127,11 @@ class Node rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ - template - typename rclcpp::service::Service::SharedPtr - create_service( - const std::string & service_name, - typename rclcpp::service::Service::CallbackType callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - - /* Create and return a Service. */ - template + template typename rclcpp::service::Service::SharedPtr create_service( const std::string & service_name, - typename rclcpp::service::Service::CallbackWithHeaderType callback_with_header, + FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); private: @@ -148,6 +158,56 @@ class Node const std::string & service_name, std::shared_ptr serv_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); + + template< + typename ServiceT, + typename FunctorT, + typename std::enable_if< + function_traits::arity == 2 && + std::is_same< + typename function_traits::template argument_type<0>, + typename std::shared_ptr + >::value && + std::is_same< + typename function_traits::template argument_type<1>, + typename std::shared_ptr + >::value>::type * = nullptr> + typename rclcpp::service::Service::SharedPtr + create_service_internal( + rmw_service_t * service_handle, + const std::string & service_name, + FunctorT callback) + { + typename rclcpp::service::Service::CallbackType callback_without_header = + callback; + return service::Service::make_shared( + service_handle, service_name, callback_without_header); + } + + template< + typename ServiceT, + typename FunctorT, + typename std::enable_if< + function_traits::arity == 3 && + std::is_same< + typename function_traits::template argument_type<0>, + std::shared_ptr + >::value && + std::is_same< + typename function_traits::template argument_type<1>, + typename std::shared_ptr + >::value>::type * = nullptr> + typename rclcpp::service::Service::SharedPtr + create_service_internal( + rmw_service_t * service_handle, + const std::string & service_name, + FunctorT callback) + { + typename rclcpp::service::Service::CallbackWithHeaderType callback_with_header = + callback; + return service::Service::make_shared( + service_handle, service_name, callback_with_header); + } }; } /* namespace node */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 363d5e7729..cb9a414add 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -182,11 +182,11 @@ Node::create_client( return cli; } -template -typename service::Service::SharedPtr +template +typename rclcpp::service::Service::SharedPtr Node::create_service( const std::string & service_name, - typename rclcpp::service::Service::CallbackType callback, + FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { using rosidl_generator_cpp::get_service_type_support_handle; @@ -196,44 +196,9 @@ Node::create_service( rmw_service_t * service_handle = rmw_create_service( this->node_handle_, service_type_support_handle, service_name.c_str()); - auto serv = service::Service::make_shared( - service_handle, - service_name, - callback); + auto serv = create_service_internal( + service_handle, service_name, callback); auto serv_base_ptr = std::dynamic_pointer_cast(serv); - register_service(service_name, serv_base_ptr, group); - return serv; -} - -template -typename service::Service::SharedPtr -Node::create_service( - const std::string & service_name, - typename rclcpp::service::Service::CallbackWithHeaderType callback_with_header, - rclcpp::callback_group::CallbackGroup::SharedPtr group) -{ - using rosidl_generator_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); - - rmw_service_t * service_handle = rmw_create_service( - this->node_handle_, service_type_support_handle, service_name.c_str()); - - auto serv = service::Service::make_shared( - service_handle, - service_name, - callback_with_header); - auto serv_base_ptr = std::dynamic_pointer_cast(serv); - register_service(service_name, serv_base_ptr, group); - return serv; -} - -void -Node::register_service( - const std::string & service_name, - std::shared_ptr serv_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) -{ if (group) { if (!group_in_node(group)) { // TODO: use custom exception @@ -244,6 +209,7 @@ Node::register_service( default_callback_group_->add_service(serv_base_ptr); } number_of_services_++; + return serv; } #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */