check for state of the controller node before cleanup (#1363)

(cherry picked from commit 6f57faf)

# Conflicts:
#	controller_manager/test/test_controller_manager_srvs.cpp

controller_manager/src/controller_manager.cpp

@@ -667,7 +667,20 @@ controller_interface::return_type ControllerManager::unload_controller(
   RCLCPP_DEBUG(get_logger(), "Cleanup controller");
   // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
   // cleaning-up controllers?
-  controller.c->get_node()->cleanup();
+  if (is_controller_inactive(*controller.c))
+  {
+    RCLCPP_DEBUG(
+      get_logger(), "Controller '%s' is cleaned-up before unloading!", controller_name.c_str());
+    // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for
+    // cleaning-up controllers?
+    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());
+    }
+  }
   executor_->remove_node(controller.c->get_node()->get_node_base_interface());
   to.erase(found_it);
 

controller_manager/test/test_controller_manager_srvs.cpp

@@ -462,9 +462,57 @@ 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, test_controller->get_state().id());
+  EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
+}
+
+<<<<<<< HEAD
+=======
+TEST_F(TestControllerManagerSrvs, robot_description_on_load_and_unload_controller)
+{
+  rclcpp::executors::SingleThreadedExecutor srv_executor;
+  rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>("srv_client");
+  srv_executor.add_node(srv_node);
+  rclcpp::Client<controller_manager_msgs::srv::UnloadController>::SharedPtr unload_client =
+    srv_node->create_client<controller_manager_msgs::srv::UnloadController>(
+      "test_controller_manager/unload_controller");
+
+  auto test_controller = std::make_shared<TestController>();
+  auto abstract_test_controller = cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+
+  // check the robot description
+  ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description());
+
+  // Now change the robot description and then see that the controller maintains the old URDF until
+  // it is unloaded and loaded again
+  auto msg = std_msgs::msg::String();
+  msg.data = ros2_control_test_assets::minimal_robot_missing_state_keys_urdf;
+  cm_->robot_description_callback(msg);
+  ASSERT_EQ(ros2_control_test_assets::minimal_robot_urdf, test_controller->get_robot_description());
+
+  // now unload and load the controller and see if the controller gets the new robot description
+  auto unload_request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>();
+  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, test_controller->get_state().id());
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
+
+  // now load it and check if it got the new robot description
+  cm_->add_controller(
+    test_controller, test_controller::TEST_CONTROLLER_NAME,
+    test_controller::TEST_CONTROLLER_CLASS_NAME);
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+  ASSERT_EQ(
+    ros2_control_test_assets::minimal_robot_missing_state_keys_urdf,
+    test_controller->get_robot_description());
 }
 
+>>>>>>> 6f57faf (check for state of the controller node before cleanup (#1363))
 TEST_F(TestControllerManagerSrvs, configure_controller_srv)
 {
   rclcpp::executors::SingleThreadedExecutor srv_executor;
@@ -473,6 +521,9 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
   rclcpp::Client<controller_manager_msgs::srv::ConfigureController>::SharedPtr client =
     srv_node->create_client<controller_manager_msgs::srv::ConfigureController>(
       "test_controller_manager/configure_controller");
+  rclcpp::Client<controller_manager_msgs::srv::UnloadController>::SharedPtr unload_client =
+    srv_node->create_client<controller_manager_msgs::srv::UnloadController>(
+      "test_controller_manager/unload_controller");
 
   auto request = std::make_shared<controller_manager_msgs::srv::ConfigureController::Request>();
   request->name = test_controller::TEST_CONTROLLER_NAME;
@@ -491,6 +542,15 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
     cm_->get_loaded_controllers()[0].c->get_state().id());
+  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
+
+  // now unload the controller and check the state
+  auto unload_request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>();
+  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, test_controller->get_state().id());
+  EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 }
 
 TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)