diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 5240c8c818..d84032ecb8 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include "rclcpp/node_interfaces/node_base.hpp" @@ -31,6 +32,68 @@ using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::node_interfaces::NodeBase; +namespace +{ +/// This class bundles together the lifetime of the rcl_node_t handle with the +/// lifetime of the RCL context. This ensures that the context will remain alive +/// for as long as the rcl_node_t handle is alive. +class NodeHandleWithContext +{ +public: + /// The make function returns a std::shared_ptr which is actually + /// an alias for a std::shared_ptr. There is no use for + /// accessing a NodeHandleWithContext instance directly, because its only job + /// is to couple together the lifetime of a context with the lifetime of a + /// node handle that depends on it. + static std::shared_ptr + make( + rclcpp::Context::SharedPtr context, + std::shared_ptr logging_mutex, + rcl_node_t * node_handle) + { + auto aliased_ptr = std::shared_ptr( + new NodeHandleWithContext( + std::move(context), + std::move(logging_mutex), + node_handle)); + + return std::shared_ptr(std::move(aliased_ptr), node_handle); + } + + ~NodeHandleWithContext() + { + std::lock_guard guard(*logging_mutex_); + // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, + // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called + // here directly. + if (rcl_node_fini(node_handle_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); + } + delete node_handle_; + } + +private: + /// The constructor is private because this class is only meant to be aliased + /// into a std::shared_ptr. + NodeHandleWithContext( + rclcpp::Context::SharedPtr context, + std::shared_ptr logging_mutex, + rcl_node_t * node_handle) + : context_(std::move(context)), + logging_mutex_(std::move(logging_mutex)), + node_handle_(node_handle) + { + // Do nothing + } + + rclcpp::Context::SharedPtr context_; + std::shared_ptr logging_mutex_; + rcl_node_t * node_handle_; +}; +} // anonymous namespace + NodeBase::NodeBase( const std::string & node_name, const std::string & namespace_, @@ -131,20 +194,8 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } - node_handle_.reset( - rcl_node.release(), - [logging_mutex](rcl_node_t * node) -> void { - std::lock_guard guard(*logging_mutex); - // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, - // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called - // here directly. - if (rcl_node_fini(node) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); - } - delete node; - }); + node_handle_ = NodeHandleWithContext::make( + context_, logging_mutex, rcl_node.release()); // Create the default callback group. using rclcpp::CallbackGroupType; diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index e034af16b2..d8c57c3605 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -97,6 +97,31 @@ TEST_F(TestNode, construction_and_destruction) { } } +/* + Testing lifecycles of subscriptions and publishers after node dies + */ +TEST_F(TestNode, pub_and_sub_lifecycles) { + using test_msgs::msg::Empty; + rclcpp::Publisher::SharedPtr pub; + rclcpp::Subscription::SharedPtr sub; + const auto callback = [](Empty::ConstSharedPtr) {}; + + { + // Create the node and context in a nested scope so that their + // std::shared_ptrs expire before we use pub and sub + auto context = std::make_shared(); + context->init(0, nullptr); + rclcpp::NodeOptions options; + options.context(context); + + const auto node = std::make_shared("my_node", "/ns", options); + pub = node->create_publisher("topic", 10); + sub = node->create_subscription("topic", 10, callback); + } + + pub->publish(Empty()); +} + TEST_F(TestNode, get_name_and_namespace) { { auto node = std::make_shared("my_node", "/ns");