diff --git a/README.MD b/README.MD index 9015817c..73afb4ab 100644 --- a/README.MD +++ b/README.MD @@ -334,8 +334,8 @@ Launch camera node source ~/ros2_ws/install/setup.bash ros2 run orbbec_camera list_devices_node # Check if the camera is connected ros2 launch orbbec_camera gemini_330_series.launch.py # Or other launch file, see below table -# Optional: enable Jet colorized depth on /camera/depth/image_raw -ros2 launch orbbec_camera gemini_330_series.launch.py enable_depth_colorizer:=true +# Optional: publish OrbbecViewer-style colorized depth on /camera/depth/image_raw +ros2 launch orbbec_camera gemini_330_series.launch.py depth_colorizer_mode:=jet ``` - On terminal 2 @@ -369,9 +369,14 @@ ros2 service call /camera/get_sdk_version orbbec_camera_msgs/srv/GetString '{}' For more usage details, please refer to the official [OrbbecSDK ROS2 documentation](https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/4_application_guide/application_guide.html) -When `enable_depth_colorizer` is enabled, `depth/image_raw` directly publishes an `rgb8` image -(OpenCV Jet). When disabled, the topic continues to publish the raw `16UC1` depth image; the -colorized image uses a 0--10 m depth range for mapping. +`depth_colorizer_mode` is a string render mode: + +- `none`: disable colorization and publish the raw `16UC1` depth image; +- `jet`: use the OrbbecViewer-style dynamic histogram, gamma, and Jet mapping; +- `jet_inv`: use the OrbbecViewer-style inverse Jet mapping; +- `gray`: use the OrbbecViewer-style grayscale mapping; + +The `jet` and `jet_inv` modes publish `rgb8`; `gray` publishes `mono8` on `depth/image_raw`. ## Supported Devices diff --git a/README_CN.MD b/README_CN.MD index 3ace3e42..962be62f 100644 --- a/README_CN.MD +++ b/README_CN.MD @@ -333,8 +333,8 @@ sudo bash install_udev_rules.sh source ~/ros2_ws/install/setup.bash ros2 run orbbec_camera list_devices_node #检查相机是否已连接 ros2 launch orbbec_camera gemini_330_series.launch.py # 或其他启动文件,见下表 -# 可选:开启 Jet 伪彩色深度图,直接发布到 /camera/depth/image_raw -ros2 launch orbbec_camera gemini_330_series.launch.py enable_depth_colorizer:=true +# 可选:使用 OrbbecViewer 风格发布伪彩色深度图到 /camera/depth/image_raw +ros2 launch orbbec_camera gemini_330_series.launch.py depth_colorizer_mode:=jet ``` - 终端 2 @@ -368,8 +368,14 @@ ros2 service call /camera/get_sdk_version orbbec_camera_msgs/srv/GetString '{}' 更多使用详情,请参考官方 [OrbbecSDK ROS2 文档](https://orbbec.github.io/OrbbecSDK_ROS2/zh/source/camera_devices/4_application_guide/application_guide.html) -开启 `enable_depth_colorizer` 后,`depth/image_raw` 直接发布 `rgb8` 彩色图(OpenCV Jet)。 -关闭时该话题仍发布原始 `16UC1` 深度图,彩色图使用 0--10 m 深度范围进行映射。 +`depth_colorizer_mode` 是字符串渲染模式: + +- `none`:关闭彩色化,`depth/image_raw` 发布原始 `16UC1` 深度图; +- `jet`:使用 OrbbecViewer 的动态直方图、伽马和 Jet 映射; +- `jet_inv`:使用 OrbbecViewer 风格的反向 Jet 映射; +- `gray`:使用 OrbbecViewer 风格的灰度映射; + +`jet` 和 `jet_inv` 模式发布 `rgb8`,`gray` 模式发布 `mono8` 到 `depth/image_raw`。 ## 支持的设备 diff --git a/orbbec_camera/config/camera_params.yaml b/orbbec_camera/config/camera_params.yaml index 6733d17c..d2cedfa3 100644 --- a/orbbec_camera/config/camera_params.yaml +++ b/orbbec_camera/config/camera_params.yaml @@ -1,7 +1,7 @@ # common params depth_registration: false enable_d2c_viewer: false -enable_depth_colorizer: false +depth_colorizer_mode: "none" enable_point_cloud: false enable_colored_point_cloud: false enable_ldp: false diff --git a/orbbec_camera/config/camera_secondary_params.yaml b/orbbec_camera/config/camera_secondary_params.yaml index eb23535e..2e865f80 100644 --- a/orbbec_camera/config/camera_secondary_params.yaml +++ b/orbbec_camera/config/camera_secondary_params.yaml @@ -1,7 +1,7 @@ # common params depth_registration: false enable_d2c_viewer: false -enable_depth_colorizer: false +depth_colorizer_mode: "none" enable_point_cloud: false enable_colored_point_cloud: false enable_ldp: false diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index f40cd81f..ba0bccf2 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -801,7 +801,7 @@ class OBCameraNode { std::string color_info_url_; std::string ir_info_url_; std::optional camera_param_; - bool colorizer_enabled_ = false; + std::string colorizer_mode_ = "none"; bool enable_d2c_viewer_ = false; std::unique_ptr d2c_viewer_ = nullptr; std::map save_images_; diff --git a/orbbec_camera/launch/astra.launch.py b/orbbec_camera/launch/astra.launch.py index ba4217b6..72bcf7a7 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/orbbec_camera/launch/astra.launch.py @@ -68,7 +68,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ordered_pc', default_value='false'), DeclareLaunchArgument('use_hardware_time', default_value='false'), diff --git a/orbbec_camera/launch/astra2.launch.py b/orbbec_camera/launch/astra2.launch.py index f8d55d97..266848cf 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/orbbec_camera/launch/astra2.launch.py @@ -100,7 +100,7 @@ def generate_launch_description(): DeclareLaunchArgument('temporal_filter_weight', default_value='-1.0'), DeclareLaunchArgument('hole_filling_filter_mode', default_value=''), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), - DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), + DeclareLaunchArgument("depth_colorizer_mode", default_value="none"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument("ordered_pc", default_value="false"), DeclareLaunchArgument("enable_depth_scale", default_value="true"), diff --git a/orbbec_camera/launch/dabai_a.launch.py b/orbbec_camera/launch/dabai_a.launch.py index f7459b5c..111b14db 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/orbbec_camera/launch/dabai_a.launch.py @@ -42,7 +42,8 @@ def load_parameters(context, args): if config_file_path: 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'} + skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', + 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -158,7 +159,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_al.launch.py b/orbbec_camera/launch/dabai_al.launch.py index d37939f4..54284e0d 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/orbbec_camera/launch/dabai_al.launch.py @@ -42,7 +42,8 @@ def load_parameters(context, args): if config_file_path: 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'} + skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', + 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -158,7 +159,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_dcw2.launch.py b/orbbec_camera/launch/dabai_dcw2.launch.py index 6110f973..356109d4 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/orbbec_camera/launch/dabai_dcw2.launch.py @@ -77,7 +77,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_level', default_value='info'), DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('enable_soft_filter', default_value='true'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'), diff --git a/orbbec_camera/launch/dabai_max_pro.launch.py b/orbbec_camera/launch/dabai_max_pro.launch.py index 16b84b45..4e3af7a2 100644 --- a/orbbec_camera/launch/dabai_max_pro.launch.py +++ b/orbbec_camera/launch/dabai_max_pro.launch.py @@ -86,7 +86,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'), DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'), DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'), diff --git a/orbbec_camera/launch/femto.launch.py b/orbbec_camera/launch/femto.launch.py index 2d4253d1..2d34a233 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/orbbec_camera/launch/femto.launch.py @@ -77,7 +77,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), - DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), + DeclareLaunchArgument("depth_colorizer_mode", default_value="none"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument("enable_frame_sync", default_value="false"), DeclareLaunchArgument("ordered_pc", default_value="false"), diff --git a/orbbec_camera/launch/femto_bolt.launch.py b/orbbec_camera/launch/femto_bolt.launch.py index 6ac85e79..68d9cb5f 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/orbbec_camera/launch/femto_bolt.launch.py @@ -80,7 +80,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), - DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), + DeclareLaunchArgument("depth_colorizer_mode", default_value="none"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'), DeclareLaunchArgument('enable_decimation_filter', default_value='false'), diff --git a/orbbec_camera/launch/femto_mega.launch.py b/orbbec_camera/launch/femto_mega.launch.py index 8b89b86d..6c98f871 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/orbbec_camera/launch/femto_mega.launch.py @@ -86,7 +86,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), - DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), + DeclareLaunchArgument("depth_colorizer_mode", default_value="none"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument('enable_noise_removal_filter', default_value='false'), DeclareLaunchArgument('enable_decimation_filter', default_value='false'), diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index 7b3bd1bc..6f26cb21 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -92,7 +92,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), - DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), + DeclareLaunchArgument("depth_colorizer_mode", default_value="none"), DeclareLaunchArgument('disparity_to_depth_mode', default_value='SW'), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument('enable_decimation_filter', default_value='false'), diff --git a/orbbec_camera/launch/gemini210.launch.py b/orbbec_camera/launch/gemini210.launch.py index 10219f69..e05413d6 100644 --- a/orbbec_camera/launch/gemini210.launch.py +++ b/orbbec_camera/launch/gemini210.launch.py @@ -90,7 +90,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), - DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), + DeclareLaunchArgument("depth_colorizer_mode", default_value="none"), DeclareLaunchArgument('disparity_to_depth_mode', default_value='SW'), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument('enable_decimation_filter', default_value='false'), diff --git a/orbbec_camera/launch/gemini2L.launch.py b/orbbec_camera/launch/gemini2L.launch.py index 64d27153..63668a47 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -64,7 +64,8 @@ def load_parameters(context, args): if config_file_path: 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'} + skip_convert = {"config_file_path", "usb_port", "serial_number", 'bag_record_filename', 'bag_filename', + "depth_colorizer_mode"} return { key: (value if key in skip_convert else convert_value(value)) @@ -146,7 +147,7 @@ def generate_launch_description(): DeclareLaunchArgument("log_file_name", default_value=""), DeclareLaunchArgument("enable_publish_extrinsic", default_value="false"), DeclareLaunchArgument("enable_d2c_viewer", default_value="false"), - DeclareLaunchArgument("enable_depth_colorizer", default_value="false"), + DeclareLaunchArgument("depth_colorizer_mode", default_value="none"), DeclareLaunchArgument("disparity_to_depth_mode", default_value="SW"), DeclareLaunchArgument("enable_ldp", default_value="true"), DeclareLaunchArgument("enable_decimation_filter", default_value="false"), diff --git a/orbbec_camera/launch/gemini345.launch.py b/orbbec_camera/launch/gemini345.launch.py index fac15ca8..53c303a3 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/orbbec_camera/launch/gemini345.launch.py @@ -42,7 +42,8 @@ def load_parameters(context, args): if config_file_path: 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'} + skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', + 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -157,7 +158,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini345_lg.launch.py b/orbbec_camera/launch/gemini345_lg.launch.py index 75159fc3..7419a3f3 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/orbbec_camera/launch/gemini345_lg.launch.py @@ -42,7 +42,8 @@ def load_parameters(context, args): if config_file_path: 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'} + skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', + 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -158,7 +159,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini435_le.launch.py b/orbbec_camera/launch/gemini435_le.launch.py index fac6695b..e0e26223 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/orbbec_camera/launch/gemini435_le.launch.py @@ -42,7 +42,8 @@ def load_parameters(context, args): if config_file_path: 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'} + skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', + 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -188,7 +189,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 88c5ed7a..ffa977d7 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -64,7 +64,8 @@ def load_parameters(context, args): if config_file_path: 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'} + skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', + 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -206,7 +207,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('sync_mode', default_value='standalone'), DeclareLaunchArgument('depth_delay_us', default_value='0'), diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py index 2b61316f..4fc4954d 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -49,7 +49,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', - 'enhanced_depth_model_path'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -212,7 +212,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='false'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py index 14364ec0..b412e194 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/orbbec_camera/launch/gemini_330_series_low_cpu.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', - 'enhanced_depth_model_path'} + 'enhanced_depth_model_path', 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -199,7 +199,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='false'), DeclareLaunchArgument('ldp_power_level', default_value='-1'), diff --git a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py index e5d04a92..9cab2b4a 100644 --- a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py +++ b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py @@ -41,7 +41,8 @@ def load_parameters(context, args): config_file_path = LaunchConfiguration('config_file_path').perform(context) if config_file_path: default_params = merge_params(default_params, load_yaml(config_file_path)) - skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename'} + skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', + 'depth_colorizer_mode'} result = {} for key, value in default_params.items(): @@ -113,7 +114,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_frame_drop_log', default_value='false'), DeclareLaunchArgument('frame_timestamp_csv_file', default_value=''), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('show_fps_enable', default_value='false'), DeclareLaunchArgument('enumerate_net_device', default_value='true'), diff --git a/orbbec_camera/launch/gemini_intra_process_demo_launch.py b/orbbec_camera/launch/gemini_intra_process_demo_launch.py index 1149f3b2..36df13fa 100644 --- a/orbbec_camera/launch/gemini_intra_process_demo_launch.py +++ b/orbbec_camera/launch/gemini_intra_process_demo_launch.py @@ -42,7 +42,8 @@ def load_parameters(context, args): if config_file_path: 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'} + skip_convert = {'config_file_path', 'usb_port', 'serial_number', 'bag_record_filename', 'bag_filename', + 'depth_colorizer_mode'} return { key: (value if key in skip_convert else convert_value(value)) for key, value in default_params.items() @@ -117,7 +118,7 @@ def generate_launch_description(): DeclareLaunchArgument('log_file_name', default_value=''), DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'), DeclareLaunchArgument('enable_d2c_viewer', default_value='false'), - DeclareLaunchArgument('enable_depth_colorizer', default_value='false'), + DeclareLaunchArgument('depth_colorizer_mode', default_value='none'), DeclareLaunchArgument('disparity_to_depth_mode', default_value='HW'), DeclareLaunchArgument('enable_ldp', default_value='true'), DeclareLaunchArgument('sync_mode', default_value='standalone'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 94fcd0f7..951dadd3 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -66,7 +67,10 @@ namespace { constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800"; constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16"; -constexpr double kColorizerMaxDistanceMm = 10000.0; +constexpr double kViewerColorizerGamma = 0.65; +constexpr uint16_t kViewerColorizerMaxDistanceMm = 10000; +constexpr uint16_t kViewerColorizerDefaultMinDistanceMm = 100; +constexpr uint16_t kViewerColorizerG305MinDistanceMm = 40; std::string getDepthFilterStatusName(const std::string &filter_name) { if (filter_name == "SpatialAdvancedFilter") { @@ -4467,7 +4471,10 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(point_cloud_decimation_filter_factor_, "point_cloud_decimation_filter_factor", 1); setAndGetNodeParameter(point_cloud_qos_, "point_cloud_qos", "default"); - setAndGetNodeParameter(colorizer_enabled_, "enable_depth_colorizer", false); + setAndGetNodeParameter(colorizer_mode_, "depth_colorizer_mode", "none"); + colorizer_mode_ = lowerParameterValue(colorizer_mode_); + colorizer_mode_ = normalizeClosedSetParameterValue( + logger_, "depth_colorizer_mode", colorizer_mode_, {"none", "jet", "jet_inv", "gray"}, "none"); setAndGetNodeParameter(enable_d2c_viewer_, "enable_d2c_viewer", false); setAndGetNodeParameter(disparity_to_depth_mode_, "disparity_to_depth_mode", ""); disparity_to_depth_mode_ = @@ -5430,9 +5437,10 @@ void OBCameraNode::setupPublishers() { } } - if (colorizer_enabled_ && enable_stream_[DEPTH]) { - RCLCPP_INFO(logger_, - "Depth colorizer enabled, publishing Jet RGB images on depth/image_raw (0-10 m)"); + if (colorizer_mode_ != "none" && enable_stream_[DEPTH]) { + RCLCPP_INFO_STREAM(logger_, "Depth colorizer mode '" + << colorizer_mode_ + << "' enabled, publishing colorized images on depth/image_raw"); } syncSoftwareAlignment(); @@ -5613,22 +5621,78 @@ cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image) { return {}; } - cv::Mat depth_8u; - if (depth_image.type() == CV_16UC1 || depth_image.type() == CV_32FC1) { - depth_image.convertTo(depth_8u, CV_8UC1, 255.0 / kColorizerMaxDistanceMm); - } else if (depth_image.type() == CV_8UC1) { - depth_image.convertTo(depth_8u, CV_8UC1); - } else { + if (colorizer_mode_ == "none") { + return {}; + } + + if (depth_image.type() != CV_16UC1 && depth_image.type() != CV_32FC1 && + depth_image.type() != CV_8UC1) { RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000, "Unsupported depth image type for colorizer: %d", depth_image.type()); return {}; } - cv::Mat colorized_bgr; - cv::applyColorMap(depth_8u, colorized_bgr, cv::COLORMAP_JET); + cv::Mat depth_16u; + depth_image.convertTo(depth_16u, CV_16UC1); + + const uint16_t min_depth = isGemini305SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm + : kViewerColorizerDefaultMinDistanceMm; + const uint16_t max_depth = kViewerColorizerMaxDistanceMm; + const uint32_t value_range = static_cast(max_depth) - min_depth + 1; + std::array(kViewerColorizerMaxDistanceMm) + 1> histogram{}; + uint32_t valid_pixel_count = 0; + + for (int row = 0; row < depth_16u.rows; ++row) { + const auto *depth_row = depth_16u.ptr(row); + for (int col = 0; col < depth_16u.cols; ++col) { + const uint16_t depth_value = depth_row[col]; + if (depth_value >= min_depth && depth_value <= max_depth) { + ++histogram[depth_value]; + ++valid_pixel_count; + } + } + } + for (uint32_t depth_value = 1; depth_value <= max_depth; ++depth_value) { + histogram[depth_value] += histogram[depth_value - 1]; + } + + cv::Mat depth_8u(depth_16u.size(), CV_8UC1); + for (int row = 0; row < depth_16u.rows; ++row) { + const auto *depth_row = depth_16u.ptr(row); + auto *mapped_row = depth_8u.ptr(row); + for (int col = 0; col < depth_16u.cols; ++col) { + uint16_t depth_value = depth_row[col]; + if (depth_value > max_depth) { + depth_value = max_depth; + } + if (valid_pixel_count != 0 && depth_value >= min_depth) { + depth_value = static_cast(static_cast(value_range) * + histogram[depth_value] / valid_pixel_count + + min_depth); + } + const double normalized_depth = std::max( + 0.0, + std::min(1.0, (static_cast(depth_value) - min_depth) / (max_depth - min_depth))); + const double scale_value = 255.0 * std::pow(normalized_depth, kViewerColorizerGamma); + mapped_row[col] = static_cast(std::max(0.0, std::min(255.0, scale_value))); + } + } cv::Mat invalid_depth_mask; cv::compare(depth_image, cv::Scalar(0), invalid_depth_mask, cv::CMP_EQ); + depth_8u.setTo(cv::Scalar::all(0), invalid_depth_mask); + + if (colorizer_mode_ == "gray") { + return depth_8u; + } + if (colorizer_mode_ == "jet_inv") { + depth_8u = 255 - depth_8u; + depth_8u.setTo(cv::Scalar::all(0), invalid_depth_mask); + } + + cv::Mat colorized_bgr; + cv::applyColorMap(depth_8u, colorized_bgr, cv::COLORMAP_JET); + colorized_bgr.setTo(cv::Scalar::all(0), invalid_depth_mask); cv::Mat colorized_rgb; @@ -6827,12 +6891,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, auto depth_scale = video_frame->as()->getValueScale(); image = image * depth_scale; image_to_publish = image; - if (colorizer_enabled_ && has_raw_image_subscriber) { + if (colorizer_mode_ != "none" && has_raw_image_subscriber) { auto colorized_image = colorizeDepthImage(image); if (!colorized_image.empty()) { image_to_publish = std::move(colorized_image); - image_encoding = sensor_msgs::image_encodings::RGB8; - image_step = static_cast(image_to_publish.cols * 3 * sizeof(uint8_t)); + image_encoding = colorizer_mode_ == "gray" ? sensor_msgs::image_encodings::MONO8 + : sensor_msgs::image_encodings::RGB8; + image_step = static_cast(image_to_publish.cols * image_to_publish.elemSize()); } } }