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

Add enable_left_ir_sequence_id_filter,left_ir_sequence_id_filter_id,enable_right_ir_sequence_id_filter and right_ir_sequence_id_filter_id in gemini_330_series.launch.py

Browse Source
This commit is contained in:
jj
2025-03-31 14:49:24 +08:00
parent dd0e3ae617
commit 54809d6b2b
3 changed files with 132 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+12
View File
@@ -330,6 +330,10 @@ class OBCameraNode {
std::shared_ptr<ob::Frame> processColorFrameFilter(std::shared_ptr<ob::Frame>& frame);
std::shared_ptr<ob::Frame> processRightIrFrameFilter(std::shared_ptr<ob::Frame>& frame);
std::shared_ptr<ob::Frame> processLeftIrFrameFilter(std::shared_ptr<ob::Frame>& frame);
uint64_t getFrameTimestampUs(const std::shared_ptr<ob::Frame>& frame);
void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
@@ -374,6 +378,8 @@ class OBCameraNode {
void setupDepthPostProcessFilter();
void setupColorPostProcessFilter();
void setupRightIrPostProcessFilter();
void setupLeftIrPostProcessFilter();
// interleave AE
int init_interleave_hdr_param();
@@ -540,6 +546,10 @@ class OBCameraNode {
int ir_gain_ = -1;
int ir_ae_max_exposure_ = -1;
int ir_brightness_ = -1;
bool enable_right_ir_sequence_id_filter_=false;
int right_ir_sequence_id_filter_id_=-1;
bool enable_left_ir_sequence_id_filter_=false;
int left_ir_sequence_id_filter_id_=-1;
int soft_filter_max_diff_ = -1;
int soft_filter_speckle_size_ = -1;
bool enable_frame_sync_ = false;
@@ -654,6 +664,8 @@ class OBCameraNode {
std::string cloud_frame_id_;
std::vector<std::shared_ptr<ob::Filter>> depth_filter_list_;
std::vector<std::shared_ptr<ob::Filter>> color_filter_list_;
std::vector<std::shared_ptr<ob::Filter>> left_ir_filter_list_;
std::vector<std::shared_ptr<ob::Filter>> right_ir_filter_list_;
// interleave AE
std::string interleave_ae_mode_ = "hdr"; // hdr or laser
orbbec_camera/launch/gemini_330_series.launch.py
+4
View File
@@ -110,6 +110,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'),
@@ -117,6 +119,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'),
orbbec_camera/src/ob_camera_node.cpp
+116 -2
View File
@@ -674,6 +674,65 @@ 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<std::string, bool> 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<ob::SequenceIdFilter>();
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<std::string, bool> 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<ob::SequenceIdFilter>();
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_);
}
}
}
}
void OBCameraNode::setupDepthPostProcessFilter() {
auto depth_sensor = device_->getSensor(OB_SENSOR_DEPTH);
// set depth sensor to filter
@@ -1445,6 +1504,13 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(ir_ae_max_exposure_, "ir_ae_max_exposure", -1);
setAndGetNodeParameter<int>(ir_brightness_, "ir_brightness", -1);
setAndGetNodeParameter<bool>(enable_ir_long_exposure_, "enable_ir_long_exposure", true);
setAndGetNodeParameter<bool>(enable_right_ir_sequence_id_filter_,
"enable_right_ir_sequence_id_filter", false);
setAndGetNodeParameter<int>(right_ir_sequence_id_filter_id_, "right_ir_sequence_id_filter_id",
-1);
setAndGetNodeParameter<bool>(enable_left_ir_sequence_id_filter_,
"enable_left_ir_sequence_id_filter", false);
setAndGetNodeParameter<int>(left_ir_sequence_id_filter_id_, "left_ir_sequence_id_filter_id", -1);
setAndGetNodeParameter<std::string>(depth_work_mode_, "depth_work_mode", "");
setAndGetNodeParameter<std::string>(sync_mode_str_, "sync_mode", "");
setAndGetNodeParameter<int>(depth_delay_us_, "depth_delay_us", 0);
@@ -1485,7 +1551,7 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(threshold_filter_max_, "threshold_filter_max", -1);
setAndGetNodeParameter<int>(threshold_filter_min_, "threshold_filter_min", -1);
setAndGetNodeParameter<float>(hardware_noise_removal_filter_threshold_,
"hardware_noise_removal_filter_threshold", -1.0);
"hardware_noise_removal_filter_threshold", -1.0);
setAndGetNodeParameter<int>(noise_removal_filter_min_diff_, "noise_removal_filter_min_diff", 256);
setAndGetNodeParameter<int>(noise_removal_filter_max_size_, "noise_removal_filter_max_size", 80);
setAndGetNodeParameter<float>(spatial_filter_alpha_, "spatial_filter_alpha", -1.0);
@@ -1603,6 +1669,8 @@ void OBCameraNode::setupTopics() {
setupDevices();
setupDepthPostProcessFilter();
setupColorPostProcessFilter();
setupRightIrPostProcessFilter();
setupLeftIrPostProcessFilter();
setupProfiles();
selectBaseStream();
setupCameraCtrlServices();
@@ -2091,6 +2159,42 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
depth_registration_cloud_pub_->publish(std::move(point_cloud_msg));
}
std::shared_ptr<ob::Frame> OBCameraNode::processRightIrFrameFilter(
std::shared_ptr<ob::Frame> &frame) {
if (frame == nullptr || frame->getType() != OB_FRAME_IR_RIGHT) {
return nullptr;
}
for (size_t i = 0; i < right_ir_filter_list_.size(); i++) {
auto filter = right_ir_filter_list_[i];
CHECK_NOTNULL(filter.get());
if (filter->isEnabled() && frame != nullptr) {
frame = filter->process(frame);
if (frame == nullptr) {
RCLCPP_ERROR_STREAM(logger_, "Right Ir filter process failed");
break;
}
}
}
return frame;
}
std::shared_ptr<ob::Frame> OBCameraNode::processLeftIrFrameFilter(
std::shared_ptr<ob::Frame> &frame) {
if (frame == nullptr || frame->getType() != OB_FRAME_IR_LEFT) {
return nullptr;
}
for (size_t i = 0; i < left_ir_filter_list_.size(); i++) {
auto filter = left_ir_filter_list_[i];
CHECK_NOTNULL(filter.get());
if (filter->isEnabled() && frame != nullptr) {
frame = filter->process(frame);
if (frame == nullptr) {
RCLCPP_ERROR_STREAM(logger_, "Left Ir filter process failed");
break;
}
}
}
return frame;
}
std::shared_ptr<ob::Frame> OBCameraNode::processColorFrameFilter(
std::shared_ptr<ob::Frame> &frame) {
if (frame == nullptr || frame->getType() != OB_FRAME_COLOR) {
@@ -2102,7 +2206,7 @@ std::shared_ptr<ob::Frame> OBCameraNode::processColorFrameFilter(
if (filter->isEnabled() && frame != nullptr) {
frame = filter->process(frame);
if (frame == nullptr) {
RCLCPP_ERROR_STREAM(logger_, "Depth filter process failed");
RCLCPP_ERROR_STREAM(logger_, "Color filter process failed");
break;
}
}
@@ -2229,6 +2333,8 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
}
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);
auto right_ir_frame = frame_set->getFrame(OB_FRAME_IR_RIGHT);
if (depth_frame) {
setDisparitySearchOffset();
setDepthAutoExposureROI();
@@ -2240,6 +2346,14 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> 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 (right_ir_frame) {
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)) {
auto new_frame_set = new_frame->as<ob::FrameSet>();