From b938bd691a8c84856ea1c32ea3b3d79ec3e08fe8 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 20 Apr 2015 13:22:54 -0700 Subject: [PATCH 1/4] Pass request header to callbacks --- rclcpp/include/rclcpp/node.hpp | 4 +--- rclcpp/include/rclcpp/node_impl.hpp | 3 +-- rclcpp/include/rclcpp/service.hpp | 7 ++++--- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 3dea295d74..333ceeed33 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -113,9 +113,7 @@ class Node typename rclcpp::service::Service::SharedPtr create_service( std::string service_name, - std::function &, - std::shared_ptr &)> callback, + typename rclcpp::service::Service::CallbackType callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); private: diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 98d404ba26..f7a8d9192d 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -186,8 +186,7 @@ template typename service::Service::SharedPtr Node::create_service( std::string service_name, - std::function &, - std::shared_ptr &)> callback, + typename rclcpp::service::Service::CallbackType callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { using rosidl_generator_cpp::get_service_type_support_handle; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index ad1376c64c..522fb81dd8 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -86,8 +86,9 @@ class Service : public ServiceBase { public: typedef std::function< - void (const std::shared_ptr &, - std::shared_ptr &)> CallbackType; + void (const std::shared_ptr &, + const std::shared_ptr &, + std::shared_ptr &)> CallbackType; RCLCPP_MAKE_SHARED_DEFINITIONS(Service); Service( @@ -114,7 +115,7 @@ 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); + callback_(typed_request_header, typed_request, response); send_response(typed_request_header, response); } From 8abae8ac829946620c9defd983e1f947839b041a Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 20 Apr 2015 15:45:01 -0700 Subject: [PATCH 2/4] Added support for callbacks with and without accepting a request header --- rclcpp/include/rclcpp/node.hpp | 16 +++++++++-- rclcpp/include/rclcpp/node_impl.hpp | 44 +++++++++++++++++++++++------ rclcpp/include/rclcpp/service.hpp | 23 +++++++++++++-- 3 files changed, 70 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 333ceeed33..701609843a 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -105,17 +105,25 @@ 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, + 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 @@ 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 f7a8d9192d..d64da26506 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 522fb81dd8..46b3294892 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 @@ 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_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 @@ class Service : public ServiceBase RCLCPP_DISABLE_COPY(Service); CallbackType callback_; + CallbackWithHeaderType callback_with_header_; }; } /* namespace service */ From 35b2c106a12d3efaf16b5796e9f5e3ebf4a5bae5 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 20 Apr 2015 15:45:58 -0700 Subject: [PATCH 3/4] Fix code style --- rclcpp/include/rclcpp/node_impl.hpp | 2 +- rclcpp/include/rclcpp/service.hpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d64da26506..363d5e7729 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -185,7 +185,7 @@ Node::create_client( template typename service::Service::SharedPtr Node::create_service( - const std::string &service_name, + const std::string & service_name, typename rclcpp::service::Service::CallbackType callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 46b3294892..c91c788d89 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -87,12 +87,12 @@ class Service : public ServiceBase public: typedef std::function< void (const std::shared_ptr &, - std::shared_ptr &)> CallbackType; + std::shared_ptr &)> CallbackType; typedef std::function< void (const std::shared_ptr &, - const std::shared_ptr &, - std::shared_ptr &)> CallbackWithHeaderType; + const std::shared_ptr &, + std::shared_ptr &)> CallbackWithHeaderType; RCLCPP_MAKE_SHARED_DEFINITIONS(Service); Service( From c3791fcab1739115283c34adbe269eba6ba9b35b Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 22 Apr 2015 12:28:30 -0700 Subject: [PATCH 4/4] Indent --- rclcpp/include/rclcpp/service.hpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index c91c788d89..ed345a3c25 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -86,13 +86,15 @@ 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; + void ( + const std::shared_ptr &, + const std::shared_ptr &, + std::shared_ptr &)> CallbackWithHeaderType; RCLCPP_MAKE_SHARED_DEFINITIONS(Service); Service(