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

Merge remote-tracking branch 'origin/feature/log_2.8.2' into merge/sdk_2.8.2

Browse Source
This commit is contained in:
ob-yalian
2026-04-15 09:25:53 +08:00
parent 6894dbeffa 953a725c3d
commit fa7e7086c9
6 changed files with 520 additions and 401 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node_driver.cpp
+61 -43
View File
@@ -22,6 +22,7 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <ament_index_cpp/get_package_prefix.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <rcutils/logging.h>
#include <csignal>
#include <sys/mman.h>
#include <unistd.h>
@@ -96,6 +97,24 @@ void signalHandler(int sig) {
namespace orbbec_camera {
backward::SignalHandling OBCameraNodeDriver::sh;
namespace {
int rosLogSeverityFromString(const std::string_view &log_level) {
if (log_level == "debug") {
return RCUTILS_LOG_SEVERITY_DEBUG;
} else if (log_level == "info") {
return RCUTILS_LOG_SEVERITY_INFO;
} else if (log_level == "warn") {
return RCUTILS_LOG_SEVERITY_WARN;
} else if (log_level == "error") {
return RCUTILS_LOG_SEVERITY_ERROR;
} else if (log_level == "fatal") {
return RCUTILS_LOG_SEVERITY_FATAL;
}
return RCUTILS_LOG_SEVERITY_UNSET;
}
} // namespace
OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options),
node_options_(node_options),
@@ -205,12 +224,19 @@ void OBCameraNodeDriver::init() {
g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
auto log_level_str = declare_parameter<std::string>("log_level", "none");
auto log_level = obLogSeverityFromString(log_level_str);
auto ros_log_level = rosLogSeverityFromString(log_level_str);
auto log_file_name = declare_parameter<std::string>("log_file_name", "");
std::string pwd_dir = std::getenv("PWD") ? std::getenv("PWD") : std::getenv("HOME");
std::string log_path = pwd_dir + "/Log/" + g_camera_name;
// Set logger to console
ob::Context::setLoggerToConsole(log_level);
ob::Context::setLoggerToFile(log_level, log_path.c_str());
if (ros_log_level != RCUTILS_LOG_SEVERITY_UNSET) {
auto ret = rcutils_logging_set_logger_level(this->get_logger().get_name(), ros_log_level);
if (ret != RCUTILS_RET_OK) {
RCLCPP_WARN_STREAM(logger_, "Failed to set ROS log level to " << log_level_str);
}
}
// Set custom log file name if specified
if (!log_file_name.empty()) {
try {
@@ -284,13 +310,14 @@ void OBCameraNodeDriver::init() {
<< device_access_mode_ << ")");
if (uvc_backend_ == "libuvc") {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
RCLCPP_INFO_STREAM(logger_, "Set UVC backend to " << uvc_backend_);
} else if (uvc_backend_ == "v4l2") {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_V4L2);
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
RCLCPP_INFO_STREAM(logger_, "Set UVC backend to " << uvc_backend_);
} else {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
RCLCPP_WARN_STREAM(logger_,
"Unsupported uvc_backend '" << uvc_backend_ << "', using default libuvc");
}
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
device_changed_callback_id_ = ctx_->registerDeviceChangedCallback(
@@ -320,17 +347,16 @@ void OBCameraNodeDriver::init() {
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(device_list);
{
RCLCPP_INFO_STREAM(logger_, "onDeviceConnected called");
RCLCPP_INFO_STREAM(logger_, "Device connected callback triggered");
std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
if (reset_device_flag_) {
RCLCPP_INFO_STREAM(logger_, "onDeviceConnected : device reset in progress, waiting...");
RCLCPP_INFO_STREAM(logger_, "Device reset in progress, waiting before connecting");
reset_device_cond_.wait(
reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
if (!is_alive_) {
return;
}
RCLCPP_INFO_STREAM(logger_,
"onDeviceConnected : device reset completed, continuing connection");
RCLCPP_INFO_STREAM(logger_, "Device reset completed, continuing connection");
}
}
if (device_list->getCount() == 0) {
@@ -378,11 +404,9 @@ void OBCameraNodeDriver::onDeviceDisconnected(const std::shared_ptr<ob::DeviceLi
RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
if (uid == device_unique_id_ || serial_number_ == serial_number) {
RCLCPP_INFO_STREAM(logger_,
"device with " << uid << " disconnected, notify reset device thread 1.");
"device with " << uid << " disconnected, notify reset device thread");
reset_device_flag_ = true;
reset_device_cond_.notify_all();
RCLCPP_INFO_STREAM(logger_,
"device with " << uid << " disconnected, notify reset device thread 2.");
break;
}
}
@@ -498,7 +522,7 @@ void OBCameraNodeDriver::resetDevice() {
}
}
RCLCPP_INFO_STREAM(logger_, "resetDevice : Reset device uid: " << device_unique_id_);
RCLCPP_INFO_STREAM(logger_, "Resetting device UID: " << device_unique_id_);
std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
{
// Mark device as disconnected immediately to prevent other threads from accessing it
@@ -517,7 +541,7 @@ void OBCameraNodeDriver::resetDevice() {
if (device_) {
try {
RCLCPP_INFO_STREAM(logger_, "Resetting device_");
RCLCPP_INFO_STREAM(logger_, "Resetting device handle");
// Force free any idle memory before device reset
if (ctx_) {
try {
@@ -527,7 +551,7 @@ void OBCameraNodeDriver::resetDevice() {
}
}
device_.reset();
RCLCPP_INFO_STREAM(logger_, "device_ reset completed");
RCLCPP_INFO_STREAM(logger_, "Device handle reset complete");
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: "
<< orbbec_camera::formatObErrorWithStatus(e));
@@ -540,9 +564,9 @@ void OBCameraNodeDriver::resetDevice() {
if (device_info_) {
try {
RCLCPP_INFO_STREAM(logger_, "Resetting device_info_");
RCLCPP_INFO_STREAM(logger_, "Resetting device info");
device_info_.reset();
RCLCPP_INFO_STREAM(logger_, "device_info_ reset completed");
RCLCPP_INFO_STREAM(logger_, "Device info reset complete");
} catch (...) {
RCLCPP_WARN_STREAM(logger_, "Exception during device_info reset");
}
@@ -555,7 +579,7 @@ void OBCameraNodeDriver::resetDevice() {
}
reset_device_cond_.notify_all();
malloc_trim(0);
RCLCPP_INFO_STREAM(logger_, "Reset device uid: " << device_unique_id_ << " done");
RCLCPP_INFO_STREAM(logger_, "Device reset complete");
}
}
@@ -813,11 +837,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
std::transform(serial_number.begin(), serial_number.end(), std::back_inserter(lower_sn),
[](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
for (size_t i = 0; i < list->getCount(); i++) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device serial number: " << serial_number);
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Selecting device by serial number: " << serial_number);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device serial number: " << serial_number);
try {
auto pid = list->getPid(i);
if (isOpenNIDevice(pid)) {
@@ -827,15 +849,16 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
if (device_info->getSerialNumber() == serial_number) {
RCLCPP_INFO_STREAM_THROTTLE(
logger_, *get_clock(), 5000,
"Device serial number " << device_info->getSerialNumber() << " matched");
"Matched device serial number: " << device_info->getSerialNumber());
return device;
}
} else {
std::string sn = list->getSerialNumber(i);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, "Device serial number: " << sn);
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Checking device serial number: " << sn);
if (sn == serial_number) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Device serial number " << sn << " matched");
"Matched device serial number: " << sn);
return list->getDevice(i, device_access_mode_);
}
}
@@ -852,15 +875,12 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
}
return nullptr;
}
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
try {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device usb port: " << usb_port);
"Selecting device by USB port: " << usb_port);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device usb port: " << usb_port);
auto device = list->getDeviceByUid(usb_port.c_str(), device_access_mode_);
if (device) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
@@ -889,11 +909,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device net ip: " << net_ip);
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Selecting device by network IP: " << net_ip);
std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"After lock: Select device net ip: " << net_ip);
std::shared_ptr<ob::Device> device = nullptr;
for (size_t i = 0; i < list->getCount(); i++) {
try {
@@ -903,8 +921,8 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
if (list->getIpAddress(i) == nullptr) {
continue;
}
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"FindDeviceByNetIP device net ip " << list->getIpAddress(i));
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"FindDeviceByNetIP device net ip " << list->getIpAddress(i));
if (std::string(list->getIpAddress(i)) == net_ip) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"getDeviceByNetIP device net ip " << net_ip << " done");
@@ -942,7 +960,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
constexpr int max_retries = 3;
bool initialized = false;
device_info_ = device_->getDeviceInfo();
RCLCPP_INFO_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
RCLCPP_DEBUG_STREAM(logger_, "Try to connect device via " << device_info_->connectionType());
while (retry_count < max_retries && !initialized) {
try {
@@ -1068,14 +1086,14 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
RCLCPP_INFO_STREAM(logger_, "Current node pid: " << getpid());
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start_time_);
RCLCPP_INFO_STREAM(logger_, "Start device cost " << time_cost.count() << " ms");
RCLCPP_DEBUG_STREAM(logger_, "Start device cost: " << time_cost.count() << " ms");
if (!upgrade_firmware_.empty()) {
// Check if this is a second update (reupdate scenario)
bool is_second_update = is_reupdating_.load();
if (is_second_update) {
RCLCPP_INFO(logger_, "Device reconnected, starting the second firmware update...");
RCLCPP_INFO(logger_, "Device reconnected, starting the second firmware update");
} else {
RCLCPP_INFO(logger_, "Starting firmware update from file: %s", upgrade_firmware_.c_str());
}
@@ -1103,7 +1121,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
if (need_reupdate_) {
// Some devices require a second update after reboot
RCLCPP_INFO(logger_, "The device will reboot and perform a second update automatically.");
RCLCPP_INFO(logger_, "The device will reboot and perform a second update automatically");
// Set flag to indicate we're waiting for device to reboot for second update
is_reupdating_ = true;
// Keep upgrade_firmware_ path and wait for device to reconnect
@@ -1113,10 +1131,10 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
if (firmware_update_success_) {
if (is_second_update) {
RCLCPP_INFO(logger_, "Second firmware update completed successfully!");
RCLCPP_INFO(logger_, "Second firmware update completed successfully");
is_reupdating_ = false;
} else {
RCLCPP_INFO(logger_, "Firmware update completed successfully!");
RCLCPP_INFO(logger_, "Firmware update completed successfully");
}
return;
}
@@ -1129,7 +1147,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
ob_lidar_node_->startStreams();
ob_lidar_node_->startIMU();
} else {
RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr");
RCLCPP_WARN_STREAM(logger_, "Camera or LiDAR node is null after device initialization");
}
} // namespace orbbec_camera
@@ -1289,7 +1307,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
// success get lock,break
break;
} else if (try_lock_result == EBUSY) {
RCLCPP_INFO_STREAM(logger_, "Device lock is held by another process, waiting 100ms");
RCLCPP_WARN_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_");
@@ -1315,12 +1333,12 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &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");
RCLCPP_DEBUG_STREAM(logger_, "Select device cost " << time_cost.count() << " ms");
start_time = std::chrono::high_resolution_clock::now();
initializeDevice(device);
end_time = std::chrono::high_resolution_clock::now();
time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO_STREAM(logger_, "Initialize device cost " << time_cost.count() << " ms");
RCLCPP_INFO_STREAM(logger_, "Initialize device cost: " << time_cost.count() << " ms");
if (firmware_update_success_) {
firmware_update_success_ = false;