diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index d32a3b335e..a0c9a02279 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -113,6 +113,7 @@ class ClientBase std::shared_ptr node_handle_; std::shared_ptr client_handle_; + std::string service_name_; }; @@ -148,7 +149,7 @@ class Client : public ClientBase auto service_type_support_handle = get_service_type_support_handle(); 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(), diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 27081b5fa0..efe6fe8f33 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -21,8 +21,6 @@ #include #include -#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(); + std::weak_ptr weak_node_handle(node_handle_); // rcl does the static memory allocation here service_handle_ = std::shared_ptr( - 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; }); diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 70034b60e0..97a414474c 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -19,8 +19,6 @@ #include #include -#include "rcutils/logging_macros.h" - #include "rcl/graph.h" #include "rcl/node.h" #include "rcl/wait.h" @@ -28,6 +26,7 @@ #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 weak_node_handle(node_handle_); client_handle_ = std::shared_ptr( - 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"); } diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index 767624931e..83401c0e80 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -14,14 +14,13 @@ #include "rclcpp/subscription.hpp" -#include - #include #include #include #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 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; }; diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp index 1045c8025d..4337119615 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/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(); } diff --git a/rclcpp/test/test_find_weak_nodes.cpp b/rclcpp/test/test_find_weak_nodes.cpp index 4d9ede7a30..bced853d60 100644 --- a/rclcpp/test/test_find_weak_nodes.cpp +++ b/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(); } diff --git a/rclcpp/test/test_logging.cpp b/rclcpp/test/test_logging.cpp index 323a3abdb0..48dcb720b8 100644 --- a/rclcpp/test/test_logging.cpp +++ b/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); } };