From 0ad2053a06499932c5e5c0073b403b083d5c096e Mon Sep 17 00:00:00 2001 From: jj Date: Sun, 20 Oct 2024 18:56:56 +0800 Subject: [PATCH] improve multi_save_rgbir_node --- ...i_camera_synced_multi_save_rgbir.launch.py | 16 +- orbbec_camera/tools/multi_save_rgbir_node.cpp | 5 +- orbbec_camera/tools/multi_save_rgbir_node.hpp | 153 +++++++++++++----- 3 files changed, 122 insertions(+), 52 deletions(-) diff --git a/orbbec_camera/launch/multi_camera_synced_multi_save_rgbir.launch.py b/orbbec_camera/launch/multi_camera_synced_multi_save_rgbir.launch.py index f229955e..8a49fc07 100644 --- a/orbbec_camera/launch/multi_camera_synced_multi_save_rgbir.launch.py +++ b/orbbec_camera/launch/multi_camera_synced_multi_save_rgbir.launch.py @@ -17,11 +17,10 @@ def generate_launch_description(): ), launch_arguments={ "camera_name": "front_camera", - "usb_port": "gmsl2-1", - "device_num": "2", + "usb_port": "2-2", + "device_num": "1", "sync_mode": "secondary", - "config_file_path": config_file_path, - "enable_gmsl_trigger": "true", + "enable_left_ir":"true", }.items(), ) # left_camera = IncludeLaunchDescription( @@ -72,17 +71,18 @@ def generate_launch_description(): { # The port number should be filled in according to the order of the port numbers above. # "usb_ports": ["gmsl2-1","gmsl2-2","gmsl2-3"], - "usb_ports": ["gmsl2-1","gmsl2-3"], + "image_number": "100", + "usb_ports": ["2-2"], "ir_topics": [ "/front_camera/left_ir/image_raw", # "/left_camera/left_ir/image_raw", - "/right_camera/left_ir/image_raw", + # "/right_camera/left_ir/image_raw", # "/rear_camera/left_ir/image_raw", ], "color_topics": [ "/front_camera/color/image_raw", # "/left_camera/color/image_raw", - "/right_camera/color/image_raw", + # "/right_camera/color/image_raw", # "/rear_camera/color/image_raw", ], } @@ -97,7 +97,7 @@ def generate_launch_description(): period=0.5, actions=[ # TimerAction(period=0.5, actions=[GroupAction([rear_camera])]), - TimerAction(period=0.5, actions=[GroupAction([right_camera])]), + # TimerAction(period=0.5, actions=[GroupAction([right_camera])]), # TimerAction(period=0.5, actions=[GroupAction([left_camera])]), TimerAction(period=0.5, actions=[GroupAction([front_camera])]), # The primary camera should be launched at last diff --git a/orbbec_camera/tools/multi_save_rgbir_node.cpp b/orbbec_camera/tools/multi_save_rgbir_node.cpp index 8e4acf4a..c6299f15 100644 --- a/orbbec_camera/tools/multi_save_rgbir_node.cpp +++ b/orbbec_camera/tools/multi_save_rgbir_node.cpp @@ -18,7 +18,8 @@ int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20); + executor.add_node(node); + executor.spin(); return 0; } diff --git a/orbbec_camera/tools/multi_save_rgbir_node.hpp b/orbbec_camera/tools/multi_save_rgbir_node.hpp index 1066ab2e..340309be 100644 --- a/orbbec_camera/tools/multi_save_rgbir_node.hpp +++ b/orbbec_camera/tools/multi_save_rgbir_node.hpp @@ -43,17 +43,19 @@ class MultiCameraSubscriber : public rclcpp::Node { this->declare_parameter>("ir_topics", std::vector()); this->declare_parameter>("color_topics", std::vector()); this->declare_parameter>("usb_ports", std::vector()); + this->declare_parameter("image_number", "100"); 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(); + image_number_ = this->get_parameter("image_number").as_string(); 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; } - + reentrant_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); for (const auto &pair : serial_numbers_) { RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "usb_port: " << pair.first << ", serial: " << pair.second); @@ -69,37 +71,48 @@ class MultiCameraSubscriber : public rclcpp::Node { 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()); + rclcpp::SubscriptionOptions ir_sub_options; + ir_sub_options.callback_group = reentrant_callback_group_; - ir_sub->registerCallback([this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) { - this->irCallback(msg, i); - }); + rclcpp::SubscriptionOptions color_sub_options; + color_sub_options.callback_group = reentrant_callback_group_; - color_sub->registerCallback([this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) { - this->colorCallback(msg, i); - }); + auto ir_sub = this->create_subscription( + ir_topics_[i], custom_qos, + [this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + this->irCallback(msg, i); + }, + ir_sub_options); + + auto color_sub = this->create_subscription( + color_topics_[i], custom_qos, + [this, i](const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + this->colorCallback(msg, i); + }, + color_sub_options); ir_subscribers_.push_back(ir_sub); color_subscribers_.push_back(color_sub); + + ir_image_buffers_.emplace_back(); + color_image_buffers_.emplace_back(); + ir_current_timestamp_buffers_.emplace_back(); + color_current_timestamp_buffers_.emplace_back(); + ir_timestamp_buffers_.emplace_back(); + color_timestamp_buffers_.emplace_back(); } } 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 generateFolderName(const std::string &serial_number, size_t serial_index) { std::string frame_rate = "30fps"; - std::string folder_name = "Star-AE-OFF-ir-" + color_resolution + "-" + "y8" + "-rgb-" + + std::string folder_name = "Star-AE-OFF-ir-" + ir_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; } @@ -121,44 +134,87 @@ class MultiCameraSubscriber : public rclcpp::Node { return timestamp.str(); } - void irCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) { + + void saveAlignedImages(size_t index) { + images_saved_ = true; + auto &ir_images = ir_image_buffers_[index]; + auto &ir_current_timestamps = ir_current_timestamp_buffers_[index]; + auto &ir_timestamps = ir_timestamp_buffers_[index]; + auto &color_images = color_image_buffers_[index]; + auto &color_current_timestamps = color_current_timestamp_buffers_[index]; + auto &color_timestamps = color_timestamp_buffers_[index]; + + if (ir_images.size() < static_cast(std::stoi(image_number_)) || + color_images.size() < static_cast(std::stoi(image_number_))) { + return; + } + 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; + + for (size_t i = 0; i < static_cast(std::stoi(image_number_)); ++i) { + std::string folder = generateFolderName(serial_index, usb_index); + std::string ir_filename = folder + "/ir#left_SN" + serial_index + "_Index" + + std::to_string(usb_index) + "_d" + ir_current_timestamps[i] + "_f" + + std::to_string(i) + "_s" + ir_timestamps[i] + "_.jpg"; + + cv::imwrite(ir_filename, ir_images[i]); + RCLCPP_INFO(this->get_logger(), "Saved IR image to: %s", ir_filename.c_str()); + + std::string color_filename = folder + "/color_SN" + serial_index + "_Index" + + std::to_string(usb_index) + "_d" + color_current_timestamps[i] + + "_f" + std::to_string(i) + "_s" + color_timestamps[i] + "_.jpg"; + cv::imwrite(color_filename, color_images[i]); + RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str()); + } + + ir_images.clear(); + color_images.clear(); + ir_current_timestamps.clear(); + color_current_timestamps.clear(); + ir_timestamps.clear(); + color_timestamps.clear(); + rclcpp::shutdown(); + } + + void irCallback(const sensor_msgs::msg::Image::ConstSharedPtr &image, size_t index) { 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()); + std::string current_timestamp_ir = getCurrentTimestamp(image); + std::string timestamp_ir = getTimestamp(); + ir_image_buffers_[index].push_back(ir_mat); + ir_current_timestamp_buffers_[index].push_back(current_timestamp_ir); + ir_timestamp_buffers_[index].push_back(timestamp_ir); + ir_resolution = std::to_string(image->width) + "x" + std::to_string(image->height); + if (!images_saved_ && + ir_image_buffers_[index].size() >= static_cast(std::stoi(image_number_)) && + color_image_buffers_[index].size() >= static_cast(std::stoi(image_number_))) { + saveAlignedImages(index); + } } 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::string current_timestamp_color = getCurrentTimestamp(image); + std::string timestamp_color = getTimestamp(); + + color_image_buffers_[index].push_back(corrected_image); + color_current_timestamp_buffers_[index].push_back(current_timestamp_color); + color_timestamp_buffers_[index].push_back(timestamp_color); + color_resolution = std::to_string(image->width) + "x" + std::to_string(image->height); + if (!images_saved_ && + ir_image_buffers_[index].size() >= static_cast(std::stoi(image_number_)) && + color_image_buffers_[index].size() >= static_cast(std::stoi(image_number_))) { + saveAlignedImages(index); + } } - std::vector>> - ir_subscribers_; - std::vector>> - color_subscribers_; + rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_; + std::vector::SharedPtr> ir_subscribers_; + std::vector::SharedPtr> color_subscribers_; std::map usb_index_map_; std::map serial_numbers_; @@ -170,4 +226,17 @@ class MultiCameraSubscriber : public rclcpp::Node { std::vector usb_params_; std::vector ir_topics_; std::vector color_topics_; + std::string image_number_; + + std::vector> ir_image_buffers_; + std::vector> color_image_buffers_; + std::vector> ir_current_timestamp_buffers_; + std::vector> color_current_timestamp_buffers_; + std::vector> ir_timestamp_buffers_; + std::vector> color_timestamp_buffers_; + + std::string color_resolution; + std::string ir_resolution; + + bool images_saved_; }; \ No newline at end of file