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

Update multi_save_rgbir tool to be compatible with other camera devices (net devices are not supported yet) and removed metadata_export, metadata_save tools

Browse Source
This commit is contained in:
jj
2025-05-07 15:41:12 +08:00
parent 0eb7f44526
commit 4c60a7f685
10 changed files with 278 additions and 1119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/CMakeLists.txt
+4 -5
View File
@@ -215,8 +215,6 @@ 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(metadata_save_files_node tools/metadata_save_files.cpp)
add_orbbec_executable(metadata_export_files_node tools/metadata_export_files.cpp)
add_orbbec_executable(ob_benchmark_node tools/ob_benchmark.cpp)
add_library(frame_latency SHARED tools/frame_latency.cpp)
@@ -239,7 +237,10 @@ rclcpp_components_register_node(start_benchmark
EXECUTABLE start_benchmark_node
)
add_library(multi_save_rgbir SHARED tools/multi_save_rgbir.cpp)
add_library(multi_save_rgbir SHARED
tools/multi_save_rgbir.cpp
src/utils.cpp
)
target_include_directories(multi_save_rgbir PUBLIC ${COMMON_INCLUDE_DIRS} )
target_link_libraries(multi_save_rgbir ${COMMON_LIBRARIES})
ament_target_dependencies(multi_save_rgbir ${dependencies})
@@ -276,8 +277,6 @@ install(TARGETS list_devices_node
list_depth_work_mode_node
list_camera_profile_mode_node
topic_statistics_node
metadata_save_files_node
metadata_export_files_node
ob_benchmark_node
DESTINATION lib/${PROJECT_NAME}/)
orbbec_camera/config/tools/metadataexport/metadata_export_params.json
-13
View File
@@ -1,13 +0,0 @@
{
"metadata_export_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"
}
}
orbbec_camera/config/tools/metadatasave/metadata_save_params.json
-10
View File
@@ -1,10 +0,0 @@
{
"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/examples/multi_camera_synced_verification_tool/gemini_330_series_synced_verify.launch.py
+34 -1
View File
@@ -57,6 +57,7 @@ def generate_launch_description():
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('preset_firmware_path', default_value=''),
DeclareLaunchArgument('uvc_backend', default_value='libuvc'),#libuvc or v4l2
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
@@ -70,13 +71,26 @@ def generate_launch_description():
DeclareLaunchArgument('enable_color', default_value='true'),
DeclareLaunchArgument('color_qos', default_value='default'),
DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'),
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
DeclareLaunchArgument('color_ae_roi_left', default_value='-1'),
DeclareLaunchArgument('color_ae_roi_right', default_value='-1'),
DeclareLaunchArgument('color_ae_roi_top', default_value='-1'),
DeclareLaunchArgument('color_ae_roi_bottom', default_value='-1'),
DeclareLaunchArgument('color_exposure', default_value='-1'),
DeclareLaunchArgument('color_gain', default_value='-1'),
DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
DeclareLaunchArgument('color_white_balance', default_value='-1'),
DeclareLaunchArgument('color_ae_max_exposure', default_value='-1'),
DeclareLaunchArgument('color_brightness', default_value='-1'),
DeclareLaunchArgument('color_sharpness', default_value='-1'),
DeclareLaunchArgument('color_gamma', default_value='-1'),
DeclareLaunchArgument('color_saturation', default_value='-1'),
DeclareLaunchArgument('color_constrast', default_value='-1'),
DeclareLaunchArgument('color_hue', default_value='-1'),
DeclareLaunchArgument('enable_color_backlight_compenstation', default_value='false'),
DeclareLaunchArgument('enable_color_decimation_filter', default_value='false'),
DeclareLaunchArgument('color_decimation_filter_scale', default_value='-1'),
DeclareLaunchArgument('depth_width', default_value='0'),
DeclareLaunchArgument('depth_height', default_value='0'),
DeclareLaunchArgument('depth_fps', default_value='0'),
@@ -84,6 +98,12 @@ def generate_launch_description():
DeclareLaunchArgument('enable_depth', default_value='true'),
DeclareLaunchArgument('depth_qos', default_value='default'),
DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'),
DeclareLaunchArgument('depth_ae_roi_left', default_value='-1'),
DeclareLaunchArgument('depth_ae_roi_right', default_value='-1'),
DeclareLaunchArgument('depth_ae_roi_top', default_value='-1'),
DeclareLaunchArgument('depth_ae_roi_bottom', default_value='-1'),
DeclareLaunchArgument('depth_brightness', default_value='-1'),
DeclareLaunchArgument('left_ir_width', default_value='0'),
DeclareLaunchArgument('left_ir_height', default_value='0'),
DeclareLaunchArgument('left_ir_fps', default_value='0'),
@@ -91,6 +111,8 @@ def generate_launch_description():
DeclareLaunchArgument('enable_left_ir', default_value='false'),
DeclareLaunchArgument('left_ir_qos', default_value='default'),
DeclareLaunchArgument('left_ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_left_ir_sequence_id_filter', default_value='false'),
DeclareLaunchArgument('left_ir_sequence_id_filter_id', default_value='-1'),
DeclareLaunchArgument('right_ir_width', default_value='0'),
DeclareLaunchArgument('right_ir_height', default_value='0'),
DeclareLaunchArgument('right_ir_fps', default_value='0'),
@@ -98,6 +120,8 @@ def generate_launch_description():
DeclareLaunchArgument('enable_right_ir', default_value='false'),
DeclareLaunchArgument('right_ir_qos', default_value='default'),
DeclareLaunchArgument('right_ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_right_ir_sequence_id_filter', default_value='false'),
DeclareLaunchArgument('right_ir_sequence_id_filter_id', default_value='-1'),
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
DeclareLaunchArgument('ir_exposure', default_value='-1'),
DeclareLaunchArgument('ir_gain', default_value='-1'),
@@ -116,11 +140,19 @@ def generate_launch_description():
DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
# Network device settings: default enumerate_net_device is set to true, which will automatically enumerate network devices
# If you do not want to automatically enumerate network devices,
# you can set enumerate_net_device to true, net_device_ip to the device's IP address, and net_device_port to the default value of 8090
DeclareLaunchArgument("enumerate_net_device", default_value="false"),
DeclareLaunchArgument("net_device_ip", default_value=""),
DeclareLaunchArgument("net_device_port", default_value="0"),
DeclareLaunchArgument("exposure_range_mode", default_value="default"),#default, ultimate or regular
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument('enable_hardware_d2d', default_value='true'),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('ldp_power_level', default_value='-1'),
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
@@ -139,7 +171,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
DeclareLaunchArgument('enable_sequence_id_filter', default_value='false'),
DeclareLaunchArgument('enable_threshold_filter', default_value='false'),
DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value='true'),
DeclareLaunchArgument('enable_hardware_noise_removal_filter', default_value='false'),
DeclareLaunchArgument('enable_noise_removal_filter', default_value='true'),
DeclareLaunchArgument('enable_spatial_filter', default_value='false'),
DeclareLaunchArgument('enable_temporal_filter', default_value='false'),
@@ -148,6 +180,7 @@ def generate_launch_description():
DeclareLaunchArgument('sequence_id_filter_id', default_value='-1'),
DeclareLaunchArgument('threshold_filter_max', default_value='-1'),
DeclareLaunchArgument('threshold_filter_min', default_value='-1'),
DeclareLaunchArgument('hardware_noise_removal_filter_threshold', default_value='-1.0'),
DeclareLaunchArgument('noise_removal_filter_min_diff', default_value='256'),
DeclareLaunchArgument('noise_removal_filter_max_size', default_value='80'),
DeclareLaunchArgument('spatial_filter_alpha', default_value='-1.0'),
orbbec_camera/examples/multi_camera_synced_verification_tool/gemini_synced_verify.launch.py
+207
View File
@@ -0,0 +1,207 @@
import os
import yaml
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, GroupAction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch.conditions import UnlessCondition
from launch_ros.actions import LoadComposableNodes
def load_yaml(file_path):
with open(file_path, 'r') as f:
return yaml.safe_load(f)
def merge_params(default_params, yaml_params):
for key, value in yaml_params.items():
if key in default_params:
default_params[key] = value
return default_params
def convert_value(value):
if isinstance(value, str):
try:
return int(value)
except ValueError:
pass
try:
return float(value)
except ValueError:
pass
if value.lower() == 'true':
return True
elif value.lower() == 'false':
return False
return value
def load_parameters(context, args):
default_params = {arg.name: LaunchConfiguration(arg.name).perform(context) for arg in args}
config_file_path = LaunchConfiguration('config_file_path').perform(context)
if config_file_path:
yaml_params = load_yaml(config_file_path)
default_params = merge_params(default_params, yaml_params)
skip_convert = {'config_file_path', 'usb_port', 'serial_number'}
return {
key: (value if key in skip_convert else convert_value(value))
for key, value in default_params.items()
}
def generate_launch_description():
args = [
DeclareLaunchArgument('camera_name', default_value='camera'),
DeclareLaunchArgument('depth_registration', default_value='true'),
DeclareLaunchArgument('serial_number', default_value=''),
DeclareLaunchArgument('usb_port', default_value=''),
DeclareLaunchArgument('device_num', default_value='1'),
DeclareLaunchArgument('uvc_backend', default_value='libuvc'),#libuvc or v4l2
DeclareLaunchArgument('product_id', default_value=''),
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
DeclareLaunchArgument('cloud_frame_id', default_value=''),
DeclareLaunchArgument('enable_colored_point_cloud', default_value='false'),
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
DeclareLaunchArgument('connection_delay', default_value='100'),
DeclareLaunchArgument('color_width', default_value='0'),
DeclareLaunchArgument('color_height', default_value='0'),
DeclareLaunchArgument('color_fps', default_value='0'),
DeclareLaunchArgument('color_format', default_value='ANY'),
DeclareLaunchArgument('enable_color', default_value='true'),
DeclareLaunchArgument('flip_color', default_value='false'),
DeclareLaunchArgument('color_qos', default_value='default'),
DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
DeclareLaunchArgument('color_ae_max_exposure', default_value='-1'),
DeclareLaunchArgument('color_ae_roi_left', default_value='-1'),
DeclareLaunchArgument('color_ae_roi_right', default_value='-1'),
DeclareLaunchArgument('color_ae_roi_top', default_value='-1'),
DeclareLaunchArgument('color_ae_roi_bottom', default_value='-1'),
DeclareLaunchArgument('color_exposure', default_value='-1'),
DeclareLaunchArgument('color_gain', default_value='-1'),
DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
DeclareLaunchArgument('color_white_balance', default_value='-1'),
DeclareLaunchArgument('color_brightness', default_value='-1'),
DeclareLaunchArgument('depth_width', default_value='0'),
DeclareLaunchArgument('depth_height', default_value='0'),
DeclareLaunchArgument('depth_fps', default_value='0'),
DeclareLaunchArgument('depth_format', default_value='ANY'),
DeclareLaunchArgument('enable_depth', default_value='true'),
DeclareLaunchArgument('flip_depth', default_value='false'),
DeclareLaunchArgument('depth_qos', default_value='default'),
DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
DeclareLaunchArgument('depth_ae_roi_left', default_value='-1'),
DeclareLaunchArgument('depth_ae_roi_right', default_value='-1'),
DeclareLaunchArgument('depth_ae_roi_top', default_value='-1'),
DeclareLaunchArgument('depth_ae_roi_bottom', default_value='-1'),
DeclareLaunchArgument('ir_width', default_value='0'),
DeclareLaunchArgument('ir_height', default_value='0'),
DeclareLaunchArgument('ir_fps', default_value='0'),
DeclareLaunchArgument('ir_format', default_value='ANY'),
DeclareLaunchArgument('enable_ir', default_value='true'),
DeclareLaunchArgument('flip_ir', default_value='false'),
DeclareLaunchArgument('ir_qos', default_value='default'),
DeclareLaunchArgument('ir_camera_info_qos', default_value='default'),
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'),
DeclareLaunchArgument('ir_exposure', default_value='-1'),
DeclareLaunchArgument('ir_gain', default_value='-1'),
DeclareLaunchArgument('ir_brightness', default_value='-1'),
DeclareLaunchArgument('config_file_path', default_value=''),
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='true'),
DeclareLaunchArgument('enable_accel', default_value='false'),
DeclareLaunchArgument('accel_rate', default_value='100hz'),
DeclareLaunchArgument('accel_range', default_value='4g'),
DeclareLaunchArgument('enable_gyro', default_value='false'),
DeclareLaunchArgument('gyro_rate', default_value='100hz'),
DeclareLaunchArgument('gyro_range', default_value='1000dps'),
DeclareLaunchArgument('liner_accel_cov', default_value='0.01'),
DeclareLaunchArgument('angular_vel_cov', default_value='0.01'),
DeclareLaunchArgument('publish_tf', default_value='true'),
DeclareLaunchArgument('tf_publish_rate', default_value='0.0'),
DeclareLaunchArgument('ir_info_url', default_value=''),
DeclareLaunchArgument('color_info_url', default_value=''),
DeclareLaunchArgument('log_level', default_value='none'),
DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
DeclareLaunchArgument('enable_ldp', default_value='true'),
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
DeclareLaunchArgument('decimation_filter_scale', default_value='-1'),
# Configure the path for depth filter file, for example: /config/depthfilter/Gemini2_v1.7.json
DeclareLaunchArgument('depth_filter_config', default_value=''),
# Depth work mode support is as follows:
# Unbinned Dense Default
# Unbinned Sparse Default
# Binned Sparse Default
# Obstacle Avoidance
DeclareLaunchArgument('depth_work_mode', default_value=''),
DeclareLaunchArgument('sync_mode', default_value='standalone'),
DeclareLaunchArgument('depth_delay_us', default_value='0'),
DeclareLaunchArgument('color_delay_us', default_value='0'),
DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('align_mode', default_value='HW'),
DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument('time_domain', default_value='global'),
DeclareLaunchArgument('use_intra_process_comms', default_value='false'),
DeclareLaunchArgument('attach_component_container_enable', default_value='false'),
DeclareLaunchArgument('attach_component_container_name', default_value='orbbec_container'),
]
def get_params(context, args):
return [load_parameters(context, args)]
def create_node_action(context, args):
params = get_params(context, args)
ros_distro = os.environ.get("ROS_DISTRO", "humble")
if ros_distro == "foxy":
return [
Node(
package="orbbec_camera",
executable="orbbec_camera_node",
name="ob_camera_node",
namespace=LaunchConfiguration("camera_name"),
parameters=params,
output="screen",
)
]
else:
attach_to_shared_component_container_arg = LaunchConfiguration('attach_to_shared_component_container', default=False)
component_container_name_arg = LaunchConfiguration('component_container_name', default='orbbec_container')
orbbec_container = Node(
name=component_container_name_arg,
package='rclcpp_components',
executable='component_container_mt',
output='screen',
condition=UnlessCondition(attach_to_shared_component_container_arg)
)
return [
orbbec_container,
LoadComposableNodes(
target_container=component_container_name_arg,
composable_node_descriptions=[
ComposableNode(
namespace=LaunchConfiguration("camera_name"),
name=LaunchConfiguration("camera_name"),
package='orbbec_camera',
plugin='orbbec_camera::OBCameraNodeDriver',
parameters=params,
extra_arguments=[{'use_intra_process_comms': LaunchConfiguration("use_intra_process_comms")}],
)
]
)
]
return LaunchDescription(
args + [
OpaqueFunction(function=lambda context: create_node_action(context, args))
]
)
orbbec_camera/tools/metadata_export_files.cpp
-8
View File
@@ -1,8 +0,0 @@
#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;
}
orbbec_camera/tools/metadata_export_files.hpp
-503
View File
@@ -1,503 +0,0 @@
#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 <opencv2/opencv.hpp>
#if defined(ROS_JAZZY) || defined(ROS_IRON)
#include <cv_bridge/cv_bridge.hpp>
#else
#include <cv_bridge/cv_bridge.h>
#endif
#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(
"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.");
return;
}
nlohmann::json json_data;
file >> json_data;
sn_ = json_data["metadata_export_params"]["sn"].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["metadata_export_params"]["left_ir_metadata_topic"].get<std::string>();
right_ir_metadata_topic_ =
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));
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(0, 100 * 1000000)); // 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(0, 100 * 1000000)); // 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(0, 100 * 1000000)); // 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(0, 100 * 1000000)); // 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
orbbec_camera/tools/metadata_save_files.cpp
-8
View File
@@ -1,8 +0,0 @@
#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;
}
orbbec_camera/tools/metadata_save_files.hpp
-507
View File
@@ -1,507 +0,0 @@
#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>
#if defined(ROS_JAZZY) || defined(ROS_IRON)
#include <cv_bridge/cv_bridge.hpp>
#else
#include <cv_bridge/cv_bridge.h>
#endif
#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") {
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();
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(0, 100 * 1000000)); // 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(0, 100 * 1000000)); // 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(0, 100 * 1000000)); // 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
orbbec_camera/tools/multi_save_rgbir.cpp
+33 -64
View File
@@ -44,13 +44,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
for (size_t i = 0; i < list->deviceCount(); i++) {
auto device = list->getDevice(i);
auto device_info = device->getDeviceInfo();
auto pid = device_info->getPid();
std::string serial = device_info->serialNumber();
std::string uid = device_info->uid();
auto usb_port = parseUsbPort(uid);
serial_numbers_[usb_port] = serial;
color_frame_counters_[count_] = 0;
ir_frame_counters_[count_] = 0;
count_++;
is_gemini330_ = isGemini335PID(pid);
}
} catch (ob::Error &e) {
RCLCPP_ERROR_STREAM(get_logger(), e.getMessage());
@@ -64,7 +63,6 @@ class MultiCameraSubscriber : public rclcpp::Node {
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);
@@ -81,6 +79,13 @@ class MultiCameraSubscriber : public rclcpp::Node {
private:
std::mutex image_mutex_;
std::mutex meta_mutex_;
bool isGemini335PID(uint32_t pid) {
return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
pid == CUSTOM_ADVANTECH_GEMINI_336L_PID;
}
void params_init() {
std::ifstream file(
"install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"
@@ -101,47 +106,14 @@ class MultiCameraSubscriber : public rclcpp::Node {
color_topics_.resize(camera_name_.size());
color_metadata_topic_.resize(camera_name_.size());
for (size_t i = 0; i < camera_name_.size(); ++i) {
left_ir_topics_[i] = "/" + camera_name_[i] + "/left_ir/image_raw";
left_ir_metadata_topic_[i] = "/" + camera_name_[i] + "/left_ir/metadata";
left_ir_topics_[i] =
"/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/image_raw";
left_ir_metadata_topic_[i] =
"/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/metadata";
color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
color_metadata_topic_[i] = "/" + camera_name_[i] + "/color/metadata";
}
}
std::string parseUsbPort(const std::string &line) {
std::string port_id;
std::regex usb_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*",
std::regex_constants::ECMAScript);
std::smatch base_match;
bool found_usb = std::regex_match(line, base_match, usb_regex);
if (found_usb) {
port_id = base_match[1].str();
std::cout << "USB port_id: " << port_id << std::endl;
if (base_match[2].str().empty()) {
std::regex end_regex(".+(-[0-9]+$)", std::regex_constants::ECMAScript);
bool found_end = std::regex_match(port_id, base_match, end_regex);
if (found_end) {
port_id = port_id.substr(0, port_id.size() - base_match[1].str().size());
std::cout << "Modified USB port_id: " << port_id << std::endl;
}
}
return port_id;
}
std::regex gmsl_regex("(gmsl[0-9]+)(?:-[0-9]+)*(-[0-9]+)$", std::regex_constants::ECMAScript);
bool found_gmsl = std::regex_match(line, base_match, gmsl_regex);
if (found_gmsl) {
port_id = base_match[1].str() + base_match[2].str();
std::cout << "Parsed GMSL Port ID: " << port_id << std::endl;
return port_id;
}
return "";
}
void topic_init() {
ir_image_buffers_.resize(left_ir_topics_.size());
color_image_buffers_.resize(left_ir_topics_.size());
@@ -155,6 +127,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
color_metadata_.gain_buffs.resize(left_ir_topics_.size());
callback_called_ = std::vector<bool>(left_ir_topics_.size(), false);
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"camera_name_.size(): " << camera_name_.size());
for (size_t i = 0; i < camera_name_.size(); ++i) {
@@ -166,7 +139,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
"color_topic: " << color_topics_[i]);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"color_metadata_topic_: " << color_metadata_topic_[i]);
reentrant_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions ir_sub_options;
ir_sub_options.callback_group = reentrant_callback_group_;
@@ -270,11 +243,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
for (size_t i = 0; i < static_cast<size_t>(saving_images_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) + time_domain_ +
ir_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
ir_timestamps[i] + "_e" + left_ir_meta_exposure[i] + "_d" +
left_ir_meta_gain[i] + "_.jpg";
std::string ir_filename =
folder + "/ir#left_SN" + serial_index + "_Index" + std::to_string(usb_index) +
time_domain_ + ir_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
ir_timestamps[i] +
(is_gemini330_ ? ("_e" + left_ir_meta_exposure[i] + "_d" + left_ir_meta_gain[i]) : "") +
"_.jpg";
if (ir_images[i].empty()) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
continue;
@@ -284,7 +258,9 @@ class MultiCameraSubscriber : public rclcpp::Node {
std::string color_filename =
folder + "/color_SN" + serial_index + "_Index" + std::to_string(usb_index) +
time_domain_ + color_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
color_timestamps[i] + "_e" + color_meta_exposure[i] + "_d" + color_meta_gain[i] + "_.jpg";
color_timestamps[i] +
(is_gemini330_ ? ("_e" + color_meta_exposure[i] + "_d" + color_meta_gain[i]) : "") +
"_.jpg";
if (color_images[i].empty()) {
continue;
}
@@ -336,15 +312,14 @@ class MultiCameraSubscriber : public rclcpp::Node {
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);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
":ir: " << index << ":" << ir_image_buffers_[index].size());
if (ir_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
color_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
color_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_) &&
left_ir_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_)) {
(!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_) &&
left_ir_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_)))) {
saveAlignedImages(index);
}
}
@@ -360,15 +335,14 @@ class MultiCameraSubscriber : public rclcpp::Node {
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);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
":color: " << index << ":" << color_image_buffers_[index].size());
if (ir_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
color_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
color_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_) &&
left_ir_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_)) {
(!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_) &&
left_ir_metadata_.exposure_buffs[index].size() >=
static_cast<size_t>(saving_images_number_)))) {
saveAlignedImages(index);
}
}
@@ -393,7 +367,6 @@ class MultiCameraSubscriber : public rclcpp::Node {
}
}
rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
ir_meta_subscribers_;
std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
@@ -405,8 +378,6 @@ class MultiCameraSubscriber : public rclcpp::Node {
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_;
std::vector<std::string> usb_params_;
std::vector<std::string> camera_name_;
@@ -425,14 +396,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
std::vector<bool> callback_called_;
std::string color_resolution_;
std::string ir_resolution_;
std::string currenttimes_;
size_t count_ = 0;
int saving_images_number_ = 100;
bool topic_init_ = false;
bool is_gemini330_ = true;
ImageMetadata left_ir_metadata_ = ImageMetadata();
ImageMetadata color_metadata_ = ImageMetadata();