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

Merge pull request #51 from ros2/memory_audit

Browse source 
Fixes to support proper lifecycle of the rmw objects and other tear down issues
This commit is contained in:
William Woodall 2015-07-15 14:23:15 -07:00
commit 62fcb3781a
10 changed files with 185 additions and 53 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -16,10 +16,13 @@
#define RCLCPP_RCLCPP_CLIENT_HPP_
#include <future>
#include <iostream>
#include <map>
#include <memory>
#include <sstream>
#include <utility>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
@ -44,15 +47,20 @@ class ClientBase
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(ClientBase);
ClientBase(rmw_client_t * client_handle, const std::string & service_name)
: client_handle_(client_handle), service_name_(service_name)
ClientBase(
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) {
rmw_destroy_client(client_handle_);
client_handle_ = nullptr;
if (client_handle_) {
if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
fprintf(stderr,
"Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe());
}
}
}
@ -74,6 +82,8 @@ public:
private:
RCLCPP_DISABLE_COPY(ClientBase);
std::shared_ptr<rmw_node_t> node_handle_;
rmw_client_t * client_handle_;
std::string service_name_;
@ -91,8 +101,11 @@ public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Client);
Client(rmw_client_t * client_handle, const std::string & service_name)
: ClientBase(client_handle, service_name)
Client(
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()
@ -133,8 +146,12 @@ public:
CallbackType cb)
{
int64_t sequence_number;
// TODO(wjwwood): Check the return code.
rmw_send_request(get_client_handle(), request.get(), &sequence_number);
if (RMW_RET_OK != 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>();
SharedFuture f(call_promise->get_future());

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

@ -163,8 +163,9 @@ protected:
subscription->handle_message(message);
}
} else {
std::cout << "[rclcpp::error] take failed for subscription on topic: " <<
subscription->get_topic_name() << std::endl;
fprintf(stderr,
"[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);
}
} else {
std::cout << "[rclcpp::error] take failed for service on service: " <<
service->get_service_name() << std::endl;
fprintf(stderr,
"[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> response = client->create_response();
bool taken = false;
rmw_take_response(
rmw_ret_t status = rmw_take_response(
client->client_handle_,
request_header.get(),
response.get(),
&taken);
if (taken) {
client->handle_response(request_header, response);
if (status == RMW_RET_OK) {
if (taken) {
client->handle_response(request_header, response);
}
} 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());
}
}

2
rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
View file

@ -48,7 +48,7 @@ public:
}
}
~MultiThreadedExecutor() {}
virtual ~MultiThreadedExecutor() {}
void
spin()

2
rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
View file

@ -43,7 +43,7 @@ public:
SingleThreadedExecutor() {}
~SingleThreadedExecutor() {}
virtual ~SingleThreadedExecutor() {}
void spin()
{

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

@ -187,7 +187,7 @@ private:
std::string name_;
rmw_node_t * node_handle_;
std::shared_ptr<rmw_node_t> node_handle_;
rclcpp::context::Context::SharedPtr context_;
@ -226,6 +226,7 @@ private:
>
typename rclcpp::service::Service<ServiceT>::SharedPtr
create_service_internal(
std::shared_ptr<rmw_node_t> node_handle,
rmw_service_t * service_handle,
const std::string & service_name,
FunctorT callback)
@ -233,7 +234,7 @@ private:
typename rclcpp::service::Service<ServiceT>::CallbackType callback_without_header =
callback;
return service::Service<ServiceT>::make_shared(
service_handle, service_name, callback_without_header);
node_handle, service_handle, service_name, callback_without_header);
}
template<
@ -271,6 +272,7 @@ private:
>
typename rclcpp::service::Service<ServiceT>::SharedPtr
create_service_internal(
std::shared_ptr<rmw_node_t> node_handle,
rmw_service_t * service_handle,
const std::string & service_name,
FunctorT callback)
@ -284,7 +286,7 @@ private:
typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header =
callback;
return service::Service<ServiceT>::make_shared(
service_handle, service_name, callback_with_header);
node_handle, service_handle, service_name, callback_with_header);
}
};

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

