From 7e3adade6559976ade537e76227ecf28e78b00b4 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Wed, 19 Aug 2026 11:15:08 +0800 Subject: [PATCH] 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; }