From 4705785926975191ac9f1a76ce4734d62eb365cd Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Sun, 22 Oct 2023 16:05:18 +0200
Subject: [PATCH] added test with loading and unloading the controller

---
 .../test/test_controller_manager_srvs.cpp     | 41 +++++++++++++++++++
 1 file changed, 41 insertions(+)

diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp
index 7873adaacf..c89762f5f0 100644
--- a/controller_manager/test/test_controller_manager_srvs.cpp
+++ b/controller_manager/test/test_controller_manager_srvs.cpp
@@ -456,6 +456,47 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv)
   EXPECT_EQ(0u, cm_->get_loaded_controllers().size());
 }
 
+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->getRobotDescription());
+
+  // Now change the robot description and then load a new controller and see if the new controller
+  // gets the new description and the old controller still maintains the configuration
+  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->getRobotDescription());
+
+  // 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(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->getRobotDescription());
+}
+
 TEST_F(TestControllerManagerSrvs, configure_controller_srv)
 {
   rclcpp::executors::SingleThreadedExecutor srv_executor;