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

Merge pull request #11 from ros2/services-post-api-review

Browse source 
Add support for services in rclcpp
This commit is contained in:
Esteve Fernandez 2015-03-09 12:01:30 -07:00
commit 3dae09f243
4 changed files with 34 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

9
rclcpp/include/rclcpp/client.hpp
View file

@ -67,7 +67,8 @@ public:
virtual std::shared_ptr<void> create_response() = 0; virtual std::shared_ptr<void> create_response() = 0;
virtual std::shared_ptr<void> create_request_header() = 0; virtual std::shared_ptr<void> create_request_header() = 0;
virtual void handle_response(std::shared_ptr<void> &response, std::shared_ptr<void> &req_id) = 0; virtual void handle_response(std::shared_ptr<void> &request_header,
std::shared_ptr<void> &response) = 0;
private: private:
RCLCPP_DISABLE_COPY(ClientBase); RCLCPP_DISABLE_COPY(ClientBase);
@ -106,11 +107,11 @@ public:
return std::shared_ptr<void>(new rmw_request_id_t); return std::shared_ptr<void>(new rmw_request_id_t);
} }
void handle_response(std::shared_ptr<void> &response, std::shared_ptr<void> &req_id) void handle_response(std::shared_ptr<void> &request_header, std::shared_ptr<void> &response)
{ {
auto typed_req_id = std::static_pointer_cast<rmw_request_id_t>(req_id); auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response); auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
int64_t sequence_number = typed_req_id->sequence_number; int64_t sequence_number = typed_request_header->sequence_number;
auto tuple = this->pending_requests_[sequence_number]; auto tuple = this->pending_requests_[sequence_number];
auto call_promise = std::get<0>(tuple); auto call_promise = std::get<0>(tuple);
auto callback = std::get<1>(tuple); auto callback = std::get<1>(tuple);

24
rclcpp/include/rclcpp/executor.hpp
View file

@ -151,8 +151,8 @@ protected:
rclcpp::subscription::SubscriptionBase::SharedPtr &subscription) rclcpp::subscription::SubscriptionBase::SharedPtr &subscription)
{ {
std::shared_ptr<void> message = subscription->create_message(); std::shared_ptr<void> message = subscription->create_message();
auto taken = rmw_take(subscription->subscription_handle_, message.get()); bool taken = false;
// TODO(wjwwood): taken is no longer a boolean, check against return types. rmw_take(subscription->subscription_handle_, message.get(), &taken);
if (taken) if (taken)
{ {
subscription->handle_message(message); subscription->handle_message(message);
@ -176,14 +176,16 @@ protected:
execute_service( execute_service(
rclcpp::service::ServiceBase::SharedPtr &service) rclcpp::service::ServiceBase::SharedPtr &service)
{ {
std::shared_ptr<void> request = service->create_request();
std::shared_ptr<void> request_header = service->create_request_header(); std::shared_ptr<void> request_header = service->create_request_header();
bool taken = rmw_take_request(service->service_handle_, std::shared_ptr<void> request = service->create_request();
bool taken = false;
rmw_take_request(service->service_handle_,
request_header.get(),
request.get(), request.get(),
request_header.get()); &taken);
if (taken) if (taken)
{ {
service->handle_request(request, request_header); service->handle_request(request_header, request);
} }
else else
{ {
@ -197,14 +199,16 @@ protected:
execute_client( execute_client(
rclcpp::client::ClientBase::SharedPtr &client) rclcpp::client::ClientBase::SharedPtr &client)
{ {
std::shared_ptr<void> response = client->create_response();
std::shared_ptr<void> request_header = client->create_request_header(); std::shared_ptr<void> request_header = client->create_request_header();
bool taken = rmw_take_response(client->client_handle_, std::shared_ptr<void> response = client->create_response();
bool taken = false;
rmw_take_response(client->client_handle_,
request_header.get(),
response.get(), response.get(),
request_header.get()); &taken);
if (taken) if (taken)
{ {
client->handle_response(response, request_header); client->handle_response(request_header, response);
} }
else else
{ {

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

@ -65,10 +65,8 @@ Node::create_publisher(std::string topic_name, size_t queue_size)
{ {
using rosidl_generator_cpp::get_type_support_handle; using rosidl_generator_cpp::get_type_support_handle;
auto type_support_handle = get_type_support_handle<MessageT>(); auto type_support_handle = get_type_support_handle<MessageT>();
auto publisher_handle = rmw_create_publisher(node_handle_, rmw_publisher_t * publisher_handle = rmw_create_publisher(
type_support_handle, node_handle_, type_support_handle, topic_name.c_str(), queue_size);
topic_name.c_str(),
queue_size);
return publisher::Publisher::make_shared(publisher_handle); return publisher::Publisher::make_shared(publisher_handle);
} }
@ -98,10 +96,8 @@ Node::create_subscription(
{ {
using rosidl_generator_cpp::get_type_support_handle; using rosidl_generator_cpp::get_type_support_handle;
auto type_support_handle = get_type_support_handle<MessageT>(); auto type_support_handle = get_type_support_handle<MessageT>();
auto subscriber_handle = rmw_create_subscription(node_handle_, rmw_subscription_t * subscriber_handle = rmw_create_subscription(
type_support_handle, node_handle_, type_support_handle, topic_name.c_str(), queue_size);
topic_name.c_str(),
queue_size);
using namespace rclcpp::subscription; using namespace rclcpp::subscription;
@ -171,9 +167,8 @@ Node::create_client(
auto service_type_support_handle = auto service_type_support_handle =
get_service_type_support_handle<ServiceT>(); get_service_type_support_handle<ServiceT>();
auto client_handle = rmw_create_client(this->node_handle_, rmw_client_t * client_handle = rmw_create_client(
service_type_support_handle, this->node_handle_, service_type_support_handle, service_name.c_str());
service_name.c_str());
using namespace rclcpp::client; using namespace rclcpp::client;
@ -211,9 +206,8 @@ Node::create_service(
auto service_type_support_handle = auto service_type_support_handle =
get_service_type_support_handle<ServiceT>(); get_service_type_support_handle<ServiceT>();
auto service_handle = rmw_create_service(this->node_handle_, rmw_service_t * service_handle = rmw_create_service(
service_type_support_handle, this->node_handle_, service_type_support_handle, service_name.c_str());
service_name.c_str());
using namespace rclcpp::service; using namespace rclcpp::service;

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

@ -66,7 +66,8 @@ public:
virtual std::shared_ptr<void> create_request() = 0; virtual std::shared_ptr<void> create_request() = 0;
virtual std::shared_ptr<void> create_request_header() = 0; virtual std::shared_ptr<void> create_request_header() = 0;
virtual void handle_request(std::shared_ptr<void> &request, std::shared_ptr<void> &req_id) = 0; virtual void handle_request(std::shared_ptr<void> &request_header,
std::shared_ptr<void> &request) = 0;
private: private:
RCLCPP_DISABLE_COPY(ServiceBase); RCLCPP_DISABLE_COPY(ServiceBase);
@ -103,13 +104,13 @@ public:
return std::shared_ptr<void>(new rmw_request_id_t); return std::shared_ptr<void>(new rmw_request_id_t);
} }
void handle_request(std::shared_ptr<void> &request, std::shared_ptr<void> &req_id) void handle_request(std::shared_ptr<void> &request_header, std::shared_ptr<void> &request)
{ {
auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request); auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
auto typed_req_id = std::static_pointer_cast<rmw_request_id_t>(req_id); 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); auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
callback_(typed_request, response); callback_(typed_request, response);
send_response(typed_req_id, response); send_response(typed_request_header, response);
} }
void send_response(std::shared_ptr<rmw_request_id_t> &req_id, void send_response(std::shared_ptr<rmw_request_id_t> &req_id,