diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index dd1efd4a..2f1be9f3 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -681,6 +681,7 @@ class OBCameraNode { static bool isGemini435LePID(uint32_t pid); static bool isPublishMetaData(uint32_t pid); static bool isDabaiASeriesForHwD2C(uint32_t pid); + static bool isLingBotSupportedPID(uint32_t pid); static bool isDepthWorkModeDevices(uint32_t pid); static bool isnotLaserDevices(uint32_t pid); diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index d23afd90..b56d603e 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/orbbec_camera/launch/dabai_a.launch.py @@ -43,7 +43,7 @@ def load_parameters(context, args): yaml_params = load_yaml(config_file_path) default_params = merge_params(default_params, yaml_params) skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', - 'depth_colorizer_mode'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -193,6 +193,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), DeclareLaunchArgument('enable_false_positive_filter', default_value='false'), + DeclareLaunchArgument('enable_enhanced_depth', default_value='false'), + DeclareLaunchArgument('enhanced_depth_model_path', default_value=''), + DeclareLaunchArgument('enhanced_depth_confidence_threshold', default_value='51'), DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'), DeclareLaunchArgument('threshold_filter_max', default_value='-1'), @@ -213,6 +216,7 @@ def generate_launch_description(): DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH + DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame, color_frame, ANY or disable DeclareLaunchArgument('diagnostic_period', default_value='1.0'), DeclareLaunchArgument('depth_precision', default_value=''), DeclareLaunchArgument('device_preset', default_value='Standard'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index 4bf3341e..5eab9010 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/orbbec_camera/launch/dabai_al.launch.py @@ -43,7 +43,7 @@ def load_parameters(context, args): yaml_params = load_yaml(config_file_path) default_params = merge_params(default_params, yaml_params) skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', - 'depth_colorizer_mode'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -193,6 +193,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), DeclareLaunchArgument('enable_false_positive_filter', default_value='false'), + DeclareLaunchArgument('enable_enhanced_depth', default_value='false'), + DeclareLaunchArgument('enhanced_depth_model_path', default_value=''), + DeclareLaunchArgument('enhanced_depth_confidence_threshold', default_value='51'), DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'), DeclareLaunchArgument('threshold_filter_max', default_value='-1'), @@ -213,6 +216,7 @@ def generate_launch_description(): DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH + DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame, color_frame, ANY or disable DeclareLaunchArgument('diagnostic_period', default_value='1.0'), DeclareLaunchArgument('enable_laser', default_value='true'), DeclareLaunchArgument('depth_precision', default_value=''), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index 26942a0a..c0149aec 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/orbbec_camera/launch/gemini345.launch.py @@ -43,7 +43,7 @@ def load_parameters(context, args): yaml_params = load_yaml(config_file_path) default_params = merge_params(default_params, yaml_params) skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', - 'depth_colorizer_mode'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -192,6 +192,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), DeclareLaunchArgument('enable_false_positive_filter', default_value='true'), + DeclareLaunchArgument('enable_enhanced_depth', default_value='false'), + DeclareLaunchArgument('enhanced_depth_model_path', default_value=''), + DeclareLaunchArgument('enhanced_depth_confidence_threshold', default_value='51'), DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'), DeclareLaunchArgument('threshold_filter_max', default_value='-1'), @@ -212,6 +215,7 @@ def generate_launch_description(): DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH + DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame, color_frame, ANY or disable DeclareLaunchArgument('diagnostic_period', default_value='1.0'), DeclareLaunchArgument('depth_precision', default_value=''), DeclareLaunchArgument('device_preset', default_value=''), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index 64739f43..1a84e50d 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/orbbec_camera/launch/gemini345_lg.launch.py @@ -43,7 +43,7 @@ def load_parameters(context, args): yaml_params = load_yaml(config_file_path) default_params = merge_params(default_params, yaml_params) skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', - 'depth_colorizer_mode'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -193,6 +193,9 @@ def generate_launch_description(): DeclareLaunchArgument('enable_disparity_to_depth', default_value='true'), DeclareLaunchArgument('enable_hole_filling_filter', default_value='false'), DeclareLaunchArgument('enable_false_positive_filter', default_value='true'), + DeclareLaunchArgument('enable_enhanced_depth', default_value='false'), + DeclareLaunchArgument('enhanced_depth_model_path', default_value=''), + DeclareLaunchArgument('enhanced_depth_confidence_threshold', default_value='51'), DeclareLaunchArgument('decimation_filter_scale', default_value='-1'), DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'), DeclareLaunchArgument('threshold_filter_max', default_value='-1'), @@ -213,6 +216,7 @@ def generate_launch_description(): DeclareLaunchArgument('hdr_merge_gain_2', default_value='-1'), DeclareLaunchArgument('align_mode', default_value='HW'), DeclareLaunchArgument('align_target_stream', default_value='COLOR'),# COLOR or DEPTH + DeclareLaunchArgument('frame_aggregate_mode', default_value='ANY'), # full_frame, color_frame, ANY or disable DeclareLaunchArgument('diagnostic_period', default_value='1.0'), DeclareLaunchArgument('enable_laser', default_value='true'), DeclareLaunchArgument('depth_precision', default_value=''), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 3dc4a0a7..0189a4a2 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -650,7 +650,7 @@ void OBCameraNode::publishDepthFiltersStatus() { if (disp_outliers_filter_supported) { append_unique_filter_name("DispOutliersFilter"); } - if (isGemini330SeriesPID(pid_)) { + if (isLingBotSupportedPID(pid_)) { append_unique_filter_name("EnhancedDepthFilter"); } @@ -3130,6 +3130,12 @@ void OBCameraNode::setupIrPostProcessFilter() { } void OBCameraNode::setupUndistortionFilters() { + // LingBot requires color undistortion before D2C alignment on Dabai A series devices. + if (enable_enhanced_depth_.load() && isDabaiASeriesForHwD2C(pid_)) { + enable_undistortion_[COLOR] = true; + RCLCPP_INFO(logger_, "Enable color undistortion for LingBot enhanced depth filter"); + } + auto remove_undistortion_filter = [](std::vector> &filters) { filters.erase(std::remove_if(filters.begin(), filters.end(), [](const std::shared_ptr &filter) { @@ -5391,8 +5397,8 @@ bool OBCameraNode::validateEnhancedDepthFilterConfig(std::string &message) const constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800"; constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16"; - if (!isGemini330SeriesPID(pid_)) { - message = "Enhanced depth filter is only supported by Gemini 330 series devices"; + if (!isLingBotSupportedPID(pid_)) { + message = "Enhanced depth filter is only supported by Gemini 330 and Dabai A series devices"; return false; } @@ -8196,6 +8202,10 @@ bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) { pid == GEMINI_345LG_PID; } +bool OBCameraNode::isLingBotSupportedPID(uint32_t pid) { + return isGemini330SeriesPID(pid) || isDabaiASeriesForHwD2C(pid); +} + bool OBCameraNode::isDepthWorkModeDevices(uint32_t pid) { return pid == GEMINI_435Le_PID; } bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini301SeriesPID(pid); } @@ -8525,8 +8535,8 @@ bool OBCameraNode::applyEnhancedDepthFilterConfig( bool enabled, const std::vector &positional_params, const std::vector &named_params, std::string &message) { - if (!isGemini330SeriesPID(pid_)) { - message = "Enhanced depth filter is only supported by Gemini 330 series devices"; + if (!isLingBotSupportedPID(pid_)) { + message = "Enhanced depth filter is only supported by Gemini 330 and Dabai A series devices"; return false; }