Merge pull request #51 from ros2/memory_audit
Fixes to support proper lifecycle of the rmw objects and other tear down issues
This commit is contained in:
commit
62fcb3781a
10 changed files with 185 additions and 53 deletions
|
@ -16,10 +16,13 @@
|
||||||
#define RCLCPP_RCLCPP_CLIENT_HPP_
|
#define RCLCPP_RCLCPP_CLIENT_HPP_
|
||||||
|
|
||||||
#include <future>
|
#include <future>
|
||||||
|
#include <iostream>
|
||||||
#include <map>
|
#include <map>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
#include <sstream>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
|
||||||
|
#include <rmw/error_handling.h>
|
||||||
#include <rmw/rmw.h>
|
#include <rmw/rmw.h>
|
||||||
|
|
||||||
#include <rclcpp/macros.hpp>
|
#include <rclcpp/macros.hpp>
|
||||||
|
@ -44,15 +47,20 @@ class ClientBase
|
||||||
public:
|
public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
|
||||||
|
|
||||||
ClientBase(rmw_client_t * client_handle, const std::string & service_name)
|
ClientBase(
|
||||||
: client_handle_(client_handle), service_name_(service_name)
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
|
rmw_client_t * client_handle,
|
||||||
|
const std::string & service_name)
|
||||||
|
: node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
~ClientBase()
|
virtual ~ClientBase()
|
||||||
{
|
{
|
||||||
if (client_handle_ != nullptr) {
|
if (client_handle_) {
|
||||||
rmw_destroy_client(client_handle_);
|
if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
|
||||||
client_handle_ = nullptr;
|
fprintf(stderr,
|
||||||
|
"Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -74,6 +82,8 @@ public:
|
||||||
private:
|
private:
|
||||||
RCLCPP_DISABLE_COPY(ClientBase);
|
RCLCPP_DISABLE_COPY(ClientBase);
|
||||||
|
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle_;
|
||||||
|
|
||||||
rmw_client_t * client_handle_;
|
rmw_client_t * client_handle_;
|
||||||
std::string service_name_;
|
std::string service_name_;
|
||||||
|
|
||||||
|
@ -91,8 +101,11 @@ public:
|
||||||
|
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
|
||||||
|
|
||||||
Client(rmw_client_t * client_handle, const std::string & service_name)
|
Client(
|
||||||
: ClientBase(client_handle, service_name)
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
|
rmw_client_t * client_handle,
|
||||||
|
const std::string & service_name)
|
||||||
|
: ClientBase(node_handle, client_handle, service_name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
std::shared_ptr<void> create_response()
|
std::shared_ptr<void> create_response()
|
||||||
|
@ -133,8 +146,12 @@ public:
|
||||||
CallbackType cb)
|
CallbackType cb)
|
||||||
{
|
{
|
||||||
int64_t sequence_number;
|
int64_t sequence_number;
|
||||||
// TODO(wjwwood): Check the return code.
|
if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
|
||||||
rmw_send_request(get_client_handle(), request.get(), &sequence_number);
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
|
throw std::runtime_error(
|
||||||
|
std::string("failed to send request: ") + rmw_get_error_string_safe());
|
||||||
|
// *INDENT-ON*
|
||||||
|
}
|
||||||
|
|
||||||
SharedPromise call_promise = std::make_shared<Promise>();
|
SharedPromise call_promise = std::make_shared<Promise>();
|
||||||
SharedFuture f(call_promise->get_future());
|
SharedFuture f(call_promise->get_future());
|
||||||
|
|
|
@ -163,8 +163,9 @@ protected:
|
||||||
subscription->handle_message(message);
|
subscription->handle_message(message);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
std::cout << "[rclcpp::error] take failed for subscription on topic: " <<
|
fprintf(stderr,
|
||||||
subscription->get_topic_name() << std::endl;
|
"[rclcpp::error] take failed for subscription on topic '%s': %s\n",
|
||||||
|
subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -192,8 +193,9 @@ protected:
|
||||||
service->handle_request(request_header, request);
|
service->handle_request(request_header, request);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
std::cout << "[rclcpp::error] take failed for service on service: " <<
|
fprintf(stderr,
|
||||||
service->get_service_name() << std::endl;
|
"[rclcpp::error] take request failed for server of service '%s': %s\n",
|
||||||
|
service->get_service_name().c_str(), rmw_get_error_string_safe());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -204,15 +206,19 @@ protected:
|
||||||
std::shared_ptr<void> request_header = client->create_request_header();
|
std::shared_ptr<void> request_header = client->create_request_header();
|
||||||
std::shared_ptr<void> response = client->create_response();
|
std::shared_ptr<void> response = client->create_response();
|
||||||
bool taken = false;
|
bool taken = false;
|
||||||
rmw_take_response(
|
rmw_ret_t status = rmw_take_response(
|
||||||
client->client_handle_,
|
client->client_handle_,
|
||||||
request_header.get(),
|
request_header.get(),
|
||||||
response.get(),
|
response.get(),
|
||||||
&taken);
|
&taken);
|
||||||
if (taken) {
|
if (status == RMW_RET_OK) {
|
||||||
client->handle_response(request_header, response);
|
if (taken) {
|
||||||
|
client->handle_response(request_header, response);
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
std::cout << "[rclcpp::error] take failed for service on client" << std::endl;
|
fprintf(stderr,
|
||||||
|
"[rclcpp::error] take response failed for client of service '%s': %s\n",
|
||||||
|
client->get_service_name().c_str(), rmw_get_error_string_safe());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -48,7 +48,7 @@ public:
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
~MultiThreadedExecutor() {}
|
virtual ~MultiThreadedExecutor() {}
|
||||||
|
|
||||||
void
|
void
|
||||||
spin()
|
spin()
|
||||||
|
|
|
@ -43,7 +43,7 @@ public:
|
||||||
|
|
||||||
SingleThreadedExecutor() {}
|
SingleThreadedExecutor() {}
|
||||||
|
|
||||||
~SingleThreadedExecutor() {}
|
virtual ~SingleThreadedExecutor() {}
|
||||||
|
|
||||||
void spin()
|
void spin()
|
||||||
{
|
{
|
||||||
|
|
|
@ -187,7 +187,7 @@ private:
|
||||||
|
|
||||||
std::string name_;
|
std::string name_;
|
||||||
|
|
||||||
rmw_node_t * node_handle_;
|
std::shared_ptr<rmw_node_t> node_handle_;
|
||||||
|
|
||||||
rclcpp::context::Context::SharedPtr context_;
|
rclcpp::context::Context::SharedPtr context_;
|
||||||
|
|
||||||
|
@ -226,6 +226,7 @@ private:
|
||||||
>
|
>
|
||||||
typename rclcpp::service::Service<ServiceT>::SharedPtr
|
typename rclcpp::service::Service<ServiceT>::SharedPtr
|
||||||
create_service_internal(
|
create_service_internal(
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
rmw_service_t * service_handle,
|
rmw_service_t * service_handle,
|
||||||
const std::string & service_name,
|
const std::string & service_name,
|
||||||
FunctorT callback)
|
FunctorT callback)
|
||||||
|
@ -233,7 +234,7 @@ private:
|
||||||
typename rclcpp::service::Service<ServiceT>::CallbackType callback_without_header =
|
typename rclcpp::service::Service<ServiceT>::CallbackType callback_without_header =
|
||||||
callback;
|
callback;
|
||||||
return service::Service<ServiceT>::make_shared(
|
return service::Service<ServiceT>::make_shared(
|
||||||
service_handle, service_name, callback_without_header);
|
node_handle, service_handle, service_name, callback_without_header);
|
||||||
}
|
}
|
||||||
|
|
||||||
template<
|
template<
|
||||||
|
@ -271,6 +272,7 @@ private:
|
||||||
>
|
>
|
||||||
typename rclcpp::service::Service<ServiceT>::SharedPtr
|
typename rclcpp::service::Service<ServiceT>::SharedPtr
|
||||||
create_service_internal(
|
create_service_internal(
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
rmw_service_t * service_handle,
|
rmw_service_t * service_handle,
|
||||||
const std::string & service_name,
|
const std::string & service_name,
|
||||||
FunctorT callback)
|
FunctorT callback)
|
||||||
|
@ -284,7 +286,7 @@ private:
|
||||||
typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header =
|
typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header =
|
||||||
callback;
|
callback;
|
||||||
return service::Service<ServiceT>::make_shared(
|
return service::Service<ServiceT>::make_shared(
|
||||||
service_handle, service_name, callback_with_header);
|
node_handle, service_handle, service_name, callback_with_header);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -16,7 +16,9 @@
|
||||||
#define RCLCPP_RCLCPP_NODE_IMPL_HPP_
|
#define RCLCPP_RCLCPP_NODE_IMPL_HPP_
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
|
#include <iostream>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
#include <sstream>
|
||||||
#include <stdexcept>
|
#include <stdexcept>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
|
@ -44,17 +46,30 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
|
||||||
: name_(node_name), context_(context),
|
: name_(node_name), context_(context),
|
||||||
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
|
number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0)
|
||||||
{
|
{
|
||||||
node_handle_ = rmw_create_node(name_.c_str());
|
// Initialize node handle shared_ptr with custom deleter.
|
||||||
|
node_handle_.reset(rmw_create_node(name_.c_str()), [ = ](rmw_node_t * node) {
|
||||||
|
if (node_handle_) {
|
||||||
|
auto ret = rmw_destroy_node(node);
|
||||||
|
if (ret != RMW_RET_OK) {
|
||||||
|
// *INDENT-OFF*
|
||||||
|
std::stringstream ss;
|
||||||
|
ss << "Error in destruction of rmw node handle: "
|
||||||
|
<< rmw_get_error_string_safe() << '\n';
|
||||||
|
// *INDENT-ON*
|
||||||
|
(std::cerr << ss.str()).flush();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
});
|
||||||
if (!node_handle_) {
|
if (!node_handle_) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
// *INDENT-OFF*
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not create node: ") +
|
std::string("could not create node: ") +
|
||||||
(rmw_get_error_string() ? rmw_get_error_string() : ""));
|
rmw_get_error_string_safe());
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
|
|
||||||
using rclcpp::callback_group::CallbackGroupType;
|
using rclcpp::callback_group::CallbackGroupType;
|
||||||
default_callback_group_ = \
|
default_callback_group_ =
|
||||||
create_callback_group(CallbackGroupType::MutuallyExclusive);
|
create_callback_group(CallbackGroupType::MutuallyExclusive);
|
||||||
// TODO(esteve): remove hardcoded values
|
// TODO(esteve): remove hardcoded values
|
||||||
events_publisher_ =
|
events_publisher_ =
|
||||||
|
@ -79,16 +94,16 @@ Node::create_publisher(std::string topic_name, size_t queue_size)
|
||||||
using rosidl_generator_cpp::get_message_type_support_handle;
|
using rosidl_generator_cpp::get_message_type_support_handle;
|
||||||
auto type_support_handle = get_message_type_support_handle<MessageT>();
|
auto type_support_handle = get_message_type_support_handle<MessageT>();
|
||||||
rmw_publisher_t * publisher_handle = rmw_create_publisher(
|
rmw_publisher_t * publisher_handle = rmw_create_publisher(
|
||||||
node_handle_, type_support_handle, topic_name.c_str(), queue_size);
|
node_handle_.get(), type_support_handle, topic_name.c_str(), queue_size);
|
||||||
if (!publisher_handle) {
|
if (!publisher_handle) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not create publisher: ") +
|
std::string("could not create publisher: ") +
|
||||||
(rmw_get_error_string() ? rmw_get_error_string() : ""));
|
rmw_get_error_string_safe());
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
|
|
||||||
return publisher::Publisher::make_shared(publisher_handle);
|
return publisher::Publisher::make_shared(node_handle_, publisher_handle);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
|
@ -116,18 +131,20 @@ Node::create_subscription(
|
||||||
using rosidl_generator_cpp::get_message_type_support_handle;
|
using rosidl_generator_cpp::get_message_type_support_handle;
|
||||||
auto type_support_handle = get_message_type_support_handle<MessageT>();
|
auto type_support_handle = get_message_type_support_handle<MessageT>();
|
||||||
rmw_subscription_t * subscriber_handle = rmw_create_subscription(
|
rmw_subscription_t * subscriber_handle = rmw_create_subscription(
|
||||||
node_handle_, type_support_handle, topic_name.c_str(), queue_size, ignore_local_publications);
|
node_handle_.get(), type_support_handle,
|
||||||
|
topic_name.c_str(), queue_size, ignore_local_publications);
|
||||||
if (!subscriber_handle) {
|
if (!subscriber_handle) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not create subscription: ") +
|
std::string("could not create subscription: ") +
|
||||||
(rmw_get_error_string() ? rmw_get_error_string() : ""));
|
rmw_get_error_string_safe());
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
|
|
||||||
using namespace rclcpp::subscription;
|
using namespace rclcpp::subscription;
|
||||||
|
|
||||||
auto sub = Subscription<MessageT>::make_shared(
|
auto sub = Subscription<MessageT>::make_shared(
|
||||||
|
node_handle_,
|
||||||
subscriber_handle,
|
subscriber_handle,
|
||||||
topic_name,
|
topic_name,
|
||||||
ignore_local_publications,
|
ignore_local_publications,
|
||||||
|
@ -189,18 +206,19 @@ Node::create_client(
|
||||||
get_service_type_support_handle<ServiceT>();
|
get_service_type_support_handle<ServiceT>();
|
||||||
|
|
||||||
rmw_client_t * client_handle = rmw_create_client(
|
rmw_client_t * client_handle = rmw_create_client(
|
||||||
this->node_handle_, service_type_support_handle, service_name.c_str());
|
this->node_handle_.get(), service_type_support_handle, service_name.c_str());
|
||||||
if (!client_handle) {
|
if (!client_handle) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not create client: ") +
|
std::string("could not create client: ") +
|
||||||
(rmw_get_error_string() ? rmw_get_error_string() : ""));
|
rmw_get_error_string_safe());
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
|
|
||||||
using namespace rclcpp::client;
|
using namespace rclcpp::client;
|
||||||
|
|
||||||
auto cli = Client<ServiceT>::make_shared(
|
auto cli = Client<ServiceT>::make_shared(
|
||||||
|
node_handle_,
|
||||||
client_handle,
|
client_handle,
|
||||||
service_name);
|
service_name);
|
||||||
|
|
||||||
|
@ -231,17 +249,17 @@ Node::create_service(
|
||||||
get_service_type_support_handle<ServiceT>();
|
get_service_type_support_handle<ServiceT>();
|
||||||
|
|
||||||
rmw_service_t * service_handle = rmw_create_service(
|
rmw_service_t * service_handle = rmw_create_service(
|
||||||
this->node_handle_, service_type_support_handle, service_name.c_str());
|
node_handle_.get(), service_type_support_handle, service_name.c_str());
|
||||||
if (!service_handle) {
|
if (!service_handle) {
|
||||||
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
std::string("could not create service: ") +
|
std::string("could not create service: ") +
|
||||||
(rmw_get_error_string() ? rmw_get_error_string() : ""));
|
rmw_get_error_string_safe());
|
||||||
// *INDENT-ON*
|
// *INDENT-ON*
|
||||||
}
|
}
|
||||||
|
|
||||||
auto serv = create_service_internal<ServiceT>(
|
auto serv = create_service_internal<ServiceT>(
|
||||||
service_handle, service_name, callback);
|
node_handle_, service_handle, service_name, callback);
|
||||||
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
|
auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
|
||||||
if (group) {
|
if (group) {
|
||||||
if (!group_in_node(group)) {
|
if (!group_in_node(group)) {
|
||||||
|
|
|
@ -15,8 +15,11 @@
|
||||||
#ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_
|
#ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_
|
||||||
#define RCLCPP_RCLCPP_PUBLISHER_HPP_
|
#define RCLCPP_RCLCPP_PUBLISHER_HPP_
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
#include <sstream>
|
||||||
|
|
||||||
|
#include <rmw/error_handling.h>
|
||||||
#include <rmw/rmw.h>
|
#include <rmw/rmw.h>
|
||||||
|
|
||||||
#include <rclcpp/macros.hpp>
|
#include <rclcpp/macros.hpp>
|
||||||
|
@ -38,18 +41,40 @@ class Publisher
|
||||||
public:
|
public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
|
||||||
|
|
||||||
Publisher(rmw_publisher_t * publisher_handle)
|
Publisher(std::shared_ptr<rmw_node_t> node_handle, rmw_publisher_t * publisher_handle)
|
||||||
: publisher_handle_(publisher_handle)
|
: node_handle_(node_handle), publisher_handle_(publisher_handle)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
virtual ~Publisher()
|
||||||
|
{
|
||||||
|
if (publisher_handle_) {
|
||||||
|
if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) {
|
||||||
|
// *INDENT-OFF*
|
||||||
|
std::stringstream ss;
|
||||||
|
ss << "Error in destruction of rmw publisher handle: "
|
||||||
|
<< rmw_get_error_string_safe() << '\n';
|
||||||
|
// *INDENT-ON*
|
||||||
|
(std::cerr << ss.str()).flush();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
template<typename MessageT>
|
template<typename MessageT>
|
||||||
void
|
void
|
||||||
publish(std::shared_ptr<MessageT> & msg)
|
publish(std::shared_ptr<MessageT> & msg)
|
||||||
{
|
{
|
||||||
rmw_publish(publisher_handle_, msg.get());
|
rmw_ret_t status = rmw_publish(publisher_handle_, msg.get());
|
||||||
|
if (status != RMW_RET_OK) {
|
||||||
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
|
throw std::runtime_error(
|
||||||
|
std::string("failed to publish message: ") + rmw_get_error_string_safe());
|
||||||
|
// *INDENT-ON*
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle_;
|
||||||
|
|
||||||
rmw_publisher_t * publisher_handle_;
|
rmw_publisher_t * publisher_handle_;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -16,9 +16,12 @@
|
||||||
#define RCLCPP_RCLCPP_SERVICE_HPP_
|
#define RCLCPP_RCLCPP_SERVICE_HPP_
|
||||||
|
|
||||||
#include <functional>
|
#include <functional>
|
||||||
|
#include <iostream>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
#include <sstream>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
|
#include <rmw/error_handling.h>
|
||||||
#include <rmw/rmw.h>
|
#include <rmw/rmw.h>
|
||||||
|
|
||||||
#include <rclcpp/macros.hpp>
|
#include <rclcpp/macros.hpp>
|
||||||
|
@ -44,16 +47,21 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
|
||||||
|
|
||||||
ServiceBase(
|
ServiceBase(
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
rmw_service_t * service_handle,
|
rmw_service_t * service_handle,
|
||||||
const std::string service_name)
|
const std::string service_name)
|
||||||
: service_handle_(service_handle), service_name_(service_name)
|
: node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
~ServiceBase()
|
virtual ~ServiceBase()
|
||||||
{
|
{
|
||||||
if (service_handle_ != nullptr) {
|
if (service_handle_) {
|
||||||
rmw_destroy_service(service_handle_);
|
if (rmw_destroy_service(service_handle_) != RMW_RET_OK) {
|
||||||
service_handle_ = nullptr;
|
std::stringstream ss;
|
||||||
|
ss << "Error in destruction of rmw service_handle_ handle: " <<
|
||||||
|
rmw_get_error_string_safe() << '\n';
|
||||||
|
(std::cerr << ss.str()).flush();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -76,6 +84,8 @@ public:
|
||||||
private:
|
private:
|
||||||
RCLCPP_DISABLE_COPY(ServiceBase);
|
RCLCPP_DISABLE_COPY(ServiceBase);
|
||||||
|
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle_;
|
||||||
|
|
||||||
rmw_service_t * service_handle_;
|
rmw_service_t * service_handle_;
|
||||||
std::string service_name_;
|
std::string service_name_;
|
||||||
|
|
||||||
|
@ -98,17 +108,20 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
|
||||||
|
|
||||||
Service(
|
Service(
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
rmw_service_t * service_handle,
|
rmw_service_t * service_handle,
|
||||||
const std::string & service_name,
|
const std::string & service_name,
|
||||||
CallbackType callback)
|
CallbackType callback)
|
||||||
: ServiceBase(service_handle, service_name), callback_(callback), callback_with_header_(nullptr)
|
: ServiceBase(node_handle, service_handle, service_name), callback_(callback),
|
||||||
|
callback_with_header_(nullptr)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
Service(
|
Service(
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
rmw_service_t * service_handle,
|
rmw_service_t * service_handle,
|
||||||
const std::string & service_name,
|
const std::string & service_name,
|
||||||
CallbackWithHeaderType callback_with_header)
|
CallbackWithHeaderType callback_with_header)
|
||||||
: ServiceBase(service_handle, service_name), callback_(nullptr),
|
: ServiceBase(node_handle, service_handle, service_name), callback_(nullptr),
|
||||||
callback_with_header_(callback_with_header)
|
callback_with_header_(callback_with_header)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
@ -141,7 +154,13 @@ public:
|
||||||
std::shared_ptr<rmw_request_id_t> & req_id,
|
std::shared_ptr<rmw_request_id_t> & req_id,
|
||||||
std::shared_ptr<typename ServiceT::Response> & response)
|
std::shared_ptr<typename ServiceT::Response> & response)
|
||||||
{
|
{
|
||||||
rmw_send_response(get_service_handle(), req_id.get(), response.get());
|
rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get());
|
||||||
|
if (status != RMW_RET_OK) {
|
||||||
|
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
|
||||||
|
throw std::runtime_error(
|
||||||
|
std::string("failed to send response: ") + rmw_get_error_string_safe());
|
||||||
|
// *INDENT-ON*
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -16,9 +16,12 @@
|
||||||
#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
|
#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
|
||||||
|
|
||||||
#include <functional>
|
#include <functional>
|
||||||
|
#include <iostream>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
#include <sstream>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
|
#include <rmw/error_handling.h>
|
||||||
#include <rmw/rmw.h>
|
#include <rmw/rmw.h>
|
||||||
|
|
||||||
#include <rclcpp/macros.hpp>
|
#include <rclcpp/macros.hpp>
|
||||||
|
@ -43,13 +46,30 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
|
||||||
|
|
||||||
SubscriptionBase(
|
SubscriptionBase(
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
rmw_subscription_t * subscription_handle,
|
rmw_subscription_t * subscription_handle,
|
||||||
std::string & topic_name,
|
std::string & topic_name,
|
||||||
bool ignore_local_publications)
|
bool ignore_local_publications)
|
||||||
: subscription_handle_(subscription_handle),
|
: node_handle_(node_handle),
|
||||||
|
subscription_handle_(subscription_handle),
|
||||||
topic_name_(topic_name),
|
topic_name_(topic_name),
|
||||||
ignore_local_publications_(ignore_local_publications)
|
ignore_local_publications_(ignore_local_publications)
|
||||||
{}
|
{
|
||||||
|
// To avoid unused private member warnings.
|
||||||
|
(void)ignore_local_publications_;
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual ~SubscriptionBase()
|
||||||
|
{
|
||||||
|
if (subscription_handle_) {
|
||||||
|
if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) {
|
||||||
|
std::stringstream ss;
|
||||||
|
ss << "Error in destruction of rmw subscription handle: " <<
|
||||||
|
rmw_get_error_string_safe() << '\n';
|
||||||
|
(std::cerr << ss.str()).flush();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
std::string get_topic_name()
|
std::string get_topic_name()
|
||||||
{
|
{
|
||||||
|
@ -62,6 +82,8 @@ public:
|
||||||
private:
|
private:
|
||||||
RCLCPP_DISABLE_COPY(SubscriptionBase);
|
RCLCPP_DISABLE_COPY(SubscriptionBase);
|
||||||
|
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle_;
|
||||||
|
|
||||||
rmw_subscription_t * subscription_handle_;
|
rmw_subscription_t * subscription_handle_;
|
||||||
std::string topic_name_;
|
std::string topic_name_;
|
||||||
bool ignore_local_publications_;
|
bool ignore_local_publications_;
|
||||||
|
@ -76,11 +98,12 @@ public:
|
||||||
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
|
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
|
||||||
|
|
||||||
Subscription(
|
Subscription(
|
||||||
|
std::shared_ptr<rmw_node_t> node_handle,
|
||||||
rmw_subscription_t * subscription_handle,
|
rmw_subscription_t * subscription_handle,
|
||||||
std::string & topic_name,
|
std::string & topic_name,
|
||||||
bool ignore_local_publications,
|
bool ignore_local_publications,
|
||||||
CallbackType callback)
|
CallbackType callback)
|
||||||
: SubscriptionBase(subscription_handle, topic_name, ignore_local_publications),
|
: SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications),
|
||||||
callback_(callback)
|
callback_(callback)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
|
|
|
@ -18,8 +18,10 @@
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
#include <sstream>
|
||||||
#include <thread>
|
#include <thread>
|
||||||
|
|
||||||
|
#include <rmw/error_handling.h>
|
||||||
#include <rmw/rmw.h>
|
#include <rmw/rmw.h>
|
||||||
|
|
||||||
#include <rclcpp/macros.hpp>
|
#include <rclcpp/macros.hpp>
|
||||||
|
@ -50,9 +52,28 @@ public:
|
||||||
TimerBase(std::chrono::nanoseconds period, CallbackType callback)
|
TimerBase(std::chrono::nanoseconds period, CallbackType callback)
|
||||||
: period_(period),
|
: period_(period),
|
||||||
callback_(callback),
|
callback_(callback),
|
||||||
|
guard_condition_(rmw_create_guard_condition()),
|
||||||
canceled_(false)
|
canceled_(false)
|
||||||
{
|
{
|
||||||
guard_condition_ = rmw_create_guard_condition();
|
if (!guard_condition_) {
|
||||||
|
// TODO(wjwwood): use custom exception
|
||||||
|
throw std::runtime_error(
|
||||||
|
std::string("failed to create guard condition: ") +
|
||||||
|
rmw_get_error_string_safe()
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
virtual ~TimerBase()
|
||||||
|
{
|
||||||
|
if (guard_condition_) {
|
||||||
|
if (rmw_destroy_guard_condition(guard_condition_) != RMW_RET_OK) {
|
||||||
|
std::stringstream ss;
|
||||||
|
ss << "Error in TimerBase destructor, rmw_destroy_guard_condition failed: " <<
|
||||||
|
rmw_get_error_string_safe() << '\n';
|
||||||
|
(std::cerr << ss.str()).flush();
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -86,9 +107,10 @@ public:
|
||||||
thread_ = std::thread(&GenericTimer<Clock>::run, this);
|
thread_ = std::thread(&GenericTimer<Clock>::run, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
~GenericTimer()
|
virtual ~GenericTimer()
|
||||||
{
|
{
|
||||||
cancel();
|
cancel();
|
||||||
|
thread_.join();
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue