Convert all rcl_*_t types to shared pointers

Converts all rcl_*_t types in the memory allocation strategy to shared pointers to prevent crash happening when a subscriber is reset.

Issue: #349

rclcpp/include/rclcpp/client.hpp

@@ -113,6 +113,7 @@ class ClientBase
   std::shared_ptr<rcl_node_t> node_handle_;
 
   std::shared_ptr<rcl_client_t> client_handle_;
+
   std::string service_name_;
 };
 
@@ -148,7 +149,7 @@ class Client : public ClientBase
     auto service_type_support_handle =
       get_service_type_support_handle<ServiceT>();
     rcl_ret_t ret = rcl_client_init(
-      client_handle_.get(),
+      get_client_handle().get(),
       this->get_rcl_node_handle(),
       service_type_support_handle,
       service_name.c_str(),

rclcpp/include/rclcpp/service.hpp

@@ -21,8 +21,6 @@
 #include <sstream>
 #include <string>
 
-#include "rcutils/logging_macros.h"
-
 #include "rcl/error_handling.h"
 #include "rcl/service.h"
 
@@ -118,16 +116,25 @@ class Service : public ServiceBase
     using rosidl_typesupport_cpp::get_service_type_support_handle;
     auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
 
+    std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
     // rcl does the static memory allocation here
     service_handle_ = std::shared_ptr<rcl_service_t>(
-      new rcl_service_t, [ = ](rcl_service_t * service)
+      new rcl_service_t, [weak_node_handle](rcl_service_t * service)
       {
-        if (rcl_service_fini(service, node_handle_.get()) != RCL_RET_OK) {
+        auto handle = weak_node_handle.lock();
+        if (handle) {
+          if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
+            RCLCPP_ERROR(
+              rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+              "Error in destruction of rcl service handle: %s",
+              rcl_get_error_string_safe());
+            rcl_reset_error();
+          }
+        } else {
           RCLCPP_ERROR(
-            rclcpp::get_logger(rcl_node_get_name(node_handle.get())).get_child("rclcpp"),
-            "Error in destruction of rcl service handle: %s",
-            rcl_get_error_string_safe());
-          rcl_reset_error();
+            rclcpp::get_logger("rclcpp"),
+            "Error in destruction of rcl service handle: "
+            "the Node Handle was destructed too early. You will leak memory");
         }
         delete service;
       });

rclcpp/src/rclcpp/client.cpp

@@ -19,15 +19,14 @@
 #include <memory>
 #include <string>
 
-#include "rcutils/logging_macros.h"
-
 #include "rcl/graph.h"
 #include "rcl/node.h"
 #include "rcl/wait.h"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/node_interfaces/node_base_interface.hpp"
 #include "rclcpp/node_interfaces/node_graph_interface.hpp"
 #include "rclcpp/utilities.hpp"
+#include "rclcpp/logging.hpp"
 
 using rclcpp::ClientBase;
 using rclcpp::exceptions::InvalidNodeError;
@@ -41,21 +40,34 @@ ClientBase::ClientBase(
   node_handle_(node_base->get_shared_rcl_node_handle()),
   service_name_(service_name)
 {
+  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
   client_handle_ = std::shared_ptr<rcl_client_t>(
-    new rcl_client_t, [ = ](rcl_client_t * client)
+    new rcl_client_t, [weak_node_handle](rcl_client_t * client)
     {
-      if (rcl_client_fini(client, node_handle_.get()) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Error in destruction of rcl client handle: %s", rcl_get_error_string_safe());
-        rcl_reset_error();
-        delete client;
+      auto handle = weak_node_handle.lock();
+      if (handle) {
+        if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+            "Error in destruction of rcl client handle: %s", rcl_get_error_string_safe());
+          rcl_reset_error();
+        }
+      } else {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "Error in destruction of rcl client handle: "
+          "the Node Handle was destructed too early. You will leak memory");
       }
+      delete client;
     });
   *client_handle_.get() = rcl_get_zero_initialized_client();
 }
 
-ClientBase::~ClientBase() {}
+ClientBase::~ClientBase()
+{
+  // Make sure the client handle is destructed as early as possible and before the node handle
+  client_handle_.reset();
+}
 
 const std::string &
 ClientBase::get_service_name() const
