diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 333ceee..7016098 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -105,17 +105,25 @@ public: template typename rclcpp::client::Client::SharedPtr create_client( - std::string service_name, + const std::string & service_name, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); /* Create and return a Service. */ template typename rclcpp::service::Service::SharedPtr create_service( - std::string service_name, + const std::string & service_name, typename rclcpp::service::Service::CallbackType callback, 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::CallbackWithHeaderType callback_with_header, + rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + private: RCLCPP_DISABLE_COPY(Node); @@ -136,6 +144,10 @@ private: size_t number_of_services_; size_t number_of_clients_; + void register_service( + const std::string & service_name, + std::shared_ptr serv_base_ptr, + rclcpp::callback_group::CallbackGroup::SharedPtr group); }; } /* namespace node */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index f7a8d91..d64da26 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -151,7 +151,7 @@ Node::create_wall_timer( template typename client::Client::SharedPtr Node::create_client( - std::string service_name, + const std::string & service_name, rclcpp::callback_group::CallbackGroup::SharedPtr group) { using rosidl_generator_cpp::get_service_type_support_handle; @@ -185,7 +185,7 @@ Node::create_client( template typename service::Service::SharedPtr Node::create_service( - std::string service_name, + const std::string &service_name, typename rclcpp::service::Service::CallbackType callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { @@ -196,14 +196,44 @@ Node::create_service( rmw_service_t * service_handle = rmw_create_service( this->node_handle_, service_type_support_handle, service_name.c_str()); - using namespace rclcpp::service; - - auto serv = Service::make_shared( + auto serv = service::Service::make_shared( service_handle, service_name, callback); - auto serv_base_ptr = std::dynamic_pointer_cast(serv); + 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 @@ -214,8 +244,6 @@ Node::create_service( default_callback_group_->add_service(serv_base_ptr); } number_of_services_++; - - return serv; } #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 522fb81..46b3294 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -85,17 +85,29 @@ template class Service : public ServiceBase { public: + typedef std::function< + void (const std::shared_ptr &, + std::shared_ptr &)> CallbackType; + typedef std::function< void (const std::shared_ptr &, const std::shared_ptr &, - std::shared_ptr &)> CallbackType; + std::shared_ptr &)> CallbackWithHeaderType; RCLCPP_MAKE_SHARED_DEFINITIONS(Service); Service( rmw_service_t * service_handle, const std::string & service_name, CallbackType callback) - : ServiceBase(service_handle, service_name), callback_(callback) + : ServiceBase(service_handle, service_name), callback_(callback), callback_with_header_(nullptr) + {} + + Service( + rmw_service_t * service_handle, + const std::string & service_name, + CallbackWithHeaderType callback_with_header) + : ServiceBase(service_handle, service_name), callback_(nullptr), + callback_with_header_(callback_with_header) {} std::shared_ptr create_request() @@ -115,7 +127,11 @@ public: auto typed_request = std::static_pointer_cast(request); auto typed_request_header = std::static_pointer_cast(request_header); auto response = std::shared_ptr(new typename ServiceT::Response); - callback_(typed_request_header, typed_request, response); + if (callback_with_header_ != nullptr) { + callback_with_header_(typed_request_header, typed_request, response); + } else { + callback_(typed_request, response); + } send_response(typed_request_header, response); } @@ -130,6 +146,7 @@ private: RCLCPP_DISABLE_COPY(Service); CallbackType callback_; + CallbackWithHeaderType callback_with_header_; }; } /* namespace service */