i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-11 10:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Write the tool node parameters into a json file

Browse Source
This commit is contained in:
jj
2024-11-06 20:04:23 +08:00
parent 10dc88861e
commit cc8dcd8a82
7 changed files with 189 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/config/metadataexport/metadata_export_params.json → orbbec_camera/config/tools/metadataexport/metadata_export_params.json
+1 -1
View File
@@ -1,5 +1,5 @@
{
"camera_params": {
"metadata_export_params": {
"sn": "CP1L44P00085",
"left_ir_image_topic": "/camera/left_ir/image_raw",
"right_ir_image_topic": "/camera/right_ir/image_raw",
orbbec_camera/config/tools/metadatasave/metadata_save_params.json
+10
View File
@@ -0,0 +1,10 @@
{
"metadata_save_params": {
"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",
"left_ir_metadata_topic": "/camera/left_ir/metadata",
"right_ir_metadata_topic": "/camera/right_ir/metadata",
"depth_metadata_topic": "/camera/depth/metadata"
}
}
orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json
+20
View File
@@ -0,0 +1,20 @@
{
"save_rgbir_params": {
"image_number": "100",
"usb_ports": [
"2-7",
"2-2",
"2-1"
],
"ir_topics": [
"/G0_4J/left_ir/image_raw",
"/G1_F0/left_ir/image_raw",
"/G2_15/left_ir/image_raw"
],
"color_topics": [
"/G0_4J/color/image_raw",
"/G1_F0/color/image_raw",
"/G2_15/left_ir/image_raw"
]
}
}
orbbec_camera/launch/multi_camera_synced.launch.py
+72 -43
View File
@@ -12,68 +12,97 @@ def generate_launch_description():
launch_file_dir = os.path.join(package_dir, "launch")
config_file_dir = os.path.join(package_dir, "config")
config_file_path = os.path.join(config_file_dir, "camera_params.yaml")
front_camera = IncludeLaunchDescription(
G0_4J = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_file_dir, "gemini_330_series.launch.py")
),
launch_arguments={
"camera_name": "front_camera",
"usb_port": "gmsl2-1",
"device_num": "2",
"sync_mode": "hardware_triggering",
"camera_name": "G0_4J",
"usb_port": "2-7",
"device_num": "5",
"sync_mode": "standalone",
"enable_left_ir":"true",
"config_file_path": config_file_path,
"enable_gmsl_trigger": "true",
}.items(),
)
# left_camera = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# os.path.join(launch_file_dir, "gemini_330_series.launch.py")
# ),
# launch_arguments={
# "camera_name": "left_camera",
# "usb_port": "gmsl2-2",
# "device_num": "3",
# "sync_mode": "secondary",
# "config_file_path": config_file_path,
# "enable_gmsl_trigger": "false",
# }.items(),
# )
right_camera = IncludeLaunchDescription(
G1_F0 = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_file_dir, "gemini_330_series.launch.py")
),
launch_arguments={
"camera_name": "right_camera",
"usb_port": "gmsl2-3",
"device_num": "2",
"sync_mode": "hardware_triggering",
"camera_name": "G1_F0",
"usb_port": "2-2",
"device_num": "5",
"sync_mode": "standalone",
"enable_left_ir":"true",
"config_file_path": config_file_path,
"enable_gmsl_trigger": "false",
}.items(),
)
# rear_camera = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# os.path.join(launch_file_dir, "gemini_330_series.launch.py")
# ),
# launch_arguments={
# "camera_name": "rear_camera",
# "usb_port": "gmsl2-4",
# "device_num": "3",
# "sync_mode": "secondary",
# "config_file_path": config_file_path,
# "enable_gmsl_trigger": "false",
# }.items(),
# )
G2_15 = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_file_dir, "gemini_330_series.launch.py")
),
launch_arguments={
"camera_name": "G2_15",
"usb_port": "2-1",
"device_num": "5",
"sync_mode": "standalone",
"enable_left_ir":"true",
"config_file_path": config_file_path,
}.items(),
)
G3_1M = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_file_dir, "gemini_330_series.launch.py")
),
launch_arguments={
"camera_name": "G3_1M",
"usb_port": "2-1.4.3",
"device_num": "5",
"sync_mode": "secondary_synced",
"enable_left_ir":"true",
"config_file_path": config_file_path,
}.items(),
)
G4_74 = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_file_dir, "gemini_330_series.launch.py")
),
launch_arguments={
"camera_name": "G4_74",
"usb_port": "2-1.4.4.1",
"device_num": "5",
"sync_mode": "secondary_synced",
"enable_left_ir":"true",
"config_file_path": config_file_path,
}.items(),
)
multi_save_rgbir_node = Node(
package="orbbec_camera",
executable="multi_save_rgbir_node",
name="multi_save_rgbir_node",
)
# If you need more cameras, just add more launch_include here, and change the usb_port and device_num
# Launch description
ld = LaunchDescription(
[
# TimerAction(period=0.5, actions=[GroupAction([rear_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
GroupAction([multi_save_rgbir_node]),
TimerAction(
period=2.0,
actions=[
TimerAction(period=0.2, actions=[GroupAction([G1_F0])]),
TimerAction(period=0.2, actions=[GroupAction([G2_15])]),
# TimerAction(period=0.2, actions=[GroupAction([G3_1M])]),
# TimerAction(period=0.2, actions=[GroupAction([G4_74])]),
TimerAction(period=0.2, actions=[GroupAction([G0_4J])]),
],
),
# The primary camera should be launched at last
]
)
orbbec_camera/tools/metadata_export_files.hpp
+16 -14
View File
@@ -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));
orbbec_camera/tools/metadata_save_files.hpp
+37 -14
View File
@@ -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();
orbbec_camera/tools/multi_save_rgbir_node.hpp
+33 -29
View File
@@ -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;
};