Skip to content

Commit 01d6510

Browse files
jian-dongjian-dong
committed
fixed start multi device
1 parent d6d32fd commit 01d6510

File tree

1 file changed

+58
-53
lines changed

1 file changed

+58
-53
lines changed

‎orbbec_camera/src/ob_camera_node_driver.cpp‎

Lines changed: 58 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ void OBCameraNodeDriver::init() {
9292
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
9393
ctx_->setDeviceChangedCallback([this](const std::shared_ptr<ob::DeviceList> &removed_list,
9494
const std::shared_ptr<ob::DeviceList> &added_list) {
95-
(void)added_list;
95+
onDeviceConnected(added_list);
9696
onDeviceDisconnected(removed_list);
9797
});
9898
check_connect_timer_ =
@@ -108,11 +108,6 @@ void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList>
108108
if (device_list->deviceCount() == 0) {
109109
return;
110110
}
111-
pthread_mutex_lock(orb_device_lock_);
112-
std::shared_ptr<int> lock_holder(nullptr,
113-
[this](int *) { pthread_mutex_unlock(orb_device_lock_); });
114-
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000,
115-
"device list count " << device_list->deviceCount());
116111
if (!device_) {
117112
startDevice(device_list);
118113
}
@@ -126,9 +121,12 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
126121
RCLCPP_INFO_STREAM(logger_, "onDeviceDisconnected");
127122
for (size_t i = 0; i < device_list->deviceCount(); i++) {
128123
std::string uid = device_list->uid(i);
129-
std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
124+
std::string serial_number = device_list->serialNumber(i);
125+
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
130126
RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
131-
if (uid == device_unique_id_) {
127+
if (uid == device_unique_id_ || serial_number_ == serial_number) {
128+
RCLCPP_INFO_STREAM(logger_,
129+
"device with " << uid << " disconnected, notify reset device thread.");
132130
std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
133131
reset_device_flag_ = true;
134132
reset_device_cond_.notify_all();
@@ -164,35 +162,13 @@ void OBCameraNodeDriver::checkConnectTimer() {
164162
}
165163

166164
void OBCameraNodeDriver::queryDevice() {
167-
while (is_alive_ && rclcpp::ok()) {
168-
if (!device_connected_.load()) {
169-
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Waiting for device connection...");
170-
auto device_list = ctx_->queryDeviceList();
171-
if (device_list->deviceCount() == 0) {
172-
std::this_thread::sleep_for(std::chrono::milliseconds(10));
173-
continue;
174-
}
175-
bool start_device_failed = false;
176-
try {
177-
onDeviceConnected(device_list);
178-
} catch (ob::Error &e) {
179-
RCLCPP_ERROR_STREAM(logger_, "Failed to start device " << e.getMessage());
180-
start_device_failed = true;
181-
} catch (std::exception &e) {
182-
RCLCPP_ERROR_STREAM(logger_, "Failed to start device " << e.what());
183-
start_device_failed = true;
184-
} catch (...) {
185-
RCLCPP_ERROR_STREAM(logger_, "Failed to start device");
186-
start_device_failed = true;
187-
}
188-
if (start_device_failed) {
189-
std::unique_lock<decltype(reset_device_mutex_)> lock(reset_device_mutex_);
190-
reset_device_flag_ = true;
191-
reset_device_cond_.notify_all();
192-
}
193-
} else {
194-
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
165+
if (!device_connected_.load()) {
166+
auto device_list = ctx_->queryDeviceList();
167+
if (device_list->deviceCount() == 0) {
168+
RCLCPP_INFO_STREAM(logger_, "queryDevice : No device found");
169+
return;
195170
}
171+
startDevice(device_list);
196172
}
197173
}
198174

@@ -213,12 +189,18 @@ void OBCameraNodeDriver::resetDevice() {
213189
if (!is_alive_ || !rclcpp::ok()) {
214190
break;
215191
}
216-
ob_camera_node_.reset();
217-
device_.reset();
218-
device_info_.reset();
219-
device_connected_ = false;
220-
device_unique_id_.clear();
221-
reset_device_flag_ = false;
192+
RCLCPP_INFO_STREAM(logger_, "resetDevice : Reset device uid: " << device_unique_id_);
193+
std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
194+
{
195+
ob_camera_node_.reset();
196+
device_.reset();
197+
device_info_.reset();
198+
device_connected_ = false;
199+
device_unique_id_.clear();
200+
serial_number_.clear();
201+
reset_device_flag_ = false;
202+
}
203+
RCLCPP_INFO_STREAM(logger_, "Reset device uid: " << device_unique_id_ << " done");
222204
}
223205
}
224206

@@ -231,12 +213,10 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
231213

232214
std::shared_ptr<ob::Device> device = nullptr;
233215
if (!serial_number_.empty()) {
234-
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000,
235-
"Connecting to device with serial number: " << serial_number_);
216+
RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_);
236217
device = selectDeviceBySerialNumber(list, serial_number_);
237218
} else if (!usb_port_.empty()) {
238-
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000,
239-
"Connecting to device with usb port: " << usb_port_);
219+
RCLCPP_INFO_STREAM(logger_, "Connecting to device with usb port: " << usb_port_);
240220
device = selectDeviceByUSBPort(list, usb_port_);
241221
}
242222
if (device == nullptr) {
@@ -288,7 +268,11 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
288268
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
289269
const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
290270
try {
271+
RCLCPP_INFO_STREAM(logger_, "Before lock: Select device usb port: " << usb_port);
272+
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
273+
RCLCPP_INFO_STREAM(logger_, "After lock: Select device usb port: " << usb_port);
291274
auto device = list->getDeviceByUid(usb_port.c_str());
275+
RCLCPP_INFO_STREAM(logger_, "Device usb port " << usb_port << " done");
292276
return device;
293277
} catch (ob::Error &e) {
294278
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.getMessage());
@@ -313,6 +297,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
313297
ob_camera_node_->startIMU();
314298
device_connected_ = true;
315299
device_info_ = device_->getDeviceInfo();
300+
serial_number_ = device_info_->serialNumber();
316301
CHECK_NOTNULL(device_info_.get());
317302
device_unique_id_ = device_info_->uid();
318303
if (!isOpenNIDevice(device_info_->pid())) {
@@ -326,7 +311,6 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
326311
}
327312

328313
void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list) {
329-
std::scoped_lock<decltype(device_lock_)> lock(device_lock_);
330314
if (device_connected_) {
331315
return;
332316
}
@@ -337,14 +321,35 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
337321
if (device_) {
338322
device_.reset();
339323
}
340-
auto device = selectDevice(list);
341-
if (device == nullptr) {
342-
RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found",
343-
serial_number_.c_str());
324+
pthread_mutex_lock(orb_device_lock_);
325+
std::shared_ptr<int> lock_holder(nullptr,
326+
[this](int *) { pthread_mutex_unlock(orb_device_lock_); });
327+
bool start_device_failed = false;
328+
try {
329+
auto device = selectDevice(list);
330+
if (device == nullptr) {
331+
RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found",
332+
serial_number_.c_str());
333+
device_connected_ = false;
334+
return;
335+
}
336+
initializeDevice(device);
337+
} catch (ob::Error &e) {
338+
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.getMessage());
339+
start_device_failed = true;
340+
} catch (std::exception &e) {
341+
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.what());
342+
start_device_failed = true;
343+
} catch (...) {
344+
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device");
345+
start_device_failed = true;
346+
}
347+
if (start_device_failed) {
344348
device_connected_ = false;
345-
return;
349+
std::unique_lock<decltype(reset_device_mutex_)> reset_device_lock(reset_device_mutex_);
350+
reset_device_flag_ = true;
351+
reset_device_cond_.notify_all();
346352
}
347-
initializeDevice(device);
348353
}
349354
} // namespace orbbec_camera
350355

0 commit comments

Comments
 (0)