From e8d150e5448aaff1cefec41857787e978c3fa0a7 Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Mon, 29 Jun 2015 13:42:36 -0700
Subject: [PATCH 1/6] 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       | 27 +++++++++++++++++-------
 rclcpp/include/rclcpp/node.hpp         |  8 ++++---
 rclcpp/include/rclcpp/node_impl.hpp    | 29 +++++++++++++++++++-------
 rclcpp/include/rclcpp/publisher.hpp    | 19 +++++++++++++++--
 rclcpp/include/rclcpp/service.hpp      | 23 ++++++++++++++------
 rclcpp/include/rclcpp/subscription.hpp | 22 +++++++++++++++++--
 6 files changed, 100 insertions(+), 28 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index f531079c89..8b41b90ef7 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/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)
-  : 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()
   {
-    if (client_handle_ != nullptr) {
-      rmw_destroy_client(client_handle_);
-      client_handle_ = nullptr;
+    if (client_handle_) {
+      if (rmw_destroy_client(client_handle_) == RMW_RET_ERROR) {
+        std::cerr << "Error in destruction of rmw client handle: " <<
+        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
+          std::endl;
+      }
     }
   }
 
@@ -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()
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index f4b5962cb1..602316a038 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/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,6 +226,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)
@@ -233,7 +234,7 @@ class Node
     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);
   }
 };
 
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 2d0aaf0c91..5f045ba7a7 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/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)) {
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index 7ae9450065..9b414d67be 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/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_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)
   {}
 
+  ~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 @@ class Publisher
   }
 
 private:
+  std::shared_ptr<rmw_node_t> node_handle_;
+
   rmw_publisher_t * publisher_handle_;
 
 };
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index ed345a3c25..9141d48da5 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/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 @@ class ServiceBase
   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) {
-      rmw_destroy_service(service_handle_);
-      service_handle_ = nullptr;
+    if (service_handle_) {
+      if (rmw_destroy_service(service_handle_) == RMW_RET_ERROR) {
+        std::cerr << "Error in destruction of rmw service_handle_ handle: " <<
+        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
+          std::endl;
+      }
     }
   }
 
@@ -76,6 +82,8 @@ class ServiceBase
 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 @@ class Service : public ServiceBase
   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)
   {}
 
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index b93dbd2ec1..40fad86231 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/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 @@ class SubscriptionBase
   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 @@ class SubscriptionBase
 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 @@ class Subscription : public SubscriptionBase
   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)
   {}
 

From 397cde7ee9c4df0e0caa9cef27261dcd14dc1e69 Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Mon, 29 Jun 2015 13:42:54 -0700
Subject: [PATCH 2/6] Fix some lifecycle issues in the TimerBase class.

---
 rclcpp/include/rclcpp/client.hpp       |  8 +++++---
 rclcpp/include/rclcpp/node_impl.hpp    |  7 +++++--
 rclcpp/include/rclcpp/publisher.hpp    |  2 ++
 rclcpp/include/rclcpp/subscription.hpp |  5 ++++-
 rclcpp/include/rclcpp/timer.hpp        | 22 +++++++++++++++++++++-
 5 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index 8b41b90ef7..757bcb7ffa 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -57,9 +57,11 @@ class ClientBase
   {
     if (client_handle_) {
       if (rmw_destroy_client(client_handle_) == RMW_RET_ERROR) {
-        std::cerr << "Error in destruction of rmw client handle: " <<
-        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
-          std::endl;
+        // *INDENT-OFF*
+        std::cerr << "Error in destruction of rmw client handle: "
+                  << (rmw_get_error_string() ? rmw_get_error_string() : "")
+                  << std::endl;
+        // *INDENT-ON*
       }
     }
   }
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 5f045ba7a7..be2d241fe7 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -50,14 +50,16 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
     if (node_handle_) {
       auto ret = rmw_destroy_node(node);
       if (ret != RMW_RET_OK) {
+        // *INDENT-OFF*
         std::cerr << "Error in destruction of rmw node handle: "
                   << (rmw_get_error_string() ? rmw_get_error_string() : "")
                   << std::endl;
+        // *INDENT-ON*
       }
     }
   });
   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() : ""));
@@ -127,7 +129,8 @@ 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_.get(), 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(
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index 9b414d67be..42a8506b59 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -48,9 +48,11 @@ class Publisher
   {
     if (publisher_handle_) {
       if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) == RMW_RET_ERROR) {
+        // *INDENT-OFF*
         std::cerr << "Error in destruction of rmw publisher handle: "
                   << (rmw_get_error_string() ? rmw_get_error_string() : "")
                   << std::endl;
+        // *INDENT-ON*
       }
     }
   }
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index 40fad86231..de00b04bb5 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -53,7 +53,10 @@ class SubscriptionBase
     subscription_handle_(subscription_handle),
     topic_name_(topic_name),
     ignore_local_publications_(ignore_local_publications)
