i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 20:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix: throttle logging frequency for device connection and selection processes

Browse Source
This commit is contained in:
obyalian
2025-10-17 15:33:59 +08:00
parent 6aaa5247c5
commit 2e074dc477
1 changed files with 26 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node_driver.cpp
+26 -20
View File
@@ -736,20 +736,23 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDevice(
const std::shared_ptr<ob::DeviceList> &list) { const std::shared_ptr<ob::DeviceList> &list) {
std::shared_ptr<ob::Device> device = nullptr; std::shared_ptr<ob::Device> device = nullptr;
if (!net_device_ip_.empty() && net_device_port_ != 0) { if (!net_device_ip_.empty() && net_device_port_ != 0) {
RCLCPP_INFO_STREAM(logger_, "Connecting to device with net ip: " << net_device_ip_); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Connecting to device with net ip: " << net_device_ip_);
device = selectDeviceByNetIP(list, net_device_ip_); device = selectDeviceByNetIP(list, net_device_ip_);
} else if (!serial_number_.empty()) { } else if (!serial_number_.empty()) {
RCLCPP_INFO_STREAM(logger_, "Connecting to device with serial number: " << serial_number_); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Connecting to device with serial number: " << serial_number_);
device = selectDeviceBySerialNumber(list, serial_number_); device = selectDeviceBySerialNumber(list, serial_number_);
} else if (!usb_port_.empty()) { } else if (!usb_port_.empty()) {
RCLCPP_INFO_STREAM(logger_, "Connecting to device with usb port: " << usb_port_); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Connecting to device with usb port: " << usb_port_);
device = selectDeviceByUSBPort(list, usb_port_); device = selectDeviceByUSBPort(list, usb_port_);
} else if (device_num_ == 1) { } else if (device_num_ == 1) {
RCLCPP_INFO_STREAM(logger_, "Connecting to the default device"); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Connecting to the default device");
return list->getDevice(0); return list->getDevice(0);
} }
if (device == nullptr) { if (device == nullptr) {
RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found", RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 5000, "Device with serial number %s not found",
serial_number_.c_str()); serial_number_.c_str());
device_connected_ = false; device_connected_ = false;
return nullptr; return nullptr;
@@ -763,9 +766,11 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn), std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
[](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); }); [](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
for (size_t i = 0; i < list->getCount(); i++) { for (size_t i = 0; i < list->getCount(); i++) {
RCLCPP_INFO_STREAM(logger_, "Before lock: Select device serial number: " << serial_number); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device serial number: " << serial_number);
std::lock_guard<decltype(device_lock_)> lock(device_lock_); std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM(logger_, "After lock: Select device serial number: " << serial_number); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device serial number: " << serial_number);
try { try {
auto pid = list->getPid(i); auto pid = list->getPid(i);
if (isOpenNIDevice(pid)) { if (isOpenNIDevice(pid)) {
@@ -773,25 +778,28 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
auto device = list->getDevice(i); auto device = list->getDevice(i);
auto device_info = device->getDeviceInfo(); auto device_info = device->getDeviceInfo();
if (device_info->getSerialNumber() == serial_number) { if (device_info->getSerialNumber() == serial_number) {
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM_THROTTLE(
logger_, "Device serial number " << device_info->getSerialNumber() << " matched"); logger_, *get_clock(), 5000,
"Device serial number " << device_info->getSerialNumber() << " matched");
return device; return device;
} }
} else { } else {
std::string sn = list->getSerialNumber(i); std::string sn = list->getSerialNumber(i);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 1000, "Device serial number: " << sn); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Device serial number: " << sn);
if (sn == serial_number) { if (sn == serial_number) {
RCLCPP_INFO_STREAM(logger_, "Device serial number " << sn << " matched"); RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Device serial number " << sn << " matched");
return list->getDevice(i); return list->getDevice(i);
} }
} }
} catch (ob::Error &e) { } catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 1000, RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.getMessage()); "Failed to get device info " << e.getMessage());
} catch (std::exception &e) { } catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info " << e.what()); RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.what());
} catch (...) { } catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to get device info"); RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Failed to get device info");
} }
} }
return nullptr; return nullptr;
@@ -1137,7 +1145,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
return; return;
} }
RCLCPP_INFO_STREAM(logger_, "startDevice called"); RCLCPP_INFO_THROTTLE(logger_, *get_clock(), 1000, "startDevice called");
start_time_ = std::chrono::high_resolution_clock::now(); start_time_ = std::chrono::high_resolution_clock::now();
if (device_) { if (device_) {
@@ -1174,15 +1182,13 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
try { try {
auto start_time = std::chrono::high_resolution_clock::now(); auto start_time = std::chrono::high_resolution_clock::now();
auto device = selectDevice(list); auto device = selectDevice(list);
auto end_time = std::chrono::high_resolution_clock::now();
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
if (device == nullptr) { if (device == nullptr) {
RCLCPP_WARN_THROTTLE(logger_, *get_clock(), 1000, "Device with serial number %s not found",
serial_number_.c_str());
device_connected_ = false; device_connected_ = false;
return; return;
} }
auto end_time = std::chrono::high_resolution_clock::now();
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
start_time = std::chrono::high_resolution_clock::now(); start_time = std::chrono::high_resolution_clock::now();
initializeDevice(device); initializeDevice(device);
end_time = std::chrono::high_resolution_clock::now(); end_time = std::chrono::high_resolution_clock::now();