From 66c7e3883b942f9631c750eb4c8540efc2c6a26a Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Wed, 29 May 2024 23:54:26 -0700 Subject: [PATCH] LifecycleNode shutdown on dtor only with valid context. Signed-off-by: Tomoya Fujita --- rclcpp_lifecycle/src/lifecycle_node.cpp | 26 +++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index ccbc388fd8..d484e9670b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -155,15 +155,23 @@ LifecycleNode::~LifecycleNode() auto current_state = LifecycleNode::get_current_state().id(); // shutdown if necessary to avoid leaving the device in any other primary state if (current_state < lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - auto finalized = LifecycleNode::shutdown(ret); - if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED || - ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) - { - RCLCPP_WARN( + if (node_base_->get_context()->is_valid()) { + auto ret = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto finalized = LifecycleNode::shutdown(ret); + if (finalized.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED || + ret != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) + { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "Shutdown error in destruction of LifecycleNode: final state(%s)", + finalized.label().c_str()); + } + } else { + // TODO(fujitatomoya): consider when context is gracefully shutdown before. + RCLCPP_DEBUG( rclcpp::get_logger("rclcpp_lifecycle"), - "Shutdown error in destruction of LifecycleNode: final state(%s)", - finalized.label().c_str()); + "Context invalid error in destruction of LifecycleNode: Node still in transition state(%u)", + current_state); } } else if (current_state > lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { RCLCPP_WARN( @@ -174,6 +182,7 @@ LifecycleNode::~LifecycleNode() // release sub-interfaces in an order that allows them to consult with node_base during tear-down node_waitables_.reset(); + node_type_descriptions_.reset(); node_time_source_.reset(); node_parameters_.reset(); node_clock_.reset(); @@ -182,6 +191,7 @@ LifecycleNode::~LifecycleNode() node_timers_.reset(); node_logging_.reset(); node_graph_.reset(); + node_base_.reset(); } const char *