@ -16,7 +16,9 @@
#define RCLCPP_RCLCPP_NODE_IMPL_HPP_
#include <algorithm>
#include <iostream>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
@ -44,17 +46,30 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
: name_(node_name), context_(context),
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_) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
// *INDENT-OFF*
throw std::runtime_error(
std::string("could not create node: ") +
(rmw_get_error_string() ? rmw_get_error_string() : ""));
rmw_get_error_string_safe());
// *INDENT-ON*
}
using rclcpp::callback_group::CallbackGroupType;
default_callback_group_ = \
default_callback_group_ =
create_callback_group(CallbackGroupType::MutuallyExclusive);
// TODO(esteve): remove hardcoded values
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;
auto type_support_handle = get_message_type_support_handle<MessageT>();
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) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create publisher: ") +
(rmw_get_error_string() ? rmw_get_error_string() : ""));
rmw_get_error_string_safe());
// *INDENT-ON*
}
return publisher::Publisher::make_shared(publisher_handle);
return publisher::Publisher::make_shared(node_handle_, publisher_handle);
}
bool
@ -116,18 +131,20 @@ Node::create_subscription(
using rosidl_generator_cpp::get_message_type_support_handle;
auto type_support_handle = get_message_type_support_handle<MessageT>();
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) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create subscription: ") +
(rmw_get_error_string() ? rmw_get_error_string() : ""));
rmw_get_error_string_safe());
// *INDENT-ON*
}
using namespace rclcpp::subscription;
auto sub = Subscription<MessageT>::make_shared(
node_handle_,
subscriber_handle,
topic_name,
ignore_local_publications,
@ -189,18 +206,19 @@ Node::create_client(
get_service_type_support_handle<ServiceT>();
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) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create client: ") +
(rmw_get_error_string() ? rmw_get_error_string() : ""));
rmw_get_error_string_safe());
// *INDENT-ON*
}
using namespace rclcpp::client;
auto cli = Client<ServiceT>::make_shared(
node_handle_,
client_handle,
service_name);
@ -231,17 +249,17 @@ Node::create_service(
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());
node_handle_.get(), service_type_support_handle, service_name.c_str());
if (!service_handle) {
// *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
throw std::runtime_error(
std::string("could not create service: ") +
(rmw_get_error_string() ? rmw_get_error_string() : ""));
rmw_get_error_string_safe());
// *INDENT-ON*
}
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);
if (group) {
if (!group_in_node(group)) {

31
rclcpp/include/rclcpp/publisher.hpp
View file

@ -15,8 +15,11 @@
#ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_
#define RCLCPP_RCLCPP_PUBLISHER_HPP_
#include <iostream>
#include <memory>
#include <sstream>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
@ -38,18 +41,40 @@ class Publisher
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher);
Publisher(rmw_publisher_t * publisher_handle)
: publisher_handle_(publisher_handle)
Publisher(std::shared_ptr<rmw_node_t> node_handle, rmw_publisher_t * 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>
void
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:
std::shared_ptr<rmw_node_t> node_handle_;
rmw_publisher_t * publisher_handle_;
};

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

@ -16,9 +16,12 @@
#define RCLCPP_RCLCPP_SERVICE_HPP_
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
@ -44,16 +47,21 @@ public:
RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase);
ServiceBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_service_t * service_handle,
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) {
rmw_destroy_service(service_handle_);
service_handle_ = nullptr;
if (service_handle_) {
if (rmw_destroy_service(service_handle_) != RMW_RET_OK) {
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:
RCLCPP_DISABLE_COPY(ServiceBase);
std::shared_ptr<rmw_node_t> node_handle_;
rmw_service_t * service_handle_;
std::string service_name_;
@ -98,17 +108,20 @@ public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
Service(
std::shared_ptr<rmw_node_t> node_handle,
rmw_service_t * service_handle,
const std::string & service_name,
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(
std::shared_ptr<rmw_node_t> node_handle,
rmw_service_t * service_handle,
const std::string & service_name,
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)
{}
@ -141,7 +154,13 @@ public:
std::shared_ptr<rmw_request_id_t> & req_id,
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:

29
rclcpp/include/rclcpp/subscription.hpp
View file

@ -16,9 +16,12 @@
#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
@ -43,13 +46,30 @@ public:
RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase);
SubscriptionBase(
std::shared_ptr<rmw_node_t> node_handle,
rmw_subscription_t * subscription_handle,
std::string & topic_name,
bool ignore_local_publications)
: subscription_handle_(subscription_handle),
: node_handle_(node_handle),
subscription_handle_(subscription_handle),
topic_name_(topic_name),
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()
{
@ -62,6 +82,8 @@ public:
private:
RCLCPP_DISABLE_COPY(SubscriptionBase);
std::shared_ptr<rmw_node_t> node_handle_;
rmw_subscription_t * subscription_handle_;
std::string topic_name_;
bool ignore_local_publications_;
@ -76,11 +98,12 @@ public:
RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription);
Subscription(
std::shared_ptr<rmw_node_t> node_handle,
rmw_subscription_t * subscription_handle,
std::string & topic_name,
bool ignore_local_publications,
CallbackType callback)
: SubscriptionBase(subscription_handle, topic_name, ignore_local_publications),
: SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications),
callback_(callback)
{}

26
rclcpp/include/rclcpp/timer.hpp
View file

@ -18,8 +18,10 @@
#include <chrono>
#include <functional>
#include <memory>
#include <sstream>
#include <thread>
#include <rmw/error_handling.h>
#include <rmw/rmw.h>
#include <rclcpp/macros.hpp>
@ -50,9 +52,28 @@ public:
TimerBase(std::chrono::nanoseconds period, CallbackType callback)
: period_(period),
callback_(callback),
guard_condition_(rmw_create_guard_condition()),
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
@ -86,9 +107,10 @@ public:
thread_ = std::thread(&GenericTimer<Clock>::run, this);
}
~GenericTimer()
virtual ~GenericTimer()
{
cancel();
thread_.join();
}
void