Skip to content

Commit

Permalink
call shutdown in LifecycleNode dtor to avoid leaving the device in un… (
Browse files Browse the repository at this point in the history 
#2450)

* call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

* add test to verify LifecycleNode::shutdown is called on destructor.

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>

---------

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
  • Loading branch information
@fujitatomoya
fujitatomoya authored Apr 6, 2024
1 parent ea4c98c commit 04ea0bb
Show file tree
Hide file tree
Showing 2 changed files with 156 additions and 0 deletions.
16 changes: 16 additions & 0 deletions rclcpp_lifecycle/src/lifecycle_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,22 @@ LifecycleNode::LifecycleNode(

LifecycleNode::~LifecycleNode()
{
// shutdown if necessary to avoid leaving the device in unknown state
if (LifecycleNode::get_current_state().id() !=
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());
}
}

// release sub-interfaces in an order that allows them to consult with node_base during tear-down
node_waitables_.reset();
node_time_source_.reset();
Expand Down
140 changes: 140 additions & 0 deletions rclcpp_lifecycle/test/test_lifecycle_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<EmptyLifecycleNode>("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<EmptyLifecycleNode>("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<EmptyLifecycleNode>("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<EmptyLifecycleNode>("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<rclcpp_lifecycle::LifecycleNode>("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<rclcpp_lifecycle::LifecycleNode>("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<rclcpp_lifecycle::LifecycleNode>("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<rclcpp_lifecycle::LifecycleNode>("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<MoodyLifecycleNode<GoodMood>>("testnode");

Expand Down

0 comments on commit 04ea0bb

Please sign in to comment.