-  {}
+  {
+    // To avoid unused private member warnings.
+    (void)ignore_local_publications_;
+  }
 
   ~SubscriptionBase()
   {
diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp
index d378669278..d87c71edec 100644
--- a/rclcpp/include/rclcpp/timer.hpp
+++ b/rclcpp/include/rclcpp/timer.hpp
@@ -20,6 +20,7 @@
 #include <memory>
 #include <thread>
 
+#include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
 #include <rclcpp/macros.hpp>
@@ -50,9 +51,27 @@ class TimerBase
   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() ? rmw_get_error_string() : "")
+      );
+    }
+  }
+
+  ~TimerBase()
+  {
+    if (guard_condition_) {
+      if (rmw_destroy_guard_condition(guard_condition_) == RMW_RET_ERROR) {
+        std::cerr << "Error in TimerBase destructor, rmw_destroy_guard_condition failed: " <<
+        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
+          std::endl;
+      }
+    }
   }
 
   void
@@ -89,6 +108,7 @@ class GenericTimer : public TimerBase
   ~GenericTimer()
   {
     cancel();
+    thread_.join();
   }
 
   void

From 2d5afac3a6252c8a2afbee7acf8d767b4705b1fd Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Tue, 30 Jun 2015 11:54:40 -0700
Subject: [PATCH 3/6] use new rmw_get_error_string_safe func

---
 rclcpp/include/rclcpp/client.hpp       |  2 +-
 rclcpp/include/rclcpp/node_impl.hpp    | 12 ++++++------
 rclcpp/include/rclcpp/publisher.hpp    |  2 +-
 rclcpp/include/rclcpp/service.hpp      |  2 +-
 rclcpp/include/rclcpp/subscription.hpp |  2 +-
 rclcpp/include/rclcpp/timer.hpp        |  4 ++--
 6 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index 757bcb7ffa..ee091939b5 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -59,7 +59,7 @@ class ClientBase
       if (rmw_destroy_client(client_handle_) == RMW_RET_ERROR) {
         // *INDENT-OFF*
         std::cerr << "Error in destruction of rmw client handle: "
-                  << (rmw_get_error_string() ? rmw_get_error_string() : "")
+                  << rmw_get_error_string_safe()
                   << std::endl;
         // *INDENT-ON*
       }
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index be2d241fe7..970ecdd791 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -52,7 +52,7 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
       if (ret != RMW_RET_OK) {
         // *INDENT-OFF*
         std::cerr << "Error in destruction of rmw node handle: "
-                  << (rmw_get_error_string() ? rmw_get_error_string() : "")
+                  << rmw_get_error_string_safe()
                   << std::endl;
         // *INDENT-ON*
       }
@@ -62,7 +62,7 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
     // *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*
   }
 
@@ -97,7 +97,7 @@ Node::create_publisher(std::string topic_name, size_t queue_size)
     // *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*
   }
 
@@ -135,7 +135,7 @@ Node::create_subscription(
     // *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*
   }
 
@@ -209,7 +209,7 @@ Node::create_client(
     // *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*
   }
 
@@ -252,7 +252,7 @@ Node::create_service(
     // *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*
   }
 
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index 42a8506b59..a16bbcba0c 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -50,7 +50,7 @@ class Publisher
       if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) == RMW_RET_ERROR) {
         // *INDENT-OFF*
         std::cerr << "Error in destruction of rmw publisher handle: "
-                  << (rmw_get_error_string() ? rmw_get_error_string() : "")
+                  << rmw_get_error_string_safe()
                   << std::endl;
         // *INDENT-ON*
       }
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index 9141d48da5..0c54fff49b 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -57,7 +57,7 @@ class ServiceBase
     if (service_handle_) {
       if (rmw_destroy_service(service_handle_) == RMW_RET_ERROR) {
         std::cerr << "Error in destruction of rmw service_handle_ handle: " <<
-        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
+          rmw_get_error_string_safe() <<
           std::endl;
       }
     }
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index de00b04bb5..63a8facee9 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -63,7 +63,7 @@ class 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() : "") <<
+          rmw_get_error_string_safe() <<
           std::endl;
       }
     }
diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp
index d87c71edec..0eec801f36 100644
--- a/rclcpp/include/rclcpp/timer.hpp
+++ b/rclcpp/include/rclcpp/timer.hpp
@@ -58,7 +58,7 @@ class TimerBase
       // TODO(wjwwood): use custom exception
       throw std::runtime_error(
               std::string("failed to create guard condition: ") +
-              (rmw_get_error_string() ? rmw_get_error_string() : "")
+              rmw_get_error_string_safe()
       );
     }
   }
