Add wall clock timeout to avoid some deadlock situation when switchi…

…ng mode (#491) Co-authored-by: Krishna <chaitanyaradon89@gmail.com>
ros-controls · Jun 16, 2021 · c338014 · c338014
1 parent 345f5c3
commit c338014
Showing 1 changed file with 14 additions and 2 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -604,17 +604,29 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
 
   // wait until switch is finished
   ROS_DEBUG("Request atomic controller switch from realtime loop");
+  auto start_time = std::chrono::system_clock::now();
+  bool timed_out = false;
   while (ros::ok() && switch_params_.do_switch)
   {
     if (!ros::ok())
     {
       return false;
     }
-    std::this_thread::sleep_for(std::chrono::microseconds(100));
+    std::chrono::duration<double> diff = std::chrono::system_clock::now() - start_time;
+    if (diff.count() < timeout+1.0 || timeout == 0){
+        std::this_thread::sleep_for(std::chrono::microseconds(100));
+    } else {
+        ROS_DEBUG("Timed out while switching controllers. Exiting...");
+        timed_out = true;
+        break;
+    }
   }
   start_request_.clear();
   stop_request_.clear();
-
+  if(timed_out){
+      ROS_DEBUG("Exited wait until switch is finished loop using non-ROS-time timeout");
+      return false;
+  }
   ROS_DEBUG("Successfully switched controllers");
   return true;
 }