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)
-  : 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 @@ class ClientBase
 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 @@ class Client : public ClientBase
 
   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 @@ class Client : public ClientBase
     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());

rclcpp/include/rclcpp/executor.hpp

@@ -162,8 +162,9 @@ class Executor
         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());
     }
   }
 
@@ -191,8 +192,9 @@ class Executor
         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());
     }
   }
 
@@ -203,15 +205,19 @@ class Executor
     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());
     }
   }
 

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

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

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -42,7 +42,7 @@ class SingleThreadedExecutor : public executor::Executor
 
   SingleThreadedExecutor() {}
 
-  ~SingleThreadedExecutor() {}
+  virtual ~SingleThreadedExecutor() {}
 
   void spin()
   {

rclcpp/include/rclcpp/node.hpp

@@ -187,7 +187,7 @@ class Node
 
   std::string name_;
 
-  rmw_node_t * node_handle_;
+  std::shared_ptr<rmw_node_t> node_handle_;
 
   rclcpp::context::Context::SharedPtr context_;
 
@@ -226,14 +226,15 @@ class Node
   >
   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)
   {
     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 @@ class Node
   >
   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 @@ class Node
     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_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_;
 
 };