From 25cf0c767b6535c08c0d75253d9140d7999d08e8 Mon Sep 17 00:00:00 2001 From: Joe Dong Date: Wed, 15 May 2024 10:13:25 +0800 Subject: [PATCH] Update default params --- include/orbbec_camera/ob_camera_node.h | 32 ++++++----- launch/dabai_dcl.launch | 2 +- launch/femto_bolt.launch | 2 +- launch/femto_mega.launch | 2 +- launch/gemini2.launch | 2 +- launch/gemini2L.launch | 2 +- launch/gemini2XL.launch | 2 +- launch/gemini2_nodelet.launch | 2 +- launch/gemini_330_series.launch | 30 +++++++---- src/ob_camera_node.cpp | 34 ++++++------ src/ros_setup.cpp | 74 ++++++++++++++++++++------ 11 files changed, 120 insertions(+), 64 deletions(-) diff --git a/include/orbbec_camera/ob_camera_node.h b/include/orbbec_camera/ob_camera_node.h index 3e84b914..7bb27c9c 100644 --- a/include/orbbec_camera/ob_camera_node.h +++ b/include/orbbec_camera/ob_camera_node.h @@ -419,7 +419,7 @@ class OBCameraNode { // Only for Gemini2 device bool enable_hardware_d2d_ = true; std::string depth_work_mode_; - OBMultiDeviceSyncMode sync_mode_ = OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN; + OBMultiDeviceSyncMode sync_mode_ = OB_MULTI_DEVICE_SYNC_MODE_STANDALONE; std::string sync_mode_str_; int depth_delay_us_ = 0; int color_delay_us_ = 0; @@ -470,19 +470,23 @@ class OBCameraNode { bool enable_temporal_filter_ = false; bool enable_hole_filling_filter_ = false; // filter params - int decimation_filter_scale_range_ = 2; - int sequence_id_filter_id_ = 1; - int threshold_filter_max_ = 16000; - int threshold_filter_min_ = 0; - int noise_removal_filter_min_diff_ = 8; - int noise_removal_filter_max_size_ = 80; - float spatial_filter_alpha_ = 0.5; - int spatial_filter_diff_threshold_ = 8; - int spatial_filter_magnitude_ = 1; - int spatial_filter_radius_ = 1; - float temporal_filter_diff_threshold_ = 0.1; - float temporal_filter_weight_ = 0.4; - std::string hole_filling_filter_mode_ = "FILL_TOP"; + int decimation_filter_scale_range_ = -1; + int sequence_id_filter_id_ = -1; + int threshold_filter_max_ = -1; + int threshold_filter_min_ = -1; + int noise_removal_filter_min_diff_ = -1; + int noise_removal_filter_max_size_ = -1; + float spatial_filter_alpha_ = -1.0; + int spatial_filter_diff_threshold_ = -1; + int spatial_filter_magnitude_ = -1; + int spatial_filter_radius_ = -1; + float temporal_filter_diff_threshold_ = -1.0; + float temporal_filter_weight_ = -1.0; + int hdr_merge_exposure_1_ = -1; + int hdr_merge_gain_1_ = -1; + int hdr_merge_exposure_2_ = -1; + int hdr_merge_gain_2_ = -1; + std::string hole_filling_filter_mode_; ros::Publisher filter_status_pub_; nlohmann::json filter_status_; std::shared_ptr diagnostic_updater_ = nullptr; diff --git a/launch/dabai_dcl.launch b/launch/dabai_dcl.launch index 446a3dd9..e2ae73e0 100644 --- a/launch/dabai_dcl.launch +++ b/launch/dabai_dcl.launch @@ -51,7 +51,7 @@ - + diff --git a/launch/femto_bolt.launch b/launch/femto_bolt.launch index cceef1d6..3731086d 100644 --- a/launch/femto_bolt.launch +++ b/launch/femto_bolt.launch @@ -48,7 +48,7 @@ - + diff --git a/launch/femto_mega.launch b/launch/femto_mega.launch index ddbd5163..fdd19d6a 100644 --- a/launch/femto_mega.launch +++ b/launch/femto_mega.launch @@ -54,7 +54,7 @@ - + diff --git a/launch/gemini2.launch b/launch/gemini2.launch index 6e2aab94..0da9c2d6 100644 --- a/launch/gemini2.launch +++ b/launch/gemini2.launch @@ -51,7 +51,7 @@ - + diff --git a/launch/gemini2L.launch b/launch/gemini2L.launch index d205184b..6a2a8031 100644 --- a/launch/gemini2L.launch +++ b/launch/gemini2L.launch @@ -52,7 +52,7 @@ - + diff --git a/launch/gemini2XL.launch b/launch/gemini2XL.launch index ebddf7a9..29c66cbf 100644 --- a/launch/gemini2XL.launch +++ b/launch/gemini2XL.launch @@ -58,7 +58,7 @@ - + diff --git a/launch/gemini2_nodelet.launch b/launch/gemini2_nodelet.launch index f2bd7a90..da3aa4b8 100644 --- a/launch/gemini2_nodelet.launch +++ b/launch/gemini2_nodelet.launch @@ -55,7 +55,7 @@ - + diff --git a/launch/gemini_330_series.launch b/launch/gemini_330_series.launch index 8cf96d17..44f7c700 100644 --- a/launch/gemini_330_series.launch +++ b/launch/gemini_330_series.launch @@ -15,7 +15,7 @@ - + @@ -58,7 +58,7 @@ - + @@ -74,17 +74,21 @@ - - + + - - - - - - - + + + + + + + + + + + @@ -175,6 +179,10 @@ + + + + diff --git a/src/ob_camera_node.cpp b/src/ob_camera_node.cpp index 23be1ef9..9d03e7a9 100644 --- a/src/ob_camera_node.cpp +++ b/src/ob_camera_node.cpp @@ -141,7 +141,7 @@ void OBCameraNode::getParameters() { enable_ir_auto_exposure_ = nh_private_.param("enable_ir_auto_exposure", true); ir_exposure_ = nh_private_.param("ir_exposure_", -1); enable_ir_long_exposure_ = nh_private_.param("enable_ir_long_exposure", false); - sync_mode_str_ = nh_private_.param("sync_mode", "free_run"); + sync_mode_str_ = nh_private_.param("sync_mode", "standalone"); std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(), ::toupper); sync_mode_ = OBSyncModeFromString(sync_mode_str_); depth_delay_us_ = nh_private_.param("depth_delay_us", 0); @@ -192,23 +192,27 @@ void OBCameraNode::getParameters() { enable_sequenced_filter_ = nh_private_.param("enable_sequenced_filter", false); enable_threshold_filter_ = nh_private_.param("enable_threshold_filter", false); enable_noise_removal_filter_ = nh_private_.param("enable_noise_removal_filter", true); - enable_spatial_filter_ = nh_private_.param("enable_spatial_filter", true); + enable_spatial_filter_ = nh_private_.param("enable_spatial_filter", false); enable_temporal_filter_ = nh_private_.param("enable_temporal_filter", false); enable_hole_filling_filter_ = nh_private_.param("enable_hole_filling_filter", false); - decimation_filter_scale_range_ = nh_private_.param("decimation_filter_scale_range", 2); - sequence_id_filter_id_ = nh_private_.param("sequence_id_filter_id", 1); - threshold_filter_max_ = nh_private_.param("threshold_filter_max", 16000); - threshold_filter_min_ = nh_private_.param("threshold_filter_min", 0); - noise_removal_filter_min_diff_ = nh_private_.param("noise_removal_filter_min_diff", 8); + decimation_filter_scale_range_ = nh_private_.param("decimation_filter_scale_range", -1); + sequence_id_filter_id_ = nh_private_.param("sequence_id_filter_id", -1); + threshold_filter_max_ = nh_private_.param("threshold_filter_max", -1); + threshold_filter_min_ = nh_private_.param("threshold_filter_min", -1); + noise_removal_filter_min_diff_ = nh_private_.param("noise_removal_filter_min_diff", 250); noise_removal_filter_max_size_ = nh_private_.param("noise_removal_filter_max_size", 80); - spatial_filter_alpha_ = nh_private_.param("spatial_filter_alpha", 0.5); - spatial_filter_diff_threshold_ = nh_private_.param("spatial_filter_diff_threshold", 8); - spatial_filter_magnitude_ = nh_private_.param("spatial_filter_magnitude", 1); - spatial_filter_radius_ = nh_private_.param("spatial_filter_radius", 1); - temporal_filter_diff_threshold_ = nh_private_.param("temporal_filter_diff_threshold", 0.1); - temporal_filter_weight_ = nh_private_.param("temporal_filter_weight", 0.4); - hole_filling_filter_mode_ = - nh_private_.param("hole_filling_filter_mode", "FILL_TOP"); + spatial_filter_alpha_ = nh_private_.param("spatial_filter_alpha", -1.0); + spatial_filter_diff_threshold_ = nh_private_.param("spatial_filter_diff_threshold", -1); + spatial_filter_magnitude_ = nh_private_.param("spatial_filter_magnitude", -1); + spatial_filter_radius_ = nh_private_.param("spatial_filter_radius", -1); + temporal_filter_diff_threshold_ = + nh_private_.param("temporal_filter_diff_threshold", -1.0); + temporal_filter_weight_ = nh_private_.param("temporal_filter_weight", -1.0); + hole_filling_filter_mode_ = nh_private_.param("hole_filling_filter_mode", ""); + hdr_merge_exposure_1_ = nh_private_.param("hdr_merge_exposure_1", -1); + hdr_merge_gain_1_ = nh_private_.param("hdr_merge_gain_1", -1); + hdr_merge_exposure_2_ = nh_private_.param("hdr_merge_exposure_2", -1); + hdr_merge_gain_2_ = nh_private_.param("hdr_merge_gain_2", -1); diagnostics_frequency_ = nh_private_.param("diagnostics_frequency", 1.0); enable_laser_ = nh_private_.param("enable_laser", true); laser_on_off_mode_ = nh_private_.param("laser_on_off_mode", 0); diff --git a/src/ros_setup.cpp b/src/ros_setup.cpp index 32627aff..4a0b7aef 100644 --- a/src/ros_setup.cpp +++ b/src/ros_setup.cpp @@ -125,6 +125,19 @@ void OBCameraNode::setupDevices() { if (!device_preset_.empty()) { device_->loadPreset(device_preset_.c_str()); } + if (sync_mode_ != OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN) { + auto sync_config = device_->getMultiDeviceSyncConfig(); + sync_config.syncMode = sync_mode_; + sync_config.depthDelayUs = depth_delay_us_; + sync_config.colorDelayUs = color_delay_us_; + sync_config.trigger2ImageDelayUs = trigger2image_delay_us_; + sync_config.triggerOutDelayUs = trigger_out_delay_us_; + sync_config.triggerOutEnable = trigger_out_enabled_; + device_->setMultiDeviceSyncConfig(sync_config); + if (device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL, + OB_PERMISSION_READ_WRITE)) { + } + } auto depth_sensor = device_->getSensor(OB_SENSOR_DEPTH); device_->setBoolProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_); device_->setBoolProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, enable_color_auto_exposure_); @@ -190,37 +203,65 @@ void OBCameraNode::setupDevices() { } if (filter_name == "DecimationFilter" && enable_decimation_filter_) { auto decimation_filter = filter->as(); - decimation_filter->setScaleValue(decimation_filter_scale_range_); + if (decimation_filter_scale_range_ != -1) { + decimation_filter->setScaleValue(decimation_filter_scale_range_); + } } else if (filter_name == "ThresholdFilter" && enable_threshold_filter_) { auto threshold_filter = filter->as(); - threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_); + if (threshold_filter_min_ != -1 && threshold_filter_max_ != -1) { + threshold_filter->setValueRange(threshold_filter_min_, threshold_filter_max_); + } } else if (filter_name == "SpatialAdvancedFilter" && enable_spatial_filter_) { auto spatial_filter = filter->as(); OBSpatialAdvancedFilterParams params{}; - params.alpha = spatial_filter_alpha_; - params.magnitude = spatial_filter_magnitude_; - params.radius = spatial_filter_radius_; - params.disp_diff = spatial_filter_diff_threshold_; - spatial_filter->setFilterParams(params); + if (spatial_filter_alpha_ != -1.0 && spatial_filter_magnitude_ != -1 && + spatial_filter_radius_ != -1 && spatial_filter_diff_threshold_ != -1) { + params.alpha = spatial_filter_alpha_; + params.magnitude = spatial_filter_magnitude_; + params.radius = spatial_filter_radius_; + params.disp_diff = spatial_filter_diff_threshold_; + spatial_filter->setFilterParams(params); + } } else if (filter_name == "TemporalFilter" && enable_temporal_filter_) { auto temporal_filter = filter->as(); - temporal_filter->setDiffScale(temporal_filter_diff_threshold_); - temporal_filter->setWeight(temporal_filter_weight_); - } else if (filter_name == "HoleFillingFilter") { + if (temporal_filter_diff_threshold_ != -1 && temporal_filter_weight_ != -1) { + temporal_filter->setDiffScale(temporal_filter_diff_threshold_); + temporal_filter->setWeight(temporal_filter_weight_); + } + } else if (filter_name == "HoleFillingFilter" && enable_hole_filling_filter_ && + !hole_filling_filter_mode_.empty()) { auto hole_filling_filter = filter->as(); OBHoleFillingMode hole_filling_mode = holeFillingModeFromString(hole_filling_filter_mode_); hole_filling_filter->setFilterMode(hole_filling_mode); } else if (filter_name == "SequenceIdFilter" && enable_sequenced_filter_) { auto sequenced_filter = filter->as(); - sequenced_filter->selectSequenceId(sequence_id_filter_id_); + if (sequence_id_filter_id_ != -1) { + sequenced_filter->selectSequenceId(sequence_id_filter_id_); + } } else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) { auto noise_removal_filter = filter->as(); OBNoiseRemovalFilterParams params{}; - params.disp_diff = noise_removal_filter_min_diff_; - params.max_size = noise_removal_filter_max_size_; - noise_removal_filter->setFilterParams(params); - } else if (filter_name == "HDRMerge") { - // do nothing + if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) { + params.disp_diff = noise_removal_filter_min_diff_; + params.max_size = noise_removal_filter_max_size_; + noise_removal_filter->setFilterParams(params); + } + } else if (filter_name == "HDRMerge" && enable_hdr_merge_) { + auto hdr_merge_filter = filter->as(); + OBHdrConfig hdr_config{}; + if (hdr_merge_exposure_1_ != -1 && hdr_merge_exposure_2_ != -1 && hdr_merge_gain_1_ != -1 && + hdr_merge_gain_2_ != -1) { + hdr_config.exposure_1 = hdr_merge_exposure_1_; + hdr_config.exposure_2 = hdr_merge_exposure_2_; + hdr_config.gain_1 = hdr_merge_gain_1_; + hdr_config.gain_2 = hdr_merge_gain_2_; + ROS_INFO_STREAM("set HDRMerge exposure_1 to " << hdr_merge_exposure_1_); + ROS_INFO_STREAM("set HDRMerge exposure_2 to " << hdr_merge_exposure_2_); + ROS_INFO_STREAM("set HDRMerge gain_1 to " << hdr_merge_gain_1_); + ROS_INFO_STREAM("set HDRMerge gain_2 to " << hdr_merge_gain_2_); + device_->setStructuredData(OB_STRUCT_DEPTH_HDR_CONFIG, &hdr_config, sizeof(OBHdrConfig)); + } + } else { ROS_INFO_STREAM("Skip setting " << filter_name); } @@ -236,7 +277,6 @@ void OBCameraNode::setupDevices() { if (device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) { device_->setBoolProperty(OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_); } - } catch (const ob::Error& e) { ROS_ERROR_STREAM("Failed to setup devices: " << e.getMessage()); } catch (const std::exception& e) {