Skip to content

Commit ed247fe

Browse files
author
xiexun
committed
Fix queryDevice thread
1 parent 755ac39 commit ed247fe

File tree

1 file changed

+33
-31
lines changed

1 file changed

+33
-31
lines changed

‎orbbec_camera/src/ob_camera_node_driver.cpp‎

Lines changed: 33 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,6 @@ void signalHandler(int sig) {
3939
if (sig == SIGINT || sig == SIGTERM) {
4040
rclcpp::shutdown();
4141
} else {
42-
4342
std::string log_dir = "Log/";
4443

4544
// get current time
@@ -226,14 +225,17 @@ void OBCameraNodeDriver::init() {
226225
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
227226
CHECK_NOTNULL(device_list);
228227
{
228+
RCLCPP_INFO_STREAM(logger_, "onDeviceConnected called");
229229
std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
230230
if (reset_device_flag_) {
231231
RCLCPP_INFO_STREAM(logger_, "onDeviceConnected : device reset in progress, waiting...");
232-
reset_device_cond_.wait(reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
232+
reset_device_cond_.wait(
233+
reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
233234
if (!is_alive_) {
234235
return;
235236
}
236-
RCLCPP_INFO_STREAM(logger_, "onDeviceConnected : device reset completed, continuing connection");
237+
RCLCPP_INFO_STREAM(logger_,
238+
"onDeviceConnected : device reset completed, continuing connection");
237239
}
238240
}
239241
if (device_list->getCount() == 0) {
@@ -256,11 +258,13 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
256258
if (device_list->getCount() == 0) {
257259
return;
258260
}
259-
RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
261+
RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected called");
260262

261263
// Check if device connection/initialization is in progress
262264
if (device_connecting_.load()) {
263-
RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected: device connection/initialization in progress, ignoring disconnect event");
265+
RCLCPP_INFO_STREAM(logger_,
266+
"onDeviceDisconnected: device connection/initialization in progress, "
267+
"ignoring disconnect event");
264268
return;
265269
}
266270

@@ -311,13 +315,14 @@ void OBCameraNodeDriver::checkConnectTimer() {
311315
}
312316

313317
void OBCameraNodeDriver::queryDevice() {
314-
while (is_alive_ && rclcpp::ok() && !device_connected_.load()) {
318+
while (is_alive_ && rclcpp::ok()) {
315319
// Check if device reset is in progress before attempting to connect
316320
{
317321
std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
318322
if (reset_device_flag_) {
319323
RCLCPP_INFO_STREAM(logger_, "queryDevice: device reset in progress, waiting...");
320-
reset_device_cond_.wait(reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
324+
reset_device_cond_.wait(
325+
reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
321326
if (!is_alive_ || !rclcpp::ok()) {
322327
return;
323328
}
@@ -327,26 +332,24 @@ void OBCameraNodeDriver::queryDevice() {
327332

328333
// Check if connection is already in progress
329334
if (device_connecting_.load()) {
330-
RCLCPP_DEBUG_STREAM(logger_, "queryDevice: device connection already in progress, waiting...");
335+
RCLCPP_INFO_STREAM(logger_, "queryDevice: device connection already in progress, waiting...");
331336
std::this_thread::sleep_for(std::chrono::milliseconds(100));
332337
continue;
333338
}
334339

335-
if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) {
336-
connectNetDevice(net_device_ip_, net_device_port_);
337-
} else {
338-
auto device_list = ctx_->queryDeviceList();
339-
if (device_list->getCount() == 0) {
340-
RCLCPP_INFO_STREAM(logger_,
341-
"queryDevice :No Device found, using usb event to trigger "
342-
"OBCameraNodeDriver::onDeviceConnected");
343-
return;
340+
// If device is already connected, skip connection attempt
341+
if (!device_connected_.load()) {
342+
if (!enumerate_net_device_ && !net_device_ip_.empty() && net_device_port_ != 0) {
343+
connectNetDevice(net_device_ip_, net_device_port_);
344+
} else {
345+
auto device_list = ctx_->queryDeviceList();
346+
if (device_list->getCount() != 0) {
347+
startDevice(device_list);
348+
}
344349
}
345-
startDevice(device_list);
346350
}
347-
348-
// Add a small delay to prevent tight loop
349-
std::this_thread::sleep_for(std::chrono::milliseconds(100));
351+
// Add a delay to prevent tight loop
352+
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
350353
}
351354
}
352355

@@ -445,8 +448,8 @@ void OBCameraNodeDriver::rebootDeviceCallback(
445448
return;
446449
}
447450

448-
std::shared_ptr<int> process_lock_guard(nullptr,
449-
[this](int *) { pthread_mutex_unlock(orb_device_lock_); });
451+
std::shared_ptr<int> process_lock_guard(
452+
nullptr, [this](int *) { pthread_mutex_unlock(orb_device_lock_); });
450453

451454
try {
452455
std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
@@ -465,6 +468,8 @@ void OBCameraNodeDriver::rebootDeviceCallback(
465468
}
466469

467470
RCLCPP_INFO(logger_, "Device reboot initiated, waiting for reconnection");
471+
reset_device_flag_ = true;
472+
reset_device_cond_.notify_all();
468473
malloc_trim(0);
469474
return;
470475

@@ -682,8 +687,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
682687
std::placeholders::_2, std::placeholders::_3),
683688
false);
684689
});
685-
if(firmware_update_success_)
686-
{
690+
if (firmware_update_success_) {
687691
return;
688692
}
689693
}
@@ -711,9 +715,8 @@ void OBCameraNodeDriver::connectNetDevice(const std::string &net_device_ip, int
711715
}
712716

713717
// Use RAII to ensure connecting flag is cleared
714-
std::shared_ptr<int> connecting_guard(nullptr, [this](int *) {
715-
device_connecting_.store(false);
716-
});
718+
std::shared_ptr<int> connecting_guard(nullptr,
719+
[this](int *) { device_connecting_.store(false); });
717720

718721
std::this_thread::sleep_for(std::chrono::milliseconds(connection_delay_));
719722
auto device = ctx_->createNetDevice(net_device_ip.c_str(), net_device_port);
@@ -807,8 +810,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
807810
time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
808811
RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
809812

810-
if(firmware_update_success_)
811-
{
813+
if (firmware_update_success_) {
812814
firmware_update_success_ = false;
813815
device_connected_ = false;
814816
std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
@@ -853,7 +855,7 @@ void OBCameraNodeDriver::updatePresetFirmware(std::string path) {
853855
}
854856
uint8_t index = 0;
855857
uint8_t count = static_cast<uint8_t>(paths.size());
856-
char (*filePaths)[OB_PATH_MAX] = new char[count][OB_PATH_MAX];
858+
char(*filePaths)[OB_PATH_MAX] = new char[count][OB_PATH_MAX];
857859
RCLCPP_INFO_STREAM(this->get_logger(), "paths.cout : " << (uint32_t)count);
858860
for (const auto &p : paths) {
859861
strcpy(filePaths[index], p.c_str());

0 commit comments

Comments
 (0)