diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0c448cf5e6..03ece2e58b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -152,8 +152,19 @@ LifecycleNode::LifecycleNode( LifecycleNode::~LifecycleNode() { + auto current_state = LifecycleNode::get_current_state().id(); + if (current_state != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + // This might be leaving sensors and devices without shutting down unintentionally. + // It is user's responsibility to call shutdown to avoid leaving them unknow states. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "LifecycleNode is not shut down: Node still in state(%u) in destructor", + current_state); + } + // 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(); @@ -162,6 +173,7 @@ LifecycleNode::~LifecycleNode() node_timers_.reset(); node_logging_.reset(); node_graph_.reset(); + node_base_.reset(); } const char * diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 8e0286086e..70993af6d5 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -431,6 +431,71 @@ TEST_F(TestDefaultStateMachine, bad_mood) { EXPECT_EQ(1u, test_node->number_of_callbacks); } +TEST_F(TestDefaultStateMachine, shutdown_from_each_primary_state) { + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + // PRIMARY_STATE_UNCONFIGURED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_INACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_ACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_FINALIZED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + ret = reset_key; + auto finalized_again = test_node->shutdown(ret); + EXPECT_EQ(reset_key, ret); + EXPECT_EQ(finalized_again.id(), State::PRIMARY_STATE_FINALIZED); + } +} + TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode");