feat: update image saving functionality and metadata handling in camera node

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.

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) 分支。

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -592,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);

orbbec_camera/src/ob_camera_node.cpp

@@ -6884,6 +6884,7 @@ 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);
 
+  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<ob::Frame> &frame,
     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) {
+    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<ob::Frame> &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<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);
@@ -6979,12 +6987,13 @@ 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,
+                                   const sensor_msgs::msg::Image &image_msg,
+                                   const std::shared_ptr<ob::Frame> &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];
+    const int index = save_images_count_[stream_index];
-    std::string file_suffix = stream_index == COLOR ? ".png" : ".raw";
+    const std::string file_name = stream_name_[stream_index] + "_" +
-    std::string filename = current_path + "/image/" + stream_name_[stream_index] + "_" +
+                                  std::to_string(image_msg.width) + "x" +
-                           std::to_string(image_msg.width) + "x" +
+                                  std::to_string(image_msg.height) + "_" + std::to_string(fps) +
-                           std::to_string(image_msg.height) + "_" + std::to_string(fps) + "hz_" +
+                                  "hz_" + ss.str() + "_" + std::to_string(index);
-                           ss.str() + "_" + std::to_string(index) + file_suffix;
+    if (!std::filesystem::exists(output_directory)) {
-    if (!std::filesystem::exists(current_path + "/image")) {
+      std::filesystem::create_directories(output_directory);
-      std::filesystem::create_directory(current_path + "/image");
     }
-    RCLCPP_INFO_STREAM(logger_, "Saving image to " << filename);
+    const auto file_stem = (output_directory / file_name).string();
-    if (stream_index.first == OB_STREAM_COLOR) {
+    const auto raw_filename = file_stem + ".raw";
-      auto image_to_save =
+    const auto png_filename = file_stem + ".png";
-          cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8)->image;
+    const auto metadata_filename = file_stem + ".json";
-      cv::imwrite(filename, image_to_save);
+    RCLCPP_INFO_STREAM(logger_, "Saving frame files to " << file_stem << " (.raw, .png, .json)");
-    } 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) {
+    const auto *frame_data = frame ? frame->getData() : nullptr;
-      std::ofstream ofs(filename, std::ios::out | std::ios::binary);
+    const auto frame_data_size = frame ? frame->getDataSize() : 0;
-      if (!ofs.is_open()) {
+    std::ofstream ofs(raw_filename, std::ios::out | std::ios::binary);
-        RCLCPP_ERROR_STREAM(logger_, "Failed to open file: " << filename);
+    if (!ofs.is_open()) {
-        return;
+      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 (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;
     }