diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 3dea295d74..701609843a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -105,17 +105,23 @@ class Node 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, - std::function &, - std::shared_ptr &)> callback, + 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: @@ -138,6 +144,10 @@ class Node 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 98d404ba26..363d5e7729 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,9 +185,8 @@ Node::create_client( template typename service::Service::SharedPtr Node::create_service( - std::string service_name, - std::function &, - std::shared_ptr &)> callback, + const std::string & service_name, + typename rclcpp::service::Service::CallbackType callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { using rosidl_generator_cpp::get_service_type_support_handle; @@ -197,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 @@ -215,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 ad1376c64c..ed345a3c25 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -86,15 +86,30 @@ class Service : public ServiceBase { public: typedef std::function< - void (const std::shared_ptr &, - std::shared_ptr &)> CallbackType; + void ( + const std::shared_ptr &, + std::shared_ptr &)> CallbackType; + + typedef std::function< + void ( + const std::shared_ptr &, + const std::shared_ptr &, + 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() @@ -114,7 +129,11 @@ class Service : public ServiceBase 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, 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); } @@ -129,6 +148,7 @@ class Service : public ServiceBase RCLCPP_DISABLE_COPY(Service); CallbackType callback_; + CallbackWithHeaderType callback_with_header_; }; } /* namespace service */