mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-09-12 11:10:19 +08:00
Write the tool node parameters into a json file
This commit is contained in:
jj
@@ -68,7 +68,7 @@ class MetadataExportFiles : public rclcpp::Node {
|
||||
|
||||
void load_parameters() {
|
||||
std::ifstream file(
|
||||
"install/orbbec_camera/share/orbbec_camera/config/metadataexport/"
|
||||
"install/orbbec_camera/share/orbbec_camera/config/tools/metadataexport/"
|
||||
"metadata_export_params.json");
|
||||
if (!file.is_open()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file.");
|
||||
@@ -78,23 +78,25 @@ class MetadataExportFiles : public rclcpp::Node {
|
||||
nlohmann::json json_data;
|
||||
file >> json_data;
|
||||
|
||||
sn_ = json_data["camera_params"]["sn"].get<std::string>();
|
||||
sn_ = json_data["metadata_export_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_image_topic_ =
|
||||
json_data["metadata_export_params"]["left_ir_image_topic"].get<std::string>();
|
||||
right_ir_image_topic_ =
|
||||
json_data["metadata_export_params"]["right_ir_image_topic"].get<std::string>();
|
||||
depth_image_topic_ =
|
||||
json_data["metadata_export_params"]["depth_image_topic"].get<std::string>();
|
||||
color_image_topic_ =
|
||||
json_data["metadata_export_params"]["color_image_topic"].get<std::string>();
|
||||
|
||||
left_ir_metadata_topic_ =
|
||||
json_data["camera_params"]["left_ir_metadata_topic"].get<std::string>();
|
||||
json_data["metadata_export_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());
|
||||
json_data["metadata_export_params"]["right_ir_metadata_topic"].get<std::string>();
|
||||
depth_metadata_topic_ =
|
||||
json_data["metadata_export_params"]["depth_metadata_topic"].get<std::string>();
|
||||
color_metadata_topic_ =
|
||||
json_data["metadata_export_params"]["color_metadata_topic"].get<std::string>();
|
||||
}
|
||||
void initialize_pub_sub() {
|
||||
auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
|
||||
|
||||
@@ -103,24 +103,47 @@ 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_params();
|
||||
initialize_directories();
|
||||
initialize_pub_sub();
|
||||
}
|
||||
void initialize_params() {
|
||||
std::ifstream file(
|
||||
"install/orbbec_camera/share/orbbec_camera/config/tools/metadatasave/"
|
||||
"metadata_save_params.json");
|
||||
if (!file.is_open()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file.");
|
||||
return;
|
||||
}
|
||||
nlohmann::json json_data;
|
||||
file >> json_data;
|
||||
left_ir_image_topic_ =
|
||||
json_data["metadata_save_params"]["left_ir_image_topic"].get<std::string>();
|
||||
right_ir_image_topic_ =
|
||||
json_data["metadata_save_params"]["right_ir_image_topic"].get<std::string>();
|
||||
depth_image_topic_ = json_data["metadata_save_params"]["depth_image_topic"].get<std::string>();
|
||||
|
||||
left_ir_metadata_topic_ =
|
||||
json_data["metadata_save_params"]["left_ir_metadata_topic"].get<std::string>();
|
||||
right_ir_metadata_topic_ =
|
||||
json_data["metadata_save_params"]["right_ir_metadata_topic"].get<std::string>();
|
||||
depth_metadata_topic_ =
|
||||
json_data["metadata_save_params"]["depth_metadata_topic"].get<std::string>();
|
||||
RCLCPP_INFO(this->get_logger(), "Parameter 2: %s", depth_image_topic_.c_str());
|
||||
// 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();
|
||||
}
|
||||
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();
|
||||
|
||||
@@ -13,8 +13,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
public:
|
||||
MultiCameraSubscriber() : Node("multi_camera_subscriber") {
|
||||
device_init();
|
||||
currenttimes_=getCurrentTimes();
|
||||
|
||||
currenttimes_ = getCurrentTimes();
|
||||
}
|
||||
void device_init() {
|
||||
try {
|
||||
@@ -28,7 +27,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
std::string uid = device_info->uid();
|
||||
auto usb_port = orbbec_camera::parseUsbPort(uid);
|
||||
serial_numbers_[usb_port] = serial;
|
||||
// RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":list->deviceCount(): " << list->deviceCount());
|
||||
// RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":list->deviceCount(): " <<
|
||||
// list->deviceCount());
|
||||
color_frame_counters_[count] = 0;
|
||||
ir_frame_counters_[count] = 0;
|
||||
count++;
|
||||
@@ -40,27 +40,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR_STREAM(get_logger(), "unknown error");
|
||||
}
|
||||
|
||||
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>());
|
||||
this->declare_parameter<std::string>("image_number", "100");
|
||||
|
||||
ir_topics_ = this->get_parameter("ir_topics").as_string_array();
|
||||
color_topics_ = this->get_parameter("color_topics").as_string_array();
|
||||
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));
|
||||
capture_control_sub_ = this->create_subscription<std_msgs::msg::Bool>(
|
||||
"start_capture", custom_qos,
|
||||
std::bind(&MultiCameraSubscriber::controlCaptureCallback, this, std::placeholders::_1));
|
||||
params_init();
|
||||
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);
|
||||
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,10 +54,29 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
"usb_port: " << pair.first << ", index: " << pair.second);
|
||||
}
|
||||
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
|
||||
capture_control_sub_ = this->create_subscription<std_msgs::msg::Bool>(
|
||||
"start_capture", custom_qos,
|
||||
std::bind(&MultiCameraSubscriber::controlCaptureCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
private:
|
||||
std::mutex buffer_mutex_;
|
||||
void params_init() {
|
||||
std::ifstream file(
|
||||
"install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"
|
||||
"multi_save_rgbir_params.json");
|
||||
if (!file.is_open()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file.");
|
||||
return;
|
||||
}
|
||||
nlohmann::json json_data;
|
||||
file >> json_data;
|
||||
image_number_ = json_data["save_rgbir_params"]["image_number"].get<std::string>();
|
||||
usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get<std::vector<std::string>>();
|
||||
ir_topics_ = json_data["save_rgbir_params"]["ir_topics"].get<std::vector<std::string>>();
|
||||
color_topics_ = json_data["save_rgbir_params"]["color_topics"].get<std::vector<std::string>>();
|
||||
}
|
||||
void topic_init() {
|
||||
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
@@ -115,7 +119,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
callback_called_ = std::vector<bool>(ir_topics_.size(), false);
|
||||
}
|
||||
}
|
||||
std::string getCurrentTimes(){
|
||||
std::string getCurrentTimes() {
|
||||
auto now = std::chrono::system_clock::now();
|
||||
auto now_time_t = std::chrono::system_clock::to_time_t(now);
|
||||
std::tm tm = *std::localtime(&now_time_t);
|
||||
@@ -126,7 +130,6 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
return date_str;
|
||||
}
|
||||
std::string generateFolderName(const std::string &serial_number, size_t serial_index) {
|
||||
|
||||
std::string path = std::string("multicamera_sync/output/") + currenttimes_ + "/" +
|
||||
"TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" +
|
||||
std::to_string(serial_index);
|
||||
@@ -165,11 +168,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
color_images.size() < static_cast<size_t>(std::stoi(image_number_))) {
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjjj1");
|
||||
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;
|
||||
if (serial_iter == serial_numbers_.end()) {
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjjj2");
|
||||
return;
|
||||
}
|
||||
std::string serial_index = serial_iter->second;
|
||||
@@ -186,7 +190,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
}
|
||||
|
||||
cv::imwrite(ir_filename, ir_images[i]);
|
||||
// RCLCPP_INFO(this->get_logger(), "Saved IR image to: %s", ir_filename.c_str());
|
||||
// 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] +
|
||||
@@ -242,8 +246,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
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);
|
||||
// RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":ir: " << index <<":"<<
|
||||
// ir_image_buffers_[index].size());
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
|
||||
":ir: " << index << ":" << ir_image_buffers_[index].size());
|
||||
if (ir_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_)) &&
|
||||
color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
|
||||
saveAlignedImages(index);
|
||||
@@ -263,8 +267,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
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);
|
||||
// RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), ":color: " << index <<":"<<
|
||||
// color_image_buffers_[index].size());
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
|
||||
":color: " << index << ":" << color_image_buffers_[index].size());
|
||||
if (ir_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_)) &&
|
||||
color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
|
||||
saveAlignedImages(index);
|
||||
@@ -300,7 +304,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
|
||||
std::string color_resolution_;
|
||||
std::string ir_resolution_;
|
||||
std::string currenttimes_;
|
||||
std::string currenttimes_;
|
||||
|
||||
bool is_saving_images_ = false;
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user