Trigger shutdown transition in destructor (backport #1979) (#2142)

Author: mergify[bot]

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -70,7 +70,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   ControllerInterfaceBase() = default;
 
   CONTROLLER_INTERFACE_PUBLIC
-  virtual ~ControllerInterfaceBase() = default;
+  virtual ~ControllerInterfaceBase();
 
   /// Get configuration for controller's required command interfaces.
   /**

controller_interface/src/controller_interface_base.cpp

@@ -24,6 +24,21 @@
 
 namespace controller_interface
 {
+ControllerInterfaceBase::~ControllerInterfaceBase()
+{
+  // check if node is initialized and we still have a valid context
+  if (
+    node_.get() && get_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 & namespace_,
   const rclcpp::NodeOptions & node_options)
@@ -47,6 +62,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;
   }
 
@@ -105,6 +125,10 @@ void ControllerInterfaceBase::release_interfaces()
 
 const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state() const
 {
+  if (!node_.get())
+  {
+    throw std::runtime_error("Lifecycle node hasn't been initialized yet!");
+  }
   return node_->get_current_state();
 }
 

controller_interface/test/test_controller_interface.cpp

@@ -17,6 +17,7 @@
 #include <gmock/gmock.h>
 #include <memory>
 
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/executor_options.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 
@@ -33,13 +34,18 @@ 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_state(), std::runtime_error);
 
   // initialize, create node
   ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK);
   ASSERT_NO_THROW(controller.get_node());
+  ASSERT_NO_THROW(const_controller.get_node());
+  ASSERT_NO_THROW(controller.get_state());
 
   // update_rate is set to default 0
   ASSERT_EQ(controller.get_update_rate(), 0u);
@@ -48,6 +54,7 @@ TEST(TestableControllerInterface, init)
   controller.configure();
   ASSERT_EQ(controller.get_update_rate(), 0u);
 
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -111,6 +118,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   ASSERT_EQ(controller.get_update_rate(), 623u);
 
   executor->cancel();
+  controller.get_node()->shutdown();
   rclcpp::shutdown();
 }
 
@@ -125,6 +133,7 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
   // initialize, create node
   ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR);
 
+  ASSERT_EQ(controller.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 
@@ -139,6 +148,7 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
   // initialize, create node
   ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR);
 
+  ASSERT_EQ(controller.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
   rclcpp::shutdown();
 }
 

controller_interface/test/test_controller_with_options.cpp

@@ -42,7 +42,7 @@ TEST(ControllerWithOption, init_with_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::OK);
+  ASSERT_EQ(controller.init("controller_name"), controller_interface::return_type::OK);
   // checks that the node options have been updated
   const auto & node_options = controller.get_node()->get_node_options();
   EXPECT_TRUE(node_options.allow_undeclared_parameters());
@@ -52,6 +52,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();
 }
 
@@ -63,12 +65,13 @@ TEST(ControllerWithOption, init_without_overrides)
   rclcpp::init(argc, argv);
   // creates the controller
   FriendControllerWithOptions controller;
-  EXPECT_EQ(controller.init("controller_name"), controller_interface::return_type::ERROR);
+  ASSERT_EQ(controller.init("controller_name"), controller_interface::return_type::ERROR);
   // checks that the node options have been updated
   const auto & node_options = controller.get_node()->get_node_options();
   EXPECT_TRUE(node_options.allow_undeclared_parameters());
   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();
 }

joint_limits/test/joint_limits_rosparam_test.cpp

@@ -19,6 +19,7 @@
 #include "gtest/gtest.h"
 
 #include "joint_limits/joint_limits_rosparam.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -283,7 +284,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_;