Fix: on_shutdown callback of controllers never get executed (#1995)

Author: Wiktor-99

controller_manager/include/controller_manager/controller_manager.hpp

@@ -317,6 +317,13 @@ class ControllerManager : public rclcpp::Node
 
   void initialize_parameters();
 
+  /**
+   * Call shutdown to change the given controller lifecycle node to the finalized state.
+   *
+   * \param[in] controller controller to be shutdown.
+   */
+  void shutdown_controller(controller_manager::ControllerSpec & controller) const;
+
   /**
    * Clear request lists used when switching controllers. The lists are shared between "callback"
    * and "control loop" threads.

controller_manager/src/controller_manager.cpp

@@ -58,6 +58,13 @@ static const rmw_qos_profile_t qos_services = {
   false};
 #endif
 
+inline bool is_controller_unconfigured(
+  const controller_interface::ControllerInterfaceBase & controller)
+{
+  return controller.get_lifecycle_state().id() ==
+         lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
+}
+
 inline bool is_controller_inactive(const controller_interface::ControllerInterfaceBase & controller)
 {
   return controller.get_lifecycle_state().id() ==
@@ -695,39 +702,17 @@ controller_interface::return_type ControllerManager::unload_controller(
     return controller_interface::return_type::ERROR;
   }
 
-  RCLCPP_DEBUG(get_logger(), "Cleanup controller");
+  RCLCPP_DEBUG(get_logger(), "Shutdown controller");
   controller_chain_spec_cleanup(controller_chain_spec_, controller_name);
   // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
   // cleaning-up controllers?
-  if (is_controller_inactive(*controller.c))
+  if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
   {
     RCLCPP_DEBUG(
-      get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str());
+      get_logger(), "Controller '%s' is shutdown before unloading!", controller_name.c_str());
     // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
     // cleaning-up controllers?
-    try
-    {
-      const auto new_state = controller.c->get_node()->cleanup();
-      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
-      {
-        RCLCPP_WARN(
-          get_logger(), "Failed to clean-up the controller '%s' before unloading!",
-          controller_name.c_str());
-      }
-    }
-    catch (const std::exception & e)
-    {
-      RCLCPP_ERROR(
-        get_logger(),
-        "Caught exception of type : %s while cleaning up the controller '%s' before unloading: %s",
-        typeid(e).name(), controller_name.c_str(), e.what());
-    }
-    catch (...)
-    {
-      RCLCPP_ERROR(
-        get_logger(), "Failed to clean-up the controller '%s' before unloading",
-        controller_name.c_str());
-    }
+    shutdown_controller(controller);
   }
   executor_->remove_node(controller.c->get_node()->get_node_base_interface());
   to.erase(found_it);
@@ -745,6 +730,33 @@ controller_interface::return_type ControllerManager::unload_controller(
   return controller_interface::return_type::OK;
 }
 
+void ControllerManager::shutdown_controller(controller_manager::ControllerSpec & controller) const
+{
+  try
+  {
+    const auto new_state = controller.c->get_node()->shutdown();
+    if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+    {
+      RCLCPP_WARN(
+        get_logger(), "Failed to shutdown the controller '%s' before unloading!",
+        controller.info.name.c_str());
+    }
+  }
+  catch (const std::exception & e)
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "Caught exception of type : %s while shutdown the controller '%s' before unloading: %s",
+      typeid(e).name(), controller.info.name.c_str(), e.what());
+  }
+  catch (...)
+  {
+    RCLCPP_ERROR(
+      get_logger(), "Failed to shutdown the controller '%s' before unloading",
+      controller.info.name.c_str());
+  }
+}
+
 std::vector<ControllerSpec> ControllerManager::get_loaded_controllers() const
 {
   std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);

controller_manager/test/test_controller/test_controller.cpp

@@ -160,6 +160,15 @@ CallbackReturn TestController::on_cleanup(const rclcpp_lifecycle::State & /*prev
   return CallbackReturn::SUCCESS;
 }
 
+CallbackReturn TestController::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  if (shutdown_calls)
+  {
+    (*shutdown_calls)++;
+  }
+  return CallbackReturn::SUCCESS;
+}
+
 void TestController::set_command_interface_configuration(
   const controller_interface::InterfaceConfiguration & cfg)
 {

controller_manager/test/test_controller/test_controller.hpp

@@ -54,6 +54,8 @@ class TestController : public controller_interface::ControllerInterface
 
   CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
 
+  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
+
   void set_command_interface_configuration(
     const controller_interface::InterfaceConfiguration & cfg);
 
@@ -66,9 +68,10 @@ class TestController : public controller_interface::ControllerInterface
   rclcpp::Service<example_interfaces::srv::SetBool>::SharedPtr service_;
   unsigned int internal_counter = 0;
   bool simulate_cleanup_failure = false;
-  // Variable where we store when cleanup was called, pointer because the controller
-  // is usually destroyed after cleanup
+  // Variable where we store when shutdown was called, pointer because the controller
+  // is usually destroyed after shutdown
   size_t * cleanup_calls = nullptr;
+  size_t * shutdown_calls = nullptr;
   controller_interface::InterfaceConfiguration cmd_iface_cfg_;
   controller_interface::InterfaceConfiguration state_iface_cfg_;
 

controller_manager/test/test_controller_manager.cpp

@@ -228,7 +228,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
   EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(1, test_controller.use_count());
 }
@@ -429,7 +429,7 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
   EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
 
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(1, test_controller.use_count());
 }

controller_manager/test/test_controller_manager_srvs.cpp

@@ -395,14 +395,14 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv)
   ASSERT_GT(test_controller.use_count(), 1)
     << "Controller manager should have have a copy of this shared ptr";
 
-  cleanup_calls = 0;
-  test_controller->cleanup_calls = &cleanup_calls;
+  size_t shutdown_calls = 0;
+  test_controller->shutdown_calls = &shutdown_calls;
   test_controller.reset();  // destroy our copy of the controller
 
   request->force_kill = false;
   result = call_service_and_wait(*client, request, srv_executor, true);
   ASSERT_TRUE(result->ok);
-  ASSERT_EQ(cleanup_calls, 1u);
+  ASSERT_EQ(shutdown_calls, 1u);
   ASSERT_EQ(test_controller.use_count(), 0)
     << "No more references to the controller after reloading.";
   test_controller.reset();
@@ -428,8 +428,8 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv)
     << "Controller manager should still have have a copy of "
        "this shared ptr, no unloading was performed";
 
-  cleanup_calls = 0;
-  test_controller->cleanup_calls = &cleanup_calls;
+  shutdown_calls = 0;
+  test_controller->shutdown_calls = &shutdown_calls;
   test_controller.reset();  // destroy our copy of the controller
 
   // Force stop active controller
@@ -439,7 +439,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv)
 
   ASSERT_EQ(test_controller_weak.use_count(), 0)
     << "No more references to the controller after reloading.";
-  ASSERT_EQ(cleanup_calls, 1u)
+  ASSERT_EQ(shutdown_calls, 1u)
     << "Controller should have been stopped and cleaned up with force_kill = true";
 }
 
@@ -491,7 +491,7 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv)
   result = call_service_and_wait(*client, request, srv_executor, true);
   ASSERT_TRUE(result->ok);
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 }
@@ -526,7 +526,7 @@ TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controlle
   unload_request->name = test_controller::TEST_CONTROLLER_NAME;
   auto result = call_service_and_wait(*unload_client, unload_request, srv_executor, true);
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 
@@ -578,7 +578,7 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
   unload_request->name = test_controller::TEST_CONTROLLER_NAME;
   ASSERT_TRUE(call_service_and_wait(*unload_client, unload_request, srv_executor, true)->ok);
   EXPECT_EQ(
-    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
     test_controller->get_lifecycle_state().id());
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 }

controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp

@@ -45,6 +45,12 @@ TestControllerWithInterfaces::on_cleanup(const rclcpp_lifecycle::State & /*previ
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerWithInterfaces::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
 }  // namespace test_controller_with_interfaces
 
 #include "pluginlib/class_list_macros.hpp"

controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp

@@ -54,6 +54,9 @@ class TestControllerWithInterfaces : public controller_interface::ControllerInte
 
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
     const rclcpp_lifecycle::State & previous_state) override;
+
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
 };
 
 }  // namespace test_controller_with_interfaces