@@ -68,7 +68,7 @@ class TimerBase
     if (guard_condition_) {
       if (rmw_destroy_guard_condition(guard_condition_) == RMW_RET_ERROR) {
         std::cerr << "Error in TimerBase destructor, rmw_destroy_guard_condition failed: " <<
-        (rmw_get_error_string() ? rmw_get_error_string() : "") <<
+          rmw_get_error_string_safe() <<
           std::endl;
       }
     }

From 16323b3f92ccb3126a09922ca095745045d5ad87 Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Wed, 1 Jul 2015 14:45:38 -0700
Subject: [PATCH 4/6] avoid streaming directly to std::cerr

---
 rclcpp/include/rclcpp/client.hpp       | 10 ++++------
 rclcpp/include/rclcpp/node_impl.hpp    |  8 +++++---
 rclcpp/include/rclcpp/publisher.hpp    | 10 ++++++----
 rclcpp/include/rclcpp/service.hpp      | 10 ++++++----
 rclcpp/include/rclcpp/subscription.hpp | 10 ++++++----
 rclcpp/include/rclcpp/timer.hpp        | 10 ++++++----
 6 files changed, 33 insertions(+), 25 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index ee091939b5..1a70723a4a 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -19,6 +19,7 @@
 #include <iostream>
 #include <map>
 #include <memory>
+#include <sstream>
 #include <utility>
 
 #include <rmw/error_handling.h>
@@ -56,12 +57,9 @@ class ClientBase
   ~ClientBase()
   {
     if (client_handle_) {
-      if (rmw_destroy_client(client_handle_) == RMW_RET_ERROR) {
-        // *INDENT-OFF*
-        std::cerr << "Error in destruction of rmw client handle: "
-                  << rmw_get_error_string_safe()
-                  << std::endl;
-        // *INDENT-ON*
+      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());
       }
     }
   }
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 970ecdd791..7e18dae662 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -18,6 +18,7 @@
 #include <algorithm>
 #include <iostream>
 #include <memory>
+#include <sstream>
 #include <stdexcept>
 #include <string>
 
@@ -51,10 +52,11 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
       auto ret = rmw_destroy_node(node);
       if (ret != RMW_RET_OK) {
         // *INDENT-OFF*
-        std::cerr << "Error in destruction of rmw node handle: "
-                  << rmw_get_error_string_safe()
-                  << std::endl;
+        std::stringstream ss;
+        ss << "Error in destruction of rmw node handle: "
+           << rmw_get_error_string_safe() << '\n';
         // *INDENT-ON*
+        (std::cerr << ss.str()).flush();
       }
     }
   });
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index a16bbcba0c..e481bfd71f 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -17,6 +17,7 @@
 
 #include <iostream>
 #include <memory>
+#include <sstream>
 
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
@@ -47,12 +48,13 @@ class Publisher
   ~Publisher()
   {
     if (publisher_handle_) {
-      if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) == RMW_RET_ERROR) {
+      if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) {
         // *INDENT-OFF*
-        std::cerr << "Error in destruction of rmw publisher handle: "
-                  << rmw_get_error_string_safe()
-                  << std::endl;
+        std::stringstream ss;
+        ss << "Error in destruction of rmw publisher handle: "
+           << rmw_get_error_string_safe() << '\n';
         // *INDENT-ON*
+        (std::cerr << ss.str()).flush();
       }
     }
   }
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index 0c54fff49b..c05282e85b 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -18,6 +18,7 @@
 #include <functional>
 #include <iostream>
 #include <memory>
+#include <sstream>
 #include <string>
 
 #include <rmw/error_handling.h>
@@ -55,10 +56,11 @@ class ServiceBase
   ~ServiceBase()
   {
     if (service_handle_) {
-      if (rmw_destroy_service(service_handle_) == RMW_RET_ERROR) {
-        std::cerr << "Error in destruction of rmw service_handle_ handle: " <<
-          rmw_get_error_string_safe() <<
-          std::endl;
+      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();
       }
     }
   }
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index 63a8facee9..b327ba0718 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -18,6 +18,7 @@
 #include <functional>
 #include <iostream>
 #include <memory>
+#include <sstream>
 #include <string>
 
 #include <rmw/error_handling.h>
@@ -61,10 +62,11 @@ class SubscriptionBase
   ~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_safe() <<
-          std::endl;
+      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();
       }
     }
   }
diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp
index 0eec801f36..3e464f5f63 100644
--- a/rclcpp/include/rclcpp/timer.hpp
+++ b/rclcpp/include/rclcpp/timer.hpp
@@ -18,6 +18,7 @@
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <sstream>
 #include <thread>
 
 #include <rmw/error_handling.h>
