i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-10 06:29:50 +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/launch/lidar.launch.py
+6 -1
View File
@@ -169,7 +169,12 @@ def generate_launch_description():
DeclareLaunchArgument( DeclareLaunchArgument(
'log_level', 'log_level',
default_value='none', default_value='none',
description='SDK log level: none, debug, info, warn, error, fatal.' description='Shared SDK and ROS log level: none, debug, info, warn, error, fatal.'
),
DeclareLaunchArgument(
'log_file_name',
default_value='',
description='Custom log file name for SDK logs. If empty, default naming is used.'
), ),
DeclareLaunchArgument( DeclareLaunchArgument(
'time_domain', 'time_domain',
orbbec_camera/launch/orbbec_camera.launch.py
+2 -1
View File
@@ -27,6 +27,7 @@ def generate_launch_arguments():
#general config #general config
DeclareLaunchArgument('camera_model', default_value=default_camera_model), DeclareLaunchArgument('camera_model', default_value=default_camera_model),
DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('config_file_path', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
#multi-device sync param #multi-device sync param
DeclareLaunchArgument('camera_name', default_value='camera'), DeclareLaunchArgument('camera_name', default_value='camera'),
DeclareLaunchArgument('usb_port', default_value=''), DeclareLaunchArgument('usb_port', default_value=''),
@@ -182,4 +183,4 @@ def generate_launch_description():
args + [ args + [
OpaqueFunction(function=lambda context: create_node_action(context, args)) OpaqueFunction(function=lambda context: create_node_action(context, args))
] ]
) )
orbbec_camera/src/ob_camera_node.cpp
+369 -283
View File
File diff suppressed because it is too large Load Diff
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_share_directory.hpp>
#include <ament_index_cpp/get_package_prefix.hpp> #include <ament_index_cpp/get_package_prefix.hpp>
#include <rclcpp_components/register_node_macro.hpp> #include <rclcpp_components/register_node_macro.hpp>
#include <rcutils/logging.h>
#include <csignal> #include <csignal>
#include <sys/mman.h> #include <sys/mman.h>
#include <unistd.h> #include <unistd.h>
@@ -96,6 +97,24 @@ void signalHandler(int sig) {
namespace orbbec_camera { namespace orbbec_camera {
backward::SignalHandling OBCameraNodeDriver::sh; 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) OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options), : Node("orbbec_camera_node", "/", node_options),
node_options_(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); 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_str = declare_parameter<std::string>("log_level", "none");
auto log_level = obLogSeverityFromString(log_level_str); 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", ""); 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 pwd_dir = std::getenv("PWD") ? std::getenv("PWD") : std::getenv("HOME");
std::string log_path = pwd_dir + "/Log/" + g_camera_name; std::string log_path = pwd_dir + "/Log/" + g_camera_name;
// Set logger to console // Set logger to console
ob::Context::setLoggerToConsole(log_level); ob::Context::setLoggerToConsole(log_level);
ob::Context::setLoggerToFile(log_level, log_path.c_str()); 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 // Set custom log file name if specified
if (!log_file_name.empty()) { if (!log_file_name.empty()) {
try { try {
@@ -284,13 +310,14 @@ void OBCameraNodeDriver::init() {
<< device_access_mode_ << ")"); << device_access_mode_ << ")");
if (uvc_backend_ == "libuvc") { if (uvc_backend_ == "libuvc") {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_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") { } else if (uvc_backend_ == "v4l2") {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_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 { } else {
ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC); 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_); ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
device_changed_callback_id_ = ctx_->registerDeviceChangedCallback( device_changed_callback_id_ = ctx_->registerDeviceChangedCallback(
@@ -320,17 +347,16 @@ void OBCameraNodeDriver::init() {
void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) { void OBCameraNodeDriver::onDeviceConnected(const std::shared_ptr<ob::DeviceList> &device_list) {
CHECK_NOTNULL(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_); std::unique_lock<decltype(reset_device_mutex_)> reset_lock(reset_device_mutex_);
if (reset_device_flag_) { 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_device_cond_.wait(
reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); }); reset_lock, [this]() { return !reset_device_flag_ || !is_alive_ || !rclcpp::ok(); });
if (!is_alive_) { if (!is_alive_) {
return; return;
} }
RCLCPP_INFO_STREAM(logger_, RCLCPP_INFO_STREAM(logger_, "Device reset completed, continuing connection");
"onDeviceConnected : device reset completed, continuing connection");
} }
} }
if (device_list->getCount() == 0) { 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"); RCLCPP_INFO_STREAM(logger_, "device with " << uid << " disconnected");
if (uid == device_unique_id_ || serial_number_ == serial_number) { if (uid == device_unique_id_ || serial_number_ == serial_number) {
RCLCPP_INFO_STREAM(logger_, 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_flag_ = true;
reset_device_cond_.notify_all(); reset_device_cond_.notify_all();
RCLCPP_INFO_STREAM(logger_,
"device with " << uid << " disconnected, notify reset device thread 2.");
break; 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_); std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
{ {
// Mark device as disconnected immediately to prevent other threads from accessing it // Mark device as disconnected immediately to prevent other threads from accessing it
@@ -517,7 +541,7 @@ void OBCameraNodeDriver::resetDevice() {
if (device_) { if (device_) {
try { try {
RCLCPP_INFO_STREAM(logger_, "Resetting device_"); RCLCPP_INFO_STREAM(logger_, "Resetting device handle");
// Force free any idle memory before device reset // Force free any idle memory before device reset
if (ctx_) { if (ctx_) {
try { try {
@@ -527,7 +551,7 @@ void OBCameraNodeDriver::resetDevice() {
} }
} }
device_.reset(); device_.reset();
RCLCPP_INFO_STREAM(logger_, "device_ reset completed"); RCLCPP_INFO_STREAM(logger_, "Device handle reset complete");
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: " RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: "
<< orbbec_camera::formatObErrorWithStatus(e)); << orbbec_camera::formatObErrorWithStatus(e));
@@ -540,9 +564,9 @@ void OBCameraNodeDriver::resetDevice() {
if (device_info_) { if (device_info_) {
try { try {
RCLCPP_INFO_STREAM(logger_, "Resetting device_info_"); RCLCPP_INFO_STREAM(logger_, "Resetting device info");
device_info_.reset(); device_info_.reset();
RCLCPP_INFO_STREAM(logger_, "device_info_ reset completed"); RCLCPP_INFO_STREAM(logger_, "Device info reset complete");
} catch (...) { } catch (...) {
RCLCPP_WARN_STREAM(logger_, "Exception during device_info reset"); RCLCPP_WARN_STREAM(logger_, "Exception during device_info reset");
} }
@@ -555,7 +579,7 @@ void OBCameraNodeDriver::resetDevice() {
} }
reset_device_cond_.notify_all(); reset_device_cond_.notify_all();
malloc_trim(0); 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), 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_THROTTLE(logger_, *get_clock(), 5000, RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device serial number: " << serial_number); "Selecting device by 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_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)) {
@@ -827,15 +849,16 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
if (device_info->getSerialNumber() == serial_number) { if (device_info->getSerialNumber() == serial_number) {
RCLCPP_INFO_STREAM_THROTTLE( RCLCPP_INFO_STREAM_THROTTLE(
logger_, *get_clock(), 5000, logger_, *get_clock(), 5000,
"Device serial number " << device_info->getSerialNumber() << " matched"); "Matched device serial number: " << device_info->getSerialNumber());
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(), 5000, "Device serial number: " << sn); RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Checking device serial number: " << sn);
if (sn == serial_number) { if (sn == serial_number) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, 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_); return list->getDevice(i, device_access_mode_);
} }
} }
@@ -852,15 +875,12 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
} }
return nullptr; return nullptr;
} }
std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort( std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) { const std::shared_ptr<ob::DeviceList> &list, const std::string &usb_port) {
try { try {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, 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_); 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_); auto device = list->getDeviceByUid(usb_port.c_str(), device_access_mode_);
if (device) { if (device) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, 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( std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) { const std::shared_ptr<ob::DeviceList> &list, const std::string &net_ip) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Before lock: Select device net ip: " << net_ip); "Selecting device by network IP: " << net_ip);
std::lock_guard<decltype(device_lock_)> lock(device_lock_); 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; std::shared_ptr<ob::Device> device = nullptr;
for (size_t i = 0; i < list->getCount(); i++) { for (size_t i = 0; i < list->getCount(); i++) {
try { try {
@@ -903,8 +921,8 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
if (list->getIpAddress(i) == nullptr) { if (list->getIpAddress(i) == nullptr) {
continue; continue;
} }
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"FindDeviceByNetIP device net ip " << list->getIpAddress(i)); "FindDeviceByNetIP device net ip " << list->getIpAddress(i));
if (std::string(list->getIpAddress(i)) == net_ip) { if (std::string(list->getIpAddress(i)) == net_ip) {
RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000, RCLCPP_INFO_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"getDeviceByNetIP device net ip " << net_ip << " done"); "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; constexpr int max_retries = 3;
bool initialized = false; bool initialized = false;
device_info_ = device_->getDeviceInfo(); 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) { while (retry_count < max_retries && !initialized) {
try { try {
@@ -1068,14 +1086,14 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
RCLCPP_INFO_STREAM(logger_, "Current node pid: " << getpid()); RCLCPP_INFO_STREAM(logger_, "Current node pid: " << getpid());
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>( auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start_time_); 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()) { if (!upgrade_firmware_.empty()) {
// Check if this is a second update (reupdate scenario) // Check if this is a second update (reupdate scenario)
bool is_second_update = is_reupdating_.load(); bool is_second_update = is_reupdating_.load();
if (is_second_update) { 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 { } else {
RCLCPP_INFO(logger_, "Starting firmware update from file: %s", upgrade_firmware_.c_str()); 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_) { if (need_reupdate_) {
// Some devices require a second update after reboot // 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 // Set flag to indicate we're waiting for device to reboot for second update
is_reupdating_ = true; is_reupdating_ = true;
// Keep upgrade_firmware_ path and wait for device to reconnect // 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 (firmware_update_success_) {
if (is_second_update) { if (is_second_update) {
RCLCPP_INFO(logger_, "Second firmware update completed successfully!"); RCLCPP_INFO(logger_, "Second firmware update completed successfully");
is_reupdating_ = false; is_reupdating_ = false;
} else { } else {
RCLCPP_INFO(logger_, "Firmware update completed successfully!"); RCLCPP_INFO(logger_, "Firmware update completed successfully");
} }
return; return;
} }
@@ -1129,7 +1147,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
ob_lidar_node_->startStreams(); ob_lidar_node_->startStreams();
ob_lidar_node_->startIMU(); ob_lidar_node_->startIMU();
} else { } 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 } // namespace orbbec_camera
@@ -1289,7 +1307,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
// success get lock,break // success get lock,break
break; break;
} else if (try_lock_result == EBUSY) { } 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)); std::this_thread::sleep_for(std::chrono::milliseconds(100));
} else { } else {
RCLCPP_ERROR_STREAM(logger_, "Failed to lock orb_device_lock_"); 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 end_time = std::chrono::high_resolution_clock::now();
auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time); 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(); 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();
time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time); 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_) { if (firmware_update_success_) {
firmware_update_success_ = false; firmware_update_success_ = false;
orbbec_camera/src/ob_lidar_node.cpp
+67 -59
View File
@@ -64,27 +64,26 @@ void OBLidarNode::setAndGetNodeParameter(
OBLidarNode::~OBLidarNode() noexcept { clean(); } OBLidarNode::~OBLidarNode() noexcept { clean(); }
void OBLidarNode::rebootDevice() { void OBLidarNode::rebootDevice() {
RCLCPP_WARN_STREAM(logger_, "Reboot device"); RCLCPP_INFO_STREAM(logger_, "Rebooting device");
clean(); clean();
if (device_) { if (device_) {
device_->reboot(); device_->reboot();
RCLCPP_WARN_STREAM(logger_, "Reboot device DONE"); RCLCPP_DEBUG_STREAM(logger_, "Reboot device complete");
} }
} }
void OBLidarNode::clean() noexcept { void OBLidarNode::clean() noexcept {
std::lock_guard<decltype(device_lock_)> lock(device_lock_); std::lock_guard<decltype(device_lock_)> lock(device_lock_);
RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBLidarNode"); RCLCPP_DEBUG_STREAM(logger_, "Destroying OBLidarNode");
is_running_.store(false); is_running_.store(false);
RCLCPP_WARN_STREAM(logger_, "Stop tf thread"); RCLCPP_DEBUG_STREAM(logger_, "Stop tf thread");
if (tf_thread_ && tf_thread_->joinable()) { if (tf_thread_ && tf_thread_->joinable()) {
tf_thread_->join(); tf_thread_->join();
} }
RCLCPP_WARN_STREAM(logger_, "Stop color frame thread"); RCLCPP_DEBUG_STREAM(logger_, "Stop streams");
RCLCPP_WARN_STREAM(logger_, "stop streams");
stopStreams(); stopStreams();
stopIMU(); stopIMU();
RCLCPP_WARN_STREAM(logger_, "Destroy ~OBLidarNode DONE"); RCLCPP_DEBUG_STREAM(logger_, "OBLidarNode cleanup complete");
} }
void OBLidarNode::setupTopics() { void OBLidarNode::setupTopics() {
@@ -95,7 +94,8 @@ void OBLidarNode::setupTopics() {
setupProfiles(); setupProfiles();
setupPublishers(); setupPublishers();
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e)); RCLCPP_ERROR_STREAM(logger_,
"Failed to setup topics: " << orbbec_camera::formatObErrorWithStatus(e));
throw std::runtime_error(orbbec_camera::formatObErrorWithStatus(e)); throw std::runtime_error(orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) { } catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what()); RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
@@ -115,9 +115,9 @@ void OBLidarNode::getParameters() {
param_name = stream_name_[stream_index] + "_rate"; param_name = stream_name_[stream_index] + "_rate";
setAndGetNodeParameter(rate_int_[stream_index], param_name, 0); setAndGetNodeParameter(rate_int_[stream_index], param_name, 0);
rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]); rate_[stream_index] = OBScanRateFromInt(rate_int_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index]) RCLCPP_DEBUG_STREAM(logger_, "Input rate: " << magic_enum::enum_name(rate_[stream_index])
<< " Input format:" << " Input format:"
<< magic_enum::enum_name(format_[stream_index])); << magic_enum::enum_name(format_[stream_index]));
param_name = stream_name_[stream_index] + "_frame_id"; param_name = stream_name_[stream_index] + "_frame_id";
std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame"; std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame";
setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id); setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id);
@@ -181,6 +181,8 @@ void OBLidarNode::getParameters() {
} }
void OBLidarNode::setupDevices() { void OBLidarNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Current time domain: " << time_domain_);
auto sensor_list = device_->getSensorList(); auto sensor_list = device_->getSensorList();
for (size_t i = 0; i < sensor_list->getCount(); i++) { for (size_t i = 0; i < sensor_list->getCount(); i++) {
auto sensor = sensor_list->getSensor(i); auto sensor = sensor_list->getSensor(i);
@@ -196,14 +198,13 @@ void OBLidarNode::setupDevices() {
} }
for (const auto &[stream_index, enable] : enable_stream_) { for (const auto &[stream_index, enable] : enable_stream_) {
if (enable && sensors_.find(stream_index) == sensors_.end()) { if (enable && sensors_.find(stream_index) == sensors_.end()) {
RCLCPP_INFO_STREAM(logger_, RCLCPP_WARN_STREAM(logger_, magic_enum::enum_name(stream_index.first)
magic_enum::enum_name(stream_index.first) << " sensor not supported by current device, skipping");
<< "sensor isn't supported by current device! -- Skipping...");
enable_stream_[stream_index] = false; enable_stream_[stream_index] = false;
} }
} }
if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) { if (device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting heartbeat to " << (enable_heartbeat_ ? "ON" : "OFF")); RCLCPP_INFO_STREAM(logger_, "Current heartbeat: " << (enable_heartbeat_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
} }
if (!echo_mode_.empty() && if (!echo_mode_.empty() &&
@@ -214,9 +215,9 @@ void OBLidarNode::setupDevices() {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_SPECIFIC_MODE_INT, 1); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_SPECIFIC_MODE_INT, 1);
} }
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(
logger_, "Setting echo mode to " logger_, "Current echo mode: " << (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT)
<< (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT) ? "First Echo" ? "First Echo"
: "Last Echo")); : "Last Echo"));
} }
if (repetitive_scan_mode_ != -1 && if (repetitive_scan_mode_ != -1 &&
device_->isPropertySupported(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT, device_->isPropertySupported(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
@@ -229,7 +230,7 @@ void OBLidarNode::setupDevices() {
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT, TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT,
repetitive_scan_mode_); repetitive_scan_mode_);
RCLCPP_INFO_STREAM(logger_, "Setting repetitive scan mode to " << device_->getIntProperty( RCLCPP_INFO_STREAM(logger_, "Current repetitive scan mode: " << device_->getIntProperty(
OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT)); OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT));
} }
} }
@@ -242,7 +243,7 @@ void OBLidarNode::setupDevices() {
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, filter_level_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT, filter_level_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_APPLY_CONFIGS_INT, 1); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LIDAR_APPLY_CONFIGS_INT, 1);
RCLCPP_INFO_STREAM(logger_, "Setting filter level to " << device_->getIntProperty( RCLCPP_INFO_STREAM(logger_, "Current filter level: " << device_->getIntProperty(
OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT)); OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT));
} }
} }
@@ -255,7 +256,7 @@ void OBLidarNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, vertical_fov_); TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT, vertical_fov_);
RCLCPP_INFO_STREAM(logger_, "Setting vertical fov to " << device_->getFloatProperty( RCLCPP_INFO_STREAM(logger_, "Current vertical fov: " << device_->getFloatProperty(
OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT)); OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT));
} }
} }
@@ -303,20 +304,20 @@ void OBLidarNode::setupProfiles() {
<< "Format:" << format_[elem]); << "Format:" << format_[elem]);
RCLCPP_INFO_STREAM(logger_, "Available profiles:"); RCLCPP_INFO_STREAM(logger_, "Available profiles:");
printSensorProfiles(sensor); printSensorProfiles(sensor);
RCLCPP_ERROR(logger_, "Because can not set this stream, so exit."); RCLCPP_ERROR(logger_, "Failed to configure the requested stream profile, exiting.");
exit(-1); exit(-1);
} }
if (!selected_profile) { if (!selected_profile) {
RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! " RCLCPP_WARN_STREAM(
<< " Stream: " << magic_enum::enum_name(elem.first) logger_, "Requested stream configuration is not supported by the device: "
<< ", Stream Index: " << elem.second << "stream=" << magic_enum::enum_name(elem.first)
<< ", Scan Rate: " << rate_[elem]); << ", stream_index=" << elem.second << ", scan_rate=" << rate_[elem]);
if (default_profile) { if (default_profile) {
RCLCPP_WARN_STREAM(logger_, "Using default profile instead."); RCLCPP_WARN_STREAM(logger_, "Using the default profile instead");
RCLCPP_WARN_STREAM(logger_, "default scan Rate " RCLCPP_WARN_STREAM(
<< magic_enum::enum_name(default_profile->getScanRate()) logger_, "Default profile: scan_rate="
<< "default format:" << magic_enum::enum_name(default_profile->getScanRate())
<< magic_enum::enum_name(default_profile->getFormat())); << ", format=" << magic_enum::enum_name(default_profile->getFormat()));
selected_profile = default_profile; selected_profile = default_profile;
} else { } else {
RCLCPP_ERROR_STREAM( RCLCPP_ERROR_STREAM(
@@ -329,11 +330,11 @@ void OBLidarNode::setupProfiles() {
stream_profile_[elem] = selected_profile; stream_profile_[elem] = selected_profile;
rate_[elem] = selected_profile->getScanRate(); rate_[elem] = selected_profile->getScanRate();
format_[elem] = selected_profile->getFormat(); format_[elem] = selected_profile->getFormat();
RCLCPP_INFO_STREAM(logger_, " stream " RCLCPP_DEBUG_STREAM(logger_, "stream "
<< stream_name_[elem] << " is enabled - scan rate: " << stream_name_[elem] << " is enabled - scan rate: "
<< magic_enum::enum_name(selected_profile->getScanRate()) << magic_enum::enum_name(selected_profile->getScanRate())
<< " format:" << " format:"
<< magic_enum::enum_name(selected_profile->getFormat())); << magic_enum::enum_name(selected_profile->getFormat()));
} }
} }
// IMU // IMU
@@ -354,12 +355,12 @@ void OBLidarNode::setupProfiles() {
auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate); auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate);
stream_profile_[stream_index] = profile; stream_profile_[stream_index] = profile;
} }
RCLCPP_INFO_STREAM(logger_, "stream " << stream_name_[stream_index] << " full scale range " RCLCPP_INFO_STREAM(logger_, "Stream " << stream_name_[stream_index] << " full scale range: "
<< (stream_index == ACCEL ? accel_range_ : gyro_range_) << (stream_index == ACCEL ? accel_range_ : gyro_range_)
<< " sample rate " << imu_rate_); << ", sample rate: " << imu_rate_);
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index] RCLCPP_INFO_STREAM(logger_, "Failed to set up " << stream_name_[stream_index] << " profile: "
<< " profile: " << orbbec_camera::formatObErrorWithStatus(e)); << orbbec_camera::formatObErrorWithStatus(e));
enable_stream_[stream_index] = false; enable_stream_[stream_index] = false;
stream_profile_[stream_index] = nullptr; stream_profile_[stream_index] = nullptr;
} }
@@ -434,7 +435,8 @@ void OBLidarNode::startStreams() {
onNewFrameSetCallback(frame_set); onNewFrameSetCallback(frame_set);
}); });
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e)); RCLCPP_ERROR_STREAM(logger_,
"Failed to start pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
setupPipelineConfig(); setupPipelineConfig();
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) { pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
onNewFrameSetCallback(frame_set); onNewFrameSetCallback(frame_set);
@@ -499,13 +501,14 @@ void OBLidarNode::startIMU() {
void OBLidarNode::stopStreams() { void OBLidarNode::stopStreams() {
if (!pipeline_started_ || !pipeline_) { if (!pipeline_started_ || !pipeline_) {
RCLCPP_INFO_STREAM(logger_, "pipeline not started or not exist, skip stop pipeline"); RCLCPP_DEBUG_STREAM(logger_, "Pipeline not started or not exist, skip stop pipeline");
return; return;
} }
try { try {
pipeline_->stop(); pipeline_->stop();
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << orbbec_camera::formatObErrorWithStatus(e)); RCLCPP_ERROR_STREAM(logger_,
"Failed to stop pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (...) { } catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline"); RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
} }
@@ -517,14 +520,16 @@ void OBLidarNode::stopIMU() {
} }
if (!imu_sync_output_start_ || !imuPipeline_) { if (!imu_sync_output_start_ || !imuPipeline_) {
RCLCPP_INFO_STREAM(logger_, "IMU pipeline not started or not exist, skip stop imu pipeline"); RCLCPP_DEBUG_STREAM(logger_,
"IMU pipeline not started or unavailable, skip stopping IMU pipeline");
return; return;
} }
try { try {
imuPipeline_->stop(); imuPipeline_->stop();
imu_sync_output_start_ = false; imu_sync_output_start_ = false;
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline: " << orbbec_camera::formatObErrorWithStatus(e)); RCLCPP_ERROR_STREAM(
logger_, "Failed to stop IMU pipeline: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (...) { } catch (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline"); RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline");
} }
@@ -537,15 +542,15 @@ void OBLidarNode::setupPipelineConfig() {
pipeline_config_ = std::make_shared<ob::Config>(); pipeline_config_ = std::make_shared<ob::Config>();
for (const auto &stream_index : LIDAR_STREAMS) { for (const auto &stream_index : LIDAR_STREAMS) {
if (enable_stream_[stream_index]) { if (enable_stream_[stream_index]) {
RCLCPP_INFO_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream"); RCLCPP_DEBUG_STREAM(logger_, "Enable " << stream_name_[stream_index] << " stream");
auto profile = stream_profile_[stream_index]->as<ob::LiDARStreamProfile>(); auto profile = stream_profile_[stream_index]->as<ob::LiDARStreamProfile>();
if (enable_stream_[stream_index]) { if (enable_stream_[stream_index]) {
auto video_profile = profile; auto video_profile = profile;
RCLCPP_INFO_STREAM(logger_, RCLCPP_DEBUG_STREAM(logger_,
"lidar profile: " << magic_enum::enum_name(video_profile->getScanRate()) "lidar profile: " << magic_enum::enum_name(video_profile->getScanRate())
<< " " << " "
<< magic_enum::enum_name(video_profile->getFormat())); << magic_enum::enum_name(video_profile->getFormat()));
} }
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(
@@ -650,7 +655,8 @@ void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set)
} }
} }
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << orbbec_camera::formatObErrorWithStatus(e)); RCLCPP_ERROR_STREAM(
logger_, "onNewFrameSetCallback error: " << orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) { } catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what()); RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
} catch (...) { } catch (...) {
@@ -1244,12 +1250,13 @@ void OBLidarNode::calcAndPublishStaticTransform() {
RCLCPP_DEBUG_STREAM(logger_, "ACCEL and GYRO extrinsics are identical"); RCLCPP_DEBUG_STREAM(logger_, "ACCEL and GYRO extrinsics are identical");
} }
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, RCLCPP_WARN_STREAM(logger_, "Could not get GYRO extrinsic for verification: "
"Could not get GYRO extrinsic for verification: " << orbbec_camera::formatObErrorWithStatus(e)); << orbbec_camera::formatObErrorWithStatus(e));
} }
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to get ACCEL extrinsic, trying GYRO: " << orbbec_camera::formatObErrorWithStatus(e)); RCLCPP_WARN_STREAM(logger_, "Failed to get ACCEL extrinsic, trying GYRO: "
<< orbbec_camera::formatObErrorWithStatus(e));
try { try {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]); ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
RCLCPP_INFO_STREAM(logger_, "Using GYRO extrinsic for IMU"); RCLCPP_INFO_STREAM(logger_, "Using GYRO extrinsic for IMU");
@@ -1272,11 +1279,12 @@ void OBLidarNode::calcAndPublishStaticTransform() {
auto timestamp = node_->now(); auto timestamp = node_->now();
publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], accel_gyro_frame_id_); publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], accel_gyro_frame_id_);
RCLCPP_INFO_STREAM(logger_, "Publishing static transform from " RCLCPP_DEBUG_STREAM(logger_, "Publishing static transform from "
<< frame_id_[base_stream_] << " to " << accel_gyro_frame_id_); << frame_id_[base_stream_] << " to " << accel_gyro_frame_id_);
RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]); RCLCPP_DEBUG_STREAM(logger_,
RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ() "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
<< ", " << Q.getW()); RCLCPP_DEBUG_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
} }
} }
orbbec_camera/src/ros_service.cpp
+15 -14
View File
@@ -645,7 +645,7 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
auto range = device_->getIntPropertyRange(prop_id); auto range = device_->getIntPropertyRange(prop_id);
if (request->data < range.min || request->data > range.max) { if (request->data < range.min || request->data > range.max) {
response->success = false; response->success = false;
RCLCPP_INFO_STREAM(logger_, "set gain value out of range"); RCLCPP_WARN_STREAM(logger_, "Gain value is out of range");
response->message = "value out of range"; response->message = "value out of range";
return; return;
} }
@@ -717,9 +717,9 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
device_->getStructuredData(OB_STRUCT_DEPTH_AE_ROI, reinterpret_cast<uint8_t*>(&config), device_->getStructuredData(OB_STRUCT_DEPTH_AE_ROI, reinterpret_cast<uint8_t*>(&config),
&data_size); &data_size);
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(
logger_, "set depth AE ROI : " logger_, "Set depth AE ROI to "
<< "[Left: " << config.x0_left << ", Right: " << config.x1_right << "[Left: " << config.x0_left << ", Right: " << config.x1_right
<< ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << " ]"); << ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << "]");
break; break;
case OB_STREAM_COLOR: case OB_STREAM_COLOR:
case OB_STREAM_COLOR_LEFT: case OB_STREAM_COLOR_LEFT:
@@ -753,9 +753,9 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
device_->getStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<uint8_t*>(&config), device_->getStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<uint8_t*>(&config),
&data_size); &data_size);
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(
logger_, "set color AE ROI : " logger_, "Set color AE ROI to "
<< "[Left: " << config.x0_left << ", Right: " << config.x1_right << "[Left: " << config.x0_left << ", Right: " << config.x1_right
<< ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << " ]"); << ", Top: " << config.y0_top << ", Bottom: " << config.y1_bottom << "]");
break; break;
default: default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__); RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
@@ -800,13 +800,13 @@ void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Requ
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT); auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT);
if (request->data < range.min || request->data > range.max) { if (request->data < range.min || request->data > range.max) {
response->success = false; response->success = false;
RCLCPP_INFO_STREAM(logger_, "set white balance value out of range"); RCLCPP_WARN_STREAM(logger_, "White balance value is out of range");
response->message = "value out of range"; response->message = "value out of range";
return; return;
} }
bool auto_white_balance = device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL); bool auto_white_balance = device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL);
if (auto_white_balance) { if (auto_white_balance) {
RCLCPP_WARN(logger_, "auto white balance is enabled, set white balance will be ignored"); RCLCPP_WARN(logger_, "Auto white balance is enabled, set white balance will be ignored");
response->success = false; response->success = false;
response->message = "auto white balance is enabled"; response->message = "auto white balance is enabled";
return; return;
@@ -887,7 +887,7 @@ void OBCameraNode::setAutoExposureCallback(
auto range = device_->getIntPropertyRange(prop_id); auto range = device_->getIntPropertyRange(prop_id);
if (request->data < range.min || request->data > range.max) { if (request->data < range.min || request->data > range.max) {
response->success = false; response->success = false;
RCLCPP_INFO_STREAM(logger_, "set auto exposure value out of range"); RCLCPP_WARN_STREAM(logger_, "Auto exposure value is out of range");
response->message = "value out of range"; response->message = "value out of range";
return; return;
} }
@@ -1274,7 +1274,7 @@ void OBCameraNode::setPtpConfigCallback(
if (!device_->isPropertySupported(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, if (!device_->isPropertySupported(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL,
OB_PERMISSION_READ_WRITE)) { OB_PERMISSION_READ_WRITE)) {
response->success = false; response->success = false;
RCLCPP_ERROR(logger_, "OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL not supported or not writable"); RCLCPP_ERROR(logger_, "PTP clock sync property is not supported or not writable");
return; return;
} }
device_->setBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, request->data); device_->setBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, request->data);
@@ -1333,18 +1333,19 @@ void OBCameraNode::toggleSensorCallback(const std::shared_ptr<SetBool::Request>&
std::string msg; std::string msg;
if (request->data) { if (request->data) {
if (enable_stream_[stream_index]) { if (enable_stream_[stream_index]) {
msg = stream_name_[stream_index] + " Already ON"; msg = stream_name_[stream_index] + " is already enabled";
} }
RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " ON"); RCLCPP_INFO_STREAM(logger_, "Request to set sensor " << stream_name_[stream_index] << " to ON");
} else { } else {
if (!enable_stream_[stream_index]) { if (!enable_stream_[stream_index]) {
msg = stream_name_[stream_index] + " Already OFF"; msg = stream_name_[stream_index] + " is already disabled";
} }
RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index] << " OFF"); RCLCPP_INFO_STREAM(logger_,
"Request to set sensor " << stream_name_[stream_index] << " to OFF");
} }
if (!msg.empty()) { if (!msg.empty()) {
RCLCPP_ERROR_STREAM(logger_, msg); RCLCPP_WARN_STREAM(logger_, msg);
response->success = true; response->success = true;
response->message = msg; response->message = msg;
return; return;