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

refactor: improve logging messages for clarity and consistency across camera and lidar nodes

Browse Source
This commit is contained in:
slz
2026-04-07 17:39:00 +08:00
parent 194de7e339
commit 5a799bfc21
4 changed files with 125 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+47 -47
View File
@@ -412,8 +412,8 @@ void OBCameraNode::setupDevices() {
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 0);
RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: disabled");
} else {
RCLCPP_WARN_STREAM(logger_, "Unknown disparity to depth mode '%s', keeping default settings"
<< disparity_to_depth_mode_.c_str());
RCLCPP_WARN_STREAM(logger_, "Unknown disparity to depth mode '"
<< disparity_to_depth_mode_ << "', keeping default settings");
}
}
if (device_->isPropertySupported(OB_PROP_LDP_BOOL, OB_PERMISSION_READ_WRITE)) {
@@ -1058,7 +1058,7 @@ void OBCameraNode::setupDevices() {
OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT));
}
if (device_->isPropertySupported(OB_PROP_DEVICE_AE_STRATEGY_INT, OB_PERMISSION_WRITE)) {
device_->setIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT, (enable_sports_mode_ ? 0 : 1));
device_->setIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT, (ae_strategy_ == "motion" ? 0 : 1));
RCLCPP_INFO_STREAM(
logger_, "Current Sports Mode: "
<< (device_->getIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT) == 0 ? "ON"
@@ -1068,11 +1068,11 @@ void OBCameraNode::setupDevices() {
if ((ae_reference_stream_ == "depth" || ae_reference_stream_ == "color") &&
device_->isPropertySupported(OB_PROP_DEVICE_AE_REFERENCE_INT, OB_PERMISSION_WRITE)) {
if (device_->isPropertySupported(OB_PROP_DEVICE_AE_REFERENCE_INT, OB_PERMISSION_WRITE)) {
auto ae_mode = ae_mode_ == "depthbased" ? 0 : 1;
device_->setIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT, ae_mode);
auto current_ae_mode = device_->getIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT);
RCLCPP_INFO_STREAM(
logger_, "Current AE Mode: " << (current_ae_mode == 0 ? "depthbased" : "colorbased"));
auto ae_reference = ae_reference_stream_ == "depth" ? 0 : 1;
device_->setIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT, ae_reference);
auto current_ae_reference = device_->getIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT);
RCLCPP_INFO_STREAM(logger_, "Current AE Reference: "
<< (current_ae_reference == 0 ? "depthbased" : "colorbased"));
}
}
}
@@ -1103,10 +1103,10 @@ void OBCameraNode::setupColorPostProcessFilter() {
{"DecimationFilter", enable_color_decimation_filter_},
};
std::string filter_name = filter->type();
RCLCPP_DEBUG_STREAM(logger_, "Setting " << filter_name << "......");
RCLCPP_DEBUG_STREAM(logger_, "Configuring color filter: " << filter_name);
if (filter_params.find(filter_name) != filter_params.end()) {
std::string value = filter_params[filter_name] ? "true" : "false";
RCLCPP_INFO_STREAM(logger_, "set color " << filter_name << " to " << value);
const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
RCLCPP_INFO_STREAM(logger_, "Set color filter " << filter_name << " to " << value);
filter->enable(filter_params[filter_name]);
}
if (filter_name == "DecimationFilter" && enable_color_decimation_filter_) {
@@ -1132,10 +1132,10 @@ void OBCameraNode::setupColorPostProcessFilter() {
{"DecimationFilter", enable_left_color_decimation_filter_},
};
std::string filter_name = filter->type();
RCLCPP_DEBUG_STREAM(logger_, "Setting left " << filter_name << "......");
RCLCPP_DEBUG_STREAM(logger_, "Configuring left color filter: " << filter_name);
if (filter_params.find(filter_name) != filter_params.end()) {
std::string value = filter_params[filter_name] ? "true" : "false";
RCLCPP_INFO_STREAM(logger_, "set left color " << filter_name << " to " << value);
const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
RCLCPP_INFO_STREAM(logger_, "Set left color filter " << filter_name << " to " << value);
filter->enable(filter_params[filter_name]);
}
if (filter_name == "DecimationFilter" && enable_left_color_decimation_filter_) {
@@ -1163,10 +1163,10 @@ void OBCameraNode::setupColorPostProcessFilter() {
{"DecimationFilter", enable_right_color_decimation_filter_},
};
std::string filter_name = filter->type();
RCLCPP_DEBUG_STREAM(logger_, "Setting right " << filter_name << "......");
RCLCPP_DEBUG_STREAM(logger_, "Configuring right color filter: " << filter_name);
if (filter_params.find(filter_name) != filter_params.end()) {
std::string value = filter_params[filter_name] ? "true" : "false";
RCLCPP_INFO_STREAM(logger_, "set right color " << filter_name << " to " << value);
const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
RCLCPP_INFO_STREAM(logger_, "Set right color filter " << filter_name << " to " << value);
filter->enable(filter_params[filter_name]);
}
if (filter_name == "DecimationFilter" && enable_right_color_decimation_filter_) {
@@ -1225,10 +1225,10 @@ void OBCameraNode::setupLeftIrPostProcessFilter() {
{"SequenceIdFilter", enable_left_ir_sequence_id_filter_},
};
std::string filter_name = filter->type();
RCLCPP_DEBUG_STREAM(logger_, "Setting " << filter_name << "......");
RCLCPP_DEBUG_STREAM(logger_, "Configuring left IR filter: " << filter_name);
if (filter_params.find(filter_name) != filter_params.end()) {
std::string value = filter_params[filter_name] ? "true" : "false";
RCLCPP_INFO_STREAM(logger_, "set left ir " << filter_name << " to " << value);
const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
RCLCPP_INFO_STREAM(logger_, "Set left IR filter " << filter_name << " to " << value);
filter->enable(filter_params[filter_name]);
}
if (filter_name == "SequenceIdFilter" && enable_left_ir_sequence_id_filter_) {
@@ -1259,10 +1259,10 @@ void OBCameraNode::setupRightIrPostProcessFilter() {
{"SequenceIdFilter", enable_right_ir_sequence_id_filter_},
};
std::string filter_name = filter->type();
RCLCPP_DEBUG_STREAM(logger_, "Setting " << filter_name << "......");
RCLCPP_DEBUG_STREAM(logger_, "Configuring right IR filter: " << filter_name);
if (filter_params.find(filter_name) != filter_params.end()) {
std::string value = filter_params[filter_name] ? "true" : "false";
RCLCPP_INFO_STREAM(logger_, "set right ir " << filter_name << " to " << value);
const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
RCLCPP_INFO_STREAM(logger_, "Set right IR filter " << filter_name << " to " << value);
filter->enable(filter_params[filter_name]);
}
if (filter_name == "SequenceIdFilter" && enable_right_ir_sequence_id_filter_) {
@@ -1303,10 +1303,10 @@ void OBCameraNode::setupDepthPostProcessFilter() {
for (size_t i = 0; i < depth_filter_list_.size(); i++) {
auto filter = depth_filter_list_[i];
std::string filter_name = filter->type();
RCLCPP_DEBUG_STREAM(logger_, "Setting " << filter_name << "......");
RCLCPP_DEBUG_STREAM(logger_, "Configuring depth filter: " << filter_name);
if (filter_params.find(filter_name) != filter_params.end()) {
std::string value = filter_params[filter_name] ? "true" : "false";
RCLCPP_INFO_STREAM(logger_, "set " << filter_name << " to " << value);
const auto *value = filter_params[filter_name] ? "enabled" : "disabled";
RCLCPP_INFO_STREAM(logger_, "Set depth filter " << filter_name << " to " << value);
filter->enable(filter_params[filter_name]);
filter_status_[filter_name] = filter_params[filter_name];
}
@@ -1560,25 +1560,24 @@ void OBCameraNode::setupProfiles() {
"configuration and try again. The current process will now exit.");
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
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);
}
if (!selected_profile) {
RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
<< " Stream: " << magic_enum::enum_name(elem.first)
<< ", Stream Index: " << elem.second
<< ", Width: " << width_[elem]
<< ", Height: " << height_[elem] << ", FPS: " << fps_[elem]
<< ", Format: " << magic_enum::enum_name(format_[elem]));
RCLCPP_WARN_STREAM(logger_,
"Requested stream configuration is not supported by the device: "
<< "stream=" << magic_enum::enum_name(elem.first)
<< ", stream_index=" << elem.second << ", width=" << width_[elem]
<< ", height=" << height_[elem] << ", fps=" << fps_[elem]
<< ", format=" << magic_enum::enum_name(format_[elem]));
if (default_profile) {
RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
RCLCPP_WARN_STREAM(logger_, "default FPS " << default_profile->getFps());
RCLCPP_WARN_STREAM(logger_, "Using the default profile instead");
RCLCPP_WARN_STREAM(logger_, "Default profile FPS: " << default_profile->getFps());
selected_profile = default_profile;
} else {
RCLCPP_ERROR_STREAM(
logger_, " NO default_profile found , Stream: " << magic_enum::enum_name(elem.first)
<< " will be disable");
RCLCPP_ERROR_STREAM(logger_, "No default profile found, disabling stream "
<< magic_enum::enum_name(elem.first));
enable_stream_[elem] = false;
continue;
}
@@ -1886,11 +1885,11 @@ void OBCameraNode::stopStreams() {
// disable interleave frame only if device is still connected
if ((interleave_ae_mode_ == "hdr") || (interleave_ae_mode_ == "laser")) {
try {
RCLCPP_DEBUG_STREAM(logger_, "current interleave_ae_mode_: " << interleave_ae_mode_);
RCLCPP_DEBUG_STREAM(logger_, "Current interleave AE mode: " << interleave_ae_mode_);
if (device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
OB_PERMISSION_WRITE)) {
interleave_frame_enable_ = false;
RCLCPP_DEBUG_STREAM(logger_, "Enable enable_interleave_depth_frame to "
RCLCPP_DEBUG_STREAM(logger_, "Set enable_interleave_depth_frame to "
<< (interleave_frame_enable_ ? "true" : "false"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
interleave_frame_enable_);
@@ -1919,7 +1918,8 @@ void OBCameraNode::stopIMU() {
if (enable_sync_output_accel_gyro_) {
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;
}
try {
@@ -1935,7 +1935,7 @@ void OBCameraNode::stopIMU() {
for (const auto &stream_index : HID_STREAMS) {
if (imu_started_[stream_index]) {
CHECK(sensors_.count(stream_index));
RCLCPP_INFO_STREAM(logger_, "stop " << stream_name_[stream_index] << " stream");
RCLCPP_DEBUG_STREAM(logger_, "Stop " << stream_name_[stream_index] << " stream");
try {
sensors_[stream_index]->stop();
} catch (const ob::Error &e) {
@@ -2373,7 +2373,7 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<std::string>(ae_reference_stream_, "ae_reference_stream", "depth");
setAndGetNodeParameter<std::string>(ae_strategy_, "ae_strategy", "motion");
RCLCPP_INFO_STREAM(logger_, "current time domain: " << time_domain_);
RCLCPP_INFO_STREAM(logger_, "Current time domain: " << time_domain_);
RCLCPP_DEBUG_STREAM(logger_, "hdr_index1_laser_control_ "
<< hdr_index1_laser_control_ << " hdr_index1_depth_exposure_ "
<< hdr_index1_depth_exposure_ << " hdr_index1_depth_gain_ "
@@ -3501,7 +3501,7 @@ void OBCameraNode::onNewColorFrameCallback() {
color_frame_queue_.pop();
}
RCLCPP_DEBUG_STREAM(logger_, "Color frame thread exit!");
RCLCPP_DEBUG_STREAM(logger_, "Color frame thread exited");
}
void OBCameraNode::onNewLeftColorFrameCallback() {
@@ -3519,7 +3519,7 @@ void OBCameraNode::onNewLeftColorFrameCallback() {
onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_LEFT), COLOR_LEFT);
left_color_frame_queue_.pop();
}
RCLCPP_DEBUG_STREAM(logger_, "Left Color frame thread exit!");
RCLCPP_DEBUG_STREAM(logger_, "Left color frame thread exited");
}
void OBCameraNode::onNewRightColorFrameCallback() {
@@ -3537,7 +3537,7 @@ void OBCameraNode::onNewRightColorFrameCallback() {
onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), COLOR_RIGHT);
right_color_frame_queue_.pop();
}
RCLCPP_DEBUG_STREAM(logger_, "Right Color frame thread exit!");
RCLCPP_DEBUG_STREAM(logger_, "Right color frame thread exited");
}
std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
@@ -4714,7 +4714,7 @@ void OBCameraNode::setFilterCallback(const std::shared_ptr<SetFilter ::Request>
if (request->filter_enable) {
device_->setFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
request->filter_param[0]);
RCLCPP_INFO_STREAM(logger_, "Setting hardware noise removal filter threshold :"
RCLCPP_INFO_STREAM(logger_, "Set hardware noise removal filter threshold to "
<< request->filter_param[0]);
}
} else {
orbbec_camera/src/ob_camera_node_driver.cpp
+38 -31
View File
@@ -244,7 +244,8 @@ void OBCameraNodeDriver::init() {
ob::Context::setLoggerFileName(log_file_name);
RCLCPP_INFO_STREAM(logger_, "SDK log file name set to: " << log_file_name);
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_, "Failed to set SDK log file name: " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_WARN_STREAM(logger_, "Failed to set SDK log file name: "
<< orbbec_camera::formatObErrorWithStatus(e));
}
}
// Force IP
@@ -310,14 +311,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_,
"not support uvc_backend:" << uvc_backend_ << ", set to default libuvc");
RCLCPP_WARN_STREAM(logger_,
"Unsupported uvc_backend '" << uvc_backend_ << "', using default libuvc");
}
ctx_->enableNetDeviceEnumeration(enumerate_net_device_);
device_changed_callback_id_ = ctx_->registerDeviceChangedCallback(
@@ -347,17 +348,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) {
@@ -525,7 +525,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
@@ -544,7 +544,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 {
@@ -554,9 +554,10 @@ 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));
RCLCPP_WARN_STREAM(logger_, "OB Exception during device reset: "
<< orbbec_camera::formatObErrorWithStatus(e));
} catch (const std::exception &e) {
RCLCPP_WARN_STREAM(logger_, "Standard exception during device reset: " << e.what());
} catch (...) {
@@ -566,9 +567,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");
}
@@ -581,7 +582,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");
}
}
@@ -866,8 +867,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceBySerialNumber(
}
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_ERROR_STREAM_THROTTLE(
logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.what());
@@ -899,8 +901,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByUSBPort(
}
return device;
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_ERROR_STREAM_THROTTLE(
logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << e.what());
@@ -935,8 +938,9 @@ std::shared_ptr<ob::Device> OBCameraNodeDriver::selectDeviceByNetIP(
return list->getDevice(i, device_access_mode_);
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_ERROR_STREAM_THROTTLE(
logger_, *get_clock(), 5000,
"Failed to get device info " << orbbec_camera::formatObErrorWithStatus(e));
continue;
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM_THROTTLE(logger_, *get_clock(), 5000,
@@ -1091,14 +1095,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());
}
@@ -1126,7 +1130,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
@@ -1136,10 +1140,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;
}
@@ -1224,7 +1228,8 @@ bool OBCameraNodeDriver::applyForceIpConfig() {
RCLCPP_ERROR(logger_, "[ForceIP] Failed to apply config (SDK returned false)");
}
} catch (const ob::Error &e) {
RCLCPP_ERROR(logger_, "[ForceIP] ob::Error: %s", orbbec_camera::formatObErrorWithStatus(e).c_str());
RCLCPP_ERROR(logger_, "[ForceIP] ob::Error: %s",
orbbec_camera::formatObErrorWithStatus(e).c_str());
} catch (const std::exception &e) {
RCLCPP_ERROR(logger_, "[ForceIP] std::exception: %s", e.what());
} catch (...) {
@@ -1342,7 +1347,7 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
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_DEBUG_STREAM(logger_, "Initialize device cost: " << time_cost.count() << " ms");
if (firmware_update_success_) {
firmware_update_success_ = false;
@@ -1364,7 +1369,8 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
ob_camera_node_->startStreams();
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_ERROR_STREAM(
logger_, "Failed to initialize device " << orbbec_camera::formatObErrorWithStatus(e));
start_device_failed = true;
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to initialize device " << e.what());
@@ -1437,7 +1443,8 @@ void OBCameraNodeDriver::updatePresetFirmware(std::string path) {
}
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware "
<< orbbec_camera::formatObErrorWithStatus(e));
} catch (std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to update Preset Firmware " << e.what());
} catch (...) {
orbbec_camera/src/ob_lidar_node.cpp
+31 -24
View File
@@ -94,7 +94,8 @@ void OBLidarNode::setupTopics() {
setupProfiles();
setupPublishers();
} 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));
} catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to setup topics: " << e.what());
@@ -303,20 +304,20 @@ void OBLidarNode::setupProfiles() {
<< "Format:" << format_[elem]);
RCLCPP_INFO_STREAM(logger_, "Available profiles:");
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);
}
if (!selected_profile) {
RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
<< " Stream: " << magic_enum::enum_name(elem.first)
<< ", Stream Index: " << elem.second
<< ", Scan Rate: " << rate_[elem]);
RCLCPP_WARN_STREAM(
logger_, "Requested stream configuration is not supported by the device: "
<< "stream=" << magic_enum::enum_name(elem.first)
<< ", stream_index=" << elem.second << ", scan_rate=" << rate_[elem]);
if (default_profile) {
RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
RCLCPP_WARN_STREAM(logger_, "default scan Rate "
<< magic_enum::enum_name(default_profile->getScanRate())
<< "default format:"
<< magic_enum::enum_name(default_profile->getFormat()));
RCLCPP_WARN_STREAM(logger_, "Using the default profile instead");
RCLCPP_WARN_STREAM(
logger_, "Default profile: scan_rate="
<< magic_enum::enum_name(default_profile->getScanRate())
<< ", format=" << magic_enum::enum_name(default_profile->getFormat()));
selected_profile = default_profile;
} else {
RCLCPP_ERROR_STREAM(
@@ -354,12 +355,12 @@ void OBLidarNode::setupProfiles() {
auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate);
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_)
<< " sample rate " << imu_rate_);
<< ", sample rate: " << imu_rate_);
} catch (const ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index]
<< " profile: " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_INFO_STREAM(logger_, "Failed to set up " << stream_name_[stream_index] << " profile: "
<< orbbec_camera::formatObErrorWithStatus(e));
enable_stream_[stream_index] = false;
stream_profile_[stream_index] = nullptr;
}
@@ -434,7 +435,8 @@ void OBLidarNode::startStreams() {
onNewFrameSetCallback(frame_set);
});
} 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();
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
onNewFrameSetCallback(frame_set);
@@ -505,7 +507,8 @@ void OBLidarNode::stopStreams() {
try {
pipeline_->stop();
} 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 (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline");
}
@@ -517,14 +520,16 @@ void OBLidarNode::stopIMU() {
}
if (!imu_sync_output_start_ || !imuPipeline_) {
RCLCPP_DEBUG_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;
}
try {
imuPipeline_->stop();
imu_sync_output_start_ = false;
} 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 (...) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop IMU pipeline");
}
@@ -537,7 +542,7 @@ void OBLidarNode::setupPipelineConfig() {
pipeline_config_ = std::make_shared<ob::Config>();
for (const auto &stream_index : LIDAR_STREAMS) {
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>();
if (enable_stream_[stream_index]) {
@@ -650,7 +655,8 @@ void OBLidarNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set)
}
}
} 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) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
} catch (...) {
@@ -1244,12 +1250,13 @@ void OBLidarNode::calcAndPublishStaticTransform() {
RCLCPP_DEBUG_STREAM(logger_, "ACCEL and GYRO extrinsics are identical");
}
} catch (const ob::Error &e) {
RCLCPP_WARN_STREAM(logger_,
"Could not get GYRO extrinsic for verification: " << orbbec_camera::formatObErrorWithStatus(e));
RCLCPP_WARN_STREAM(logger_, "Could not get GYRO extrinsic for verification: "
<< orbbec_camera::formatObErrorWithStatus(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 {
ex = base_stream_profile->getExtrinsicTo(stream_profile_[GYRO]);
RCLCPP_INFO_STREAM(logger_, "Using GYRO extrinsic for IMU");
orbbec_camera/src/ros_service.cpp
+9 -8
View File
@@ -643,7 +643,7 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
auto range = device_->getIntPropertyRange(prop_id);
if (request->data < range.min || request->data > range.max) {
response->success = false;
RCLCPP_INFO_STREAM(logger_, "set gain value out of range");
RCLCPP_INFO_STREAM(logger_, "Set gain value out of range");
response->message = "value out of range";
return;
}
@@ -751,9 +751,9 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
device_->getStructuredData(OB_STRUCT_COLOR_AE_ROI, reinterpret_cast<uint8_t*>(&config),
&data_size);
RCLCPP_INFO_STREAM(
logger_, "set color AE ROI : "
logger_, "Set color AE ROI : "
<< "[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;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
@@ -1331,18 +1331,19 @@ void OBCameraNode::toggleSensorCallback(const std::shared_ptr<SetBool::Request>&
std::string msg;
if (request->data) {
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 {
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()) {
RCLCPP_ERROR_STREAM(logger_, msg);
RCLCPP_WARN_STREAM(logger_, msg);
response->success = true;
response->message = msg;
return;