From 5c2cc5551016764dd34c3352bb19f552de5110be Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Mon, 28 Aug 2023 00:05:12 +0200
Subject: [PATCH] use ControllerManagerRunner to avoid blocking when switching
 lists

---
 .../test/test_controller_manager.cpp          | 22 ++++++++++++++-----
 .../test/test_load_controller.cpp             | 15 ++++++++++---
 2 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
index 6db4cfd1b2c..a5566ee9b59 100644
--- a/controller_manager/test/test_controller_manager.cpp
+++ b/controller_manager/test/test_controller_manager.cpp
@@ -104,8 +104,11 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
 
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
-  cm_->configure_controller(TEST_CONTROLLER2_NAME);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+    cm_->configure_controller(TEST_CONTROLLER2_NAME);
+  }
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
@@ -217,7 +220,10 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
 
   test_controller->get_node()->set_parameter({"update_rate", 4});
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  }
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
@@ -296,7 +302,10 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
   rclcpp::Parameter update_rate_parameter("update_rate", static_cast<int>(ctrl_update_rate));
   test_controller->get_node()->set_parameter(update_rate_parameter);
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  }
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
@@ -390,7 +399,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
 
   test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  }
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp
index ca2f7f6b1cb..390a7365f0e 100644
--- a/controller_manager/test/test_load_controller.cpp
+++ b/controller_manager/test/test_load_controller.cpp
@@ -162,7 +162,10 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller)
       lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id());
 
     // Activate configured controller
-    cm_->configure_controller(controller_name1);
+    {
+      ControllerManagerRunner cm_runner(this);
+      cm_->configure_controller(controller_name1);
+    }
     start_test_controller(test_param.strictness);
     ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id());
   }
@@ -246,7 +249,10 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure
   test_controller->cleanup_calls = &cleanup_calls;
   // Configure from inactive state
   test_controller->simulate_cleanup_failure = false;
-  EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK);
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK);
+  }
   ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id());
   EXPECT_EQ(1u, cleanup_calls);
 }
@@ -421,7 +427,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers)
   ASSERT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id());
 
-  cm_->configure_controller(controller_name2);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->configure_controller(controller_name2);
+  }
 
   // Stop controller 1
   RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1");