diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
index 2f326ff302..810936554e 100644
--- a/controller_manager/include/controller_manager/controller_manager.hpp
+++ b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -583,14 +583,25 @@ class ControllerManager : public rclcpp::Node
 
   struct SwitchParams
   {
-    bool do_switch = {false};
-    bool started = {false};
-    rclcpp::Time init_time = {rclcpp::Time::max()};
+    void reset()
+    {
+      do_switch = false;
+      started = false;
+      strictness = 0;
+      activate_asap = false;
+    }
+
+    bool do_switch;
+    bool started;
 
     // Switch options
-    int strictness = {0};
-    bool activate_asap = {false};
-    rclcpp::Duration timeout = rclcpp::Duration{0, 0};
+    int strictness;
+    bool activate_asap;
+    std::chrono::nanoseconds timeout;
+
+    // conditional variable and mutex to wait for the switch to complete
+    std::condition_variable cv;
+    std::mutex mutex;
   };
 
   SwitchParams switch_params_;
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 48dbbc720a..68ee9e6b52 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -877,7 +877,8 @@ controller_interface::return_type ControllerManager::switch_controller(
     return controller_interface::return_type::ERROR;
   }
 
-  switch_params_ = SwitchParams();
+  // reset the switch param internal variables
+  switch_params_.reset();
 
   if (!deactivate_request_.empty() || !activate_request_.empty())
   {
@@ -1296,21 +1297,30 @@ controller_interface::return_type ControllerManager::switch_controller(
   }
 
   // start the atomic controller switching
+  std::unique_lock<std::mutex> switch_params_guard(switch_params_.mutex);
   switch_params_.strictness = strictness;
   switch_params_.activate_asap = activate_asap;
-  switch_params_.init_time = rclcpp::Clock().now();
-  switch_params_.timeout = timeout;
+  if (timeout == rclcpp::Duration{0, 0})
+  {
+    RCLCPP_INFO_ONCE(get_logger(), "Switch controller timeout is set to 0, using default 1s!");
+    switch_params_.timeout = std::chrono::nanoseconds(1'000'000'000);
+  }
+  else
+  {
+    switch_params_.timeout = timeout.to_chrono<std::chrono::nanoseconds>();
+  }
   switch_params_.do_switch = true;
 
   // wait until switch is finished
   RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop");
-  while (rclcpp::ok() && switch_params_.do_switch)
+  if (!switch_params_.cv.wait_for(
+        switch_params_guard, switch_params_.timeout, [this] { return !switch_params_.do_switch; }))
   {
-    if (!rclcpp::ok())
-    {
-      return controller_interface::return_type::ERROR;
-    }
-    std::this_thread::sleep_for(std::chrono::microseconds(100));
+    RCLCPP_ERROR(
+      get_logger(), "Switch controller timed out after %f seconds!",
+      static_cast<double>(switch_params_.timeout.count()) / 1e9);
+    clear_requests();
+    return controller_interface::return_type::ERROR;
   }
 
   // copy the controllers spec from the used to the unused list
@@ -2146,6 +2156,7 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
 
 void ControllerManager::manage_switch()
 {
+  std::lock_guard<std::mutex> guard(switch_params_.mutex);
   // Ask hardware interfaces to change mode
   if (!resource_manager_->perform_command_mode_switch(
         activate_command_interface_request_, deactivate_command_interface_request_))
@@ -2174,6 +2185,7 @@ void ControllerManager::manage_switch()
 
   // All controllers switched --> switching done
   switch_params_.do_switch = false;
+  switch_params_.cv.notify_all();
 }
 
 controller_interface::return_type ControllerManager::update(