@@ -80,7 +92,9 @@ ClientBase::service_is_ready() const
 {
   bool is_ready;
   rcl_ret_t ret =
-    rcl_service_server_is_available(this->get_rcl_node_handle(), client_handle_.get(), &is_ready);
+    rcl_service_server_is_available(this->get_rcl_node_handle(),
+                                    this->get_client_handle().get(),
+                                    &is_ready);
   if (ret != RCL_RET_OK) {
     throw_from_rcl_error(ret, "rcl_service_server_is_available failed");
   }

rclcpp/src/rclcpp/subscription.cpp

@@ -14,14 +14,13 @@
 
 #include "rclcpp/subscription.hpp"
 
-#include <rcutils/logging_macros.h>
-
 #include <cstdio>
 #include <memory>
 #include <string>
 
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/expand_topic_or_service_name.hpp"
+#include "rclcpp/logging.hpp"
 
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
@@ -35,14 +34,23 @@ SubscriptionBase::SubscriptionBase(
   const rcl_subscription_options_t & subscription_options)
 : node_handle_(node_handle)
 {
-  auto custom_deletor = [ = ](rcl_subscription_t * rcl_subs)
+  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
+  auto custom_deletor = [weak_node_handle](rcl_subscription_t * rcl_subs)
     {
-      if (rcl_subscription_fini(rcl_subs, node_handle_.get()) != RCL_RET_OK) {
-        RCUTILS_LOG_ERROR_NAMED(
-          "rclcpp",
-          "Error in destruction of rcl subscription handle: %s",
-          rcl_get_error_string_safe());
-        rcl_reset_error();
+      auto handle = weak_node_handle.lock();
+      if (handle) {
+        if (rcl_subscription_fini(rcl_subs, handle.get()) != RCL_RET_OK) {
+          RCLCPP_ERROR(
+            rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp"),
+            "Error in destruction of rcl subscription handle: %s",
+            rcl_get_error_string_safe());
+          rcl_reset_error();
+        }
+      } else {
+        RCLCPP_ERROR(
+          rclcpp::get_logger("rclcpp"),
+          "Error in destruction of rcl subscription handle: "
+          "the Node Handle was destructed too early. You will leak memory");
       }
       delete rcl_subs;
     };

rclcpp/test/test_externally_defined_services.cpp

@@ -106,6 +106,15 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) {
     return;
   }
 
+  // Destruct the service
+  ret = rcl_service_fini(
+    &service_handle,
+    node_handle->get_node_base_interface()->get_rcl_node_handle());
+  if (ret != RCL_RET_OK) {
+    FAIL();
+    return;
+  }
+
   SUCCEED();
 }
 
@@ -139,5 +148,15 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) {
     FAIL();
     return;
   }
+
+  // Destruct the service
+  ret = rcl_service_fini(
+    &service_handle,
+    node_handle->get_node_base_interface()->get_rcl_node_handle());
+  if (ret != RCL_RET_OK) {
+    FAIL();
+    return;
+  }
+
   SUCCEED();
 }

rclcpp/test/test_find_weak_nodes.cpp

@@ -52,6 +52,9 @@ TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) {
   // THEN
   // The result of finding dangling node pointers should be true
   ASSERT_TRUE(has_invalid_weak_nodes);
+
+  // Prevent memory leak due to the order of destruction
+  memory_strategy->clear_handles();
 }
 
 TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) {
@@ -73,4 +76,7 @@ TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) {
   // THEN
   // The result of finding dangling node pointers should be false
   ASSERT_FALSE(has_invalid_weak_nodes);
+
+  // Prevent memory leak due to the order of destruction
+  memory_strategy->clear_handles();
 }

rclcpp/test/test_logging.cpp

@@ -68,7 +68,7 @@ class TestLoggingMacros : public ::testing::Test
   void TearDown()
   {
     rcutils_logging_set_output_handler(this->previous_output_handler);
-    g_rcutils_logging_initialized = false;
+    ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown());
     EXPECT_FALSE(g_rcutils_logging_initialized);
   }
 };