add try to lock mutex and print to not to obsruct RT thread

ros-controls · Jul 23, 2024 · 0ce923d · 0ce923d
1 parent c2336ae
commit 0ce923d
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -2156,7 +2156,12 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
 
 void ControllerManager::manage_switch()
 {
-  std::lock_guard<std::mutex> guard(switch_params_.mutex);
+  std::unique_lock<std::mutex> guard(switch_params_.mutex, std::try_to_lock);
+  if (!guard.owns_lock())
+  {
+    RCLCPP_WARN(get_logger(), "Unable to lock switch mutex. Retrying in next cycle.");
+    return;
+  }
   // Ask hardware interfaces to change mode
   if (!resource_manager_->perform_command_mode_switch(
         activate_command_interface_request_, deactivate_command_interface_request_))