share ownership of the rmw node handle

This prevents the node handle from getting
deleted before things it created can be deleted.

I also added destructors where necessary.

rclcpp/include/rclcpp/client.hpp

@@ -16,10 +16,12 @@
 #define RCLCPP_RCLCPP_CLIENT_HPP_
 
 #include <future>
+#include <iostream>
 #include <map>
 #include <memory>
 #include <utility>
 
+#include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
 #include <rclcpp/macros.hpp>
@@ -44,15 +46,21 @@ 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()
   {
-    if (client_handle_ != nullptr) {
+    if (client_handle_) {
-      rmw_destroy_client(client_handle_);
+      if (rmw_destroy_client(client_handle_) == RMW_RET_ERROR) {
-      client_handle_ = nullptr;
+        std::cerr << "Error in destruction of rmw client handle: " <<
+        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
+          std::endl;
+      }
     }
   }
 
@@ -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()

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,6 +16,7 @@
 #define RCLCPP_RCLCPP_NODE_IMPL_HPP_
 
 #include <algorithm>
+#include <iostream>
 #include <memory>
 #include <stdexcept>
 #include <string>
@@ -44,7 +45,17 @@ 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) {
+        std::cerr << "Error in destruction of rmw node handle: "
+                  << (rmw_get_error_string() ? rmw_get_error_string() : "")
+                  << std::endl;
+      }
+    }
+  });
   if (!node_handle_) {
     // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
     throw std::runtime_error(
@@ -54,7 +65,7 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
   }
 
   using rclcpp::callback_group::CallbackGroupType;
-  default_callback_group_ = \
+  default_callback_group_ =
     create_callback_group(CallbackGroupType::MutuallyExclusive);
   // TODO(esteve): remove hardcoded values
   events_publisher_ =
@@ -79,7 +90,7 @@ 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(
@@ -88,7 +99,7 @@ Node::create_publisher(std::string topic_name, size_t queue_size)
     // *INDENT-ON*
   }
 
-  return publisher::Publisher::make_shared(publisher_handle);
+  return publisher::Publisher::make_shared(node_handle_, publisher_handle);
 }
 
 bool
@@ -116,7 +127,7 @@ 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(
@@ -128,6 +139,7 @@ Node::create_subscription(
   using namespace rclcpp::subscription;
 
   auto sub = Subscription<MessageT>::make_shared(
+    node_handle_,
     subscriber_handle,
     topic_name,
     ignore_local_publications,
@@ -189,7 +201,7 @@ 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(
@@ -201,6 +213,7 @@ Node::create_client(
   using namespace rclcpp::client;
 
   auto cli = Client<ServiceT>::make_shared(
+    node_handle_,
     client_handle,
     service_name);
 
@@ -231,7 +244,7 @@ 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(
@@ -241,7 +254,7 @@ Node::create_service(
   }
 
   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,10 @@
 #ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_
 #define RCLCPP_RCLCPP_PUBLISHER_HPP_
 
+#include <iostream>
 #include <memory>
 
+#include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
 #include <rclcpp/macros.hpp>
@@ -38,10 +40,21 @@ 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)
   {}
 
+  ~Publisher()
+  {
+    if (publisher_handle_) {
+      if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) == RMW_RET_ERROR) {
+        std::cerr << "Error in destruction of rmw publisher handle: "
+                  << (rmw_get_error_string() ? rmw_get_error_string() : "")
+                  << std::endl;
+      }
+    }
+  }
+
   template<typename MessageT>
   void
   publish(std::shared_ptr<MessageT> & msg)
@@ -50,6 +63,8 @@ public:
   }
 
 private:
+  std::shared_ptr<rmw_node_t> node_handle_;
+
   rmw_publisher_t * publisher_handle_;
 
 };

rclcpp/include/rclcpp/service.hpp

@@ -16,9 +16,11 @@
 #define RCLCPP_RCLCPP_SERVICE_HPP_
 
 #include <functional>
+#include <iostream>
 #include <memory>
 #include <string>
 
+#include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
 #include <rclcpp/macros.hpp>
@@ -44,16 +46,20 @@ 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()
   {
-    if (service_handle_ != nullptr) {
+    if (service_handle_) {
-      rmw_destroy_service(service_handle_);
+      if (rmw_destroy_service(service_handle_) == RMW_RET_ERROR) {
-      service_handle_ = nullptr;
+        std::cerr << "Error in destruction of rmw service_handle_ handle: " <<
+        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
+          std::endl;
+      }
     }
   }
 
@@ -76,6 +82,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 +106,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)
   {}
 

rclcpp/include/rclcpp/subscription.hpp

@@ -16,9 +16,11 @@
 #define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
 
 #include <functional>
+#include <iostream>
 #include <memory>
 #include <string>
 
+#include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
 #include <rclcpp/macros.hpp>
@@ -43,14 +45,27 @@ 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)
   {}
 
+  ~SubscriptionBase()
+  {
+    if (subscription_handle_) {
+      if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) == RMW_RET_ERROR) {
+        std::cerr << "Error in destruction of rmw subscription handle: " <<
+        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
+          std::endl;
+      }
+    }
+  }
+
   std::string get_topic_name()
   {
     return this->topic_name_;
@@ -62,6 +77,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 +93,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)
   {}