diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 3fa0b34f94..aace768983 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -411,7 +411,8 @@ controller_interface::return_type ControllerManager::configure_controller(
 {
   RCLCPP_INFO(get_logger(), "Configuring controller '%s'", controller_name.c_str());
 
-  const auto & controllers = get_loaded_controllers();
+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
+  const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(guard);
 
   auto found_it = std::find_if(
     controllers.begin(), controllers.end(),
@@ -428,6 +429,7 @@ controller_interface::return_type ControllerManager::configure_controller(
   auto controller = found_it->c;
 
   auto state = controller->get_state();
+
   if (
     state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
     state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
@@ -481,10 +483,112 @@ controller_interface::return_type ControllerManager::configure_controller(
       return controller_interface::return_type::ERROR;
     }
     resource_manager_->import_controller_reference_interfaces(controller_name, interfaces);
+  }
+
+  std::vector<ControllerSpec> & unused_controller_list =
+    rt_controllers_wrapper_.get_unused_list(guard);
+  unused_controller_list = controllers;
+
+  std::unordered_map<std::string, int> preceding_controller_count = {};
+  std::unordered_map<std::string, std::vector<std::string>> following_controllers = {};
+
+  for (const auto & controller1 : unused_controller_list)
+  {
+    if (
+      !controller1.c->is_chainable() ||
+      (!is_controller_active(controller1.c) && !is_controller_inactive(controller1.c)))
+    {
+      continue;
+    }
+    for (const auto & controller2 : unused_controller_list)
+    {
+      if (!is_controller_active(controller2.c) && !is_controller_inactive(controller2.c))
+      {
+        continue;
+      }
+      if (controller2.info.name == controller1.info.name)
+      {
+        continue;
+      }
+      for (const auto & reference1 :
+           resource_manager_->get_controller_reference_interface_names(controller1.info.name))
+      {
+        const auto controller2_commands = controller2.c->command_interface_configuration().names;
+
+        std::string commands_string = "";
+        for (auto command : controller2_commands)
+        {
+          commands_string += " " + command;
+        }
+        auto same = std::find(controller2_commands.begin(), controller2_commands.end(), reference1);
+        if (same != controller2_commands.end())
+        {
+          // controller 2 precedes controller1
+          if (following_controllers.find(controller2.info.name) != following_controllers.end())
+          {
+            following_controllers[controller2.info.name].push_back(controller1.info.name);
+          }
+          else
+          {
+            following_controllers.emplace(
+              controller2.info.name, std::initializer_list<std::string>{controller1.info.name});
+          }
+          if (
+            preceding_controller_count.find(controller1.info.name) !=
+            preceding_controller_count.end())
+          {
+            preceding_controller_count[controller1.info.name] += 1;
+          }
+          else
+          {
+            preceding_controller_count[controller1.info.name] = 1;
+          }
+          break;
+        }
+      }
+    }
+  }
+
+  // Use Kahn's algorithm for topological sorting
+  std::deque<std::string> controllers_to_add = {};
+  for (const auto & controller : unused_controller_list)
+  {
+    if (preceding_controller_count.find(controller.info.name) == preceding_controller_count.end())
+    {
+      controllers_to_add.push_back(controller.info.name);
+    }
+  }
 
-    // TODO(destogl): check and resort controllers in the vector
+  for (auto i = unused_controller_list.begin(); i != unused_controller_list.end(); i++)
+  {
+    std::string controller_name_to_find = controllers_to_add.front();
+    controllers_to_add.pop_front();
+    auto found_it = std::find_if(
+      unused_controller_list.begin(), unused_controller_list.end(),
+      std::bind(controller_name_compare, std::placeholders::_1, controller_name_to_find));
+    iter_swap(i, found_it);
+    if (following_controllers.find(controller_name_to_find) != following_controllers.end())
+    {
+      for (const auto & controller_name : following_controllers[controller_name_to_find])
+      {
+        preceding_controller_count[controller_name] -= 1;
+        if (preceding_controller_count[controller_name] == 0)
+        {
+          controllers_to_add.push_back(controller_name);
+        }
+      }
+    }
+  }
+
+  std::string controller_names = "";
+  for (auto & cont : unused_controller_list)
+  {
+    controller_names += cont.info.name + " ";
   }
 
+  rt_controllers_wrapper_.switch_updated_list(guard);
+  rt_controllers_wrapper_.get_unused_list(guard).clear();
+
   return controller_interface::return_type::OK;
 }
 
diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
index 22221b6e21..54ec14f562 100644
--- a/controller_manager/test/test_controller_manager.cpp
+++ b/controller_manager/test/test_controller_manager.cpp
@@ -104,11 +104,24 @@ TEST_P(TestControllerManager, 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);
+  auto configure_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
+    test_controller::TEST_CONTROLLER_NAME);
+  configure_future.wait_for(std::chrono::milliseconds(100));
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK);
+
+  configure_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
+    TEST_CONTROLLER2_NAME);
+  configure_future.wait_for(std::chrono::milliseconds(100));
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK);
+
   EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
   EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";
 
