diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0c448cf5e6..ccbc388fd8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -152,6 +152,26 @@ LifecycleNode::LifecycleNode( 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( + rclcpp::get_logger("rclcpp_lifecycle"), + "Shutdown error in destruction of LifecycleNode: final state(%s)", + finalized.label().c_str()); + } + } else if (current_state > lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "Shutdown error in destruction of LifecycleNode: Node still in transition state(%u)", + current_state); + } + // release sub-interfaces in an order that allows them to consult with node_base during tear-down node_waitables_.reset(); node_time_source_.reset(); diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index fdb9be6153..58ce89897c 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -447,6 +447,146 @@ 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, test_shutdown_on_dtor) { + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + bool shutdown_cb_called = false; + auto on_shutdown_callback = + [&shutdown_cb_called](const rclcpp_lifecycle::State &) -> + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn { + shutdown_cb_called = true; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + }; + + // PRIMARY_STATE_UNCONFIGURED to shutdown via dtor + shutdown_cb_called = false; + { + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_INACTIVE to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_ACTIVE to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + 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); + EXPECT_FALSE(shutdown_cb_called); + } + EXPECT_TRUE(shutdown_cb_called); + + // PRIMARY_STATE_FINALIZED to shutdown via dtor + shutdown_cb_called = false; + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + test_node->register_on_shutdown(std::bind(on_shutdown_callback, std::placeholders::_1)); + 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); + EXPECT_TRUE(shutdown_cb_called); // should be called already + } + EXPECT_TRUE(shutdown_cb_called); +} + TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { auto test_node = std::make_shared>("testnode");