Merge pull request #51 from ros2/memory_audit

Fixes to support proper lifecycle of the rmw objects and other tear down issues

rclcpp/include/rclcpp/client.hpp

@@ -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)
+  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:
   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)
+  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()
@@ -133,8 +146,12 @@ public:
     CallbackType cb)
   {
     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>();
     SharedFuture f(call_promise->get_future());

rclcpp/include/rclcpp/executor.hpp

@@ -163,8 +163,9 @@ protected:
         subscription->handle_message(message);
       }
     } 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);
       }
     } 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> 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) {
+    if (status == RMW_RET_OK) {
-      client->handle_response(request_header, response);
+      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());
     }
   }
 

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

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

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

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

rclcpp/include/rclcpp/node.hpp

@@ -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);
   }
 };
 

rclcpp/include/rclcpp/node_impl.hpp

@@ -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)) {

rclcpp/include/rclcpp/publisher.hpp

@@ -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(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>
   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_;
 
 };

rclcpp/include/rclcpp/service.hpp

@@ -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) {
+    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:
   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:

rclcpp/include/rclcpp/subscription.hpp

@@ -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)
   {}
 

rclcpp/include/rclcpp/timer.hpp

@@ -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