@@ -216,11 +229,16 @@ TEST_P(TestControllerManager, per_controller_update_rate)
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
 
   test_controller->get_node()->set_parameter({"update_rate", 4});
+
   // configure controller
-  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  auto configure_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
+    test_controller::TEST_CONTROLLER_NAME);
+  configure_future.wait_for(std::chrono::milliseconds(100));
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK);
   EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
 
   EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp
index adb227d6fe..f001a8aa9f 100644
--- a/controller_manager/test/test_controller_manager_srvs.cpp
+++ b/controller_manager/test/test_controller_manager_srvs.cpp
@@ -262,24 +262,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
   result = call_service_and_wait(*client, request, srv_executor);
   ASSERT_EQ(2u, result->controller.size());
   // check chainable controller
-  ASSERT_EQ(result->controller[0].state, "inactive");
-  ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u);
-  ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u);
-  ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u);
-  ASSERT_EQ(result->controller[0].is_chainable, true);
-  ASSERT_EQ(result->controller[0].is_chained, false);
-  ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u);
-  ;
-  ASSERT_EQ(result->controller[0].chain_connections.size(), 0u);
-  // check test controller
+
+  // cm_->configure_controller() sorts the controllers
+  // test controller now precedes chainable controller
+  ASSERT_EQ(result->controller[1].name, "test_chainable_controller_name");
+  ASSERT_EQ(result->controller[1].type, "controller_manager/test_chainable_controller");
   ASSERT_EQ(result->controller[1].state, "inactive");
   ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u);
-  ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u);
+  ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u);
   ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u);
-  ASSERT_EQ(result->controller[1].is_chainable, false);
+  ASSERT_EQ(result->controller[1].is_chainable, true);
   ASSERT_EQ(result->controller[1].is_chained, false);
-  ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u);
-  ASSERT_EQ(result->controller[1].chain_connections.size(), 1u);
+  ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u);
+  ASSERT_EQ(result->controller[1].chain_connections.size(), 0u);
+  // check test controller
+  ASSERT_EQ(result->controller[0].state, "inactive");
+  ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u);
+  ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u);
+  ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u);
+  ASSERT_EQ(result->controller[0].is_chainable, false);
+  ASSERT_EQ(result->controller[0].is_chained, false);
+  ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u);
+  ASSERT_EQ(result->controller[0].chain_connections.size(), 1u);
   // activate controllers
   cm_->switch_controller(
     {test_chainable_controller::TEST_CONTROLLER_NAME}, {},
@@ -290,18 +294,18 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
   // get controller list after activate
   result = call_service_and_wait(*client, request, srv_executor);
   // check chainable controller
-  ASSERT_EQ(result->controller[0].state, "active");
-  ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 1u);
-  ASSERT_EQ(result->controller[0].is_chained, true);
-  // check test controller
   ASSERT_EQ(result->controller[1].state, "active");
-  ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 3u);
+  ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 1u);
+  ASSERT_EQ(result->controller[1].is_chained, true);
+  // check test controller
+  ASSERT_EQ(result->controller[0].state, "active");
+  ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 3u);
   ASSERT_EQ(
     test_chainable_controller::TEST_CONTROLLER_NAME,
-    result->controller[1].chain_connections[0].name);
-  ASSERT_EQ(2u, result->controller[1].chain_connections[0].reference_interfaces.size());
-  ASSERT_EQ("joint1/position", result->controller[1].chain_connections[0].reference_interfaces[0]);
-  ASSERT_EQ("joint1/velocity", result->controller[1].chain_connections[0].reference_interfaces[1]);
+    result->controller[0].chain_connections[0].name);
+  ASSERT_EQ(2u, result->controller[0].chain_connections[0].reference_interfaces.size());
+  ASSERT_EQ("joint1/position", result->controller[0].chain_connections[0].reference_interfaces[0]);
+  ASSERT_EQ("joint1/velocity", result->controller[0].chain_connections[0].reference_interfaces[1]);
 }
 
 TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv)
diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp
index 62babe998d..f12fc0761c 100644
--- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp
+++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp
@@ -52,6 +52,7 @@ class TestableTestChainableController : public test_chainable_controller::TestCh
   FRIEND_TEST(
     TestControllerChainingWithControllerManager,
     test_chained_controllers_deactivation_error_handling);
+  FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_random_order);
 };
 
 class TestableControllerManager : public controller_manager::ControllerManager
@@ -84,6 +85,7 @@ class TestableControllerManager : public controller_manager::ControllerManager
   FRIEND_TEST(
     TestControllerChainingWithControllerManager,
     test_chained_controllers_deactivation_error_handling);
+  FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_random_order);
 
 public:
   TestableControllerManager(
@@ -975,10 +977,91 @@ TEST_P(
     lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id());
 }
 
-// TODO(destogl): Add test case with controllers added in "random" order
-//
 // new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain
 //
+TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_random_order)
+{
+  SetupControllers();
+
+  // add all controllers - CONTROLLERS HAVE TO ADDED IN REVERSE EXECUTION ORDER
+  cm_->add_controller(
+    pid_left_wheel_controller, PID_LEFT_WHEEL,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    pid_right_wheel_controller, PID_RIGHT_WHEEL,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    diff_drive_controller, DIFF_DRIVE_CONTROLLER,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    position_tracking_controller, POSITION_TRACKING_CONTROLLER,
+    test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
+
+  CheckIfControllersAreAddedCorrectly();
+
+  ConfigureAndCheckControllers();
+
+  SetToChainedModeAndMakeReferenceInterfacesAvailable(
+    pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES);
+  SetToChainedModeAndMakeReferenceInterfacesAvailable(
+    pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES);
+  SetToChainedModeAndMakeReferenceInterfacesAvailable(
+    diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES);
+
+  EXPECT_THROW(
+    cm_->resource_manager_->make_controller_reference_interfaces_available(
+      POSITION_TRACKING_CONTROLLER),
+    std::out_of_range);
+
+  // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
+  cm_->get_logger().set_level(rclcpp::Logger::Level::Debug);
+
+  // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER
+  // (otherwise, interface will be missing)
+  ActivateAndCheckController(
+    pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u);
+  ActivateAndCheckController(
+    pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u);
+  ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u);
+
+  // Diff-Drive Controller claims the reference interfaces of PID controllers
+  ActivateAndCheckController(
+    diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u);
+  ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u);
+  ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u);
+
+  // Position-Tracking Controller uses reference interfaces of Diff-Drive Controller
+  ActivateAndCheckController(
+    position_tracking_controller, POSITION_TRACKING_CONTROLLER,
+    POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u);
+  // 'rot_z' reference interface is not claimed
+  for (const auto & interface : {"diff_drive_controller/rot_z"})
+  {
+    EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface));
+    EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface));
+    EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface));
+  }
+  ASSERT_EQ(diff_drive_controller->internal_counter, 3u);
+  ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u);
+  ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u);
+
+  // update controllers
+  std::vector<double> reference = {32.0, 128.0};
+
+  // update 'Position Tracking' controller
+  for (auto & value : diff_drive_controller->reference_interfaces_)
+  {
+    ASSERT_EQ(value, 0.0);  // default reference values are 0.0
+  }
+
+  // update all controllers at once and see that all have expected values --> also checks the order
+  // of controller execution
+  reference = {1024.0, 4096.0};
+  UpdateAllControllerAndCheck(reference, 2u);
+}
 
 INSTANTIATE_TEST_SUITE_P(
   test_strict_best_effort, TestControllerChainingWithControllerManager,
diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp
index ca2f7f6b1c..3ff6ee67a2 100644
--- a/controller_manager/test/test_load_controller.cpp
+++ b/controller_manager/test/test_load_controller.cpp
@@ -162,8 +162,17 @@ 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);
+    auto configure_future = std::async(
+      std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
+      controller_name1);
+    configure_future.wait_for(std::chrono::milliseconds(100));
+
+    EXPECT_EQ(
+      controller_interface::return_type::OK,
+      cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+    configure_future.get();
     start_test_controller(test_param.strictness);
+
     ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id());
   }
 
@@ -246,7 +255,17 @@ 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);
+
+  auto configure_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
+    controller_name1);
+  configure_future.wait_for(std::chrono::milliseconds(100));
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(configure_future.get(), 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 +440,13 @@ 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);
+  auto configure_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
+    controller_name2);
+  configure_future.wait_for(std::chrono::milliseconds(1000));
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
 
   // Stop controller 1
   RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1");