diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9dee4528a3..a548cd5856 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -99,7 +99,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy public: ControllerInterfaceBase() = default; - virtual ~ControllerInterfaceBase() = default; + virtual ~ControllerInterfaceBase(); /// Get configuration for controller's required command interfaces. /** diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 03d45fa384..b8dd9f770d 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -22,6 +22,22 @@ namespace controller_interface { +ControllerInterfaceBase::~ControllerInterfaceBase() +{ + // check if node is initialized and we still have a valid context + if ( + node_.get() && + get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED && + rclcpp::ok()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), + "Calling shutdown transition of controller node '%s' due to destruction.", + get_node()->get_name()); + node_->shutdown(); + } +} + return_type ControllerInterfaceBase::init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & node_namespace, const rclcpp::NodeOptions & node_options) @@ -52,6 +68,11 @@ return_type ControllerInterfaceBase::init( break; case LifecycleNodeInterface::CallbackReturn::ERROR: case LifecycleNodeInterface::CallbackReturn::FAILURE: + RCLCPP_DEBUG( + get_node()->get_logger(), + "Calling shutdown transition of controller node '%s' due to init failure.", + get_node()->get_name()); + node_->shutdown(); return return_type::ERROR; } @@ -158,6 +179,10 @@ void ControllerInterfaceBase::release_interfaces() const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const { + if (!node_.get()) + { + throw std::runtime_error("Lifecycle node hasn't been initialized yet!"); + } return node_->get_current_state(); } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 4204d32f45..c5197becdc 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -36,9 +36,12 @@ TEST(TestableControllerInterface, init) rclcpp::init(argc, argv); TestableControllerInterface controller; + const TestableControllerInterface & const_controller = controller; // try to get node when not initialized ASSERT_THROW(controller.get_node(), std::runtime_error); + ASSERT_THROW(const_controller.get_node(), std::runtime_error); + ASSERT_THROW(controller.get_lifecycle_state(), std::runtime_error); // initialize, create node const auto node_options = controller.define_custom_node_options(); @@ -46,6 +49,8 @@ TEST(TestableControllerInterface, init) controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options), controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); + ASSERT_NO_THROW(const_controller.get_node()); + ASSERT_NO_THROW(controller.get_lifecycle_state()); // update_rate is set to controller_manager's rate ASSERT_EQ(controller.get_update_rate(), 10u); @@ -54,6 +59,7 @@ TEST(TestableControllerInterface, init) controller.configure(); ASSERT_EQ(controller.get_update_rate(), 10u); + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -80,6 +86,8 @@ TEST(TestableControllerInterface, setting_negative_update_rate_in_configure) // The configure should fail and the update rate should stay the same ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ(controller.get_update_rate(), 1000u); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -149,6 +157,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) ASSERT_EQ(controller.get_update_rate(), 623u); executor->cancel(); + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -166,6 +175,8 @@ TEST(TestableControllerInterfaceInitError, init_with_error) controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); rclcpp::shutdown(); } @@ -183,6 +194,8 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure) controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options), controller_interface::return_type::ERROR); + ASSERT_EQ( + controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); rclcpp::shutdown(); } diff --git a/controller_interface/test/test_controller_with_options.cpp b/controller_interface/test/test_controller_with_options.cpp index 5829bcbcdd..eb60c19aca 100644 --- a/controller_interface/test/test_controller_with_options.cpp +++ b/controller_interface/test/test_controller_with_options.cpp @@ -41,7 +41,7 @@ TEST(ControllerWithOption, init_with_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), controller_interface::return_type::OK); // checks that the node options have been updated @@ -54,6 +54,8 @@ TEST(ControllerWithOption, init_with_overrides) EXPECT_EQ(controller.params["parameter1"], 1.); EXPECT_EQ(controller.params["parameter2"], 2.); EXPECT_EQ(controller.params["parameter3"], 3.); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -68,7 +70,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters) controller_node_options.arguments( {"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.", "-p", "parameter_list.parameter3:=3."}); - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller_node_options), controller_interface::return_type::OK); // checks that the node options have been updated @@ -81,6 +83,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters) EXPECT_EQ(controller.params["parameter1"], 1.); EXPECT_EQ(controller.params["parameter2"], 2.); EXPECT_EQ(controller.params["parameter3"], 3.); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -96,7 +100,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file) std::cerr << params_file_path << std::endl; auto controller_node_options = controller.define_custom_node_options(); controller_node_options.arguments({"--ros-args", "--params-file", params_file_path}); - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller_node_options), controller_interface::return_type::OK); // checks that the node options have been updated @@ -112,6 +116,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file) bool use_sim_time(true); controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); ASSERT_FALSE(use_sim_time); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -130,7 +136,7 @@ TEST( controller_node_options.arguments( {"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785", "-p", "use_sim_time:=true"}); - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller_node_options), controller_interface::return_type::OK); // checks that the node options have been updated @@ -146,6 +152,8 @@ TEST( bool use_sim_time(false); controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false); ASSERT_TRUE(use_sim_time); + + controller.get_node()->shutdown(); rclcpp::shutdown(); } @@ -157,7 +165,7 @@ TEST(ControllerWithOption, init_without_overrides) rclcpp::init(argc, argv); // creates the controller FriendControllerWithOptions controller; - EXPECT_EQ( + ASSERT_EQ( controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()), controller_interface::return_type::ERROR); // checks that the node options have been updated @@ -166,5 +174,6 @@ TEST(ControllerWithOption, init_without_overrides) EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides()); // checks that no parameter has been declared from overrides EXPECT_EQ(controller.params.size(), 0u); + rclcpp::shutdown(); } diff --git a/joint_limits/test/joint_limits_rosparam_test.cpp b/joint_limits/test/joint_limits_rosparam_test.cpp index 7d03eb20ca..8b0b6e62d8 100644 --- a/joint_limits/test/joint_limits_rosparam_test.cpp +++ b/joint_limits/test/joint_limits_rosparam_test.cpp @@ -17,6 +17,7 @@ #include #include "joint_limits/joint_limits_rosparam.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" class JointLimitsRosParamTest : public ::testing::Test @@ -294,7 +295,17 @@ class JointLimitsLifecycleNodeUndeclaredRosParamTest : public ::testing::Test lifecycle_node_ = rclcpp_lifecycle::LifecycleNode::make_shared("JointLimitsRosparamTestNode"); } - void TearDown() { lifecycle_node_.reset(); } + void TearDown() + { + if ( + lifecycle_node_->get_current_state().id() != + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED && + rclcpp::ok()) + { + lifecycle_node_->shutdown(); + } + lifecycle_node_.reset(); + } protected: rclcpp_lifecycle::LifecycleNode::SharedPtr lifecycle_node_;