mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-09-12 11:10:19 +08:00
Add some tools
This commit is contained in:
jj
@@ -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)
|
||||
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
#include "metadata_export_files.hpp"
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<orbbec_camera::tools::MetadataExportFiles>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,495 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/utils.h>
|
||||
#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/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<std::string>();
|
||||
|
||||
left_ir_image_topic_ = json_data["camera_params"]["left_ir_image_topic"].get<std::string>();
|
||||
right_ir_image_topic_ = json_data["camera_params"]["right_ir_image_topic"].get<std::string>();
|
||||
depth_image_topic_ = json_data["camera_params"]["depth_image_topic"].get<std::string>();
|
||||
color_image_topic_ = json_data["camera_params"]["color_image_topic"].get<std::string>();
|
||||
|
||||
left_ir_metadata_topic_ =
|
||||
json_data["camera_params"]["left_ir_metadata_topic"].get<std::string>();
|
||||
right_ir_metadata_topic_ =
|
||||
json_data["camera_params"]["right_ir_metadata_topic"].get<std::string>();
|
||||
depth_metadata_topic_ = json_data["camera_params"]["depth_metadata_topic"].get<std::string>();
|
||||
color_metadata_topic_ = json_data["camera_params"]["color_metadata_topic"].get<std::string>();
|
||||
|
||||
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<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
this, left_ir_image_topic_, qos_filters);
|
||||
left_ir_metadata_sub_ =
|
||||
std::make_shared<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>(
|
||||
this, left_ir_metadata_topic_, qos_filters);
|
||||
|
||||
left_ir_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
|
||||
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<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
this, right_ir_image_topic_, qos_filters);
|
||||
right_ir_metadata_sub_ =
|
||||
std::make_shared<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>(
|
||||
this, right_ir_metadata_topic_, qos_filters);
|
||||
|
||||
right_ir_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
|
||||
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<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
this, depth_image_topic_, qos_filters);
|
||||
depth_metadata_sub_ =
|
||||
std::make_shared<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>(
|
||||
this, depth_metadata_topic_, qos_filters);
|
||||
|
||||
depth_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
|
||||
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<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
this, color_image_topic_, qos_filters);
|
||||
color_metadata_sub_ =
|
||||
std::make_shared<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>(
|
||||
this, color_metadata_topic_, qos_filters);
|
||||
|
||||
color_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
|
||||
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<ob::Context>();
|
||||
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<long long>(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<message_filters::Subscriber<sensor_msgs::msg::Image>> left_ir_image_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>
|
||||
left_ir_metadata_sub_;
|
||||
|
||||
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> right_ir_image_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>
|
||||
right_ir_metadata_sub_;
|
||||
|
||||
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> depth_image_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>
|
||||
depth_metadata_sub_;
|
||||
|
||||
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> color_image_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>
|
||||
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<sensor_msgs::msg::Image,
|
||||
orbbec_camera_msgs::msg::Metadata>;
|
||||
std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> left_ir_sync_;
|
||||
std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> right_ir_sync_;
|
||||
std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> depth_sync_;
|
||||
std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> color_sync_;
|
||||
};
|
||||
|
||||
} // namespace tools
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,8 @@
|
||||
#include "metadata_save_files.hpp"
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<orbbec_camera::tools::MetadataSaveFiles>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,482 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/utils.h>
|
||||
#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<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
this, left_ir_image_topic_, qos_filters);
|
||||
left_ir_metadata_sub_ =
|
||||
std::make_shared<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>(
|
||||
this, left_ir_metadata_topic_, qos_filters);
|
||||
|
||||
left_ir_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
|
||||
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<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
this, right_ir_image_topic_, qos_filters);
|
||||
right_ir_metadata_sub_ =
|
||||
std::make_shared<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>(
|
||||
this, right_ir_metadata_topic_, qos_filters);
|
||||
|
||||
right_ir_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
|
||||
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<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
this, depth_image_topic_, qos_filters);
|
||||
depth_metadata_sub_ =
|
||||
std::make_shared<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>(
|
||||
this, depth_metadata_topic_, qos_filters);
|
||||
|
||||
depth_sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(
|
||||
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<ob::Context>();
|
||||
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<long long>(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<message_filters::Subscriber<sensor_msgs::msg::Image>> left_ir_image_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>
|
||||
left_ir_metadata_sub_;
|
||||
|
||||
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> right_ir_image_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>
|
||||
right_ir_metadata_sub_;
|
||||
|
||||
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> depth_image_sub_;
|
||||
std::shared_ptr<message_filters::Subscriber<orbbec_camera_msgs::msg::Metadata>>
|
||||
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<sensor_msgs::msg::Image,
|
||||
orbbec_camera_msgs::msg::Metadata>;
|
||||
std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> left_ir_sync_;
|
||||
std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> right_ir_sync_;
|
||||
std::shared_ptr<message_filters::Synchronizer<MySyncPolicy>> depth_sync_;
|
||||
};
|
||||
|
||||
} // namespace tools
|
||||
} // namespace orbbec_camera
|
||||
@@ -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<MultiCameraSubscriber>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
#pragma once
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/utils.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
|
||||
class MultiCameraSubscriber : public rclcpp::Node {
|
||||
public:
|
||||
MultiCameraSubscriber() : Node("multi_camera_subscriber") {
|
||||
auto context = std::make_unique<ob::Context>();
|
||||
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<std::vector<std::string>>("ir_topics", std::vector<std::string>());
|
||||
this->declare_parameter<std::vector<std::string>>("color_topics", std::vector<std::string>());
|
||||
this->declare_parameter<std::vector<std::string>>("usb_ports", std::vector<std::string>());
|
||||
|
||||
std::vector<std::string> ir_topics_ = this->get_parameter("ir_topics").as_string_array();
|
||||
std::vector<std::string> color_topics_ = this->get_parameter("color_topics").as_string_array();
|
||||
std::vector<std::string> 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<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
this, ir_topics_[i], custom_qos.get_rmw_qos_profile());
|
||||
auto color_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
|
||||
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<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>>>
|
||||
ir_subscribers_;
|
||||
std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>>>
|
||||
color_subscribers_;
|
||||
|
||||
std::map<std::string, int> usb_index_map_;
|
||||
std::map<std::string, std::string> serial_numbers_;
|
||||
std::array<std::string, 10> usb_numbers_;
|
||||
std::array<size_t, 10> color_frame_counters_;
|
||||
std::array<size_t, 10> ir_frame_counters_;
|
||||
size_t count = 0;
|
||||
|
||||
std::vector<std::string> usb_params_;
|
||||
std::vector<std::string> ir_topics_;
|
||||
std::vector<std::string> color_topics_;
|
||||
};
|
||||
Reference in New Issue
Block a user