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 branch 'feature/log' into merge/sdk_2.8.2

Browse Source
This commit is contained in:
slz
2026-04-07 16:45:55 +08:00
parent 31baab180c 78f6ce7d05
commit 194de7e339
19 changed files with 338 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node_driver.cpp
+38 -10
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),
@@ -204,13 +223,21 @@ void OBCameraNodeDriver::init() {
ob::Context::setExtensionsDirectory(extension_path_.c_str());
g_camera_name = declare_parameter<std::string>("camera_name", g_camera_name);
auto log_level_str = declare_parameter<std::string>("log_level", "none");
auto ros_log_level_str = declare_parameter<std::string>("ros_log_level", "info");
auto log_level = obLogSeverityFromString(log_level_str);
auto ros_log_level = rosLogSeverityFromString(ros_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 " << ros_log_level_str);
}
}
// Set custom log file name if specified
if (!log_file_name.empty()) {
try {
@@ -289,7 +316,8 @@ void OBCameraNodeDriver::init() {
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
} else {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
RCLCPP_INFO_STREAM(logger_, "setUvcBackendType:" << uvc_backend_);
RCLCPP_INFO_STREAM(logger_,
"not support uvc_backend:" << uvc_backend_ << ", set to default libuvc");
}
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
device_changed_callback_id_ = ctx_->registerDeviceChangedCallback(
@@ -885,11 +913,11 @@ 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,
"Before lock: Select device net 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);
RCLCPP_DEBUG_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 {
@@ -899,8 +927,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");
@@ -937,7 +965,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 {
@@ -1283,7 +1311,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_");
@@ -1309,7 +1337,7 @@ 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();