diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h index 826d1cbe..f454561d 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h @@ -98,5 +98,6 @@ class OBCameraNodeDriver : public rclcpp::Node { // net config std::string net_device_ip_; int net_device_port_ = 0; + int connection_delay_ = 100; }; } // namespace orbbec_camera diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp index ae9e8fd2..2d12180e 100644 --- a/orbbec_camera/src/ob_camera_node_driver.cpp +++ b/orbbec_camera/src/ob_camera_node_driver.cpp @@ -60,6 +60,7 @@ OBCameraNodeDriver::~OBCameraNodeDriver() { void OBCameraNodeDriver::init() { auto log_level_str = declare_parameter("log_level", "none"); + connection_delay_ = declare_parameter("connection_delay", 100); auto log_level = obLogSeverityFromString(log_level_str); ob::Context::setLoggerSeverity(log_level); orb_device_lock_shm_fd_ = shm_open(ORB_DEFAULT_LOCK_NAME.c_str(), O_CREAT | O_RDWR, 0666); @@ -164,7 +165,7 @@ void OBCameraNodeDriver::checkConnectTimer() { } void OBCameraNodeDriver::queryDevice() { - if (!device_connected_.load()) { + while (rclcpp::ok() && !device_connected_.load()) { if (!net_device_ip_.empty() && net_device_port_ != 0) { connectNetDevice(net_device_ip_, net_device_port_); } else { @@ -175,6 +176,7 @@ void OBCameraNodeDriver::queryDevice() { } startDevice(device_list); } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } @@ -343,6 +345,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr &list if (device_) { device_.reset(); } + std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_)); pthread_mutex_lock(orb_device_lock_); std::shared_ptr lock_holder(nullptr, [this](int *) { pthread_mutex_unlock(orb_device_lock_); });