@@ -66,10 +67,11 @@ class TimerBase
   ~TimerBase()
   {
     if (guard_condition_) {
-      if (rmw_destroy_guard_condition(guard_condition_) == RMW_RET_ERROR) {
-        std::cerr << "Error in TimerBase destructor, rmw_destroy_guard_condition failed: " <<
-          rmw_get_error_string_safe() <<
-          std::endl;
+      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();
       }
     }
   }

From 9df50f53553ce8c86262bc6949f3657269e18c4b Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Wed, 1 Jul 2015 17:51:15 -0700
Subject: [PATCH 5/6] make all destructors virtual

---
 rclcpp/include/rclcpp/client.hpp                             | 2 +-
 rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp  | 2 +-
 rclcpp/include/rclcpp/executors/single_threaded_executor.hpp | 2 +-
 rclcpp/include/rclcpp/publisher.hpp                          | 2 +-
 rclcpp/include/rclcpp/service.hpp                            | 2 +-
 rclcpp/include/rclcpp/subscription.hpp                       | 2 +-
 rclcpp/include/rclcpp/timer.hpp                              | 4 ++--
 7 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index 1a70723a4a..6b957d4238 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -54,7 +54,7 @@ class ClientBase
   : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name)
   {}
 
-  ~ClientBase()
+  virtual ~ClientBase()
   {
     if (client_handle_) {
       if (rmw_destroy_client(client_handle_) != RMW_RET_OK) {
diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
index d16135c675..5eb2bc393e 100644
--- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
+++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
@@ -48,7 +48,7 @@ class MultiThreadedExecutor : public executor::Executor
     }
   }
 
-  ~MultiThreadedExecutor() {}
+  virtual ~MultiThreadedExecutor() {}
 
   void
   spin()
diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
index ebcd51dd8d..b54b445c7b 100644
--- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
+++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
@@ -42,7 +42,7 @@ class SingleThreadedExecutor : public executor::Executor
 
   SingleThreadedExecutor() {}
 
-  ~SingleThreadedExecutor() {}
+  virtual ~SingleThreadedExecutor() {}
 
   void spin()
   {
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index e481bfd71f..f7810bd723 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -45,7 +45,7 @@ class Publisher
   : node_handle_(node_handle), publisher_handle_(publisher_handle)
   {}
 
-  ~Publisher()
+  virtual ~Publisher()
   {
     if (publisher_handle_) {
       if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) {
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index c05282e85b..3c9d7c31c2 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -53,7 +53,7 @@ class ServiceBase
   : node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name)
   {}
 
-  ~ServiceBase()
+  virtual ~ServiceBase()
   {
     if (service_handle_) {
       if (rmw_destroy_service(service_handle_) != RMW_RET_OK) {
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index b327ba0718..5aa08786d0 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -59,7 +59,7 @@ class SubscriptionBase
     (void)ignore_local_publications_;
   }
 
-  ~SubscriptionBase()
+  virtual ~SubscriptionBase()
   {
     if (subscription_handle_) {
       if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) {
diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp
index 3e464f5f63..6b7c96998d 100644
--- a/rclcpp/include/rclcpp/timer.hpp
+++ b/rclcpp/include/rclcpp/timer.hpp
@@ -64,7 +64,7 @@ class TimerBase
     }
   }
 
-  ~TimerBase()
+  virtual ~TimerBase()
   {
     if (guard_condition_) {
       if (rmw_destroy_guard_condition(guard_condition_) != RMW_RET_OK) {
@@ -107,7 +107,7 @@ class GenericTimer : public TimerBase
     thread_ = std::thread(&GenericTimer<Clock>::run, this);
   }
 
-  ~GenericTimer()
+  virtual ~GenericTimer()
   {
     cancel();
     thread_.join();

From edc18861739985db875130ae350e79697cd0160f Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Tue, 14 Jul 2015 19:34:28 -0700
Subject: [PATCH 6/6] incorporating changes from #53

---
 rclcpp/include/rclcpp/client.hpp    |  8 ++++++--
 rclcpp/include/rclcpp/executor.hpp  | 22 ++++++++++++++--------
 rclcpp/include/rclcpp/publisher.hpp |  8 +++++++-
 rclcpp/include/rclcpp/service.hpp   |  8 +++++++-
 4 files changed, 34 insertions(+), 12 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index 6b957d4238..553c9b546d 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -146,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());
diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index d3e9051824..d2e28a8def 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/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());
     }
   }
 
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index f7810bd723..b31b2dc97f 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -63,7 +63,13 @@ class Publisher
   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:
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index 3c9d7c31c2..6c756c13db 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -154,7 +154,13 @@ class Service : public ServiceBase
     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: