diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index b58f0b4..9231066 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -158,6 +158,26 @@ private: const std::string & service_name, std::shared_ptr serv_base_ptr, rclcpp::callback_group::CallbackGroup::SharedPtr group); + + template + typename function_arity< + FunctorT, + 2, + typename rclcpp::service::Service::SharedPtr>::type + create_service_internal( + rmw_service_t * service_handle, + const std::string & service_name, + FunctorT callback); + + template + typename function_arity< + FunctorT, + 3, + typename rclcpp::service::Service::SharedPtr>::type + create_service_internal( + rmw_service_t * service_handle, + const std::string & service_name, + FunctorT callback); }; } /* namespace node */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 795dc0a..1d87b83 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 +template typename rclcpp::service::Service::SharedPtr Node::create_service( const std::string & service_name, - F callback, + FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { using rosidl_generator_cpp::get_service_type_support_handle; @@ -196,21 +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; -} - -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 @@ -221,6 +209,38 @@ Node::register_service( default_callback_group_->add_service(serv_base_ptr); } number_of_services_++; + return serv; +} + +template +typename function_arity< + FunctorT, + 2, + typename rclcpp::service::Service::SharedPtr>::type +Node::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 function_arity< + FunctorT, + 3, + typename rclcpp::service::Service::SharedPtr>::type +Node::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); } #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */