Merge branch 'feat/depth_colorizer' into v2/develop

orbbec_camera/config/camera_params.yaml

@@ -1,6 +1,7 @@
 # common params
 depth_registration: false
 enable_d2c_viewer: false
+depth_colorizer_mode: "none"
 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"
-
-

orbbec_camera/config/camera_secondary_params.yaml

@@ -1,6 +1,7 @@
 # common params
 depth_registration: false
 enable_d2c_viewer: false
+depth_colorizer_mode: "none"
 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"
-
-

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -558,6 +558,8 @@ class OBCameraNode {
 
   void publishRawDepthImage(const std::shared_ptr<ob::Frame>& depth_frame);
 
+  cv::Mat colorizeDepthImage(const cv::Mat& depth_image, const std::string& colorizer_mode);
+
   std::shared_ptr<ob::Frame> processDepthFrameFilter(std::shared_ptr<ob::Frame>& frame);
 
   std::shared_ptr<ob::Frame> processColorFrameFilter(std::shared_ptr<ob::Frame>& frame);
@@ -590,14 +592,17 @@ class OBCameraNode {
   void publishMetadata(const std::shared_ptr<ob::Frame>& frame,
                        const stream_index_pair& stream_index, const std_msgs::msg::Header& header);
 
+  std::string createFrameMetadataJson(const std::shared_ptr<ob::Frame>& frame) const;
+
   void onNewColorFrameCallback();
 
   void onNewLeftColorFrameCallback();
 
   void onNewRightColorFrameCallback();
 
-  void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
+  void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& raw_image,
-                       const sensor_msgs::msg::Image& image_msg);
+                       const cv::Mat& image_to_save, const sensor_msgs::msg::Image& image_msg,
+                       const std::shared_ptr<ob::Frame>& frame);
 
   void onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Frame>& accelframe,
                                        const std::shared_ptr<ob::Frame>& gryoframe);
@@ -799,10 +804,12 @@ class OBCameraNode {
   std::string color_info_url_;
   std::string ir_info_url_;
   std::optional<OBCameraParam> camera_param_;
+  std::string colorizer_mode_ = "none";
   bool enable_d2c_viewer_ = false;
   std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
   std::map<stream_index_pair, std::atomic_bool> save_images_;
   std::map<stream_index_pair, int> 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};

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('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'),

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("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"),

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,6 +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('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'),

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,6 +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('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'),

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('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'),

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('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'),

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("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"),

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("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'),

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("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'),

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("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'),

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("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'),

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,6 +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("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"),

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,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('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'),

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,6 +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('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'),

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,6 +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('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'),

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,6 +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('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'),

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,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('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'),

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,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('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'),

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,6 +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('depth_colorizer_mode', default_value='none'),
         DeclareLaunchArgument('show_fps_enable', default_value='false'),
 
         DeclareLaunchArgument('enumerate_net_device', default_value='true'),

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,6 +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('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'),

orbbec_camera/src/ob_camera_node.cpp

@@ -19,6 +19,7 @@
 #include <thread>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <sstream>
+#include <array>
 #include <algorithm>
 #include <cctype>
 #include <cmath>
@@ -66,6 +67,10 @@ namespace {
 
 constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
 constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";
+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") {
@@ -4468,6 +4473,10 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(point_cloud_decimation_filter_factor_,
                               "point_cloud_decimation_filter_factor", 1);
   setAndGetNodeParameter<std::string>(point_cloud_qos_, "point_cloud_qos", "default");
+  setAndGetNodeParameter<std::string>(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<bool>(enable_d2c_viewer_, "enable_d2c_viewer", false);
   setAndGetNodeParameter<std::string>(disparity_to_depth_mode_, "disparity_to_depth_mode", "");
   disparity_to_depth_mode_ =
@@ -5430,6 +5439,12 @@ void OBCameraNode::setupPublishers() {
     }
   }
 
+  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();
 
   if (enable_sync_output_accel_gyro_) {
@@ -5603,6 +5618,91 @@ void OBCameraNode::publishRawDepthImage(const std::shared_ptr<ob::Frame> &depth_
   depth_unaligned_publisher_->publish(std::move(image_msg));
 }
 
+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") {
+    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 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<uint32_t>(max_depth) - min_depth + 1;
+  std::array<uint32_t, static_cast<size_t>(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<uint16_t>(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<uint16_t>(row);
+    auto *mapped_row = depth_8u.ptr<uint8_t>(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<uint16_t>(static_cast<float>(value_range) *
+                                                histogram[depth_value] / valid_pixel_count +
+                                            min_depth);
+      }
+      const double normalized_depth = std::max(
+          0.0,
+          std::min(1.0, (static_cast<double>(depth_value) - min_depth) / (max_depth - min_depth)));
+      const double scale_value = 255.0 * std::pow(normalized_depth, kViewerColorizerGamma);
+      mapped_row[col] = static_cast<uint8_t>(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;
+  cv::cvtColor(colorized_bgr, colorized_rgb, cv::COLOR_BGR2RGB);
+  return colorized_rgb;
+}
+
 void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
   if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 ||
       !enable_point_cloud_) {
@@ -6787,24 +6887,38 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &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]) {
+  cv::Mat raw_image = image;
-    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<ob::DepthFrame>()->getValueScale();
     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, colorizer_mode_);
+      if (!colorized_image.empty()) {
+        image_to_publish = std::move(colorized_image);
+        image_encoding = colorizer_mode_ == "gray" ? sensor_msgs::image_encodings::MONO8
+                                                   : sensor_msgs::image_encodings::RGB8;
+        image_step = static_cast<uint32_t>(image_to_publish.cols * image_to_publish.elemSize());
+      }
+    }
+  }
+  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);
+    saveImageToFile(stream_index, raw_image, image_to_publish, *image_msg, frame);
     if (!has_raw_image_subscriber) {
       record_image_publish_skipped();
       return;
@@ -6858,8 +6972,15 @@ void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &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<ob::Frame> &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<OBFrameMetadataType>(i);
     std::string field_name = metaDataTypeToString(meta_data_type);
@@ -6869,59 +6990,124 @@ void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
     int64_t value = frame->getMetadataValue(meta_data_type);
     json_data[field_name] = value;
   }
-  metadata_msg.json_data = json_data.dump(2);
+  return json_data.dump(2);
-  metadata_publisher->publish(metadata_msg);
 }
 
-void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image,
+void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &raw_image,
-                                   const sensor_msgs::msg::Image &image_msg) {
+                                   const cv::Mat &image_to_save,
-  if (save_images_[stream_index]) {
+                                   const sensor_msgs::msg::Image &image_msg,
+                                   const std::shared_ptr<ob::Frame> &frame) {
+  if (save_images_[stream_index].load(std::memory_order_acquire)) {
+    int index = 0;
+    {
+      std::lock_guard<std::mutex> 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<std::chrono::microseconds>(now.time_since_epoch()) % 1000000;
 
-    std::stringstream ss;
+    std::tm local_time{};
-    ss << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S");
+    if (localtime_r(&in_time_t, &local_time) == nullptr) {
-    ss << "_" << std::setw(6) << std::setfill('0') << us.count();
+      RCLCPP_ERROR_STREAM(logger_, "Failed to convert image save timestamp to local time");
-    auto current_path = std::filesystem::current_path().string();
+      return;
-    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");
     }
-    RCLCPP_INFO_STREAM(logger_, "Saving image to " << filename);
+    std::stringstream ss;
-    if (stream_index.first == OB_STREAM_COLOR) {
+    ss << std::put_time(&local_time, "%Y%m%d_%H%M%S");
-      auto image_to_save =
+    ss << "_" << std::setw(6) << std::setfill('0') << us.count();
-          cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8)->image;
+    const auto output_directory = std::filesystem::current_path() / "image";
-      cv::imwrite(filename, image_to_save);
+    auto fps = fps_[stream_index];
-    } else if (stream_index.first == OB_STREAM_IR || stream_index.first == OB_STREAM_IR_LEFT ||
+    const std::string file_name = stream_name_[stream_index] + "_" +
-               stream_index.first == OB_STREAM_IR_RIGHT || stream_index.first == OB_STREAM_DEPTH) {
+                                  std::to_string(image_msg.width) + "x" +
-      std::ofstream ofs(filename, std::ios::out | std::ios::binary);
+                                  std::to_string(image_msg.height) + "_" + std::to_string(fps) +
-      if (!ofs.is_open()) {
+                                  "hz_" + ss.str() + "_" + std::to_string(index);
-        RCLCPP_ERROR_STREAM(logger_, "Failed to open file: " << filename);
+    if (!std::filesystem::exists(output_directory)) {
-        return;
+      std::filesystem::create_directories(output_directory);
+    }
+    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<const char *>(frame_data),
+                static_cast<std::streamsize>(frame_data_size));
+      if (!ofs.good()) {
+        RCLCPP_ERROR_STREAM(logger_, "Failed to write raw file: " << raw_filename);
       }
-      if (image.isContinuous()) {
+    } else if (!raw_image.empty()) {
-        ofs.write(reinterpret_cast<const char *>(image.data), image.total() * image.elemSize());
+      if (raw_image.isContinuous()) {
+        ofs.write(reinterpret_cast<const char *>(raw_image.data),
+                  static_cast<std::streamsize>(raw_image.total() * raw_image.elemSize()));
       } else {
-        int rows = image.rows;
+        const auto row_size = static_cast<std::streamsize>(raw_image.cols * raw_image.elemSize());
-        int cols = image.cols * image.channels();
+        for (int row = 0; row < raw_image.rows; ++row) {
-        for (int r = 0; r < rows; ++r) {
+          ofs.write(reinterpret_cast<const char *>(raw_image.ptr<uchar>(row)), row_size);
-          ofs.write(reinterpret_cast<const char *>(image.ptr<uchar>(r)), cols);
         }
       }
-      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 (++save_images_count_[stream_index] >= max_save_images_count_) {
+    if (ofs.is_open()) {
-      save_images_[stream_index] = false;
+      ofs.close();
+    }
+
+    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 {
+      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();
     }
   }
 }

orbbec_camera/src/ros_service.cpp

@@ -1963,10 +1963,11 @@ void OBCameraNode::saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty:
                                      std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
   (void)request;
   (void)response;
+  std::lock_guard<std::mutex> 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);
     }
   }
 }