From 475dd3923e87575ac0369478236f1d0299b9326b Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Tue, 18 Aug 2026 14:17:26 +0800 Subject: [PATCH 1/5] feat: add support for colorized depth images in camera node and update launch configurations --- README.MD | 6 ++ README_CN.MD | 5 ++ orbbec_camera/config/camera_params.yaml | 3 +- .../config/camera_secondary_params.yaml | 3 +- .../include/orbbec_camera/ob_camera_node.h | 3 + orbbec_camera/launch/astra.launch.py | 1 + orbbec_camera/launch/astra2.launch.py | 1 + orbbec_camera/launch/dabai_a.launch.py | 1 + orbbec_camera/launch/dabai_al.launch.py | 1 + orbbec_camera/launch/dabai_dcw2.launch.py | 1 + orbbec_camera/launch/dabai_max_pro.launch.py | 1 + orbbec_camera/launch/femto.launch.py | 1 + orbbec_camera/launch/femto_bolt.launch.py | 1 + orbbec_camera/launch/femto_mega.launch.py | 1 + orbbec_camera/launch/gemini2.launch.py | 1 + orbbec_camera/launch/gemini210.launch.py | 1 + orbbec_camera/launch/gemini2L.launch.py | 1 + orbbec_camera/launch/gemini345.launch.py | 1 + orbbec_camera/launch/gemini345_lg.launch.py | 1 + orbbec_camera/launch/gemini435_le.launch.py | 1 + .../launch/gemini_301_series.launch.py | 1 + .../launch/gemini_330_series.launch.py | 1 + .../gemini_330_series_low_cpu.launch.py | 1 + .../gemini_330_series_sdk_json.launch.py | 1 + .../gemini_intra_process_demo_launch.py | 1 + orbbec_camera/src/ob_camera_node.cpp | 57 +++++++++++++++++-- 26 files changed, 88 insertions(+), 9 deletions(-) diff --git a/README.MD b/README.MD index 0eef390a..9015817c 100644 --- a/README.MD +++ b/README.MD @@ -334,6 +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 ``` - On terminal 2 @@ -367,6 +369,10 @@ 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. + ## Supported Devices Currently, the following devices are supported by the OrbbecSDK ROS2 Wrapper v2-main branch. More devices support will be added in the near future. If you can not find your device in the table below, try the [main](https://github.com/orbbec/OrbbecSDK_ROS2/tree/main) branch. diff --git a/README_CN.MD b/README_CN.MD index 0ae10dea..3ace3e42 100644 --- a/README_CN.MD +++ b/README_CN.MD @@ -333,6 +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 ``` - 终端 2 @@ -366,6 +368,9 @@ 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 深度范围进行映射。 + ## 支持的设备 目前 v2-main 分支支持以下设备。更多设备支持将陆续增加。如果没有找到您的设备，请尝试 [main](https://github.com/orbbec/OrbbecSDK_ROS2/tree/main) 分支。 diff --git a/orbbec_camera/config/camera_params.yaml b/orbbec_camera/config/camera_params.yaml index 73587fce..6733d17c 100644 --- a/orbbec_camera/config/camera_params.yaml +++ b/orbbec_camera/config/camera_params.yaml @@ -1,6 +1,7 @@ # common params depth_registration: false enable_d2c_viewer: false +enable_depth_colorizer: false enable_point_cloud: false enable_colored_point_cloud: false enable_ldp: false @@ -61,5 +62,3 @@ right_ir_format: "ANY" # ir_height: 0 # ir_fps: 0 # ir_format: "ANY" - - diff --git a/orbbec_camera/config/camera_secondary_params.yaml b/orbbec_camera/config/camera_secondary_params.yaml index cf60bae9..eb23535e 100644 --- a/orbbec_camera/config/camera_secondary_params.yaml +++ b/orbbec_camera/config/camera_secondary_params.yaml @@ -1,6 +1,7 @@ # common params depth_registration: false enable_d2c_viewer: false +enable_depth_colorizer: false enable_point_cloud: false enable_colored_point_cloud: false enable_ldp: false @@ -61,5 +62,3 @@ right_ir_format: "ANY" # ir_height: 0 # ir_fps: 0 # ir_format: "ANY" - - diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index f21ad31c..f40cd81f 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -558,6 +558,8 @@ class OBCameraNode { void publishRawDepthImage(const std::shared_ptr& depth_frame); + cv::Mat colorizeDepthImage(const cv::Mat& depth_image); + std::shared_ptr processDepthFrameFilter(std::shared_ptr& frame); std::shared_ptr processColorFrameFilter(std::shared_ptr& frame); @@ -799,6 +801,7 @@ class OBCameraNode { std::string color_info_url_; std::string ir_info_url_; std::optional camera_param_; + bool colorizer_enabled_ = false; 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 0a498db9..ba4217b6 100644 --- a/orbbec_camera/launch/astra.launch.py +++ b/orbbec_camera/launch/astra.launch.py @@ -68,6 +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('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 d17f738d..f8d55d97 100644 --- a/orbbec_camera/launch/astra2.launch.py +++ b/orbbec_camera/launch/astra2.launch.py @@ -100,6 +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("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 33f2a11c..f7459b5c 100644 --- a/orbbec_camera/launch/dabai_a.launch.py +++ b/orbbec_camera/launch/dabai_a.launch.py @@ -158,6 +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('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 1773024c..d37939f4 100644 --- a/orbbec_camera/launch/dabai_al.launch.py +++ b/orbbec_camera/launch/dabai_al.launch.py @@ -158,6 +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('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 95a1eec0..6110f973 100644 --- a/orbbec_camera/launch/dabai_dcw2.launch.py +++ b/orbbec_camera/launch/dabai_dcw2.launch.py @@ -77,6 +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('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 02757996..16b84b45 100644 --- a/orbbec_camera/launch/dabai_max_pro.launch.py +++ b/orbbec_camera/launch/dabai_max_pro.launch.py @@ -86,6 +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('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 6ea3f7aa..2d4253d1 100644 --- a/orbbec_camera/launch/femto.launch.py +++ b/orbbec_camera/launch/femto.launch.py @@ -77,6 +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("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 7283f469..6ac85e79 100644 --- a/orbbec_camera/launch/femto_bolt.launch.py +++ b/orbbec_camera/launch/femto_bolt.launch.py @@ -80,6 +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("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 040e0696..8b89b86d 100644 --- a/orbbec_camera/launch/femto_mega.launch.py +++ b/orbbec_camera/launch/femto_mega.launch.py @@ -86,6 +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("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 d0beb879..7b3bd1bc 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -92,6 +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('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 e5da7d5f..10219f69 100644 --- a/orbbec_camera/launch/gemini210.launch.py +++ b/orbbec_camera/launch/gemini210.launch.py @@ -90,6 +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('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 81405065..64d27153 100644 --- a/orbbec_camera/launch/gemini2L.launch.py +++ b/orbbec_camera/launch/gemini2L.launch.py @@ -146,6 +146,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("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 fd1e3167..fac15ca8 100644 --- a/orbbec_camera/launch/gemini345.launch.py +++ b/orbbec_camera/launch/gemini345.launch.py @@ -157,6 +157,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('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 87e46fdd..75159fc3 100644 --- a/orbbec_camera/launch/gemini345_lg.launch.py +++ b/orbbec_camera/launch/gemini345_lg.launch.py @@ -158,6 +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('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 726472be..fac6695b 100644 --- a/orbbec_camera/launch/gemini435_le.launch.py +++ b/orbbec_camera/launch/gemini435_le.launch.py @@ -188,6 +188,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('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 4339900b..88c5ed7a 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -206,6 +206,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('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 a8860183..2b61316f 100644 --- a/orbbec_camera/launch/gemini_330_series.launch.py +++ b/orbbec_camera/launch/gemini_330_series.launch.py @@ -212,6 +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('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 36be361e..14364ec0 100644 --- a/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py +++ b/orbbec_camera/launch/gemini_330_series_low_cpu.launch.py @@ -199,6 +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('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 a41be6b1..e5d04a92 100644 --- a/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py +++ b/orbbec_camera/launch/gemini_330_series_sdk_json.launch.py @@ -113,6 +113,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('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 21cc4ae2..1149f3b2 100644 --- a/orbbec_camera/launch/gemini_intra_process_demo_launch.py +++ b/orbbec_camera/launch/gemini_intra_process_demo_launch.py @@ -117,6 +117,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('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 b4874abe..94fcd0f7 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -66,6 +66,7 @@ namespace { constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800"; constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16"; +constexpr double kColorizerMaxDistanceMm = 10000.0; std::string getDepthFilterStatusName(const std::string &filter_name) { if (filter_name == "SpatialAdvancedFilter") { @@ -4466,6 +4467,7 @@ 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(enable_d2c_viewer_, "enable_d2c_viewer", false); setAndGetNodeParameter(disparity_to_depth_mode_, "disparity_to_depth_mode", ""); disparity_to_depth_mode_ = @@ -5428,6 +5430,11 @@ 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)"); + } + syncSoftwareAlignment(); if (enable_sync_output_accel_gyro_) { @@ -5601,6 +5608,34 @@ void OBCameraNode::publishRawDepthImage(const std::shared_ptr &depth_ depth_unaligned_publisher_->publish(std::move(image_msg)); } +cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image) { + if (depth_image.empty() || depth_image.channels() != 1) { + 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 { + 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 invalid_depth_mask; + cv::compare(depth_image, cv::Scalar(0), invalid_depth_mask, cv::CMP_EQ); + colorized_bgr.setTo(cv::Scalar::all(0), invalid_depth_mask); + + cv::Mat colorized_rgb; + cv::cvtColor(colorized_bgr, colorized_rgb, cv::COLOR_BGR2RGB); + return colorized_rgb; +} + void OBCameraNode::publishDepthPointCloud(const std::shared_ptr &frame_set) { if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 || !enable_point_cloud_) { @@ -6785,22 +6820,34 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, camera_info_publishers_[stream_index]->publish(camera_info); publishMetadata(frame, stream_index, camera_info.header); - if (!has_raw_image_subscriber && !save_images_[stream_index]) { - return; - } + cv::Mat image_to_publish = image; + std::string image_encoding = encoding_[stream_index]; + uint32_t image_step = width * unit_step_size_[stream_index]; if (stream_index == DEPTH) { auto depth_scale = video_frame->as()->getValueScale(); image = image * depth_scale; + image_to_publish = image; + if (colorizer_enabled_ && 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)); + } + } + } + if (!has_raw_image_subscriber && !save_images_[stream_index]) { + return; } CHECK(image_publishers_.count(stream_index) > 0); if (has_raw_image_subscriber || save_images_[stream_index]) { sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image()); - cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image) + cv_bridge::CvImage(std_msgs::msg::Header(), image_encoding, image_to_publish) .toImageMsg(*image_msg); CHECK_NOTNULL(image_msg.get()); image_msg->header.stamp = timestamp; image_msg->is_bigendian = false; - image_msg->step = width * unit_step_size_[stream_index]; + image_msg->step = image_step; image_msg->header.frame_id = frame_id; saveImageToFile(stream_index, image, *image_msg); if (!has_raw_image_subscriber) { From afc64cbc7093400e5c6407f61dcf91c9533f12ca Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Tue, 18 Aug 2026 17:36:51 +0800 Subject: [PATCH 2/5] feat: update depth colorizer implementation and configuration options --- README.MD | 15 ++- README_CN.MD | 14 ++- orbbec_camera/config/camera_params.yaml | 2 +- .../config/camera_secondary_params.yaml | 2 +- .../include/orbbec_camera/ob_camera_node.h | 2 +- orbbec_camera/launch/astra.launch.py | 2 +- orbbec_camera/launch/astra2.launch.py | 2 +- orbbec_camera/launch/dabai_a.launch.py | 5 +- orbbec_camera/launch/dabai_al.launch.py | 5 +- orbbec_camera/launch/dabai_dcw2.launch.py | 2 +- orbbec_camera/launch/dabai_max_pro.launch.py | 2 +- orbbec_camera/launch/femto.launch.py | 2 +- orbbec_camera/launch/femto_bolt.launch.py | 2 +- orbbec_camera/launch/femto_mega.launch.py | 2 +- orbbec_camera/launch/gemini2.launch.py | 2 +- orbbec_camera/launch/gemini210.launch.py | 2 +- orbbec_camera/launch/gemini2L.launch.py | 5 +- orbbec_camera/launch/gemini345.launch.py | 5 +- orbbec_camera/launch/gemini345_lg.launch.py | 5 +- orbbec_camera/launch/gemini435_le.launch.py | 5 +- .../launch/gemini_301_series.launch.py | 5 +- .../launch/gemini_330_series.launch.py | 4 +- .../gemini_330_series_low_cpu.launch.py | 4 +- .../gemini_330_series_sdk_json.launch.py | 5 +- .../gemini_intra_process_demo_launch.py | 5 +- orbbec_camera/src/ob_camera_node.cpp | 97 ++++++++++++++++--- 26 files changed, 144 insertions(+), 59 deletions(-) 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()); } } } From 7e3adade6559976ade537e76227ecf28e78b00b4 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Wed, 19 Aug 2026 11:15:08 +0800 Subject: [PATCH 3/5] feat: update image saving functionality and metadata handling in camera node --- README.MD | 11 -- README_CN.MD | 11 -- .../include/orbbec_camera/ob_camera_node.h | 7 +- orbbec_camera/src/ob_camera_node.cpp | 124 +++++++++++++----- 4 files changed, 94 insertions(+), 59 deletions(-) diff --git a/README.MD b/README.MD index 73afb4ab..0eef390a 100644 --- a/README.MD +++ b/README.MD @@ -334,8 +334,6 @@ 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: 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,15 +367,6 @@ 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) -`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 Currently, the following devices are supported by the OrbbecSDK ROS2 Wrapper v2-main branch. More devices support will be added in the near future. If you can not find your device in the table below, try the [main](https://github.com/orbbec/OrbbecSDK_ROS2/tree/main) branch. diff --git a/README_CN.MD b/README_CN.MD index 962be62f..0ae10dea 100644 --- a/README_CN.MD +++ b/README_CN.MD @@ -333,8 +333,6 @@ 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 # 或其他启动文件，见下表 -# 可选：使用 OrbbecViewer 风格发布伪彩色深度图到 /camera/depth/image_raw -ros2 launch orbbec_camera gemini_330_series.launch.py depth_colorizer_mode:=jet ``` - 终端 2 @@ -368,15 +366,6 @@ 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) -`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`。 - ## 支持的设备 目前 v2-main 分支支持以下设备。更多设备支持将陆续增加。如果没有找到您的设备，请尝试 [main](https://github.com/orbbec/OrbbecSDK_ROS2/tree/main) 分支。 diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index ba0bccf2..0e1b7610 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -592,14 +592,17 @@ class OBCameraNode { void publishMetadata(const std::shared_ptr& frame, const stream_index_pair& stream_index, const std_msgs::msg::Header& header); + std::string createFrameMetadataJson(const std::shared_ptr& frame) const; + void onNewColorFrameCallback(); void onNewLeftColorFrameCallback(); void onNewRightColorFrameCallback(); - void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image, - const sensor_msgs::msg::Image& image_msg); + void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& raw_image, + const cv::Mat& image_to_save, const sensor_msgs::msg::Image& image_msg, + const std::shared_ptr& frame); void onNewIMUFrameSyncOutputCallback(const std::shared_ptr& accelframe, const std::shared_ptr& gryoframe); diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 951dadd3..3faa6eae 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -6884,6 +6884,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, camera_info_publishers_[stream_index]->publish(camera_info); publishMetadata(frame, stream_index, camera_info.header); + cv::Mat raw_image = image; cv::Mat image_to_publish = image; std::string image_encoding = encoding_[stream_index]; uint32_t image_step = width * unit_step_size_[stream_index]; @@ -6891,7 +6892,7 @@ 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_mode_ != "none" && has_raw_image_subscriber) { + if (colorizer_mode_ != "none" && (has_raw_image_subscriber || save_images_[stream_index])) { auto colorized_image = colorizeDepthImage(image); if (!colorized_image.empty()) { image_to_publish = std::move(colorized_image); @@ -6914,7 +6915,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, image_msg->is_bigendian = false; image_msg->step = image_step; image_msg->header.frame_id = frame_id; - saveImageToFile(stream_index, image, *image_msg); + saveImageToFile(stream_index, raw_image, image_to_publish, *image_msg, frame); if (!has_raw_image_subscriber) { record_image_publish_skipped(); return; @@ -6968,8 +6969,15 @@ void OBCameraNode::publishMetadata(const std::shared_ptr &frame, } orbbec_camera_msgs::msg::Metadata metadata_msg; metadata_msg.header = header; - nlohmann::json json_data; + metadata_msg.json_data = createFrameMetadataJson(frame); + metadata_publisher->publish(metadata_msg); +} +std::string OBCameraNode::createFrameMetadataJson(const std::shared_ptr &frame) const { + nlohmann::json json_data; + if (frame == nullptr) { + return json_data.dump(2); + } for (int i = 0; i < OB_FRAME_METADATA_TYPE_COUNT; i++) { auto meta_data_type = static_cast(i); std::string field_name = metaDataTypeToString(meta_data_type); @@ -6979,12 +6987,13 @@ void OBCameraNode::publishMetadata(const std::shared_ptr &frame, int64_t value = frame->getMetadataValue(meta_data_type); json_data[field_name] = value; } - metadata_msg.json_data = json_data.dump(2); - metadata_publisher->publish(metadata_msg); + return json_data.dump(2); } -void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image, - const sensor_msgs::msg::Image &image_msg) { +void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &raw_image, + const cv::Mat &image_to_save, + const sensor_msgs::msg::Image &image_msg, + const std::shared_ptr &frame) { if (save_images_[stream_index]) { auto now = std::chrono::system_clock::now(); auto in_time_t = std::chrono::system_clock::to_time_t(now); @@ -6994,42 +7003,87 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const std::stringstream ss; ss << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S"); ss << "_" << std::setw(6) << std::setfill('0') << us.count(); - auto current_path = std::filesystem::current_path().string(); + const auto output_directory = std::filesystem::current_path() / "image"; auto fps = fps_[stream_index]; - int index = save_images_count_[stream_index]; - std::string file_suffix = stream_index == COLOR ? ".png" : ".raw"; - std::string filename = current_path + "/image/" + stream_name_[stream_index] + "_" + - std::to_string(image_msg.width) + "x" + - std::to_string(image_msg.height) + "_" + std::to_string(fps) + "hz_" + - ss.str() + "_" + std::to_string(index) + file_suffix; - if (!std::filesystem::exists(current_path + "/image")) { - std::filesystem::create_directory(current_path + "/image"); + const int index = save_images_count_[stream_index]; + const std::string file_name = stream_name_[stream_index] + "_" + + std::to_string(image_msg.width) + "x" + + std::to_string(image_msg.height) + "_" + std::to_string(fps) + + "hz_" + ss.str() + "_" + std::to_string(index); + if (!std::filesystem::exists(output_directory)) { + std::filesystem::create_directories(output_directory); } - RCLCPP_INFO_STREAM(logger_, "Saving image to " << filename); - if (stream_index.first == OB_STREAM_COLOR) { - auto image_to_save = - cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8)->image; - cv::imwrite(filename, image_to_save); - } else if (stream_index.first == OB_STREAM_IR || stream_index.first == OB_STREAM_IR_LEFT || - stream_index.first == OB_STREAM_IR_RIGHT || stream_index.first == OB_STREAM_DEPTH) { - std::ofstream ofs(filename, std::ios::out | std::ios::binary); - if (!ofs.is_open()) { - RCLCPP_ERROR_STREAM(logger_, "Failed to open file: " << filename); - return; + const auto file_stem = (output_directory / file_name).string(); + const auto raw_filename = file_stem + ".raw"; + const auto png_filename = file_stem + ".png"; + const auto metadata_filename = file_stem + ".json"; + RCLCPP_INFO_STREAM(logger_, "Saving frame files to " << file_stem << " (.raw, .png, .json)"); + + const auto *frame_data = frame ? frame->getData() : nullptr; + const auto frame_data_size = frame ? frame->getDataSize() : 0; + std::ofstream ofs(raw_filename, std::ios::out | std::ios::binary); + if (!ofs.is_open()) { + RCLCPP_ERROR_STREAM(logger_, "Failed to open raw file: " << raw_filename); + } else if (frame_data != nullptr && frame_data_size > 0) { + ofs.write(reinterpret_cast(frame_data), + static_cast(frame_data_size)); + if (!ofs.good()) { + RCLCPP_ERROR_STREAM(logger_, "Failed to write raw file: " << raw_filename); } - if (image.isContinuous()) { - ofs.write(reinterpret_cast(image.data), image.total() * image.elemSize()); + } else if (!raw_image.empty()) { + if (raw_image.isContinuous()) { + ofs.write(reinterpret_cast(raw_image.data), + static_cast(raw_image.total() * raw_image.elemSize())); } else { - int rows = image.rows; - int cols = image.cols * image.channels(); - for (int r = 0; r < rows; ++r) { - ofs.write(reinterpret_cast(image.ptr(r)), cols); + const auto row_size = static_cast(raw_image.cols * raw_image.elemSize()); + for (int row = 0; row < raw_image.rows; ++row) { + ofs.write(reinterpret_cast(raw_image.ptr(row)), row_size); } } - ofs.close(); + if (!ofs.good()) { + RCLCPP_ERROR_STREAM(logger_, "Failed to write raw file: " << raw_filename); + } } else { - RCLCPP_ERROR_STREAM(logger_, "Unsupported stream type: " << stream_index.first); + RCLCPP_ERROR_STREAM(logger_, "Failed to save raw image: frame data and image are empty"); } + if (ofs.is_open()) { + ofs.close(); + } + + cv::Mat png_image = image_to_save.empty() ? raw_image : image_to_save; + if (png_image.empty()) { + RCLCPP_ERROR_STREAM(logger_, "Failed to save PNG image: image is empty"); + } else { + cv::Mat converted_png_image; + if (image_msg.encoding == sensor_msgs::image_encodings::RGB8 && png_image.channels() == 3) { + cv::cvtColor(png_image, converted_png_image, cv::COLOR_RGB2BGR); + png_image = converted_png_image; + } else if (image_msg.encoding == sensor_msgs::image_encodings::RGBA8 && + png_image.channels() == 4) { + cv::cvtColor(png_image, converted_png_image, cv::COLOR_RGBA2BGRA); + png_image = converted_png_image; + } + try { + if (!cv::imwrite(png_filename, png_image)) { + RCLCPP_ERROR_STREAM(logger_, "Failed to write PNG file: " << png_filename); + } + } catch (const cv::Exception &exception) { + RCLCPP_ERROR_STREAM( + logger_, "Failed to write PNG file " << png_filename << ": " << exception.what()); + } + } + + std::ofstream metadata_ofs(metadata_filename); + if (!metadata_ofs.is_open()) { + RCLCPP_ERROR_STREAM(logger_, "Failed to open metadata file: " << metadata_filename); + } else { + metadata_ofs << createFrameMetadataJson(frame) << '\n'; + if (!metadata_ofs.good()) { + RCLCPP_ERROR_STREAM(logger_, "Failed to write metadata file: " << metadata_filename); + } + metadata_ofs.close(); + } + if (++save_images_count_[stream_index] >= max_save_images_count_) { save_images_[stream_index] = false; } From 54d29f15d815d8cf6434929a92d7b437ad11cf38 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Wed, 19 Aug 2026 18:05:27 +0800 Subject: [PATCH 4/5] feat: enhance colorizeDepthImage function to accept colorizer mode parameter --- .../include/orbbec_camera/ob_camera_node.h | 2 +- orbbec_camera/src/ob_camera_node.cpp | 18 +++++++++++++----- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 0e1b7610..8ad62006 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -558,7 +558,7 @@ class OBCameraNode { void publishRawDepthImage(const std::shared_ptr& depth_frame); - cv::Mat colorizeDepthImage(const cv::Mat& depth_image); + cv::Mat colorizeDepthImage(const cv::Mat& depth_image, const std::string& colorizer_mode); std::shared_ptr processDepthFrameFilter(std::shared_ptr& frame); diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 3faa6eae..5fba339f 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -5616,12 +5616,13 @@ void OBCameraNode::publishRawDepthImage(const std::shared_ptr &depth_ depth_unaligned_publisher_->publish(std::move(image_msg)); } -cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image) { +cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image, + const std::string &colorizer_mode) { if (depth_image.empty() || depth_image.channels() != 1) { return {}; } - if (colorizer_mode_ == "none") { + if (colorizer_mode == "none") { return {}; } @@ -5682,10 +5683,10 @@ cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image) { 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") { + if (colorizer_mode == "gray") { return depth_8u; } - if (colorizer_mode_ == "jet_inv") { + if (colorizer_mode == "jet_inv") { depth_8u = 255 - depth_8u; depth_8u.setTo(cv::Scalar::all(0), invalid_depth_mask); } @@ -6893,7 +6894,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr &frame, image = image * depth_scale; image_to_publish = image; if (colorizer_mode_ != "none" && (has_raw_image_subscriber || save_images_[stream_index])) { - auto colorized_image = colorizeDepthImage(image); + auto colorized_image = colorizeDepthImage(image, colorizer_mode_); if (!colorized_image.empty()) { image_to_publish = std::move(colorized_image); image_encoding = colorizer_mode_ == "gray" ? sensor_msgs::image_encodings::MONO8 @@ -7051,6 +7052,13 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const } cv::Mat png_image = image_to_save.empty() ? raw_image : image_to_save; + if (stream_index == DEPTH && colorizer_mode_ == "none") { + // Keep the ROS topic raw in none mode, but save a viewable depth preview. + auto depth_preview = colorizeDepthImage(png_image, "gray"); + if (!depth_preview.empty()) { + png_image = std::move(depth_preview); + } + } if (png_image.empty()) { RCLCPP_ERROR_STREAM(logger_, "Failed to save PNG image: image is empty"); } else { From ada0122f9876c134d5b1bbde16e308fc71b04ae3 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Thu, 20 Aug 2026 17:12:20 +0800 Subject: [PATCH 5/5] feat: implement thread-safe image saving with mutex and update save logic --- .../include/orbbec_camera/ob_camera_node.h | 1 + orbbec_camera/src/ob_camera_node.cpp | 26 ++++++++++++++----- orbbec_camera/src/ros_service.cpp | 3 ++- 3 files changed, 22 insertions(+), 8 deletions(-) diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 8ad62006..4685b321 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -809,6 +809,7 @@ class OBCameraNode { std::unique_ptr d2c_viewer_ = nullptr; std::map save_images_; std::map save_images_count_; + std::mutex save_images_mutex_; int max_save_images_count_ = 10; std::atomic_bool save_point_cloud_{false}; std::atomic_bool save_colored_point_cloud_{false}; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 5fba339f..241a5e6c 100755 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -6995,18 +6995,34 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const const cv::Mat &image_to_save, const sensor_msgs::msg::Image &image_msg, const std::shared_ptr &frame) { - if (save_images_[stream_index]) { + if (save_images_[stream_index].load(std::memory_order_acquire)) { + int index = 0; + { + std::lock_guard lock(save_images_mutex_); + if (!save_images_[stream_index].load(std::memory_order_relaxed)) { + return; + } + index = save_images_count_[stream_index]++; + if (save_images_count_[stream_index] >= max_save_images_count_) { + save_images_[stream_index].store(false, std::memory_order_release); + } + } + auto now = std::chrono::system_clock::now(); auto in_time_t = std::chrono::system_clock::to_time_t(now); auto us = std::chrono::duration_cast(now.time_since_epoch()) % 1000000; + std::tm local_time{}; + if (localtime_r(&in_time_t, &local_time) == nullptr) { + RCLCPP_ERROR_STREAM(logger_, "Failed to convert image save timestamp to local time"); + return; + } std::stringstream ss; - ss << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S"); + ss << std::put_time(&local_time, "%Y%m%d_%H%M%S"); ss << "_" << std::setw(6) << std::setfill('0') << us.count(); const auto output_directory = std::filesystem::current_path() / "image"; auto fps = fps_[stream_index]; - const int index = save_images_count_[stream_index]; const std::string file_name = stream_name_[stream_index] + "_" + std::to_string(image_msg.width) + "x" + std::to_string(image_msg.height) + "_" + std::to_string(fps) + @@ -7091,10 +7107,6 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const } metadata_ofs.close(); } - - if (++save_images_count_[stream_index] >= max_save_images_count_) { - save_images_[stream_index] = false; - } } } diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 8c4be54b..fbb68ab7 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -1961,10 +1961,11 @@ void OBCameraNode::saveImageCallback(const std::shared_ptr& response) { (void)request; (void)response; + std::lock_guard lock(save_images_mutex_); for (const auto& stream_index : IMAGE_STREAMS) { if (enable_stream_[stream_index]) { - save_images_[stream_index] = true; save_images_count_[stream_index] = 0; + save_images_[stream_index].store(true, std::memory_order_release); } } }