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

fix: guard property readback failures

Browse Source
This commit is contained in:
slz
2026-09-03 15:06:42 +08:00
parent 27e0c7dc97
commit e48e10f5e9
2 changed files with 221 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+207 -160
View File
@@ -1036,8 +1036,9 @@ void OBCameraNode::setupDevices() {
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT, TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT,
sync_io_voltage_level_); sync_io_voltage_level_);
RCLCPP_INFO_STREAM(logger_, "Current sync IO voltage level: " << device_->getIntProperty( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT)); logger_, "Current sync IO voltage level: "
<< device_->getIntProperty(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT)));
} }
} }
if (monitor_poll_interval_sec_ != -1) { if (monitor_poll_interval_sec_ != -1) {
@@ -1054,16 +1055,16 @@ void OBCameraNode::setupDevices() {
if (should_apply_launch_config("enable_heartbeat") && if (should_apply_launch_config("enable_heartbeat") &&
device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_, "Current heartbeat: "
"Current heartbeat: " << (device_->getBoolProperty(OB_PROP_HEARTBEAT_BOOL) ? "ON" : "OFF")); << (device_->getBoolProperty(OB_PROP_HEARTBEAT_BOOL) ? "ON" : "OFF")));
} }
if (should_apply_launch_config("enable_fps_boost") && if (should_apply_launch_config("enable_fps_boost") &&
device_->isPropertySupported(OB_PROP_FPS_BOOST_BOOL, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_FPS_BOOST_BOOL, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FPS_BOOST_BOOL, enable_fps_boost_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FPS_BOOST_BOOL, enable_fps_boost_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_, "Current fps boost: "
"Current fps boost: " << (device_->getBoolProperty(OB_PROP_FPS_BOOST_BOOL) ? "ON" : "OFF")); << (device_->getBoolProperty(OB_PROP_FPS_BOOST_BOOL) ? "ON" : "OFF")));
} }
if (should_apply_launch_config("enable_firmware_log")) { if (should_apply_launch_config("enable_firmware_log")) {
device_->enableFirmwareLog(enable_firmware_log_); device_->enableFirmwareLog(enable_firmware_log_);
@@ -1072,14 +1073,14 @@ void OBCameraNode::setupDevices() {
if (max_depth_limit_ > 0 && if (max_depth_limit_ > 0 &&
device_->isPropertySupported(OB_PROP_MAX_DEPTH_INT, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_MAX_DEPTH_INT, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MAX_DEPTH_INT, max_depth_limit_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MAX_DEPTH_INT, max_depth_limit_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current max depth limit: " << device_->getIntProperty(OB_PROP_MAX_DEPTH_INT)); logger_, "Current max depth limit: " << device_->getIntProperty(OB_PROP_MAX_DEPTH_INT)));
} }
if (min_depth_limit_ > 0 && if (min_depth_limit_ > 0 &&
device_->isPropertySupported(OB_PROP_MIN_DEPTH_INT, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_MIN_DEPTH_INT, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MIN_DEPTH_INT, min_depth_limit_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MIN_DEPTH_INT, min_depth_limit_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current min depth limit: " << device_->getIntProperty(OB_PROP_MIN_DEPTH_INT)); logger_, "Current min depth limit: " << device_->getIntProperty(OB_PROP_MIN_DEPTH_INT)));
} }
if (laser_energy_level_ != -1 && if (laser_energy_level_ != -1 &&
device_->isPropertySupported(OB_PROP_LASER_ENERGY_LEVEL_INT, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_LASER_ENERGY_LEVEL_INT, OB_PERMISSION_READ_WRITE)) {
@@ -1089,8 +1090,10 @@ void OBCameraNode::setupDevices() {
"Laser energy level is out of range " << range.min << " - " << range.max); "Laser energy level is out of range " << range.min << " - " << range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_ENERGY_LEVEL_INT, laser_energy_level_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_ENERGY_LEVEL_INT, laser_energy_level_);
auto new_laser_energy_level = device_->getIntProperty(OB_PROP_LASER_ENERGY_LEVEL_INT); TRY_EXECUTE_BLOCK({
RCLCPP_INFO_STREAM(logger_, "Current energy level: " << new_laser_energy_level); auto new_laser_energy_level = device_->getIntProperty(OB_PROP_LASER_ENERGY_LEVEL_INT);
RCLCPP_INFO_STREAM(logger_, "Current energy level: " << new_laser_energy_level);
});
} }
} }
if (depth_registration_ && align_mode_ == "SW") { if (depth_registration_ && align_mode_ == "SW") {
@@ -1103,16 +1106,16 @@ void OBCameraNode::setupDevices() {
device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE) && device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE) &&
device_->isPropertySupported(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
if (disparity_to_depth_mode_ == "HW") { if (disparity_to_depth_mode_ == "HW") {
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, 1); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DISPARITY_TO_DEPTH_BOOL, 1);
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 0); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 0);
RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: HW"); RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: HW");
} else if (disparity_to_depth_mode_ == "SW") { } else if (disparity_to_depth_mode_ == "SW") {
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, 0); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DISPARITY_TO_DEPTH_BOOL, 0);
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 1); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 1);
RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: SW"); RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: SW");
} else if (disparity_to_depth_mode_ == "disable") { } else if (disparity_to_depth_mode_ == "disable") {
device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, 0); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DISPARITY_TO_DEPTH_BOOL, 0);
device_->setBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 0); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, 0);
RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: disabled"); RCLCPP_INFO_STREAM(logger_, "Disparity to depth mode: disabled");
} else { } else {
RCLCPP_WARN_STREAM(logger_, "Unknown disparity to depth mode '" RCLCPP_WARN_STREAM(logger_, "Unknown disparity to depth mode '"
@@ -1153,23 +1156,24 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_POWER_LEVEL_CONTROL_INT, ldp_power_level_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_POWER_LEVEL_CONTROL_INT, ldp_power_level_);
RCLCPP_INFO_STREAM(logger_, "Current lrm power level: " << device_->getIntProperty( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
OB_PROP_LASER_POWER_LEVEL_CONTROL_INT)); logger_, "Current lrm power level: "
<< device_->getIntProperty(OB_PROP_LASER_POWER_LEVEL_CONTROL_INT)));
} }
} }
if (should_apply_launch_config("enable_laser") && if (should_apply_launch_config("enable_laser") &&
device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, enable_laser_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, enable_laser_);
RCLCPP_INFO_STREAM(logger_, TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
"Current G300 laser control: " logger_, "Current G300 laser control: "
<< (device_->getIntProperty(OB_PROP_LASER_CONTROL_INT) ? "ON" : "OFF")); << (device_->getIntProperty(OB_PROP_LASER_CONTROL_INT) ? "ON" : "OFF")));
} }
if (should_apply_launch_config("enable_laser") && if (should_apply_launch_config("enable_laser") &&
device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, enable_laser_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, enable_laser_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Current laser control: " << (device_->getIntProperty(OB_PROP_LASER_BOOL) ? "ON" : "OFF")); "Current laser control: " << (device_->getIntProperty(OB_PROP_LASER_BOOL) ? "ON" : "OFF")));
} }
if (!sync_mode_str_.empty()) { if (!sync_mode_str_.empty()) {
auto sync_config = device_->getMultiDeviceSyncConfig(); auto sync_config = device_->getMultiDeviceSyncConfig();
@@ -1183,9 +1187,11 @@ void OBCameraNode::setupDevices() {
sync_config.triggerOutEnable = trigger_out_enabled_; sync_config.triggerOutEnable = trigger_out_enabled_;
sync_config.framesPerTrigger = frames_per_trigger_; sync_config.framesPerTrigger = frames_per_trigger_;
TRY_EXECUTE_BLOCK(device_->setMultiDeviceSyncConfig(sync_config)); TRY_EXECUTE_BLOCK(device_->setMultiDeviceSyncConfig(sync_config));
sync_config = device_->getMultiDeviceSyncConfig(); TRY_EXECUTE_BLOCK({
RCLCPP_INFO_STREAM(logger_, sync_config = device_->getMultiDeviceSyncConfig();
"Current sync mode: " << magic_enum::enum_name(sync_config.syncMode)); RCLCPP_INFO_STREAM(logger_,
"Current sync mode: " << magic_enum::enum_name(sync_config.syncMode));
});
if (sync_mode_ == OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING) { if (sync_mode_ == OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING) {
RCLCPP_INFO_STREAM(logger_, "Frames per trigger: " << sync_config.framesPerTrigger); RCLCPP_INFO_STREAM(logger_, "Frames per trigger: " << sync_config.framesPerTrigger);
RCLCPP_INFO_STREAM(logger_, RCLCPP_INFO_STREAM(logger_,
@@ -1200,22 +1206,24 @@ void OBCameraNode::setupDevices() {
if (should_apply_launch_config("enable_ptp_config") && if (should_apply_launch_config("enable_ptp_config") &&
device_->isPropertySupported(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, device_->isPropertySupported(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL,
OB_PERMISSION_READ_WRITE)) { OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, enable_ptp_config_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL, enable_ptp_config_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current PTP Config: " logger_, "Current PTP Config: "
<< (device_->getBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL) ? "ON" << (device_->getBoolProperty(OB_DEVICE_PTP_CLOCK_SYNC_ENABLE_BOOL) ? "ON"
: "OFF")); : "OFF")));
} }
if (device_->isPropertySupported(OB_PROP_DEPTH_PRECISION_LEVEL_INT, OB_PERMISSION_READ_WRITE) && if (device_->isPropertySupported(OB_PROP_DEPTH_PRECISION_LEVEL_INT, OB_PERMISSION_READ_WRITE) &&
!depth_precision_str_.empty()) { !depth_precision_str_.empty()) {
auto default_precision_level = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT); auto default_precision_level = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
if (default_precision_level != depth_precision_) { if (default_precision_level != depth_precision_) {
device_->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
const auto current_depth_precision = TRY_EXECUTE_BLOCK({
device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT); const auto current_depth_precision =
RCLCPP_INFO_STREAM(logger_, "Current depth precision: " device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
<< depthPrecisionLevelToString(current_depth_precision)); RCLCPP_INFO_STREAM(logger_, "Current depth precision: "
<< depthPrecisionLevelToString(current_depth_precision));
});
} }
} else if (device_->isPropertySupported(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT, } else if (device_->isPropertySupported(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
OB_PERMISSION_READ_WRITE) && OB_PERMISSION_READ_WRITE) &&
@@ -1230,10 +1238,10 @@ void OBCameraNode::setupDevices() {
} else { } else {
TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT, TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
depth_unit_flexible_adjustment); depth_unit_flexible_adjustment);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current depth unit: " logger_, "Current depth unit: "
<< device_->getFloatProperty(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT) << device_->getFloatProperty(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT)
<< "mm"); << "mm"));
} }
} }
@@ -1258,9 +1266,9 @@ void OBCameraNode::setupDevices() {
if (should_apply_launch_config(stream_name_[stream_index] + "_mirror") && if (should_apply_launch_config(stream_name_[stream_index] + "_mirror") &&
device_->isPropertySupported(mirrorPropertyID, OB_PERMISSION_WRITE)) { device_->isPropertySupported(mirrorPropertyID, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, mirrorPropertyID, mirror_stream_[stream_index]); TRY_TO_SET_PROPERTY(setBoolProperty, mirrorPropertyID, mirror_stream_[stream_index]);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current " << stream_name_[stream_index] << " mirror: " logger_, "Current " << stream_name_[stream_index] << " mirror: "
<< (device_->getBoolProperty(mirrorPropertyID) ? "ON" : "OFF")); << (device_->getBoolProperty(mirrorPropertyID) ? "ON" : "OFF")));
} }
OBPropertyID flipPropertyID = OB_PROP_DEPTH_FLIP_BOOL; OBPropertyID flipPropertyID = OB_PROP_DEPTH_FLIP_BOOL;
if (stream_index == COLOR) { if (stream_index == COLOR) {
@@ -1281,9 +1289,9 @@ void OBCameraNode::setupDevices() {
if (should_apply_launch_config(stream_name_[stream_index] + "_flip") && if (should_apply_launch_config(stream_name_[stream_index] + "_flip") &&
device_->isPropertySupported(flipPropertyID, OB_PERMISSION_WRITE)) { device_->isPropertySupported(flipPropertyID, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, flipPropertyID, flip_stream_[stream_index]); TRY_TO_SET_PROPERTY(setBoolProperty, flipPropertyID, flip_stream_[stream_index]);
RCLCPP_INFO_STREAM(logger_, TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
"Current " << stream_name_[stream_index] << " flip: " logger_, "Current " << stream_name_[stream_index] << " flip: "
<< (device_->getBoolProperty(flipPropertyID) ? "ON" : "OFF")); << (device_->getBoolProperty(flipPropertyID) ? "ON" : "OFF")));
} }
OBPropertyID rotationPropertyID = OB_PROP_DEPTH_ROTATE_INT; OBPropertyID rotationPropertyID = OB_PROP_DEPTH_ROTATE_INT;
if (stream_index == COLOR) { if (stream_index == COLOR) {
@@ -1304,8 +1312,9 @@ void OBCameraNode::setupDevices() {
if (rotation_stream_[stream_index] != -1 && if (rotation_stream_[stream_index] != -1 &&
device_->isPropertySupported(rotationPropertyID, OB_PERMISSION_WRITE)) { device_->isPropertySupported(rotationPropertyID, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, rotationPropertyID, rotation_stream_[stream_index]); TRY_TO_SET_PROPERTY(setIntProperty, rotationPropertyID, rotation_stream_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Current " << stream_name_[stream_index] << " rotation: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(rotationPropertyID)); logger_, "Current " << stream_name_[stream_index]
<< " rotation: " << device_->getIntProperty(rotationPropertyID)));
} }
} }
} }
@@ -1314,10 +1323,10 @@ void OBCameraNode::setupDevices() {
device_->isPropertySupported(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL,
enable_color_auto_white_balance_); enable_color_auto_white_balance_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Current color auto white balance: " "Current color auto white balance: "
<< (device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL) ? "ON" : "OFF")); << (device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL) ? "ON" : "OFF")));
} }
if (should_apply_launch_config("color_preset") && !color_preset_.empty()) { if (should_apply_launch_config("color_preset") && !color_preset_.empty()) {
try { try {
@@ -1370,8 +1379,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_EXPOSURE_INT, color_exposure_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_EXPOSURE_INT, color_exposure_);
RCLCPP_INFO_STREAM(logger_, "Current color exposure: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT)); logger_,
"Current color exposure: " << device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT)));
} }
} }
if (color_gain_ != -1 && if (color_gain_ != -1 &&
@@ -1382,8 +1392,8 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAIN_INT, color_gain_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAIN_INT, color_gain_);
RCLCPP_INFO_STREAM(logger_, TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
"Current color gain: " << device_->getIntProperty(OB_PROP_COLOR_GAIN_INT)); logger_, "Current color gain: " << device_->getIntProperty(OB_PROP_COLOR_GAIN_INT)));
} }
} }
if (color_mjpeg_quality_ != -1) { if (color_mjpeg_quality_ != -1) {
@@ -1397,8 +1407,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MJPEG_QUALITY_INT, color_mjpeg_quality_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_MJPEG_QUALITY_INT, color_mjpeg_quality_);
RCLCPP_INFO_STREAM(logger_, "Current color MJPEG quality: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_MJPEG_QUALITY_INT)); logger_,
"Current color MJPEG quality: " << device_->getIntProperty(OB_PROP_MJPEG_QUALITY_INT)));
} }
} }
} }
@@ -1407,19 +1418,19 @@ void OBCameraNode::setupDevices() {
int set_enable_color_auto_exposure_priority = enable_color_auto_exposure_priority_ ? 1 : 0; int set_enable_color_auto_exposure_priority = enable_color_auto_exposure_priority_ ? 1 : 0;
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT, TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT,
set_enable_color_auto_exposure_priority); set_enable_color_auto_exposure_priority);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Current color auto exposure priority: " "Current color auto exposure priority: "
<< (device_->getIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF")); << (device_->getIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF")));
} }
if (should_apply_launch_config("enable_color_auto_exposure") && if (should_apply_launch_config("enable_color_auto_exposure") &&
device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL,
enable_color_auto_exposure_); enable_color_auto_exposure_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Current color auto exposure: " "Current color auto exposure: "
<< (device_->getBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL) ? "ON" : "OFF")); << (device_->getBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL) ? "ON" : "OFF")));
} }
if (color_white_balance_ != -1 && if (color_white_balance_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_WHITE_BALANCE_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_COLOR_WHITE_BALANCE_INT, OB_PERMISSION_WRITE)) {
@@ -1430,8 +1441,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_WHITE_BALANCE_INT, color_white_balance_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_WHITE_BALANCE_INT, color_white_balance_);
RCLCPP_INFO_STREAM(logger_, "Current color white balance: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT)); logger_, "Current color white balance: "
<< device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT)));
} }
} }
@@ -1445,8 +1457,9 @@ void OBCameraNode::setupDevices() {
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_EXPOSURE_INT, TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_EXPOSURE_INT,
color_ae_max_exposure_); color_ae_max_exposure_);
RCLCPP_INFO_STREAM(logger_, "Current color AE max exposure: " << device_->getIntProperty( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
OB_PROP_COLOR_AE_MAX_EXPOSURE_INT)); logger_, "Current color AE max exposure: "
<< device_->getIntProperty(OB_PROP_COLOR_AE_MAX_EXPOSURE_INT)));
} }
} }
if (color_ae_max_gain_ != -1 && if (color_ae_max_gain_ != -1 &&
@@ -1458,8 +1471,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_GAIN_INT, color_ae_max_gain_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_GAIN_INT, color_ae_max_gain_);
RCLCPP_INFO_STREAM(logger_, "Current color AE max gain: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_AE_MAX_GAIN_INT)); logger_,
"Current color AE max gain: " << device_->getIntProperty(OB_PROP_COLOR_AE_MAX_GAIN_INT)));
} }
} }
if (color_brightness_ != -1 && if (color_brightness_ != -1 &&
@@ -1470,8 +1484,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BRIGHTNESS_INT, color_brightness_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BRIGHTNESS_INT, color_brightness_);
RCLCPP_INFO_STREAM(logger_, "Current color brightness: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_BRIGHTNESS_INT)); logger_,
"Current color brightness: " << device_->getIntProperty(OB_PROP_COLOR_BRIGHTNESS_INT)));
} }
} }
if (color_roi_brightness_ != -1 && if (color_roi_brightness_ != -1 &&
@@ -1483,8 +1498,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_ROI_BRIGHTNESS_INT, color_roi_brightness_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_ROI_BRIGHTNESS_INT, color_roi_brightness_);
RCLCPP_INFO_STREAM(logger_, "Current color roi brightness: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_ROI_BRIGHTNESS_INT)); logger_, "Current color roi brightness: "
<< device_->getIntProperty(OB_PROP_COLOR_ROI_BRIGHTNESS_INT)));
} }
} }
if (color_sharpness_ != -1 && if (color_sharpness_ != -1 &&
@@ -1495,8 +1511,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_SHARPNESS_INT, color_sharpness_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_SHARPNESS_INT, color_sharpness_);
RCLCPP_INFO_STREAM(logger_, "Current color sharpness: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_SHARPNESS_INT)); logger_,
"Current color sharpness: " << device_->getIntProperty(OB_PROP_COLOR_SHARPNESS_INT)));
} }
} }
if (color_gamma_ != -1 && if (color_gamma_ != -1 &&
@@ -1507,8 +1524,8 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAMMA_INT, color_gamma_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAMMA_INT, color_gamma_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color gamma: " << device_->getIntProperty(OB_PROP_COLOR_GAMMA_INT)); logger_, "Current color gamma: " << device_->getIntProperty(OB_PROP_COLOR_GAMMA_INT)));
} }
} }
if (color_saturation_ != -1 && if (color_saturation_ != -1 &&
@@ -1519,8 +1536,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_SATURATION_INT, color_saturation_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_SATURATION_INT, color_saturation_);
RCLCPP_INFO_STREAM(logger_, "Current color saturation: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_SATURATION_INT)); logger_,
"Current color saturation: " << device_->getIntProperty(OB_PROP_COLOR_SATURATION_INT)));
} }
} }
if (color_contrast_ != -1 && if (color_contrast_ != -1 &&
@@ -1531,8 +1549,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_CONTRAST_INT, color_contrast_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_CONTRAST_INT, color_contrast_);
RCLCPP_INFO_STREAM(logger_, "Current color contrast: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_CONTRAST_INT)); logger_,
"Current color contrast: " << device_->getIntProperty(OB_PROP_COLOR_CONTRAST_INT)));
} }
} }
if (color_hue_ != -1 && if (color_hue_ != -1 &&
@@ -1543,29 +1562,32 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_HUE_INT, color_hue_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_HUE_INT, color_hue_);
RCLCPP_INFO_STREAM(logger_, TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
"Current color hue: " << device_->getIntProperty(OB_PROP_COLOR_HUE_INT)); logger_, "Current color hue: " << device_->getIntProperty(OB_PROP_COLOR_HUE_INT)));
} }
} }
if (color_backlight_compensation_ != -1 && if (color_backlight_compensation_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT, TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT,
color_backlight_compensation_); color_backlight_compensation_);
RCLCPP_INFO_STREAM(logger_, "Current color backlight compensation: " << device_->getIntProperty( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT)); logger_, "Current color backlight compensation: "
<< device_->getIntProperty(OB_PROP_COLOR_BACKLIGHT_COMPENSATION_INT)));
} }
if (color_denoising_level_ != -1 && if (color_denoising_level_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_DENOISING_LEVEL_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_COLOR_DENOISING_LEVEL_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_DENOISING_LEVEL_INT, color_denoising_level_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_DENOISING_LEVEL_INT, color_denoising_level_);
RCLCPP_INFO_STREAM(logger_, "Current color denoising level: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_COLOR_DENOISING_LEVEL_INT)); logger_, "Current color denoising level: "
<< device_->getIntProperty(OB_PROP_COLOR_DENOISING_LEVEL_INT)));
} }
if (should_apply_launch_config("color_anti_flicker") && if (should_apply_launch_config("color_anti_flicker") &&
device_->isPropertySupported(OB_PROP_COLOR_ANTI_FLICKER_BOOL, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_COLOR_ANTI_FLICKER_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_ANTI_FLICKER_BOOL, color_anti_flicker_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_ANTI_FLICKER_BOOL, color_anti_flicker_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current color anti-flicker to " logger_,
<< (device_->getBoolProperty(OB_PROP_COLOR_ANTI_FLICKER_BOOL) ? "ON" : "OFF")); "Current color anti-flicker to "
<< (device_->getBoolProperty(OB_PROP_COLOR_ANTI_FLICKER_BOOL) ? "ON" : "OFF")));
} }
if (!color_powerline_freq_.empty() && if (!color_powerline_freq_.empty() &&
device_->isPropertySupported(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, OB_PERMISSION_WRITE)) {
@@ -1578,10 +1600,13 @@ void OBCameraNode::setupDevices() {
} else if (color_powerline_freq_ == "auto") { } else if (color_powerline_freq_ == "auto") {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 3); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 3);
} }
const auto current_color_powerline_freq = TRY_EXECUTE_BLOCK({
device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT); const auto current_color_powerline_freq =
RCLCPP_INFO_STREAM(logger_, "Current color powerline freq: " << colorPowerLineFrequencyToString( device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT);
current_color_powerline_freq)); RCLCPP_INFO_STREAM(logger_,
"Current color powerline freq: "
<< colorPowerLineFrequencyToString(current_color_powerline_freq));
});
} }
if (depth_exposure_ != -1 && if (depth_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_DEPTH_EXPOSURE_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_DEPTH_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
@@ -1591,8 +1616,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_EXPOSURE_INT, depth_exposure_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_EXPOSURE_INT, depth_exposure_);
RCLCPP_INFO_STREAM(logger_, "Current depth exposure: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT)); logger_,
"Current depth exposure: " << device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT)));
} }
} }
if (depth_gain_ != -1 && if (depth_gain_ != -1 &&
@@ -1603,8 +1629,8 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_GAIN_INT, depth_gain_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_GAIN_INT, depth_gain_);
RCLCPP_INFO_STREAM(logger_, TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
"Current depth gain: " << device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT)); logger_, "Current depth gain: " << device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT)));
} }
} }
if (should_apply_launch_config("enable_depth_auto_exposure_priority") && if (should_apply_launch_config("enable_depth_auto_exposure_priority") &&
@@ -1613,17 +1639,17 @@ void OBCameraNode::setupDevices() {
int set_enable_depth_auto_exposure_priority = enable_depth_auto_exposure_priority_ ? 1 : 0; int set_enable_depth_auto_exposure_priority = enable_depth_auto_exposure_priority_ ? 1 : 0;
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT, TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT,
set_enable_depth_auto_exposure_priority); set_enable_depth_auto_exposure_priority);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Current depth auto exposure priority: " "Current depth auto exposure priority: "
<< (device_->getIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF")); << (device_->getIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF")));
} }
if (should_apply_launch_config("enable_ir_auto_exposure") && if (should_apply_launch_config("enable_ir_auto_exposure") &&
device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR auto exposure: " logger_, "Current IR auto exposure: "
<< (device_->getBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL) ? "ON" : "OFF")); << (device_->getBoolProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL) ? "ON" : "OFF")));
} }
if (mean_intensity_set_point_ != -1 && if (mean_intensity_set_point_ != -1 &&
device_->isPropertySupported(OB_PROP_IR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_IR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
@@ -1633,8 +1659,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, mean_intensity_set_point_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, mean_intensity_set_point_);
RCLCPP_INFO_STREAM(logger_, "Current depth brightness: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_IR_BRIGHTNESS_INT)); logger_,
"Current depth brightness: " << device_->getIntProperty(OB_PROP_IR_BRIGHTNESS_INT)));
} }
} }
// ir ae max // ir ae max
@@ -1647,8 +1674,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure_);
RCLCPP_INFO_STREAM(logger_, "Current IR AE max exposure: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT)); logger_, "Current IR AE max exposure: "
<< device_->getIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT)));
} }
} }
// ir brightness // ir brightness
@@ -1660,8 +1688,9 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, ir_brightness_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, ir_brightness_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR brightness: " << device_->getIntProperty(OB_PROP_IR_BRIGHTNESS_INT)); logger_,
"Current IR brightness: " << device_->getIntProperty(OB_PROP_IR_BRIGHTNESS_INT)));
} }
} }
if (ir_exposure_ != -1 && if (ir_exposure_ != -1 &&
@@ -1672,8 +1701,8 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_EXPOSURE_INT, ir_exposure_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_EXPOSURE_INT, ir_exposure_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR exposure: " << device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT)); logger_, "Current IR exposure: " << device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT)));
} }
} }
if (ir_gain_ != -1 && device_->isPropertySupported(OB_PROP_IR_GAIN_INT, OB_PERMISSION_WRITE)) { if (ir_gain_ != -1 && device_->isPropertySupported(OB_PROP_IR_GAIN_INT, OB_PERMISSION_WRITE)) {
@@ -1683,16 +1712,16 @@ void OBCameraNode::setupDevices() {
range.min, range.max); range.min, range.max);
} else { } else {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_GAIN_INT, ir_gain_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_GAIN_INT, ir_gain_);
RCLCPP_INFO_STREAM(logger_, TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
"Current IR gain: " << device_->getIntProperty(OB_PROP_IR_GAIN_INT)); logger_, "Current IR gain: " << device_->getIntProperty(OB_PROP_IR_GAIN_INT)));
} }
} }
if (should_apply_launch_config("enable_ir_long_exposure") && if (should_apply_launch_config("enable_ir_long_exposure") &&
device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current IR long exposure: " logger_, "Current IR long exposure: "
<< (device_->getBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL) ? "ON" : "OFF")); << (device_->getBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL) ? "ON" : "OFF")));
} }
if (should_apply_launch_config("enable_noise_removal_filter") && enable_noise_removal_filter_ && if (should_apply_launch_config("enable_noise_removal_filter") && enable_noise_removal_filter_ &&
@@ -1710,11 +1739,13 @@ void OBCameraNode::setupDevices() {
"the value", "the value",
range.min, range.max); range.min, range.max);
} else { } else {
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, noise_removal_filter_min_diff_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_MAX_DIFF_INT,
noise_removal_filter_min_diff_);
} }
} }
RCLCPP_INFO_STREAM(logger_, "Current noise_removal_filter_min_diff: " TRY_EXECUTE_BLOCK(
<< device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT)); RCLCPP_INFO_STREAM(logger_, "Current noise_removal_filter_min_diff: "
<< device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT)));
} }
if (should_apply_launch_config("enable_noise_removal_filter") && enable_noise_removal_filter_ && if (should_apply_launch_config("enable_noise_removal_filter") && enable_noise_removal_filter_ &&
@@ -1732,34 +1763,38 @@ void OBCameraNode::setupDevices() {
"the value", "the value",
range.min, range.max); range.min, range.max);
} else { } else {
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, noise_removal_filter_max_size_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT,
noise_removal_filter_max_size_);
} }
} }
RCLCPP_INFO_STREAM(logger_, "Current noise_removal_filter_max_size: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT)); logger_, "Current noise_removal_filter_max_size: "
<< device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT)));
} }
if (should_apply_launch_config("enable_noise_removal_filter") && if (should_apply_launch_config("enable_noise_removal_filter") &&
sensors_.find(DEPTH) != sensors_.end() && sensors_.find(DEPTH) != sensors_.end() &&
device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_noise_removal_filter_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_SOFT_FILTER_BOOL,
enable_noise_removal_filter_);
RCLCPP_INFO_STREAM(logger_, "Set noise removal filter to " RCLCPP_INFO_STREAM(logger_, "Set noise removal filter to "
<< (enable_noise_removal_filter_ ? "true" : "false")); << (enable_noise_removal_filter_ ? "true" : "false"));
} }
if (should_apply_launch_config("enable_disp_outliers_filter") && if (should_apply_launch_config("enable_disp_outliers_filter") &&
sensors_.find(DEPTH) != sensors_.end() && sensors_.find(DEPTH) != sensors_.end() &&
device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL, enable_disp_outliers_filter_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEPTH_OUTLIERS_FILTER_BOOL,
enable_disp_outliers_filter_);
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(
logger_, "Set DispOutliersFilter to " << (enable_disp_outliers_filter_ ? "true" : "false")); logger_, "Set DispOutliersFilter to " << (enable_disp_outliers_filter_ ? "true" : "false"));
} }
if (disp_outliers_filter_search_mode_ != -1 && sensors_.find(DEPTH) != sensors_.end() && if (disp_outliers_filter_search_mode_ != -1 && sensors_.find(DEPTH) != sensors_.end() &&
device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT, device_->isPropertySupported(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
OB_PERMISSION_READ_WRITE)) { OB_PERMISSION_READ_WRITE)) {
device_->setIntProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT, TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT,
disp_outliers_filter_search_mode_); disp_outliers_filter_search_mode_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current DispOutliersFilter search mode: " logger_, "Current DispOutliersFilter search mode: "
<< device_->getIntProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT)); << device_->getIntProperty(OB_PROP_DEPTH_OUTLIERS_FILTER_SEARCH_MODE_INT)));
} }
if (disparity_range_mode_ != -1 && if (disparity_range_mode_ != -1 &&
device_->isPropertySupported(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, OB_PERMISSION_WRITE)) {
@@ -1772,30 +1807,33 @@ void OBCameraNode::setupDevices() {
} else { } else {
RCLCPP_ERROR(logger_, "disparity range mode does not support this setting"); RCLCPP_ERROR(logger_, "disparity range mode does not support this setting");
} }
const auto current_disparity_range_mode = TRY_EXECUTE_BLOCK({
device_->getIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT); const auto current_disparity_range_mode =
RCLCPP_INFO_STREAM(logger_, "Current disparity range mode: " device_->getIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT);
<< disparityRangeModeToString(current_disparity_range_mode)); RCLCPP_INFO_STREAM(logger_, "Current disparity range mode: "
<< disparityRangeModeToString(current_disparity_range_mode));
});
} }
if (should_apply_launch_config("enable_hardware_noise_removal_filter") && if (should_apply_launch_config("enable_hardware_noise_removal_filter") &&
device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL, device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
OB_PERMISSION_READ_WRITE)) { OB_PERMISSION_READ_WRITE)) {
device_->setBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL, TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL,
enable_hardware_noise_removal_filter_); enable_hardware_noise_removal_filter_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Set hardware noise removal filter to " "Set hardware noise removal filter to "
<< (device_->getBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL) ? "true" << (device_->getBoolProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_ENABLE_BOOL) ? "true"
: "false")); : "false")));
if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT, if (device_->isPropertySupported(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
OB_PERMISSION_READ_WRITE)) { OB_PERMISSION_READ_WRITE)) {
if (hardware_noise_removal_filter_threshold_ != -1.0 && if (hardware_noise_removal_filter_threshold_ != -1.0 &&
enable_hardware_noise_removal_filter_) { enable_hardware_noise_removal_filter_) {
device_->setFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT, TRY_TO_SET_PROPERTY(setFloatProperty, OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT,
hardware_noise_removal_filter_threshold_); hardware_noise_removal_filter_threshold_);
RCLCPP_INFO_STREAM(logger_, "Current hardware noise removal filter threshold: " TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
<< device_->getFloatProperty( logger_,
OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT)); "Current hardware noise removal filter threshold: "
<< device_->getFloatProperty(OB_PROP_HW_NOISE_REMOVE_FILTER_THRESHOLD_FLOAT)));
} }
} }
} }
@@ -1808,28 +1846,31 @@ void OBCameraNode::setupDevices() {
} else { } else {
RCLCPP_ERROR(logger_, "exposure range mode does not support this setting"); RCLCPP_ERROR(logger_, "exposure range mode does not support this setting");
} }
const auto current_exposure_range_mode = TRY_EXECUTE_BLOCK({
device_->getIntProperty(OB_PROP_DEVICE_PERFORMANCE_MODE_INT); const auto current_exposure_range_mode =
RCLCPP_INFO_STREAM(logger_, "Current exposure range mode: " device_->getIntProperty(OB_PROP_DEVICE_PERFORMANCE_MODE_INT);
<< exposureRangeModeToString(current_exposure_range_mode)); RCLCPP_INFO_STREAM(logger_, "Current exposure range mode: "
<< exposureRangeModeToString(current_exposure_range_mode));
});
} }
if (should_apply_launch_config("enable_accel_data_correction") && if (should_apply_launch_config("enable_accel_data_correction") &&
device_->isPropertySupported(OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL, TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL,
enable_accel_data_correction_); enable_accel_data_correction_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Current accel data correction: " "Current accel data correction: "
<< (device_->getBoolProperty(OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL) ? "ON" : "OFF")); << (device_->getBoolProperty(OB_PROP_SDK_ACCEL_FRAME_TRANSFORMED_BOOL) ? "ON"
: "OFF")));
} }
if (should_apply_launch_config("enable_gyro_data_correction") && if (should_apply_launch_config("enable_gyro_data_correction") &&
device_->isPropertySupported(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL, TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL,
enable_gyro_data_correction_); enable_gyro_data_correction_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Current gyro data correction: " "Current gyro data correction: "
<< (device_->getBoolProperty(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL) ? "ON" : "OFF")); << (device_->getBoolProperty(OB_PROP_SDK_GYRO_FRAME_TRANSFORMED_BOOL) ? "ON" : "OFF")));
} }
if (!intra_camera_sync_reference_.empty() && if (!intra_camera_sync_reference_.empty() &&
device_->isPropertySupported(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT, OB_PERMISSION_WRITE)) {
@@ -1842,19 +1883,22 @@ void OBCameraNode::setupDevices() {
} else { } else {
RCLCPP_ERROR(logger_, "intra camera sync reference does not support this setting"); RCLCPP_ERROR(logger_, "intra camera sync reference does not support this setting");
} }
const auto current_intra_camera_sync_reference = TRY_EXECUTE_BLOCK({
device_->getIntProperty(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT); const auto current_intra_camera_sync_reference =
RCLCPP_INFO_STREAM( device_->getIntProperty(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT);
logger_, "Current intra camera sync reference: " RCLCPP_INFO_STREAM(
<< intraCameraSyncReferenceToString(current_intra_camera_sync_reference)); logger_, "Current intra camera sync reference: "
<< intraCameraSyncReferenceToString(current_intra_camera_sync_reference));
});
} }
if (should_apply_launch_config("ae_strategy") && if (should_apply_launch_config("ae_strategy") &&
device_->isPropertySupported(OB_PROP_DEVICE_AE_STRATEGY_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_DEVICE_AE_STRATEGY_INT, OB_PERMISSION_WRITE)) {
device_->setIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT, (ae_strategy_ == "motion" ? 0 : 1)); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEVICE_AE_STRATEGY_INT,
RCLCPP_INFO_STREAM( (ae_strategy_ == "motion" ? 0 : 1));
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current Sports Mode: " logger_, "Current Sports Mode: "
<< (device_->getIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT) == 0 ? "ON" << (device_->getIntProperty(OB_PROP_DEVICE_AE_STRATEGY_INT) == 0 ? "ON"
: "OFF")); : "OFF")));
} }
if (should_apply_launch_config("ae_reference_stream") && if (should_apply_launch_config("ae_reference_stream") &&
@@ -1862,10 +1906,13 @@ void OBCameraNode::setupDevices() {
device_->isPropertySupported(OB_PROP_DEVICE_AE_REFERENCE_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_DEVICE_AE_REFERENCE_INT, OB_PERMISSION_WRITE)) {
if (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_reference = ae_reference_stream_ == "depth" ? 0 : 1; auto ae_reference = ae_reference_stream_ == "depth" ? 0 : 1;
device_->setIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT, ae_reference); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEVICE_AE_REFERENCE_INT, ae_reference);
auto current_ae_reference = device_->getIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT); TRY_EXECUTE_BLOCK({
RCLCPP_INFO_STREAM(logger_, "Current AE Reference: " auto current_ae_reference = device_->getIntProperty(OB_PROP_DEVICE_AE_REFERENCE_INT);
<< (current_ae_reference == 0 ? "depthbased" : "colorbased")); RCLCPP_INFO_STREAM(
logger_,
"Current AE Reference: " << (current_ae_reference == 0 ? "depthbased" : "colorbased"));
});
} }
} }
} }
@@ -4154,11 +4201,11 @@ void OBCameraNode::startStreams() {
if (device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL, OB_PERMISSION_WRITE)) { if (device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL, TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
interleave_frame_enable_); interleave_frame_enable_);
RCLCPP_INFO_STREAM( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, logger_,
"Enable enable_interleave_depth_frame to " "Enable enable_interleave_depth_frame to "
<< (device_->getBoolProperty(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL) ? "true" << (device_->getBoolProperty(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL) ? "true"
: "false")); : "false")));
} }
} }
// set interleave larse PATTERN_SYNC_DELAY // set interleave larse PATTERN_SYNC_DELAY
@@ -4168,9 +4215,9 @@ void OBCameraNode::startStreams() {
(sync_mode_str_ == "PRIMARY" || sync_mode_str_ == "SOFTWARE_TRIGGERING")) { (sync_mode_str_ == "PRIMARY" || sync_mode_str_ == "SOFTWARE_TRIGGERING")) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::this_thread::sleep_for(std::chrono::milliseconds(1000));
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT, 0); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT, 0);
RCLCPP_INFO_STREAM(logger_, TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
"Current interleave laser pattern sync delay: " << device_->getIntProperty( logger_, "Current interleave laser pattern sync delay: " << device_->getIntProperty(
OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT)); OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT)));
} }
pipeline_started_.store(true); pipeline_started_.store(true);
} }
orbbec_camera/src/ob_lidar_node.cpp
+14 -9
View File
@@ -205,8 +205,10 @@ void OBLidarNode::setupDevices() {
} }
} }
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_, "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_);
TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current heartbeat: "
<< (device_->getBoolProperty(OB_PROP_HEARTBEAT_BOOL) ? "ON" : "OFF")));
} }
if (!echo_mode_.empty() && if (!echo_mode_.empty() &&
device_->isPropertySupported(OB_PROP_LIDAR_SPECIFIC_MODE_INT, OB_PERMISSION_READ_WRITE)) { device_->isPropertySupported(OB_PROP_LIDAR_SPECIFIC_MODE_INT, OB_PERMISSION_READ_WRITE)) {
@@ -215,10 +217,10 @@ void OBLidarNode::setupDevices() {
} else if (echo_mode_ == "First Echo") { } else if (echo_mode_ == "First Echo") {
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( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
logger_, "Current echo mode: " << (device_->getIntProperty(OB_PROP_LIDAR_SPECIFIC_MODE_INT) logger_, "Current echo mode: " << (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,
@@ -231,8 +233,9 @@ 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_, "Current repetitive scan mode: " << device_->getIntProperty( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT)); logger_, "Current repetitive scan mode: "
<< device_->getIntProperty(OB_PROP_LIDAR_REPETITIVE_SCAN_MODE_INT)));
} }
} }
if (filter_level_ != -1 && if (filter_level_ != -1 &&
@@ -244,8 +247,9 @@ 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_, "Current filter level: " << device_->getIntProperty( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT)); logger_, "Current filter level: "
<< device_->getIntProperty(OB_PROP_LIDAR_TAIL_FILTER_LEVEL_INT)));
} }
} }
@@ -257,8 +261,9 @@ 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_, "Current vertical fov: " << device_->getFloatProperty( TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT)); logger_, "Current vertical fov: "
<< device_->getFloatProperty(OB_PROP_LIDAR_MEMS_FOV_SIZE_FLOAT)));
} }
} }
} }