diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index ca4f05c487..901ba469e3 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -165,7 +165,7 @@ class GraphListener : public std::enable_shared_from_this void __shutdown(bool); - rclcpp::Context::SharedPtr parent_context_; + rclcpp::Context::WeakPtr parent_context_; std::thread listener_thread_; bool is_started_; diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index b097564333..29369329e7 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -80,7 +80,7 @@ GraphListener::start_if_not_started() 0, // number_of_clients 0, // number_of_services 0, // number_of_events - this->parent_context_->get_rcl_context().get(), + interrupt_guard_condition_context_.get(), rcl_get_default_allocator()); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to initialize wait set"); @@ -355,10 +355,24 @@ GraphListener::__shutdown(bool should_throw) throw_from_rcl_error(ret, "failed to finalize interrupt guard condition"); } if (shutdown_guard_condition_) { - if (should_throw) { - parent_context_->release_interrupt_guard_condition(&wait_set_); + auto parent_context_ptr = parent_context_.lock(); + if (parent_context_ptr) { + if (should_throw) { + parent_context_ptr->release_interrupt_guard_condition(&wait_set_); + } else { + parent_context_ptr->release_interrupt_guard_condition(&wait_set_, std::nothrow); + } } else { - parent_context_->release_interrupt_guard_condition(&wait_set_, std::nothrow); + ret = rcl_guard_condition_fini(shutdown_guard_condition_); + if (RCL_RET_OK != ret) { + if (should_throw) { + throw_from_rcl_error(ret, "failed to finalize shutdown guard condition"); + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "failed to finalize shutdown guard condition"); + } + } } shutdown_guard_condition_ = nullptr; }