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_))