From 0a4c931b06844f098197f202977186c2e323a113 Mon Sep 17 00:00:00 2001 From: jj <957713278@qq.com> Date: Mon, 31 Mar 2025 15:16:43 +0800 Subject: [PATCH] =?UTF-8?q?Update=20gemini2.launch.py=20=E2=80=8B=E2=80=8B?= =?UTF-8?q?to=20be=20compatible=20with=20Gemini=20210=20and=20Gemini=20215?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- orbbec_camera/launch/gemini2.launch.py | 24 ++--- orbbec_camera/src/ob_camera_node.cpp | 118 ++++++++++++++----------- 2 files changed, 78 insertions(+), 64 deletions(-) diff --git a/orbbec_camera/launch/gemini2.launch.py b/orbbec_camera/launch/gemini2.launch.py index a3783d6e..abe9c4c7 100644 --- a/orbbec_camera/launch/gemini2.launch.py +++ b/orbbec_camera/launch/gemini2.launch.py @@ -24,10 +24,10 @@ def generate_launch_description(): 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='640'), - DeclareLaunchArgument('color_height', default_value='360'), - DeclareLaunchArgument('color_fps', default_value='15'), - DeclareLaunchArgument('color_format', default_value='MJPG'), + 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'), @@ -39,18 +39,18 @@ def generate_launch_description(): 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='640'), - DeclareLaunchArgument('depth_height', default_value='400'), - DeclareLaunchArgument('depth_fps', default_value='15'), - DeclareLaunchArgument('depth_format', default_value='Y14'), + 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('ir_width', default_value='640'), - DeclareLaunchArgument('ir_height', default_value='400'), - DeclareLaunchArgument('ir_fps', default_value='15'), - DeclareLaunchArgument('ir_format', default_value='Y8'), + 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'), diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index dd7abaec..ba7dd671 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -164,6 +164,7 @@ void OBCameraNode::setupDevices() { } } auto info = device_->getDeviceInfo(); + if (retry_on_usb3_detection_failure_ && device_->isPropertySupported(OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL, OB_PERMISSION_READ_WRITE)) { @@ -675,60 +676,70 @@ void OBCameraNode::setupColorPostProcessFilter() { } } void OBCameraNode::setupLeftIrPostProcessFilter() { - auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT); - left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters(); - if (left_ir_filter_list_.empty()) { - RCLCPP_WARN_STREAM(logger_, "Failed to get left ir sensor filter list"); - return; - } - for (size_t i = 0; i < left_ir_filter_list_.size(); i++) { - auto filter = left_ir_filter_list_[i]; - std::map filter_params = { - {"SequenceIdFilter", enable_left_ir_sequence_id_filter_}, - }; - std::string filter_name = filter->type(); - RCLCPP_INFO_STREAM(logger_, "Setting " << filter_name << "......"); - if (filter_params.find(filter_name) != filter_params.end()) { - std::string value = filter_params[filter_name] ? "true" : "false"; - RCLCPP_INFO_STREAM(logger_, "set left ir " << filter_name << " to " << value); - filter->enable(filter_params[filter_name]); + auto device_info = device_->getDeviceInfo(); + CHECK_NOTNULL(device_info); + auto pid = device_info->getPid(); + if (isGemini335PID(pid)) { + auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT); + left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters(); + if (left_ir_filter_list_.empty()) { + RCLCPP_WARN_STREAM(logger_, "Failed to get left ir sensor filter list"); + return; } - if (filter_name == "SequenceIdFilter" && enable_left_ir_sequence_id_filter_) { - auto sequenced_filter = filter->as(); - if (left_ir_sequence_id_filter_id_ != -1) { - sequenced_filter->selectSequenceId(left_ir_sequence_id_filter_id_); - RCLCPP_INFO_STREAM(logger_, - "Set left ir SequenceIdFilter ID to " << left_ir_sequence_id_filter_id_); + for (size_t i = 0; i < left_ir_filter_list_.size(); i++) { + auto filter = left_ir_filter_list_[i]; + std::map filter_params = { + {"SequenceIdFilter", enable_left_ir_sequence_id_filter_}, + }; + std::string filter_name = filter->type(); + RCLCPP_INFO_STREAM(logger_, "Setting " << filter_name << "......"); + if (filter_params.find(filter_name) != filter_params.end()) { + std::string value = filter_params[filter_name] ? "true" : "false"; + RCLCPP_INFO_STREAM(logger_, "set left ir " << filter_name << " to " << value); + filter->enable(filter_params[filter_name]); + } + if (filter_name == "SequenceIdFilter" && enable_left_ir_sequence_id_filter_) { + auto sequenced_filter = filter->as(); + if (left_ir_sequence_id_filter_id_ != -1) { + sequenced_filter->selectSequenceId(left_ir_sequence_id_filter_id_); + RCLCPP_INFO_STREAM( + logger_, "Set left ir SequenceIdFilter ID to " << left_ir_sequence_id_filter_id_); + } } } } } void OBCameraNode::setupRightIrPostProcessFilter() { - auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT); - right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters(); - if (right_ir_filter_list_.empty()) { - RCLCPP_WARN_STREAM(logger_, "Failed to get right ir sensor filter list"); - return; - } - for (size_t i = 0; i < right_ir_filter_list_.size(); i++) { - auto filter = right_ir_filter_list_[i]; - std::map filter_params = { - {"SequenceIdFilter", enable_right_ir_sequence_id_filter_}, - }; - std::string filter_name = filter->type(); - RCLCPP_INFO_STREAM(logger_, "Setting " << filter_name << "......"); - if (filter_params.find(filter_name) != filter_params.end()) { - std::string value = filter_params[filter_name] ? "true" : "false"; - RCLCPP_INFO_STREAM(logger_, "set right ir " << filter_name << " to " << value); - filter->enable(filter_params[filter_name]); + auto device_info = device_->getDeviceInfo(); + CHECK_NOTNULL(device_info); + auto pid = device_info->getPid(); + if (isGemini335PID(pid)) { + auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT); + right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters(); + if (right_ir_filter_list_.empty()) { + RCLCPP_WARN_STREAM(logger_, "Failed to get right ir sensor filter list"); + return; } - if (filter_name == "SequenceIdFilter" && enable_right_ir_sequence_id_filter_) { - auto sequenced_filter = filter->as(); - if (right_ir_sequence_id_filter_id_ != -1) { - sequenced_filter->selectSequenceId(right_ir_sequence_id_filter_id_); - RCLCPP_INFO_STREAM( - logger_, "Set right ir SequenceIdFilter ID to " << right_ir_sequence_id_filter_id_); + for (size_t i = 0; i < right_ir_filter_list_.size(); i++) { + auto filter = right_ir_filter_list_[i]; + std::map filter_params = { + {"SequenceIdFilter", enable_right_ir_sequence_id_filter_}, + }; + std::string filter_name = filter->type(); + RCLCPP_INFO_STREAM(logger_, "Setting " << filter_name << "......"); + if (filter_params.find(filter_name) != filter_params.end()) { + std::string value = filter_params[filter_name] ? "true" : "false"; + RCLCPP_INFO_STREAM(logger_, "set right ir " << filter_name << " to " << value); + filter->enable(filter_params[filter_name]); + } + if (filter_name == "SequenceIdFilter" && enable_right_ir_sequence_id_filter_) { + auto sequenced_filter = filter->as(); + if (right_ir_sequence_id_filter_id_ != -1) { + sequenced_filter->selectSequenceId(right_ir_sequence_id_filter_id_); + RCLCPP_INFO_STREAM( + logger_, "Set right ir SequenceIdFilter ID to " << right_ir_sequence_id_filter_id_); + } } } } @@ -2331,6 +2342,9 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set publishStaticTransforms(); tf_published_ = true; } + auto device_info = device_->getDeviceInfo(); + CHECK_NOTNULL(device_info); + auto pid = device_info->getPid(); auto depth_frame = frame_set->getFrame(OB_FRAME_DEPTH); auto color_frame = frame_set->getFrame(OB_FRAME_COLOR); auto left_ir_frame = frame_set->getFrame(OB_FRAME_IR_LEFT); @@ -2346,13 +2360,13 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr frame_set color_frame = processColorFrameFilter(color_frame); frame_set->pushFrame(color_frame); } - if (left_ir_frame) { - left_ir_frame=processLeftIrFrameFilter(left_ir_frame); - frame_set->pushFrame(left_ir_frame); + if (left_ir_frame && isGemini335PID(pid)) { + left_ir_frame = processLeftIrFrameFilter(left_ir_frame); + frame_set->pushFrame(left_ir_frame); } - if (right_ir_frame) { - right_ir_frame=processRightIrFrameFilter(right_ir_frame); - frame_set->pushFrame(right_ir_frame); + if (right_ir_frame && isGemini335PID(pid)) { + right_ir_frame = processRightIrFrameFilter(right_ir_frame); + frame_set->pushFrame(right_ir_frame); } if (depth_registration_ && align_filter_ && depth_frame) { if (auto new_frame = align_filter_->process(frame_set)) {