added suggestions from ros-controls#1236

Author: soham2560

controller_manager/src/controller_manager.cpp

@@ -947,12 +947,6 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
 controller_interface::return_type ControllerManager::unload_controller(
   const std::string & controller_name)
 {
-  // first find and clean controller if it is inactive
-  if (cleanup_controller(controller_name) != controller_interface::return_type::OK)
-  {
-    return controller_interface::return_type::ERROR;
-  }
-
   RCLCPP_INFO(get_logger(), "Unloading controller: '%s'", controller_name.c_str());
   std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
   std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);
@@ -986,9 +980,14 @@ controller_interface::return_type ControllerManager::unload_controller(
     return controller_interface::return_type::ERROR;
   }
 
+  // find and clean controller if it is inactive
+  if (cleanup_controller(controller_name) != controller_interface::return_type::OK)
+  {
+    return controller_interface::return_type::ERROR;
+  }
+
   RCLCPP_DEBUG(get_logger(), "Shutdown controller");
   controller_chain_spec_cleanup(controller_chain_spec_, controller_name);
-  cleanup_controller_exported_interfaces(controller);
   if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
   {
     RCLCPP_DEBUG(

controller_manager/test/test_controller_manager_srvs.cpp

@@ -490,6 +490,9 @@ TEST_F(TestControllerManagerSrvs, unconfigure_controller_srv)
   // scenario: call the cleanup service when no controllers are loaded
   // expected: it should return ERROR as no controllers will be found to cleanup
   auto result = call_service_and_wait(*client, request, srv_executor);
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
   ASSERT_FALSE(result->ok) << "Controller not loaded: " << request->name;
 
   // variation - 2:
@@ -503,6 +506,9 @@ TEST_F(TestControllerManagerSrvs, unconfigure_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,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
   EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
 
   // variation - 3:
@@ -520,6 +526,9 @@ TEST_F(TestControllerManagerSrvs, unconfigure_controller_srv)
     cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
   result = call_service_and_wait(*client, request, srv_executor, true);
   ASSERT_FALSE(result->ok) << "Controller can not be cleaned in active state: " << request->name;
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
   EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
 
   // variation - 4:
@@ -533,6 +542,9 @@ TEST_F(TestControllerManagerSrvs, unconfigure_controller_srv)
     cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
   result = call_service_and_wait(*client, request, srv_executor, true);
   ASSERT_TRUE(result->ok) << "Controller cleaned in inactive state: " << request->name;
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
   EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
 }