niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge pull request #22 from ros2/request-header

Browse source 
Pass request header to callbacks
This commit is contained in:
Esteve Fernandez 2015-04-22 14:05:29 -07:00
commit 1bf595dfed
3 changed files with 76 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

20
rclcpp/include/rclcpp/node.hpp
View file

@ -105,17 +105,23 @@ public:
template<typename ServiceT>
typename rclcpp::client::Client<ServiceT>::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 ServiceT>
typename rclcpp::service::Service<ServiceT>::SharedPtr
create_service(
std::string service_name,
std::function<void(
const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response> &)> callback,
const std::string & service_name,
typename rclcpp::service::Service<ServiceT>::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
/* Create and return a Service. */
template<typename ServiceT>
typename rclcpp::service::Service<ServiceT>::SharedPtr
create_service(
const std::string & service_name,
typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header,
rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
private:
@ -138,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<rclcpp::service::ServiceBase> serv_base_ptr,
rclcpp::callback_group::CallbackGroup::SharedPtr group);
};
} /* namespace node */

47
rclcpp/include/rclcpp/node_impl.hpp
View file

@ -151,7 +151,7 @@ Node::create_wall_timer(
template<typename ServiceT>
typename client::Client<ServiceT>::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 ServiceT>
typename service::Service<ServiceT>::SharedPtr
Node::create_service(
std::string service_name,
std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response> &)> callback,
const std::string & service_name,
typename rclcpp::service::Service<ServiceT>::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<ServiceT>::make_shared(
auto serv = service::Service<ServiceT>::make_shared(
service_handle,
service_name,
callback);
auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
register_service(service_name, serv_base_ptr, group);
return serv;
}
template<typename ServiceT>
typename service::Service<ServiceT>::SharedPtr
Node::create_service(
const std::string & service_name,
typename rclcpp::service::Service<ServiceT>::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<ServiceT>();
rmw_service_t * service_handle = rmw_create_service(
this->node_handle_, service_type_support_handle, service_name.c_str());
auto serv = service::Service<ServiceT>::make_shared(
service_handle,
service_name,
callback_with_header);
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
register_service(service_name, serv_base_ptr, group);
return serv;
}
void
Node::register_service(
const std::string & service_name,
std::shared_ptr<rclcpp::service::ServiceBase> 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_ */

28
rclcpp/include/rclcpp/service.hpp
View file

@ -86,15 +86,30 @@ class Service : public ServiceBase
{
public:
typedef std::function<
void (const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response> &)> CallbackType;
void (
const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response> &)> CallbackType;
typedef std::function<
void (
const std::shared_ptr<rmw_request_id_t> &,
const std::shared_ptr<typename ServiceT::Request> &,
std::shared_ptr<typename ServiceT::Response> &)> 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<void> create_request()
@ -114,7 +129,11 @@ public:
auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
auto response = std::shared_ptr<typename ServiceT::Response>(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 @@ private:
RCLCPP_DISABLE_COPY(Service);
CallbackType callback_;
CallbackWithHeaderType callback_with_header_;
};
} /* namespace service */