i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 03:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'v2-main' into test_interleave_mode_opdk

Browse Source
This commit is contained in:
datean
2024-12-09 20:23:46 +08:00
parent 1b1653cfbc a8a4ee2ae3
commit 681d104822
19 changed files with 1976 additions and 3151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/config/OrbbecSDKConfig_v1.0.xml
-2979
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/config/OrbbecSDKConfig_v2.0.xml
+1605
View File
File diff suppressed because it is too large Load Diff
orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json
+14 -11
View File
@@ -1,23 +1,26 @@
{
"save_rgbir_params": {
"time_domain": "device",
"image_number": "100",
"usb_ports": [
"2-2",
"2-3.3",
"2-3.1",
"2-3",
"2-1"
],
"ir_topics": [
"/G0_51/left_ir/image_raw",
"/G1_54/left_ir/image_raw",
"/G2_5Y/left_ir/image_raw",
"/G3_47/left_ir/image_raw"
"/G330_0/left_ir/image_raw",
"/G330_1/left_ir/image_raw"
],
"left_ir_metadata_topic": [
"/G330_0/left_ir/metadata",
"/G330_1/left_ir/metadata"
],
"color_topics": [
"/G0_51/color/image_raw",
"/G1_54/color/image_raw",
"/G2_5Y/color/image_raw",
"/G3_47/color/image_raw"
"/G330_0/color/image_raw",
"/G330_1/color/image_raw"
],
"color_metadata_topic": [
"/G330_0/color/metadata",
"/G330_1/color/metadata"
]
}
}
orbbec_camera/launch/astra2.launch.py
+1 -1
View File
@@ -76,11 +76,11 @@ def generate_launch_description():
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='false'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('align_mode', default_value='HW'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument('time_domain', default_value='device'),
]
# Node configuration
orbbec_camera/launch/femto.launch.py
+1 -1
View File
@@ -78,10 +78,10 @@ def generate_launch_description():
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
DeclareLaunchArgument('enable_frame_sync', default_value='false'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='false'),
DeclareLaunchArgument('align_mode', default_value='HW'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument('time_domain', default_value='device'),
]
# Node configuration
orbbec_camera/launch/femto_bolt.launch.py
+1 -1
View File
@@ -84,11 +84,11 @@ def generate_launch_description():
DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='true'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('align_mode', default_value='SW'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument('time_domain', default_value='device'),
]
# Node configuration
orbbec_camera/launch/femto_mega.launch.py
+1 -1
View File
@@ -90,11 +90,11 @@ def generate_launch_description():
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='false'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('align_mode', default_value='HW'),
DeclareLaunchArgument('laser_energy_level', default_value='-1'),
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument('time_domain', default_value='device'),
]
# Node configuration
orbbec_camera/launch/gemini2.launch.py
+1 -1
View File
@@ -89,12 +89,12 @@ def generate_launch_description():
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='true'),
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='device'),
]
# Node configuration
orbbec_camera/launch/gemini2L.launch.py
+1 -1
View File
@@ -89,13 +89,13 @@ def generate_launch_description():
DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='true'),
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('enable_noise_removal_filter', default_value='false'),
DeclareLaunchArgument('time_domain', default_value='device'),
]
# Node configuration
orbbec_camera/launch/gemini_330_series.launch.py
+27 -1
View File
@@ -132,7 +132,6 @@ def generate_launch_description():
DeclareLaunchArgument('software_trigger_period', default_value='33'), # ms
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='true'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
@@ -175,6 +174,33 @@ def generate_launch_description():
DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
DeclareLaunchArgument('interleave_ae_mode', default_value='laser'), # 'hdr' or 'laser'
DeclareLaunchArgument('interleave_frame_enable', default_value='false'),
DeclareLaunchArgument('interleave_skip_enable', default_value='false'),
DeclareLaunchArgument('interleave_skip_index', default_value='1'), # 0:skip pattern ir 1: skip flood ir
DeclareLaunchArgument('hdr_index1_laser_control', default_value='1'),#interleave_hdr_param
DeclareLaunchArgument('hdr_index1_depth_exposure', default_value='1'),
DeclareLaunchArgument('hdr_index1_depth_gain', default_value='16'),
DeclareLaunchArgument('hdr_index1_ir_brightness', default_value='20'),
DeclareLaunchArgument('hdr_index1_ir_ae_max_exposure', default_value='2000'),
DeclareLaunchArgument('hdr_index0_laser_control', default_value='1'),
DeclareLaunchArgument('hdr_index0_depth_exposure', default_value='7500'),
DeclareLaunchArgument('hdr_index0_depth_gain', default_value='16'),
DeclareLaunchArgument('hdr_index0_ir_brightness', default_value='60'),
DeclareLaunchArgument('hdr_index0_ir_ae_max_exposure', default_value='10000'),
DeclareLaunchArgument('laser_index1_laser_control', default_value='0'),#interleave_laser_param
DeclareLaunchArgument('laser_index1_depth_exposure', default_value='3000'),
DeclareLaunchArgument('laser_index1_depth_gain', default_value='16'),
DeclareLaunchArgument('laser_index1_ir_brightness', default_value='60'),
DeclareLaunchArgument('laser_index1_ir_ae_max_exposure', default_value='17000'),
DeclareLaunchArgument('laser_index0_laser_control', default_value='1'),
DeclareLaunchArgument('laser_index0_depth_exposure', default_value='3000'),
DeclareLaunchArgument('laser_index0_depth_gain', default_value='16'),
DeclareLaunchArgument('laser_index0_ir_brightness', default_value='60'),
DeclareLaunchArgument('laser_index0_ir_ae_max_exposure', default_value='30000'),
]
def get_params(context, args):
orbbec_camera/launch/gemini_intra_process_demo_launch.py
-1
View File
@@ -128,7 +128,6 @@ def generate_launch_description():
DeclareLaunchArgument('software_trigger_period', default_value='33'), # ms
DeclareLaunchArgument('enable_frame_sync', default_value='true'),
DeclareLaunchArgument('ordered_pc', default_value='false'),
DeclareLaunchArgument('use_hardware_time', default_value='true'),
DeclareLaunchArgument('enable_depth_scale', default_value='true'),
DeclareLaunchArgument('enable_decimation_filter', default_value='false'),
DeclareLaunchArgument('enable_hdr_merge', default_value='false'),
orbbec_camera/launch/ob_camera.launch.py
+1
View File
@@ -66,6 +66,7 @@ def generate_launch_description():
DeclareLaunchArgument('enable_soft_filter', default_value='true'),
DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
DeclareLaunchArgument('time_domain', default_value='device'),
]
# Node configuration
orbbec_camera/src/ob_camera_node.cpp
+45 -52
View File
@@ -251,8 +251,7 @@ void OBCameraNode::setupDevices() {
RCLCPP_INFO_STREAM(logger_, "Set depth work mode: " << depth_work_mode_);
TRY_EXECUTE_BLOCK(device_->switchDepthWorkMode(depth_work_mode_.c_str()));
}
if (!sync_mode_str_.empty() && device_->isPropertySupported(OB_PROP_SYNC_SIGNAL_TRIGGER_OUT_BOOL,
OB_PERMISSION_READ_WRITE)) {
if (!sync_mode_str_.empty()) {
auto sync_config = device_->getMultiDeviceSyncConfig();
RCLCPP_INFO_STREAM(logger_,
"Current sync mode: " << magic_enum::enum_name(sync_config.syncMode));
@@ -329,12 +328,6 @@ void OBCameraNode::setupDevices() {
device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_noise_removal_filter_);
}
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(
logger_, "Setting color auto exposure to " << (enable_color_auto_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL,
enable_color_auto_exposure_);
}
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting color auto white balance to "
<< (enable_color_auto_white_balance_ ? "ON" : "OFF"));
@@ -343,7 +336,6 @@ void OBCameraNode::setupDevices() {
}
if (color_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_EXPOSURE_INT);
if (color_exposure_ < range.min || color_exposure_ > range.max) {
RCLCPP_ERROR(logger_, "color exposure value is out of range[%d,%d], please check the value",
@@ -355,7 +347,6 @@ void OBCameraNode::setupDevices() {
}
if (color_gain_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_GAIN_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_GAIN_INT);
if (color_gain_ < range.min || color_gain_ > range.max) {
RCLCPP_ERROR(logger_, "color gain value is out of range[%d,%d], please check the value",
@@ -365,6 +356,12 @@ void OBCameraNode::setupDevices() {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_GAIN_INT, color_gain_);
}
}
if (device_->isPropertySupported(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(
logger_, "Setting color auto exposure to " << (enable_color_auto_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_EXPOSURE_BOOL,
enable_color_auto_exposure_);
}
if (color_white_balance_ != -1 &&
device_->isPropertySupported(OB_PROP_COLOR_WHITE_BALANCE_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL, false);
@@ -402,14 +399,8 @@ void OBCameraNode::setupDevices() {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, ir_brightness_);
}
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_,
"Setting IR auto exposure to " << (enable_ir_auto_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
}
if (ir_exposure_ != -1 &&
device_->isPropertySupported(OB_PROP_IR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_IR_EXPOSURE_INT);
RCLCPP_ERROR(logger_, "ir exposure value is out of range[%d,%d], please check the value",
range.min, range.max);
@@ -422,7 +413,6 @@ void OBCameraNode::setupDevices() {
}
}
if (ir_gain_ != -1 && device_->isPropertySupported(OB_PROP_IR_GAIN_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_IR_GAIN_INT);
RCLCPP_ERROR(logger_, "ir gain value is out of range[%d,%d], please check the value", range.min,
range.max);
@@ -434,7 +424,11 @@ void OBCameraNode::setupDevices() {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_GAIN_INT, ir_gain_);
}
}
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_,
"Setting IR auto exposure to " << (enable_ir_auto_exposure_ ? "ON" : "OFF"));
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
}
if (device_->isPropertySupported(OB_PROP_IR_LONG_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_,
"Setting IR long exposure to " << (enable_ir_long_exposure_ ? "ON" : "OFF"));
@@ -442,28 +436,31 @@ void OBCameraNode::setupDevices() {
}
if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
auto default_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
RCLCPP_INFO_STREAM(logger_, "default_soft_filter_max_diff: " << default_soft_filter_max_diff);
if (soft_filter_max_diff_ != -1 && default_soft_filter_max_diff != soft_filter_max_diff_) {
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, soft_filter_max_diff_);
auto new_soft_filter_max_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
RCLCPP_INFO_STREAM(logger_, "after set soft_filter_max_diff: " << new_soft_filter_max_diff);
auto default_noise_removal_filter_min_diff =
device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
RCLCPP_INFO_STREAM(logger_, "default_noise_removal_filter_min_diff: "
<< default_noise_removal_filter_min_diff);
if (noise_removal_filter_min_diff_ != -1 &&
default_noise_removal_filter_min_diff != noise_removal_filter_min_diff_) {
device_->setIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT, noise_removal_filter_min_diff_);
auto new_noise_removal_filter_min_diff = device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
RCLCPP_INFO_STREAM(logger_, "after set noise_removal_filter_min_diff: "
<< new_noise_removal_filter_min_diff);
}
}
if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE)) {
auto default_soft_filter_speckle_size =
auto default_noise_removal_filter_max_size =
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
RCLCPP_INFO_STREAM(logger_,
"default_soft_filter_speckle_size: " << default_soft_filter_speckle_size);
if (soft_filter_speckle_size_ != -1 &&
default_soft_filter_speckle_size != soft_filter_speckle_size_) {
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT,
soft_filter_speckle_size_);
auto new_soft_filter_speckle_size =
RCLCPP_INFO_STREAM(logger_, "default_noise_removal_filter_max_size: "
<< default_noise_removal_filter_max_size);
if (noise_removal_filter_max_size_ != -1 &&
default_noise_removal_filter_max_size != noise_removal_filter_max_size_) {
device_->setIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, noise_removal_filter_max_size_);
auto new_noise_removal_filter_max_size =
device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
RCLCPP_INFO_STREAM(logger_,
"after set soft_filter_speckle_size: " << new_soft_filter_speckle_size);
RCLCPP_INFO_STREAM(logger_, "after set noise_removal_filter_max_size: "
<< new_noise_removal_filter_max_size);
}
}
}
@@ -789,7 +786,6 @@ void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) {
unit_step_size_[stream_index] = sizeof(uint16_t);
}
}
int OBCameraNode::init_interleave_hdr_param() {
device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 1);
device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, hdr_index1_laser_control_);
@@ -825,7 +821,6 @@ int OBCameraNode::init_interleave_laser_param() {
device_->setIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT, laser_index0_ir_ae_max_exposure_);
return 0;
}
void OBCameraNode::startStreams() {
if (pipeline_ != nullptr) {
pipeline_.reset();
@@ -834,20 +829,17 @@ void OBCameraNode::startStreams() {
try {
setupPipelineConfig();
if (interleave_frame_enable_) {
// set interleave mode
if (interleave_ae_mode_ == "hdr") {
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to hdr");
device_->loadFrameInterleave("hdr interleave");
init_interleave_hdr_param();
} else if (interleave_ae_mode_ == "laser") {
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to laser");
device_->loadFrameInterleave("laser interleave");
init_interleave_laser_param();
} else {
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to nothing");
}
// set interleave mode
if (interleave_ae_mode_ == "hdr" && interleave_frame_enable_) {
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to hdr");
device_->loadFrameInterleave("hdr interleave");
init_interleave_hdr_param();
} else if (interleave_ae_mode_ == "laser" && interleave_frame_enable_) {
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to laser");
device_->loadFrameInterleave("laser interleave");
init_interleave_laser_param();
} else {
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to nothing");
}
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
@@ -971,7 +963,6 @@ void OBCameraNode::stopStreams() {
}
try {
pipeline_->stop();
// disable interleave frame
if ((interleave_ae_mode_ == "hdr") || (interleave_ae_mode_ == "laser")) {
RCLCPP_INFO_STREAM(logger_, "current interleave_ae_mode_: " << interleave_ae_mode_);
@@ -983,7 +974,6 @@ void OBCameraNode::stopStreams() {
interleave_frame_enable_);
}
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
} catch (...) {
@@ -1286,6 +1276,9 @@ void OBCameraNode::getParameters() {
if (isOpenNIDevice(pid)) {
time_domain_ = "system";
}
if (time_domain_ == "global") {
device_->enableGlobalTimestamp(true);
}
RCLCPP_INFO_STREAM(logger_, "current time domain: " << time_domain_);
setAndGetNodeParameter<int>(frames_per_trigger_, "frames_per_trigger", 2);
long software_trigger_period = 33;
orbbec_camera/src/ob_camera_node_driver.cpp
+2 -2
View File
@@ -75,7 +75,7 @@ OBCameraNodeDriver::OBCameraNodeDriver(const rclcpp::NodeOptions &node_options)
: Node("orbbec_camera_node", "/", node_options),
node_options_(node_options),
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
"/config/OrbbecSDKConfig_v1.0.xml"),
"/config/OrbbecSDKConfig_v2.0.xml"),
logger_(this->get_logger()),
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
init();
@@ -86,7 +86,7 @@ OBCameraNodeDriver::OBCameraNodeDriver(const std::string &node_name, const std::
: Node(node_name, ns, node_options),
node_options_(node_options),
config_path_(ament_index_cpp::get_package_share_directory("orbbec_camera") +
"/config/OrbbecSDKConfig_v1.0.xml"),
"/config/OrbbecSDKConfig_v2.0.xml"),
logger_(this->get_logger()),
extension_path_(ament_index_cpp::get_package_prefix("orbbec_camera") + "/lib/extensions") {
init();
orbbec_camera/src/ros_service.cpp
+1 -1
View File
@@ -479,7 +479,6 @@ void OBCameraNode::setLaserEnableCallback(
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
(void)request_header;
(void)response;
auto device_info = device_->getDeviceInfo();
int laser_enable = request->data ? 1 : 0;
try {
if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
@@ -812,6 +811,7 @@ void OBCameraNode::setRESETTimestampCallback(
(void)request;
try {
device_->setBoolProperty(OB_PROP_TIMER_RESET_TRIGGER_OUT_ENABLE_BOOL, true);
device_->setBoolProperty(OB_PROP_TIMER_RESET_SIGNAL_BOOL, true);
response->success = true;
} catch (const ob::Error& e) {
response->message = e.getMessage();
orbbec_camera/tools/multi_save_rgbir_node.cpp
+1 -1
View File
@@ -17,7 +17,7 @@
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<MultiCameraSubscriber>();
auto node = std::make_shared<orbbec_camera::tools::MultiCameraSubscriber>();
rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20);
executor.add_node(node);
executor.spin();
orbbec_camera/tools/multi_save_rgbir_node.hpp
+89 -27
View File
@@ -3,11 +3,18 @@
#include <orbbec_camera/ob_camera_node_driver.h>
#include <orbbec_camera/utils.h>
#include "orbbec_camera_msgs/msg/metadata.hpp"
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <std_msgs/msg/bool.hpp>
#include <filesystem>
namespace orbbec_camera {
namespace tools {
struct ImageMetadata {
std::vector<std::vector<std::string>> exposure_buffs;
std::vector<std::vector<std::string>> gain_buffs;
};
class MultiCameraSubscriber : public rclcpp::Node {
public:
@@ -61,7 +68,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
}
private:
std::mutex buffer_mutex_;
std::mutex image_mutex_;
std::mutex meta_mutex_;
void params_init() {
std::ifstream file(
"install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"
@@ -72,10 +80,16 @@ class MultiCameraSubscriber : public rclcpp::Node {
}
nlohmann::json json_data;
file >> json_data;
time_domain_= json_data["save_rgbir_params"]["time_domain"].get<std::string>();
time_domain_ =(time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown");
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>>();
left_ir_metadata_topic_ =
json_data["save_rgbir_params"]["left_ir_metadata_topic"].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>>();
color_metadata_topic_ =
json_data["save_rgbir_params"]["color_metadata_topic"].get<std::vector<std::string>>();
}
void topic_init() {
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
@@ -84,8 +98,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
for (size_t i = 0; i < ir_topics_.size(); ++i) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"ir_topic: " << ir_topics_[i]);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"left_ir_metadata_topic_: " << left_ir_metadata_topic_[i]);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"color_topic: " << color_topics_[i]);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"color_metadata_topic_: " << color_metadata_topic_[i]);
rclcpp::SubscriptionOptions ir_sub_options;
ir_sub_options.callback_group = reentrant_callback_group_;
@@ -100,6 +118,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
},
ir_sub_options);
auto ir_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
left_ir_metadata_topic_[i], custom_qos,
[this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
this->ir_meta_Callback(msg, i);
});
auto color_sub = this->create_subscription<sensor_msgs::msg::Image>(
color_topics_[i], custom_qos,
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
@@ -107,8 +131,16 @@ class MultiCameraSubscriber : public rclcpp::Node {
},
color_sub_options);
auto color_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
color_metadata_topic_[i], custom_qos,
[this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
this->color_meta_Callback(msg, i);
});
ir_subscribers_.push_back(ir_sub);
ir_meta_subscribers_.push_back(ir_metadata_sub);
color_subscribers_.push_back(color_sub);
color_meta_subscribers_.push_back(color_metadata_sub);
ir_image_buffers_.resize(ir_topics_.size());
color_image_buffers_.resize(ir_topics_.size());
@@ -116,6 +148,10 @@ class MultiCameraSubscriber : public rclcpp::Node {
color_current_timestamp_buffers_.resize(ir_topics_.size());
ir_timestamp_buffers_.resize(ir_topics_.size());
color_timestamp_buffers_.resize(ir_topics_.size());
left_ir_metadata_.exposure_buffs.resize(ir_topics_.size());
left_ir_metadata_.gain_buffs.resize(ir_topics_.size());
color_metadata_.exposure_buffs.resize(ir_topics_.size());
color_metadata_.gain_buffs.resize(ir_topics_.size());
callback_called_ = std::vector<bool>(ir_topics_.size(), false);
}
}
@@ -163,17 +199,21 @@ class MultiCameraSubscriber : public rclcpp::Node {
auto &color_images = color_image_buffers_[index];
auto &color_current_timestamps = color_current_timestamp_buffers_[index];
auto &color_timestamps = color_timestamp_buffers_[index];
auto &left_ir_meta_exposure = left_ir_metadata_.exposure_buffs[index];
auto &left_ir_meta_gain = left_ir_metadata_.gain_buffs[index];
auto &color_meta_exposure = color_metadata_.exposure_buffs[index];
auto &color_meta_gain = color_metadata_.gain_buffs[index];
callback_called_[index] = true;
if (ir_images.size() < static_cast<size_t>(std::stoi(image_number_)) ||
color_images.size() < static_cast<size_t>(std::stoi(image_number_))) {
return;
}
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjjj1");
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "index:"<<index);
auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
int usb_index = usb_iter->second;
if (serial_iter == serial_numbers_.end()) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjjj2");
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "serial_iter is empty");
return;
}
std::string serial_index = serial_iter->second;
@@ -181,46 +221,42 @@ class MultiCameraSubscriber : public rclcpp::Node {
for (size_t i = 0; i < static_cast<size_t>(std::stoi(image_number_)); i++) {
std::string folder = generateFolderName(serial_index, usb_index);
std::string ir_filename = folder + "/ir#left_SN" + serial_index + "_Index" +
std::to_string(usb_index) + "_d" + ir_current_timestamps[i] + "_f" +
std::to_string(i) + "_s" + ir_timestamps[i] + "_.jpg";
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] + "_g" + left_ir_meta_gain[i] + "_.jpg";
if (ir_images[i].empty()) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "over ");
// rclcpp::shutdown();
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
continue;
}
cv::imwrite(ir_filename, ir_images[i]);
// RCLCPP_INFO(this->get_logger(), "Saved IR image to: %s", ir_filename.c_str());
std::string color_filename = folder + "/color_SN" + serial_index + "_Index" +
std::to_string(usb_index) + "_d" + color_current_timestamps[i] +
"_f" + std::to_string(i) + "_s" + color_timestamps[i] + "_.jpg";
if (ir_images[i].empty()) {
// rclcpp::shutdown();
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] + "_g" + color_meta_gain[i] +"_.jpg";
if (color_images[i].empty()) {
continue;
}
cv::imwrite(color_filename, color_images[i]);
// RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str());
}
ir_images.clear();
color_images.clear();
ir_current_timestamps.clear();
color_current_timestamps.clear();
ir_timestamps.clear();
color_timestamps.clear();
ir_image_buffers_[index].clear();
ir_current_timestamp_buffers_[index].clear();
ir_timestamp_buffers_[index].clear();
color_image_buffers_[index].clear();
color_current_timestamp_buffers_[index].clear();
color_timestamp_buffers_[index].clear();
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"),
left_ir_metadata_.exposure_buffs[index].clear();
left_ir_metadata_.gain_buffs[index].clear();
color_metadata_.exposure_buffs[index].clear();
color_metadata_.gain_buffs[index].clear();
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
"callback_called_ " << index << ":" << callback_called_[index]);
bool all_true =
std::all_of(callback_called_.begin(), callback_called_.end(), [](bool v) { return v; });
if (all_true) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "over ");
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
ir_image_buffers_.clear();
ir_current_timestamp_buffers_.clear();
ir_timestamp_buffers_.clear();
@@ -234,10 +270,10 @@ class MultiCameraSubscriber : public rclcpp::Node {
void controlCaptureCallback(const std_msgs::msg::Bool::SharedPtr msg) {
is_saving_images_ = msg->data;
topic_init();
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "jjjj " << is_saving_images_);
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "jjjj " << is_saving_images_);
}
void irCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
std::lock_guard<std::mutex> lock(buffer_mutex_);
std::lock_guard<std::mutex> lock(image_mutex_);
if (!callback_called_[index] && is_saving_images_) {
cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
std::string current_timestamp_ir = getCurrentTimestamp(image);
@@ -246,7 +282,7 @@ 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"),
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>(std::stoi(image_number_)) &&
color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
@@ -254,9 +290,8 @@ class MultiCameraSubscriber : public rclcpp::Node {
}
}
}
void colorCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
std::lock_guard<std::mutex> lock(buffer_mutex_);
std::lock_guard<std::mutex> lock(image_mutex_);
if (!callback_called_[index] && is_saving_images_) {
cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
cv::Mat corrected_image;
@@ -267,7 +302,7 @@ 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"),
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>(std::stoi(image_number_)) &&
color_image_buffers_[index].size() >= static_cast<size_t>(std::stoi(image_number_))) {
@@ -276,7 +311,26 @@ class MultiCameraSubscriber : public rclcpp::Node {
}
}
void ir_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
size_t index) {
std::lock_guard<std::mutex> lock(meta_mutex_);
nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
left_ir_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
left_ir_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
}
void color_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
size_t index) {
std::lock_guard<std::mutex> lock(meta_mutex_);
nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
color_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
color_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
}
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>
color_meta_subscribers_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> ir_subscribers_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subscribers_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr capture_control_sub_;
@@ -289,9 +343,12 @@ class MultiCameraSubscriber : public rclcpp::Node {
size_t count = 0;
std::vector<std::string> usb_params_;
std::vector<std::string> left_ir_metadata_topic_;
std::vector<std::string> color_metadata_topic_;
std::vector<std::string> ir_topics_;
std::vector<std::string> color_topics_;
std::string image_number_;
std::string time_domain_;
std::vector<std::vector<cv::Mat>> ir_image_buffers_;
std::vector<std::vector<cv::Mat>> color_image_buffers_;
@@ -307,4 +364,9 @@ class MultiCameraSubscriber : public rclcpp::Node {
std::string currenttimes_;
bool is_saving_images_ = false;
ImageMetadata left_ir_metadata_ = ImageMetadata();
ImageMetadata color_metadata_ = ImageMetadata();
};
} // namespace tools
} // namespace orbbec_camera