diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index 66961d87..0ddc7763 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -472,16 +472,30 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr &list device_.reset(); } std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_)); - auto try_lock_result = pthread_mutex_trylock(orb_device_lock_); - if (try_lock_result != 0) { - if (try_lock_result == EBUSY) { - RCLCPP_INFO_STREAM(logger_, "Device lock locked by other process, wait for 10ms"); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + int try_lock_count = 0; + int max_try_lock_count = 5; + + while (try_lock_count < max_try_lock_count) { + int try_lock_result = pthread_mutex_trylock(orb_device_lock_); + + if (try_lock_result == 0) { + // success get lock,break + break; + } else if (try_lock_result == EBUSY) { + RCLCPP_INFO_STREAM(logger_, "Device lock is held by another process, waiting 100ms"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } else { RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_"); + return; // Not EBUSY, return } + + try_lock_count++; + } + if (try_lock_count >= max_try_lock_count) { + RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_"); return; } + std::shared_ptr lock_holder(nullptr, [this](int *) { pthread_mutex_unlock(orb_device_lock_); }); bool start_device_failed = false;