From 13dce00d56f747586c548c95f8187c38e34ec4a3 Mon Sep 17 00:00:00 2001 From: jj Date: Sat, 28 Sep 2024 15:32:51 +0800 Subject: [PATCH] Add export_params --- orbbec_camera/CMakeLists.txt | 7 +- .../metadata_export_params.json | 13 + orbbec_camera/tools/metadata_export_files.cpp | 8 + orbbec_camera/tools/metadata_export_files.hpp | 495 ++++++++++++++++++ orbbec_camera/tools/metadata_save_files.cpp | 8 + orbbec_camera/tools/metadata_save_files.hpp | 482 +++++++++++++++++ orbbec_camera/tools/multi_save_rgbir_node.cpp | 24 + orbbec_camera/tools/multi_save_rgbir_node.hpp | 164 ++++++ 8 files changed, 1200 insertions(+), 1 deletion(-) create mode 100644 orbbec_camera/config/metadataexport/metadata_export_params.json create mode 100644 orbbec_camera/tools/metadata_export_files.cpp create mode 100644 orbbec_camera/tools/metadata_export_files.hpp create mode 100644 orbbec_camera/tools/metadata_save_files.cpp create mode 100644 orbbec_camera/tools/metadata_save_files.hpp create mode 100644 orbbec_camera/tools/multi_save_rgbir_node.cpp create mode 100644 orbbec_camera/tools/multi_save_rgbir_node.hpp diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt index 5ce17286..207d6bbe 100644 --- a/orbbec_camera/CMakeLists.txt +++ b/orbbec_camera/CMakeLists.txt @@ -214,8 +214,10 @@ rclcpp_components_register_node(${PROJECT_NAME} add_orbbec_executable(list_devices_node tools/list_devices_node.cpp) add_orbbec_executable(list_depth_work_mode_node tools/list_depth_work_mode.cpp) add_orbbec_executable(list_camera_profile_mode_node tools/list_camera_profile.cpp) - add_orbbec_executable(topic_statistics_node tools/topic_statistics.cpp) +add_orbbec_executable(multi_save_rgbir_node tools/multi_save_rgbir_node.cpp) +add_orbbec_executable(metadata_save_files_node tools/metadata_save_files.cpp) +add_orbbec_executable(metadata_export_files_node tools/metadata_export_files.cpp) add_library(frame_latency SHARED tools/frame_latency.cpp) target_include_directories(frame_latency PUBLIC ${COMMON_INCLUDE_DIRS}) @@ -251,6 +253,9 @@ install(TARGETS list_devices_node list_depth_work_mode_node list_camera_profile_mode_node topic_statistics_node + multi_save_rgbir_node + metadata_save_files_node + metadata_export_files_node DESTINATION lib/${PROJECT_NAME}/) if (BUILD_TESTING) diff --git a/orbbec_camera/config/metadataexport/metadata_export_params.json b/orbbec_camera/config/metadataexport/metadata_export_params.json new file mode 100644 index 00000000..fad7f60b --- /dev/null +++ b/orbbec_camera/config/metadataexport/metadata_export_params.json @@ -0,0 +1,13 @@ +{ + "camera_params": { + "sn": "CP1L44P00085", + "left_ir_image_topic": "/camera/left_ir/image_raw", + "right_ir_image_topic": "/camera/right_ir/image_raw", + "depth_image_topic": "/camera/depth/image_raw", + "color_image_topic": "/camera/color/image_raw", + "left_ir_metadata_topic": "/camera/left_ir/metadata", + "right_ir_metadata_topic": "/camera/right_ir/metadata", + "depth_metadata_topic": "/camera/depth/metadata", + "color_metadata_topic": "/camera/color/metadata" + } +} \ No newline at end of file diff --git a/orbbec_camera/tools/metadata_export_files.cpp b/orbbec_camera/tools/metadata_export_files.cpp new file mode 100644 index 00000000..d98b130d --- /dev/null +++ b/orbbec_camera/tools/metadata_export_files.cpp @@ -0,0 +1,8 @@ +#include "metadata_export_files.hpp" + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/orbbec_camera/tools/metadata_export_files.hpp b/orbbec_camera/tools/metadata_export_files.hpp new file mode 100644 index 00000000..932e1bdc --- /dev/null +++ b/orbbec_camera/tools/metadata_export_files.hpp @@ -0,0 +1,495 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "orbbec_camera_msgs/msg/metadata.hpp" + +namespace orbbec_camera { +namespace tools { +const int kMetadataVectorSize = 10; + +struct ImageMetadata { + int exposure; + int frame_emitter_mode; + int frame_number; + int64_t frame_timestamp; + int gain; + int64_t sensor_timestamp; + + ImageMetadata() + : exposure(0), + frame_emitter_mode(0), + frame_number(0), + frame_timestamp(0), + gain(0), + sensor_timestamp(0) {} + friend std::ostream &operator<<(std::ostream &os, const ImageMetadata &metadata) { + os << "exposure: " << metadata.exposure << "\n" + << "frame_emitter_mode: " << metadata.frame_emitter_mode + << "frame_number: " << metadata.frame_number << "\n" + << "frame_timestamp: " << metadata.frame_timestamp << "\n" + << "gain: " << metadata.gain << "\n" + << "sensor_timestamp: " << metadata.sensor_timestamp; + return os; + } +}; + +using std::placeholders::_1; +using std::placeholders::_2; + +class MetadataExportFiles : public rclcpp::Node { + public: + MetadataExportFiles() : Node("metadata_export_files") { + load_parameters(); + initialize_directories(); + initialize_pub_sub(); + } + + void load_parameters() { + std::ifstream file("src/OrbbecSDK_ROS2/orbbec_camera/config/metadataexport/metadata_export_params.json"); + if (!file.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file."); + return; + } + + nlohmann::json json_data; + file >> json_data; + + sn_ = json_data["camera_params"]["sn"].get(); + + left_ir_image_topic_ = json_data["camera_params"]["left_ir_image_topic"].get(); + right_ir_image_topic_ = json_data["camera_params"]["right_ir_image_topic"].get(); + depth_image_topic_ = json_data["camera_params"]["depth_image_topic"].get(); + color_image_topic_ = json_data["camera_params"]["color_image_topic"].get(); + + left_ir_metadata_topic_ = + json_data["camera_params"]["left_ir_metadata_topic"].get(); + right_ir_metadata_topic_ = + json_data["camera_params"]["right_ir_metadata_topic"].get(); + depth_metadata_topic_ = json_data["camera_params"]["depth_metadata_topic"].get(); + color_metadata_topic_ = json_data["camera_params"]["color_metadata_topic"].get(); + + RCLCPP_INFO(this->get_logger(), "Parameter 1: %s", sn_.c_str()); + RCLCPP_INFO(this->get_logger(), "Parameter 2: %s", depth_image_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), "Parameter 3: %s", color_image_topic_.c_str()); + } + void initialize_pub_sub() { + auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); + const rmw_qos_profile_t qos_filters = qos.get_rmw_qos_profile(); + + left_ir_image_sub_ = std::make_shared>( + this, left_ir_image_topic_, qos_filters); + left_ir_metadata_sub_ = + std::make_shared>( + this, left_ir_metadata_topic_, qos_filters); + + left_ir_sync_ = std::make_shared>( + MySyncPolicy(10), *left_ir_image_sub_, *left_ir_metadata_sub_); + left_ir_sync_->setMaxIntervalDuration( + rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + + left_ir_sync_->registerCallback( + std::bind(&MetadataExportFiles::left_ir_metadata_sync_callback, this, _1, _2)); + + right_ir_image_sub_ = std::make_shared>( + this, right_ir_image_topic_, qos_filters); + right_ir_metadata_sub_ = + std::make_shared>( + this, right_ir_metadata_topic_, qos_filters); + + right_ir_sync_ = std::make_shared>( + MySyncPolicy(10), *right_ir_image_sub_, *right_ir_metadata_sub_); + right_ir_sync_->setMaxIntervalDuration( + rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + + right_ir_sync_->registerCallback( + std::bind(&MetadataExportFiles::right_ir_metadata_sync_callback, this, _1, _2)); + + depth_image_sub_ = std::make_shared>( + this, depth_image_topic_, qos_filters); + depth_metadata_sub_ = + std::make_shared>( + this, depth_metadata_topic_, qos_filters); + + depth_sync_ = std::make_shared>( + MySyncPolicy(10), *depth_image_sub_, *depth_metadata_sub_); + depth_sync_->setMaxIntervalDuration(rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + + depth_sync_->registerCallback( + std::bind(&MetadataExportFiles::depth_metadata_sync_callback, this, _1, _2)); + + color_image_sub_ = std::make_shared>( + this, color_image_topic_, qos_filters); + color_metadata_sub_ = + std::make_shared>( + this, color_metadata_topic_, qos_filters); + + color_sync_ = std::make_shared>( + MySyncPolicy(10), *color_image_sub_, *color_metadata_sub_); + color_sync_->setMaxIntervalDuration(rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + + color_sync_->registerCallback( + std::bind(&MetadataExportFiles::color_metadata_sync_callback, this, _1, _2)); + + RCLCPP_INFO(get_logger(), "Subscribed to left_ir_image_topic_ %s", + left_ir_image_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to right_ir_image_topic_ %s", + right_ir_image_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to depth_image_topic_ %s", depth_image_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to left_ir_metadata_topic_ %s", + left_ir_metadata_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to right_ir_metadata_topic_ %s", + right_ir_metadata_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to depth_metadata_topic_ %s", + depth_metadata_topic_.c_str()); + } + + void initialize_directories() { + try { + auto context = std::make_unique(); + context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE); + auto list = context->queryDeviceList(); + for (size_t i = 0; i < list->deviceCount(); i++) { + auto device = list->getDevice(i); + auto device_info = device->getDeviceInfo(); + // serial_ = device_info->serialNumber(); + std::string uid = device_info->uid(); + auto usb_port = orbbec_camera::parseUsbPort(uid); + // RCLCPP_INFO_STREAM(get_logger(), "serial: " << serial_); + RCLCPP_INFO_STREAM(get_logger(), "usb port: " << usb_port); + } + } catch (ob::Error &e) { + RCLCPP_ERROR_STREAM(get_logger(), e.getMessage()); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } catch (...) { + RCLCPP_ERROR_STREAM(get_logger(), "unknown error"); + } + + std::filesystem::path cwd = std::filesystem::current_path(); + serial_path_left_ir_ = cwd.string() + "/" + "sn_" + sn_ + "/IR_LEFT"; + serial_path_right_ir_ = cwd.string() + "/" + "sn_" + sn_ + "/IR_RIGHT"; + serial_path_color_ = cwd.string() + "/" + "sn_" + sn_ + "/Color"; + serial_path_depth_ = cwd.string() + "/" + "sn_" + sn_ + "/Depth"; + createDirectory(serial_path_left_ir_); + createDirectory(serial_path_right_ir_); + createDirectory(serial_path_color_); + createDirectory(serial_path_depth_); + } + + private: + void left_ir_metadata_sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr &image_msg, + const orbbec_camera_msgs::msg::Metadata::ConstSharedPtr &metadata_msg) { + try { + nlohmann::json json_data = nlohmann::json::parse(metadata_msg->json_data); + + left_ir_metadata_.exposure = json_data["exposure"]; + left_ir_metadata_.frame_emitter_mode = json_data["frame_emitter_mode"]; + left_ir_metadata_.frame_number = json_data["frame_number"]; + left_ir_metadata_.frame_timestamp = json_data["frame_timestamp"]; + left_ir_metadata_.gain = json_data["gain"]; + left_ir_metadata_.sensor_timestamp = json_data["sensor_timestamp"]; + + save_metadata_to_file(left_ir_metadata_, "irleft"); + } catch (const nlohmann::json::exception &e) { + RCLCPP_ERROR(get_logger(), "Failed to parse metadata JSON: %s", e.what()); + } + if (image_msg) { + leftir_frame_index_++; + } + try { + save_image_to_file(image_msg, "irleft", left_ir_image_count_, image_msg->header.stamp, + leftir_frame_index_); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error saving image: " << e.what()); + return; + } + left_ir_image_count_++; + } + void right_ir_metadata_sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr &image_msg, + const orbbec_camera_msgs::msg::Metadata::ConstSharedPtr &metadata_msg) { + try { + nlohmann::json json_data = nlohmann::json::parse(metadata_msg->json_data); + + right_ir_metadata_.exposure = json_data["exposure"]; + right_ir_metadata_.frame_emitter_mode = json_data["frame_emitter_mode"]; + right_ir_metadata_.frame_number = json_data["frame_number"]; + right_ir_metadata_.frame_timestamp = json_data["frame_timestamp"]; + right_ir_metadata_.gain = json_data["gain"]; + right_ir_metadata_.sensor_timestamp = json_data["sensor_timestamp"]; + + save_metadata_to_file(right_ir_metadata_, "irright"); + } catch (const nlohmann::json::exception &e) { + RCLCPP_ERROR(get_logger(), "Failed to parse metadata JSON: %s", e.what()); + } + if (image_msg) { + rightir_frame_index_++; + } + try { + save_image_to_file(image_msg, "irright", right_ir_image_count_, image_msg->header.stamp, + rightir_frame_index_); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error saving image: " << e.what()); + return; + } + right_ir_image_count_++; + } + void depth_metadata_sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr &image_msg, + const orbbec_camera_msgs::msg::Metadata::ConstSharedPtr &metadata_msg) { + try { + nlohmann::json json_data = nlohmann::json::parse(metadata_msg->json_data); + + depth_metadata_.exposure = json_data["exposure"]; + depth_metadata_.frame_emitter_mode = json_data["frame_emitter_mode"]; + depth_metadata_.frame_number = json_data["frame_number"]; + depth_metadata_.frame_timestamp = json_data["frame_timestamp"]; + depth_metadata_.gain = json_data["gain"]; + depth_metadata_.sensor_timestamp = json_data["sensor_timestamp"]; + + save_metadata_to_file(depth_metadata_, "depth"); + } catch (const nlohmann::json::exception &e) { + RCLCPP_ERROR(get_logger(), "Failed to parse metadata JSON: %s", e.what()); + } + if (image_msg) { + depth_frame_index_++; + } + try { + save_image_to_file(image_msg, "depth", depth_image_count_, image_msg->header.stamp, + depth_frame_index_); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error saving image: " << e.what()); + return; + } + depth_image_count_++; + } + + void color_metadata_sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr &image_msg, + const orbbec_camera_msgs::msg::Metadata::ConstSharedPtr &metadata_msg) { + try { + nlohmann::json json_data = nlohmann::json::parse(metadata_msg->json_data); + + color_metadata_.exposure = json_data["exposure"]; + color_metadata_.frame_number = json_data["frame_number"]; + color_metadata_.frame_timestamp = json_data["frame_timestamp"]; + color_metadata_.gain = json_data["gain"]; + color_metadata_.sensor_timestamp = json_data["sensor_timestamp"]; + + save_metadata_to_file(color_metadata_, "color"); + } catch (const nlohmann::json::exception &e) { + RCLCPP_ERROR(get_logger(), "Failed to parse metadata JSON: %s", e.what()); + } + if (image_msg) { + color_frame_index_++; + } + try { + save_image_to_file(image_msg, "color", color_image_count_, image_msg->header.stamp, + color_frame_index_); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error saving image: " << e.what()); + return; + } + } + void createDirectory(const std::string &path) { + RCLCPP_INFO(this->get_logger(), "Creating directory: %s", path.c_str()); + std::filesystem::path dir_path(path); + if (!std::filesystem::exists(dir_path)) { + if (std::filesystem::create_directories(dir_path)) { + std::cout << "Directory created: " << path << std::endl; + } else { + std::cerr << "Failed to create directory: " << path << std::endl; + } + } else { + std::cout << "Directory already exists: " << path << std::endl; + } + } + void save_image_to_file(const sensor_msgs::msg::Image::ConstSharedPtr &msg, + const std::string &prefix, int count, + const builtin_interfaces::msg::Time &stamp, int frame_index) { + (void)count; + long long camera_timestamp_us = + static_cast(stamp.sec) * 1000000LL + stamp.nanosec / 1000LL; + auto now = this->get_clock()->now(); + int64_t seconds = now.seconds(); + int64_t nanoseconds = now.nanoseconds() % 1000000000; + int64_t microseconds = nanoseconds / 1000; + std::ostringstream timestamp_us; + timestamp_us << seconds << std::setw(3) << std::setfill('0') << microseconds; + std::string resolution = std::to_string(msg->width) + "x" + std::to_string(msg->height); + std::string serial_path{}; + cv_bridge::CvImagePtr cv_ptr; + if (prefix == "irleft") { + try { + serial_path = serial_path_left_ir_; + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + } else if (prefix == "irright") { + try { + serial_path = serial_path_right_ir_; + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + } else if (prefix == "depth") { + try { + serial_path = serial_path_depth_; + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + } else if (prefix == "color") { + try { + serial_path = serial_path_color_; + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + } else { + RCLCPP_ERROR(get_logger(), "Unknown prefix: %s", prefix.c_str()); + return; + } + + std::ostringstream ss; + ss << serial_path << "/" << std::to_string(frame_index) << "_" << "0000" << "_" + << camera_timestamp_us << "_" << timestamp_us.str() << "_" << prefix << "_" << resolution + << ".png"; + cv::imwrite(ss.str(), cv_ptr->image); + } + void save_metadata_to_file(const ImageMetadata &metadata, const std::string &prefix) { + std::ostringstream ss; + std::string serial_path{}; + if (prefix == "irleft") { + try { + serial_path = serial_path_left_ir_; + } catch (cv_bridge::Exception &e) { + return; + } + } else if (prefix == "irright") { + try { + serial_path = serial_path_right_ir_; + } catch (cv_bridge::Exception &e) { + return; + } + } else if (prefix == "depth") { + try { + serial_path = serial_path_depth_; + } catch (cv_bridge::Exception &e) { + return; + } + } else if (prefix == "color") { + try { + serial_path = serial_path_color_; + } catch (cv_bridge::Exception &e) { + return; + } + } + ss << serial_path << "/" << metadata.frame_timestamp << "_" << prefix << "_e" + << metadata.exposure << "_g" << metadata.gain << ".txt"; + std::string file_path = ss.str(); + + std::ofstream outfile(file_path); + if (outfile.is_open()) { + if (prefix != "color") { + outfile << "Frame Emitter Mode: " << metadata.frame_emitter_mode << "\n"; + } else { + outfile << "Frame Emitter Mode: " << 0 << "\n"; + } + outfile << "Exposure: " << metadata.exposure << "\n"; + outfile << "Gain: " << metadata.gain << "\n"; + outfile << "Frame Timestamp: " << metadata.frame_timestamp << "\n"; + outfile << "Sensor Timestamp: " << metadata.sensor_timestamp << "\n"; + outfile << "Frame Number: " << metadata.frame_number << "\n"; + + outfile.close(); + } else { + RCLCPP_ERROR(get_logger(), "Failed to open file: %s", file_path.c_str()); + } + } + + std::shared_ptr> left_ir_image_sub_; + std::shared_ptr> + left_ir_metadata_sub_; + + std::shared_ptr> right_ir_image_sub_; + std::shared_ptr> + right_ir_metadata_sub_; + + std::shared_ptr> depth_image_sub_; + std::shared_ptr> + depth_metadata_sub_; + + std::shared_ptr> color_image_sub_; + std::shared_ptr> + color_metadata_sub_; + + std::string left_ir_image_topic_; + std::string right_ir_image_topic_; + std::string depth_image_topic_; + std::string color_image_topic_; + std::string left_ir_metadata_topic_; + std::string right_ir_metadata_topic_; + std::string depth_metadata_topic_; + std::string color_metadata_topic_; + std::string sn_; + + int left_ir_image_count_ = 0; + int right_ir_image_count_ = 0; + int depth_image_count_ = 0; + int color_image_count_ = 0; + int left_ir_metadata_count_ = 0; + int right_ir_metadata_count_ = 0; + int depth_metadata_count_ = 0; + int color_metadata_count_ = 0; + int leftir_frame_index_ = 0; + int rightir_frame_index_ = 0; + int depth_frame_index_ = 0; + int color_frame_index_ = 0; + + bool directories_initialized_ = false; + + std::string serial_path_left_ir_; + std::string serial_path_right_ir_; + std::string serial_path_color_; + std::string serial_path_depth_; + + ImageMetadata left_ir_metadata_ = ImageMetadata(); + ImageMetadata right_ir_metadata_ = ImageMetadata(); + ImageMetadata depth_metadata_ = ImageMetadata(); + ImageMetadata color_metadata_ = ImageMetadata(); + + using MySyncPolicy = + message_filters::sync_policies::ApproximateTime; + std::shared_ptr> left_ir_sync_; + std::shared_ptr> right_ir_sync_; + std::shared_ptr> depth_sync_; + std::shared_ptr> color_sync_; +}; + +} // namespace tools +} // namespace orbbec_camera diff --git a/orbbec_camera/tools/metadata_save_files.cpp b/orbbec_camera/tools/metadata_save_files.cpp new file mode 100644 index 00000000..10713048 --- /dev/null +++ b/orbbec_camera/tools/metadata_save_files.cpp @@ -0,0 +1,8 @@ +#include "metadata_save_files.hpp" + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/orbbec_camera/tools/metadata_save_files.hpp b/orbbec_camera/tools/metadata_save_files.hpp new file mode 100644 index 00000000..0033b276 --- /dev/null +++ b/orbbec_camera/tools/metadata_save_files.hpp @@ -0,0 +1,482 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "orbbec_camera_msgs/msg/metadata.hpp" + +namespace orbbec_camera { +namespace tools { +const int kMetadataVectorSize = 10; + +struct ImageMetadata { + int actual_frame_rate; + int ae_roi_bottom; + int ae_roi_left; + int ae_roi_right; + int ae_roi_top; + int auto_exposure; + int exposure; + int exposure_priority; + int frame_emitter_mode; + int frame_laser_power; + int frame_laser_power_mode; + int frame_number; + int64_t frame_timestamp; + int gain; + int gpio_input_data; + int hdr_sequence_index; + int hdr_sequence_name; + int hdr_sequence_size; + int64_t sensor_timestamp; + + ImageMetadata() + : actual_frame_rate(0), + ae_roi_bottom(0), + ae_roi_left(0), + ae_roi_right(0), + ae_roi_top(0), + auto_exposure(0), + exposure(0), + exposure_priority(0), + frame_emitter_mode(0), + frame_laser_power(0), + frame_laser_power_mode(0), + frame_number(0), + frame_timestamp(0), + gain(0), + gpio_input_data(0), + hdr_sequence_index(0), + hdr_sequence_name(0), + hdr_sequence_size(0), + sensor_timestamp(0) {} + friend std::ostream &operator<<(std::ostream &os, const ImageMetadata &metadata) { + os + // << "actual_frame_rate: " << metadata.actual_frame_rate << "\n" + // << "ae_roi_bottom: " << metadata.ae_roi_bottom << "\n" + // << "ae_roi_left: " << metadata.ae_roi_left << "\n" + // << "ae_roi_right: " << metadata.ae_roi_right << "\n" + // << "ae_roi_top: " << metadata.ae_roi_top << "\n" + << "auto_exposure: " << metadata.auto_exposure << "\n" + << "exposure: " << metadata.exposure + << "\n" + // << "exposure_priority: " << metadata.exposure_priority << "\n" + << "frame_emitter_mode: " << metadata.frame_emitter_mode + << "\n" + // << "frame_laser_power: " << metadata.frame_laser_power << "\n" + // << "frame_laser_power_mode: " << metadata.frame_laser_power_mode << "\n" + << "frame_number: " << metadata.frame_number << "\n" + << "frame_timestamp: " << metadata.frame_timestamp << "\n" + << "gain: " << metadata.gain + << "\n" + // << "gpio_input_data: " << metadata.gpio_input_data << "\n" + // << "hdr_sequence_index: " << metadata.hdr_sequence_index << "\n" + // << "hdr_sequence_name: " << metadata.hdr_sequence_name << "\n" + // << "hdr_sequence_size: " << metadata.hdr_sequence_size << "\n" + << "sensor_timestamp: " << metadata.sensor_timestamp; + return os; + } +}; + +using std::placeholders::_1; +using std::placeholders::_2; + +class MetadataSaveFiles : public rclcpp::Node { + public: + MetadataSaveFiles() : Node("metadata_save_files") { + this->declare_parameter("left_ir_image_topic", "/camera/left_ir/image_raw"); + this->declare_parameter("right_ir_image_topic", "/camera/right_ir/image_raw"); + this->declare_parameter("depth_image_topic", "/camera/depth/image_raw"); + this->declare_parameter("left_ir_metadata_topic", "/camera/left_ir/metadata"); + this->declare_parameter("right_ir_metadata_topic", "/camera/right_ir/metadata"); + this->declare_parameter("depth_metadata_topic", "/camera/depth/metadata"); + + left_ir_image_topic_ = this->get_parameter("left_ir_image_topic").as_string(); + right_ir_image_topic_ = this->get_parameter("right_ir_image_topic").as_string(); + depth_image_topic_ = this->get_parameter("depth_image_topic").as_string(); + left_ir_metadata_topic_ = this->get_parameter("left_ir_metadata_topic").as_string(); + right_ir_metadata_topic_ = this->get_parameter("right_ir_metadata_topic").as_string(); + depth_metadata_topic_ = this->get_parameter("depth_metadata_topic").as_string(); + + initialize_directories(); + initialize_pub_sub(); + } + + void initialize_pub_sub() { + auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); + const rmw_qos_profile_t qos_filters = qos.get_rmw_qos_profile(); + + left_ir_image_sub_ = std::make_shared>( + this, left_ir_image_topic_, qos_filters); + left_ir_metadata_sub_ = + std::make_shared>( + this, left_ir_metadata_topic_, qos_filters); + + left_ir_sync_ = std::make_shared>( + MySyncPolicy(10), *left_ir_image_sub_, *left_ir_metadata_sub_); + left_ir_sync_->setMaxIntervalDuration( + rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + + left_ir_sync_->registerCallback( + std::bind(&MetadataSaveFiles::left_ir_metadata_sync_callback, this, _1, _2)); + + right_ir_image_sub_ = std::make_shared>( + this, right_ir_image_topic_, qos_filters); + right_ir_metadata_sub_ = + std::make_shared>( + this, right_ir_metadata_topic_, qos_filters); + + right_ir_sync_ = std::make_shared>( + MySyncPolicy(10), *right_ir_image_sub_, *right_ir_metadata_sub_); + right_ir_sync_->setMaxIntervalDuration( + rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + + right_ir_sync_->registerCallback( + std::bind(&MetadataSaveFiles::right_ir_metadata_sync_callback, this, _1, _2)); + + depth_image_sub_ = std::make_shared>( + this, depth_image_topic_, qos_filters); + depth_metadata_sub_ = + std::make_shared>( + this, depth_metadata_topic_, qos_filters); + + depth_sync_ = std::make_shared>( + MySyncPolicy(10), *depth_image_sub_, *depth_metadata_sub_); + depth_sync_->setMaxIntervalDuration(rclcpp::Duration::from_nanoseconds(100000000LL)); // 100 ms + + depth_sync_->registerCallback( + std::bind(&MetadataSaveFiles::depth_metadata_sync_callback, this, _1, _2)); + + RCLCPP_INFO(get_logger(), "Subscribed to left_ir_image_topic_ %s", + left_ir_image_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to right_ir_image_topic_ %s", + right_ir_image_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to depth_image_topic_ %s", depth_image_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to left_ir_metadata_topic_ %s", + left_ir_metadata_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to right_ir_metadata_topic_ %s", + right_ir_metadata_topic_.c_str()); + RCLCPP_INFO(get_logger(), "Subscribed to depth_metadata_topic_ %s", + depth_metadata_topic_.c_str()); + } + + void initialize_directories() { + try { + auto context = std::make_unique(); + context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE); + auto list = context->queryDeviceList(); + for (size_t i = 0; i < list->deviceCount(); i++) { + auto device = list->getDevice(i); + auto device_info = device->getDeviceInfo(); + serial_ = device_info->serialNumber(); + std::string uid = device_info->uid(); + auto usb_port = orbbec_camera::parseUsbPort(uid); + RCLCPP_INFO_STREAM(get_logger(), "serial: " << serial_); + RCLCPP_INFO_STREAM(get_logger(), "usb port: " << usb_port); + } + } catch (ob::Error &e) { + RCLCPP_ERROR_STREAM(get_logger(), e.getMessage()); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), e.what()); + } catch (...) { + RCLCPP_ERROR_STREAM(get_logger(), "unknown error"); + } + + std::filesystem::path cwd = std::filesystem::current_path(); + serial_path_zero_ = cwd.string() + "/" + serial_ + "/0"; + serial_path_one_ = cwd.string() + "/" + serial_ + "/1"; + createDirectory(serial_path_zero_); + createDirectory(serial_path_one_); + } + + private: + void left_ir_metadata_sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr &image_msg, + const orbbec_camera_msgs::msg::Metadata::ConstSharedPtr &metadata_msg) { + // RCLCPP_INFO_STREAM(get_logger(), + // "Left IR stamp: sec " << image_msg->header.stamp.sec << " nanosec " << + // image_msg->header.stamp.nanosec); + // RCLCPP_INFO_STREAM(get_logger(), + // "Left IR metadata stamp: sec " << metadata_msg->header.stamp.sec << " + // nanosec " << metadata_msg->header.stamp.nanosec); + int frame_emitter_mode{0}; + try { + nlohmann::json json_data = nlohmann::json::parse(metadata_msg->json_data); + + left_ir_metadata_.actual_frame_rate = json_data["actual_frame_rate"]; + left_ir_metadata_.ae_roi_bottom = json_data["ae_roi_bottom"]; + left_ir_metadata_.ae_roi_left = json_data["ae_roi_left"]; + left_ir_metadata_.ae_roi_right = json_data["ae_roi_right"]; + left_ir_metadata_.ae_roi_top = json_data["ae_roi_top"]; + left_ir_metadata_.auto_exposure = json_data["auto_exposure"]; + left_ir_metadata_.exposure = json_data["exposure"]; + left_ir_metadata_.exposure_priority = json_data["exposure_priority"]; + left_ir_metadata_.frame_emitter_mode = json_data["frame_emitter_mode"]; + left_ir_metadata_.frame_laser_power = json_data["frame_laser_power"]; + left_ir_metadata_.frame_laser_power_mode = json_data["frame_laser_power_mode"]; + left_ir_metadata_.frame_number = json_data["frame_number"]; + left_ir_metadata_.frame_timestamp = json_data["frame_timestamp"]; + left_ir_metadata_.gain = json_data["gain"]; + left_ir_metadata_.gpio_input_data = json_data["gpio_input_data"]; + left_ir_metadata_.hdr_sequence_index = json_data["hdr_sequence_index"]; + left_ir_metadata_.hdr_sequence_name = json_data["hdr_sequence_name"]; + left_ir_metadata_.hdr_sequence_size = json_data["hdr_sequence_size"]; + left_ir_metadata_.sensor_timestamp = json_data["sensor_timestamp"]; + + frame_emitter_mode = left_ir_metadata_.frame_emitter_mode; + + // RCLCPP_INFO_STREAM(get_logger(), "Left IR metadata: \n" << left_ir_metadata_); + + save_metadata_to_file(left_ir_metadata_, "irleft"); + } catch (const nlohmann::json::exception &e) { + RCLCPP_ERROR(get_logger(), "Failed to parse metadata JSON: %s", e.what()); + } + + try { + save_image_to_file(image_msg, "irleft", left_ir_image_count_, image_msg->header.stamp, + frame_emitter_mode); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error saving image: " << e.what()); + return; + } + left_ir_image_count_++; + } + void right_ir_metadata_sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr &image_msg, + const orbbec_camera_msgs::msg::Metadata::ConstSharedPtr &metadata_msg) { + // RCLCPP_INFO_STREAM(get_logger(), + // "Right IR stamp: sec " << image_msg->header.stamp.sec << " nanosec " << + // image_msg->header.stamp.nanosec); + // RCLCPP_INFO_STREAM(get_logger(), + // "Right IR metadata stamp: sec " << metadata_msg->header.stamp.sec << " + // nanosec " << metadata_msg->header.stamp.nanosec); + + int frame_emitter_mode{0}; + try { + nlohmann::json json_data = nlohmann::json::parse(metadata_msg->json_data); + + right_ir_metadata_.actual_frame_rate = json_data["actual_frame_rate"]; + right_ir_metadata_.ae_roi_bottom = json_data["ae_roi_bottom"]; + right_ir_metadata_.ae_roi_left = json_data["ae_roi_left"]; + right_ir_metadata_.ae_roi_right = json_data["ae_roi_right"]; + right_ir_metadata_.ae_roi_top = json_data["ae_roi_top"]; + right_ir_metadata_.auto_exposure = json_data["auto_exposure"]; + right_ir_metadata_.exposure = json_data["exposure"]; + right_ir_metadata_.exposure_priority = json_data["exposure_priority"]; + right_ir_metadata_.frame_emitter_mode = json_data["frame_emitter_mode"]; + right_ir_metadata_.frame_laser_power = json_data["frame_laser_power"]; + right_ir_metadata_.frame_laser_power_mode = json_data["frame_laser_power_mode"]; + right_ir_metadata_.frame_number = json_data["frame_number"]; + right_ir_metadata_.frame_timestamp = json_data["frame_timestamp"]; + right_ir_metadata_.gain = json_data["gain"]; + right_ir_metadata_.gpio_input_data = json_data["gpio_input_data"]; + right_ir_metadata_.hdr_sequence_index = json_data["hdr_sequence_index"]; + right_ir_metadata_.hdr_sequence_name = json_data["hdr_sequence_name"]; + right_ir_metadata_.hdr_sequence_size = json_data["hdr_sequence_size"]; + right_ir_metadata_.sensor_timestamp = json_data["sensor_timestamp"]; + + frame_emitter_mode = right_ir_metadata_.frame_emitter_mode; + + // RCLCPP_INFO_STREAM(get_logger(), "Right IR metadata: \n" << right_ir_metadata_); + + save_metadata_to_file(right_ir_metadata_, "irright"); + } catch (const nlohmann::json::exception &e) { + RCLCPP_ERROR(get_logger(), "Failed to parse metadata JSON: %s", e.what()); + } + + try { + save_image_to_file(image_msg, "irright", right_ir_image_count_, image_msg->header.stamp, + frame_emitter_mode); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error saving image: " << e.what()); + return; + } + right_ir_image_count_++; + } + void depth_metadata_sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr &image_msg, + const orbbec_camera_msgs::msg::Metadata::ConstSharedPtr &metadata_msg) { + // RCLCPP_INFO_STREAM(get_logger(), + // "Depth stamp: sec " << image_msg->header.stamp.sec << " nanosec " << + // image_msg->header.stamp.nanosec); + // RCLCPP_INFO_STREAM(get_logger(), + // "Depth metadata stamp: sec " << metadata_msg->header.stamp.sec << " + // nanosec " << metadata_msg->header.stamp.nanosec); + int frame_emitter_mode{0}; + try { + nlohmann::json json_data = nlohmann::json::parse(metadata_msg->json_data); + + depth_metadata_.actual_frame_rate = json_data["actual_frame_rate"]; + depth_metadata_.ae_roi_bottom = json_data["ae_roi_bottom"]; + depth_metadata_.ae_roi_left = json_data["ae_roi_left"]; + depth_metadata_.ae_roi_right = json_data["ae_roi_right"]; + depth_metadata_.ae_roi_top = json_data["ae_roi_top"]; + depth_metadata_.auto_exposure = json_data["auto_exposure"]; + depth_metadata_.exposure = json_data["exposure"]; + depth_metadata_.exposure_priority = json_data["exposure_priority"]; + depth_metadata_.frame_emitter_mode = json_data["frame_emitter_mode"]; + depth_metadata_.frame_laser_power = json_data["frame_laser_power"]; + depth_metadata_.frame_laser_power_mode = json_data["frame_laser_power_mode"]; + depth_metadata_.frame_number = json_data["frame_number"]; + depth_metadata_.frame_timestamp = json_data["frame_timestamp"]; + depth_metadata_.gain = json_data["gain"]; + depth_metadata_.gpio_input_data = json_data["gpio_input_data"]; + depth_metadata_.hdr_sequence_index = json_data["hdr_sequence_index"]; + depth_metadata_.hdr_sequence_name = json_data["hdr_sequence_name"]; + depth_metadata_.hdr_sequence_size = json_data["hdr_sequence_size"]; + depth_metadata_.sensor_timestamp = json_data["sensor_timestamp"]; + + frame_emitter_mode = depth_metadata_.frame_emitter_mode; + + // RCLCPP_INFO_STREAM(get_logger(), "Depth metadata: \n" << depth_metadata_); + + save_metadata_to_file(depth_metadata_, "depth"); + } catch (const nlohmann::json::exception &e) { + RCLCPP_ERROR(get_logger(), "Failed to parse metadata JSON: %s", e.what()); + } + + try { + save_image_to_file(image_msg, "depth", depth_image_count_, image_msg->header.stamp, + frame_emitter_mode); + } catch (const std::exception &e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error saving image: " << e.what()); + return; + } + depth_image_count_++; + } + + void createDirectory(const std::string &path) { + RCLCPP_INFO(this->get_logger(), "Creating directory: %s", path.c_str()); + std::filesystem::path dir_path(path); + if (!std::filesystem::exists(dir_path)) { + if (std::filesystem::create_directories(dir_path)) { + std::cout << "Directory created: " << path << std::endl; + } else { + std::cerr << "Failed to create directory: " << path << std::endl; + } + } else { + std::cout << "Directory already exists: " << path << std::endl; + } + } + void save_image_to_file(const sensor_msgs::msg::Image::ConstSharedPtr &msg, + const std::string &prefix, int count, + const builtin_interfaces::msg::Time &stamp, int frame_emitter_mode) { + (void)count; + long long timestamp_us = static_cast(stamp.sec) * 1000000LL + stamp.nanosec / 1000LL; + std::string serial_path{}; + cv_bridge::CvImagePtr cv_ptr; + + if ((prefix == "irleft") || (prefix == "irright")) { + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + } else if (prefix == "depth") { + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + } else { + RCLCPP_ERROR(get_logger(), "Unknown prefix: %s", prefix.c_str()); + return; + } + + serial_path = frame_emitter_mode == 0 ? serial_path_zero_ : serial_path_one_; + + std::ostringstream ss; + // ss << serial_path << "/" << prefix << "_" << timestamp_us << ".png"; + ss << serial_path << "/" << timestamp_us << "_" << prefix << ".png"; + + cv::imwrite(ss.str(), cv_ptr->image); + } + void save_metadata_to_file(const ImageMetadata &metadata, const std::string &prefix) { + std::ostringstream ss; + ss << (metadata.frame_emitter_mode == 0 ? serial_path_zero_ : serial_path_one_) << "/" + << metadata.frame_timestamp << "_" << prefix << "_e" << metadata.exposure << "_g" + << metadata.gain << ".txt"; + + std::string file_path = ss.str(); + // RCLCPP_INFO(get_logger(), "Saving metadata to file: %s", file_path.c_str()); + + std::ofstream outfile(file_path); + if (outfile.is_open()) { + outfile << "Frame Emitter Mode: " << metadata.frame_emitter_mode << "\n"; + outfile << "Exposure: " << metadata.exposure << "\n"; + outfile << "Gain: " << metadata.gain << "\n"; + outfile << "Hdr Sequence Index: " << metadata.hdr_sequence_index << "\n"; + outfile << "Frame Timestamp: " << metadata.frame_timestamp << "\n"; + outfile << "Sensor Timestamp: " << metadata.sensor_timestamp << "\n"; + outfile << "Frame Number: " << metadata.frame_number << "\n"; + + outfile.close(); + // RCLCPP_INFO(get_logger(), "Metadata successfully saved to file."); + } else { + RCLCPP_ERROR(get_logger(), "Failed to open file: %s", file_path.c_str()); + } + } + + std::shared_ptr> left_ir_image_sub_; + std::shared_ptr> + left_ir_metadata_sub_; + + std::shared_ptr> right_ir_image_sub_; + std::shared_ptr> + right_ir_metadata_sub_; + + std::shared_ptr> depth_image_sub_; + std::shared_ptr> + depth_metadata_sub_; + + std::string left_ir_image_topic_; + std::string right_ir_image_topic_; + std::string depth_image_topic_; + std::string left_ir_metadata_topic_; + std::string right_ir_metadata_topic_; + std::string depth_metadata_topic_; + + int left_ir_image_count_ = 0; + int right_ir_image_count_ = 0; + int depth_image_count_ = 0; + int left_ir_metadata_count_ = 0; + int right_ir_metadata_count_ = 0; + int depth_metadata_count_ = 0; + + bool directories_initialized_ = false; + + std::string serial_; + std::string serial_path_zero_; + std::string serial_path_one_; + + ImageMetadata left_ir_metadata_ = ImageMetadata(); + ImageMetadata right_ir_metadata_ = ImageMetadata(); + ImageMetadata depth_metadata_ = ImageMetadata(); + + using MySyncPolicy = + message_filters::sync_policies::ApproximateTime; + std::shared_ptr> left_ir_sync_; + std::shared_ptr> right_ir_sync_; + std::shared_ptr> depth_sync_; +}; + +} // namespace tools +} // namespace orbbec_camera diff --git a/orbbec_camera/tools/multi_save_rgbir_node.cpp b/orbbec_camera/tools/multi_save_rgbir_node.cpp new file mode 100644 index 00000000..8e4acf4a --- /dev/null +++ b/orbbec_camera/tools/multi_save_rgbir_node.cpp @@ -0,0 +1,24 @@ +/******************************************************************************* + * Copyright (c) 2023 Orbbec 3D Technology, Inc + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *******************************************************************************/ +#include "multi_save_rgbir_node.hpp" + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/orbbec_camera/tools/multi_save_rgbir_node.hpp b/orbbec_camera/tools/multi_save_rgbir_node.hpp new file mode 100644 index 00000000..c61acbdb --- /dev/null +++ b/orbbec_camera/tools/multi_save_rgbir_node.hpp @@ -0,0 +1,164 @@ +#pragma once +#include + +#include +#include +#include +#include +#include + +class MultiCameraSubscriber : public rclcpp::Node { + public: + MultiCameraSubscriber() : Node("multi_camera_subscriber") { + auto context = std::make_unique(); + context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE); + auto list = context->queryDeviceList(); + for (size_t i = 0; i < list->deviceCount(); i++) { + auto device = list->getDevice(i); + auto device_info = device->getDeviceInfo(); + std::string serial = device_info->serialNumber(); + std::string uid = device_info->uid(); + auto usb_port = orbbec_camera::parseUsbPort(uid); + int vid = device_info->vid(); + int pid = device_info->pid(); + serial_numbers_[usb_port] = serial; + RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":vid: " << std::hex << vid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":pid: " << std::hex << pid); + RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":serial: " << serial); + RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":usb_port: " << usb_port); + color_frame_counters_[count] = 0; + ir_frame_counters_[count] = 0; + count++; + } + + this->declare_parameter>("ir_topics", std::vector()); + this->declare_parameter>("color_topics", std::vector()); + this->declare_parameter>("usb_ports", std::vector()); + + std::vector ir_topics_ = this->get_parameter("ir_topics").as_string_array(); + std::vector color_topics_ = this->get_parameter("color_topics").as_string_array(); + std::vector usb_params_ = this->get_parameter("usb_ports").as_string_array(); + + auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); + for (size_t i = 0; i < usb_params_.size(); i++) { + usb_numbers_[i] = usb_params_[i]; + usb_index_map_[usb_params_[i]] = i; + } + + for (const auto &pair : serial_numbers_) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), + "usb_port: " << pair.first << ", serial: " << pair.second); + } + for (const auto &pair : usb_index_map_) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), + "usb_port: " << pair.first << ", index: " << pair.second); + } + + for (size_t i = 0; i < ir_topics_.size(); ++i) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), + "ir_topic: " << ir_topics_[i]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), + "color_topic: " << color_topics_[i]); + + auto ir_sub = std::make_shared>( + this, ir_topics_[i], custom_qos.get_rmw_qos_profile()); + auto color_sub = std::make_shared>( + this, color_topics_[i], custom_qos.get_rmw_qos_profile()); + + ir_sub->registerCallback([this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + this->irCallback(msg, i); + }); + + color_sub->registerCallback([this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + this->colorCallback(msg, i); + }); + + ir_subscribers_.push_back(ir_sub); + color_subscribers_.push_back(color_sub); + } + } + + private: + std::string generateFolderName(const sensor_msgs::msg::Image::ConstSharedPtr &color_msg, + const std::string &serial_number, size_t serial_index) { + std::string color_resolution = + std::to_string(color_msg->width) + "x" + std::to_string(color_msg->height); + std::string color_encoding = color_msg->encoding; + std::string frame_rate = "30fps"; + std::string folder_name = "Star-AE-OFF-ir-" + color_resolution + "-" + "y8" + "-rgb-" + + color_resolution + "-" + "mjpg" + "-" + frame_rate; + + std::string path = std::string("multicamera_sync/output/") + folder_name + "/" + + "TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" + + std::to_string(serial_index); + std::filesystem::create_directories(path); + return path; + } + std::string getTimestamp() { + auto now = this->get_clock()->now(); + int64_t seconds = now.seconds(); + int64_t nanoseconds = now.nanoseconds() % 1000000000; + int64_t milliseconds = nanoseconds / 1000000; + return std::to_string(seconds) + std::to_string(milliseconds); + } + std::string getCurrentTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image_msg) { + int64_t seconds = image_msg->header.stamp.sec; + int64_t nanoseconds = image_msg->header.stamp.nanosec; + + int64_t milliseconds = nanoseconds / 1000000; + + std::ostringstream timestamp; + timestamp << seconds << std::setw(3) << std::setfill('0') << milliseconds; + + return timestamp.str(); + } + void irCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) { + auto usb_iter = usb_index_map_.find(usb_numbers_[index]); + auto serial_iter = serial_numbers_.find(usb_numbers_[index]); + int usb_index = usb_iter->second; + std::string serial_index = serial_iter->second; + cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image; + std::string timestamp_ir = getCurrentTimestamp(image); + size_t frame_index = ir_frame_counters_[index]++; + std::string folder = generateFolderName(image, serial_index, usb_index); + std::string filename = folder + "/ir#left_SN" + serial_index + "_Index" + + std::to_string(usb_index) + "_d" + timestamp_ir + "_f" + + std::to_string(frame_index) + "_s" + getTimestamp() + "_.jpg"; + cv::imwrite(filename, ir_mat); + RCLCPP_INFO(this->get_logger(), "Saved IR image for camera to: %s", filename.c_str()); + } + + void colorCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) { + auto usb_iter = usb_index_map_.find(usb_numbers_[index]); + auto serial_iter = serial_numbers_.find(usb_numbers_[index]); + int usb_index = usb_iter->second; + std::string serial_index = serial_iter->second; + cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image; + cv::Mat corrected_image; + cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR); + std::string timestamp_color = getCurrentTimestamp(image); + size_t frame_index = color_frame_counters_[index]++; + std::string folder = generateFolderName(image, serial_index, usb_index); + std::string filename = folder + "/color_SN" + serial_index + "_Index" + + std::to_string(usb_index) + "_d" + timestamp_color + "_f" + + std::to_string(frame_index) + "_s" + getTimestamp() + "_.jpg"; + cv::imwrite(filename, corrected_image); + RCLCPP_INFO(this->get_logger(), "Saved Color image for camera to: %s", filename.c_str()); + } + + std::vector>> + ir_subscribers_; + std::vector>> + color_subscribers_; + + std::map usb_index_map_; + std::map serial_numbers_; + std::array usb_numbers_; + std::array color_frame_counters_; + std::array ir_frame_counters_; + size_t count = 0; + + std::vector usb_params_; + std::vector ir_topics_; + std::vector color_topics_; +}; \ No newline at end of file