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

only set the interleave param when interleave_frame_enable is enabled

Browse Source
This commit is contained in:
datean
2024-12-05 19:56:43 +08:00
parent ee7f5b866b
commit f1134728f2
5 changed files with 61 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/config/camera_params.yaml
+7 -6
View File
@@ -4,10 +4,11 @@ enable_point_cloud: false
enable_colored_point_cloud: false enable_colored_point_cloud: false
device_preset: "Default" device_preset: "Default"
laser_on_off_mode: 0 # 0: off, 1: on-off, 1: off-on laser_on_off_mode: 0 # 0: off, 1: on-off, 1: off-on
time_domain: "device" # global, device, system time_domain: "global" # global, device, system
enable_sync_host_time: true enable_sync_host_time: true
frames_per_trigger: 1 frames_per_trigger: 1
log_level: "warning" log_level: "warning"
enable_laser: false
# When 3D reconstruction mode is enabled: # When 3D reconstruction mode is enabled:
# - The laser will switch to on-off mode # - The laser will switch to on-off mode
@@ -19,7 +20,7 @@ enable_3d_reconstruction_mode: false
enable_color: true enable_color: true
color_width: 640 color_width: 640
color_height: 480 color_height: 480
color_fps: 60 color_fps: 30
color_format: "YUYV" color_format: "YUYV"
enable_color_auto_exposure: false enable_color_auto_exposure: false
color_exposure: 50 # 5ms color_exposure: 50 # 5ms
@@ -29,20 +30,20 @@ color_qos: "sensor_data"
# depth params # depth params
depth_width: 640 depth_width: 640
depth_height: 480 depth_height: 480
depth_fps: 60 depth_fps: 30
depth_format: "Y16" depth_format: "Y16"
depth_qos: "sensor_data" depth_qos: "sensor_data"
# ir exposure # ir exposure
enable_ir_auto_exposure: false enable_ir_auto_exposure: false
ir_exposure: 15000 # 5ms ir_exposure: 5000 # 5ms
ir_gain: 40 ir_gain: 40
#left ir params #left ir params
enable_left_ir: true enable_left_ir: true
left_ir_width: 640 left_ir_width: 640
left_ir_height: 480 left_ir_height: 480
left_ir_fps: 60 left_ir_fps: 30
left_ir_format: "Y8" left_ir_format: "Y8"
left_ir_qos: "sensor_data" left_ir_qos: "sensor_data"
@@ -50,6 +51,6 @@ left_ir_qos: "sensor_data"
enable_right_ir: true enable_right_ir: true
right_ir_width: 640 right_ir_width: 640
right_ir_height: 480 right_ir_height: 480
right_ir_fps: 60 right_ir_fps: 30
right_ir_format: "Y8" right_ir_format: "Y8"
right_ir_qos: "sensor_data" right_ir_qos: "sensor_data"
orbbec_camera/launch/gemini_330_series_interleave_laser_g335L.launch.py
+3 -3
View File
@@ -182,9 +182,9 @@ def generate_launch_description():
DeclareLaunchArgument('config_file_path', default_value=''), DeclareLaunchArgument('config_file_path', default_value=''),
DeclareLaunchArgument('enable_heartbeat', default_value='false'), DeclareLaunchArgument('enable_heartbeat', default_value='false'),
DeclareLaunchArgument('enable_hardware_reset', default_value='false'), DeclareLaunchArgument('enable_hardware_reset', default_value='false'),
DeclareLaunchArgument('interleave_ae_mode', default_value='laser'), # 'hdr' or 'laser' DeclareLaunchArgument('interleave_ae_mode', default_value='hdr'), # 'hdr' or 'laser'
DeclareLaunchArgument('interleave_frame_enable', default_value='true'), DeclareLaunchArgument('interleave_frame_enable', default_value='false'),
DeclareLaunchArgument('interleave_skip_enable', default_value='true'), DeclareLaunchArgument('interleave_skip_enable', default_value='false'),
DeclareLaunchArgument('interleave_skip_index', default_value='0'), # 0:skip pattern ir 1: skip flood ir DeclareLaunchArgument('interleave_skip_index', default_value='0'), # 0:skip pattern ir 1: skip flood ir
DeclareLaunchArgument('use_intra_process_comms', default_value='false'), DeclareLaunchArgument('use_intra_process_comms', default_value='false'),
DeclareLaunchArgument('attach_to_shared_component_container', default_value='false'), DeclareLaunchArgument('attach_to_shared_component_container', default_value='false'),
orbbec_camera/launch/multi_camera_synced.launch.py
+12 -12
View File
@@ -45,17 +45,17 @@ def generate_launch_description():
), ),
launch_arguments={ launch_arguments={
"camera_name": "front_camera", "camera_name": "front_camera",
# "usb_port": "2-1", "usb_port": "2-1",
"usb_port": "gmsl2-4", # "usb_port": "gmsl2-4",
"device_num": "4", "device_num": "4",
# "sync_mode": "primary", "sync_mode": "primary",
"sync_mode": "secondary_synced", # "sync_mode": "secondary_synced",
"config_file_path": config_file_path, "config_file_path": config_file_path,
'use_intra_process_comms': LaunchConfiguration("use_intra_process_comms"), 'use_intra_process_comms': LaunchConfiguration("use_intra_process_comms"),
'attach_to_shared_component_container': attach_to_shared_component_container_arg, 'attach_to_shared_component_container': attach_to_shared_component_container_arg,
'component_container_name': component_container_name_arg, 'component_container_name': component_container_name_arg,
'gmsl_trigger_fps': "5990", # 'gmsl_trigger_fps': "5990",
'enable_gmsl_trigger': "true", # 'enable_gmsl_trigger': "true",
}.items(), }.items(),
) )
left_camera = IncludeLaunchDescription( left_camera = IncludeLaunchDescription(
@@ -64,8 +64,8 @@ def generate_launch_description():
), ),
launch_arguments={ launch_arguments={
"camera_name": "left_camera", "camera_name": "left_camera",
# "usb_port": "2-3.1", "usb_port": "2-3.1",
"usb_port": "gmsl2-3", # "usb_port": "gmsl2-3",
"device_num": "4", "device_num": "4",
"sync_mode": "secondary_synced", "sync_mode": "secondary_synced",
"config_file_path": config_file_path, "config_file_path": config_file_path,
@@ -80,8 +80,8 @@ def generate_launch_description():
), ),
launch_arguments={ launch_arguments={
"camera_name": "right_camera", "camera_name": "right_camera",
# "usb_port": "2-3.3", "usb_port": "2-3.3",
"usb_port": "gmsl2-1", # "usb_port": "gmsl2-1",
"device_num": "4", "device_num": "4",
"sync_mode": "secondary_synced", "sync_mode": "secondary_synced",
"config_file_path": config_file_path, "config_file_path": config_file_path,
@@ -96,8 +96,8 @@ def generate_launch_description():
), ),
launch_arguments={ launch_arguments={
"camera_name": "rear_camera", "camera_name": "rear_camera",
# "usb_port": "2-2", "usb_port": "2-2",
"usb_port": "gmsl2-7", # "usb_port": "gmsl2-7",
"device_num": "4", "device_num": "4",
"sync_mode": "secondary_synced", "sync_mode": "secondary_synced",
"config_file_path": config_file_path, "config_file_path": config_file_path,
orbbec_camera/src/ob_camera_node.cpp
+35 -16
View File
@@ -218,9 +218,13 @@ void OBCameraNode::setupDevices() {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_LDP_BOOL, enable_ldp_);
} }
if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) { if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting laser control to " << enable_laser_); RCLCPP_INFO_STREAM(logger_, "Setting G300 laser control to " << enable_laser_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, enable_laser_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, enable_laser_);
} }
if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting laser control to " << enable_laser_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, enable_laser_);
}
if (device_->isPropertySupported(OB_PROP_LASER_ON_OFF_MODE_INT, OB_PERMISSION_READ_WRITE)) { if (device_->isPropertySupported(OB_PROP_LASER_ON_OFF_MODE_INT, OB_PERMISSION_READ_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting laser on off mode to " << laser_on_off_mode_); RCLCPP_INFO_STREAM(logger_, "Setting laser on off mode to " << laser_on_off_mode_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_ON_OFF_MODE_INT, laser_on_off_mode_); TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_ON_OFF_MODE_INT, laser_on_off_mode_);
@@ -407,6 +411,8 @@ void OBCameraNode::setupDevices() {
device_->isPropertySupported(OB_PROP_IR_EXPOSURE_INT, OB_PERMISSION_WRITE)) { device_->isPropertySupported(OB_PROP_IR_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_IR_EXPOSURE_INT); 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);
if (ir_exposure_ < range.min || ir_exposure_ > range.max) { if (ir_exposure_ < range.min || ir_exposure_ > range.max) {
RCLCPP_ERROR(logger_, "ir exposure value is out of range[%d,%d], please check the value", RCLCPP_ERROR(logger_, "ir exposure value is out of range[%d,%d], please check the value",
range.min, range.max); range.min, range.max);
@@ -418,6 +424,8 @@ void OBCameraNode::setupDevices() {
if (ir_gain_ != -1 && device_->isPropertySupported(OB_PROP_IR_GAIN_INT, OB_PERMISSION_WRITE)) { 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); TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, false);
auto range = device_->getIntPropertyRange(OB_PROP_IR_GAIN_INT); 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);
if (ir_gain_ < range.min || ir_gain_ > range.max) { if (ir_gain_ < range.min || ir_gain_ > range.max) {
RCLCPP_ERROR(logger_, "ir gain value is out of range[%d,%d], please check the value", RCLCPP_ERROR(logger_, "ir gain value is out of range[%d,%d], please check the value",
range.min, range.max); range.min, range.max);
@@ -828,22 +836,33 @@ void OBCameraNode::startStreams() {
try { try {
setupPipelineConfig(); setupPipelineConfig();
// set interleave mode if (interleave_frame_enable_) {
if (interleave_ae_mode_ == "hdr") { // set interleave mode
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to hdr"); if (interleave_ae_mode_ == "hdr") {
device_->loadFrameInterleave("hdr interleave"); RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to hdr");
init_interleave_hdr_param(); device_->loadFrameInterleave("hdr interleave");
} else if (interleave_ae_mode_ == "laser") { init_interleave_hdr_param();
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to laser"); } else if (interleave_ae_mode_ == "laser") {
device_->loadFrameInterleave("laser interleave"); RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to laser");
init_interleave_laser_param(); device_->loadFrameInterleave("laser interleave");
} else { init_interleave_laser_param();
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to nothing"); } else {
RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to nothing");
}
} }
pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) { pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
onNewFrameSetCallback(frame_set); onNewFrameSetCallback(frame_set);
}); });
// if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
// RCLCPP_INFO_STREAM(logger_, "Setting G300 laser control to " << enable_laser_);
// TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_CONTROL_INT, enable_laser_);
// }
// if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
// RCLCPP_INFO_STREAM(logger_, "Setting laser control to " << enable_laser_);
// TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, enable_laser_);
// }
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << e.getMessage()); RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << e.getMessage());
RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again"); RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again");
@@ -2065,7 +2084,7 @@ void OBCameraNode::updateStreamInfo(const std::shared_ptr<ob::Frame> &frame,
return; return;
} }
if (interleave_skip_enable_) { if (interleave_frame_enable_ && interleave_skip_enable_) {
dst_fps = fps_[dst_frame_type] / 2; dst_fps = fps_[dst_frame_type] / 2;
} else { } else {
dst_fps = fps_[dst_frame_type]; dst_fps = fps_[dst_frame_type];
@@ -2130,7 +2149,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
std::shared_ptr<ob::VideoFrame> video_frame; std::shared_ptr<ob::VideoFrame> video_frame;
if (frame->getType() == OB_FRAME_COLOR) { if (frame->getType() == OB_FRAME_COLOR) {
// updateStreamInfo(frame, color_stream_info_); // updateStreamInfo(frame, color_stream_info_);
if (interleave_skip_enable_) { if (interleave_frame_enable_ && interleave_skip_enable_) {
interleave_skip_color_index_++; interleave_skip_color_index_++;
RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_color_index_: %d", RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_color_index_: %d",
interleave_skip_color_index_); interleave_skip_color_index_);
@@ -2143,7 +2162,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
video_frame = frame->as<ob::ColorFrame>(); video_frame = frame->as<ob::ColorFrame>();
} else if (frame->getType() == OB_FRAME_DEPTH) { } else if (frame->getType() == OB_FRAME_DEPTH) {
// updateStreamInfo(frame, depth_stream_info_); // updateStreamInfo(frame, depth_stream_info_);
if (interleave_skip_enable_) { if (interleave_frame_enable_ && interleave_skip_enable_) {
interleave_skip_depth_index_++; interleave_skip_depth_index_++;
RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d", RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d",
interleave_skip_depth_index_); interleave_skip_depth_index_);
@@ -2159,7 +2178,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
video_frame = frame->as<ob::IRFrame>(); video_frame = frame->as<ob::IRFrame>();
// interleave filter speckle or flood ir // interleave filter speckle or flood ir
if (interleave_skip_enable_) { if (interleave_frame_enable_ && interleave_skip_enable_) {
RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d", RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d",
interleave_skip_index_); interleave_skip_index_);
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) == if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) ==
orbbec_camera/src/ros_service.cpp
+4 -5
View File
@@ -184,9 +184,9 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetInt32::Response> response) { std::shared_ptr<SetInt32::Response> response) {
setSYNCInterleaveLaserCallback(request, response); setSYNCInterleaveLaserCallback(request, response);
}); });
set_sync_host_time_srv_ = node_->create_service<SetBool>( set_sync_host_time_srv_ = node_->create_service<SetBool>(
"set_sync_hosttime", [this](const std::shared_ptr<SetBool::Request> request, "set_sync_hosttime", [this](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) { std::shared_ptr<SetBool::Response> response) {
setSYNCHostimeCallback(request, response); setSYNCHostimeCallback(request, response);
}); });
} }
@@ -480,12 +480,11 @@ void OBCameraNode::setLaserEnableCallback(
(void)request_header; (void)request_header;
(void)response; (void)response;
auto device_info = device_->getDeviceInfo(); auto device_info = device_->getDeviceInfo();
auto pid = device_info->getPid();
int laser_enable = request->data ? 1 : 0; int laser_enable = request->data ? 1 : 0;
try { try {
if (isGemini335PID(pid)) { if (device_->isPropertySupported(OB_PROP_LASER_CONTROL_INT, OB_PERMISSION_READ_WRITE)) {
device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, laser_enable); device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, laser_enable);
} else { } else if (device_->isPropertySupported(OB_PROP_LASER_BOOL, OB_PERMISSION_READ_WRITE)) {
device_->setIntProperty(OB_PROP_LASER_BOOL, laser_enable); device_->setIntProperty(OB_PROP_LASER_BOOL, laser_enable);
} }
response->success = true; response->success = true;