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

fix g300 turning off color and depth causes other streams to behave abnormally

Browse Source
This commit is contained in:
jj
2024-10-18 19:09:33 +08:00
parent faa076d229
commit e581d4334a
2 changed files with 114 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.vscode/settings.json
Vendored
+2 -1
View File
@@ -71,6 +71,7 @@
"thread": "cpp", "thread": "cpp",
"typeindex": "cpp", "typeindex": "cpp",
"typeinfo": "cpp", "typeinfo": "cpp",
"variant": "cpp" "variant": "cpp",
"*.ipp": "cpp"
} }
} }
orbbec_camera/src/ob_camera_node.cpp
+34 -32
View File
@@ -552,13 +552,13 @@ void OBCameraNode::setupDepthPostProcessFilter() {
} else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) { } else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>(); auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams(); OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: " RCLCPP_INFO_STREAM(
<< "disp_diff: " << params.disp_diff logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size); << ", max_size: " << params.max_size);
params.disp_diff = noise_removal_filter_min_diff_; params.disp_diff = noise_removal_filter_min_diff_;
params.max_size = noise_removal_filter_max_size_; params.max_size = noise_removal_filter_max_size_;
RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: " RCLCPP_INFO_STREAM(logger_,
<< "disp_diff: " << params.disp_diff "Set noise removal filter params: " << "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size); << ", max_size: " << params.max_size);
if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) { if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
noise_removal_filter->setFilterParams(params); noise_removal_filter->setFilterParams(params);
@@ -568,8 +568,8 @@ void OBCameraNode::setupDepthPostProcessFilter() {
hdr_merge_gain_2_ != -1) { hdr_merge_gain_2_ != -1) {
auto hdr_merge_filter = filter->as<ob::HdrMerge>(); auto hdr_merge_filter = filter->as<ob::HdrMerge>();
hdr_merge_filter->enable(true); hdr_merge_filter->enable(true);
RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: " RCLCPP_INFO_STREAM(
<< "exposure_1: " << hdr_merge_exposure_1_ logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
<< ", gain_1: " << hdr_merge_gain_1_ << ", gain_1: " << hdr_merge_gain_1_
<< ", exposure_2: " << hdr_merge_exposure_2_ << ", exposure_2: " << hdr_merge_exposure_2_
<< ", gain_2: " << hdr_merge_gain_2_); << ", gain_2: " << hdr_merge_gain_2_);
@@ -784,6 +784,7 @@ void OBCameraNode::startStreams() {
pipeline_.reset(); pipeline_.reset();
} }
pipeline_ = std::make_unique<ob::Pipeline>(device_); pipeline_ = std::make_unique<ob::Pipeline>(device_);
try { try {
setupPipelineConfig(); setupPipelineConfig();
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) {
@@ -931,29 +932,29 @@ int OBCameraNode::openSocSyncPwmTrigger(uint16_t fps) {
const int TRIGGER_MODE_DISABLE = 0; const int TRIGGER_MODE_DISABLE = 0;
int ret = -1; int ret = -1;
cs_param_t param = { TRIGGER_MODE_ENABLE, fps }; cs_param_t param = {TRIGGER_MODE_ENABLE, fps};
cs_param_t rd_par = { TRIGGER_MODE_DISABLE, 0 }; cs_param_t rd_par = {TRIGGER_MODE_DISABLE, 0};
if(access(devicePath, F_OK) != 0) { if (access(devicePath, F_OK) != 0) {
std::cerr << "Device node " << devicePath << " does not exist." << std::endl; std::cerr << "Device node " << devicePath << " does not exist." << std::endl;
return ret; return ret;
} }
gmsl_trigger_fd_ = open(DEVICE_PATH, O_RDWR); gmsl_trigger_fd_ = open(DEVICE_PATH, O_RDWR);
if(gmsl_trigger_fd_ < 0) { if (gmsl_trigger_fd_ < 0) {
perror("open device failed\n"); perror("open device failed\n");
return gmsl_trigger_fd_; return gmsl_trigger_fd_;
} }
std::cout << "Written param mode=" << param.mode << ", fps=" << param.fps << std::endl; std::cout << "Written param mode=" << param.mode << ", fps=" << param.fps << std::endl;
ret = write(gmsl_trigger_fd_, &param, sizeof(param)); ret = write(gmsl_trigger_fd_, &param, sizeof(param));
if(ret < 0) { if (ret < 0) {
perror("write device failed\n"); perror("write device failed\n");
close(gmsl_trigger_fd_); close(gmsl_trigger_fd_);
return ret; return ret;
} }
ret = read(gmsl_trigger_fd_, &rd_par, sizeof(rd_par)); ret = read(gmsl_trigger_fd_, &rd_par, sizeof(rd_par));
if(ret < 0) { if (ret < 0) {
perror("read device failed\n"); perror("read device failed\n");
close(gmsl_trigger_fd_); close(gmsl_trigger_fd_);
return ret; return ret;
@@ -965,7 +966,7 @@ int OBCameraNode::openSocSyncPwmTrigger(uint16_t fps) {
return 0; return 0;
} }
int OBCameraNode::closeSocSyncPwmTrigger() { int OBCameraNode::closeSocSyncPwmTrigger() {
if(gmsl_trigger_fd_ >= 0) { if (gmsl_trigger_fd_ >= 0) {
close(gmsl_trigger_fd_); close(gmsl_trigger_fd_);
gmsl_trigger_fd_ = -1; // Reset file descriptors gmsl_trigger_fd_ = -1; // Reset file descriptors
std::cout << "close camSync success" << std::endl; std::cout << "close camSync success" << std::endl;
@@ -975,17 +976,17 @@ int OBCameraNode::closeSocSyncPwmTrigger() {
} }
void OBCameraNode::startGmslTrigger() { void OBCameraNode::startGmslTrigger() {
if(gmsl_trigger_fps_ > 0 && enable_gmsl_trigger_) { if (gmsl_trigger_fps_ > 0 && enable_gmsl_trigger_) {
RCLCPP_WARN_STREAM(logger_, "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_: " << gmsl_trigger_fps_); RCLCPP_WARN_STREAM(logger_, "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_: "
<< gmsl_trigger_fps_);
openSocSyncPwmTrigger(gmsl_trigger_fps_); openSocSyncPwmTrigger(gmsl_trigger_fps_);
} } else {
else { RCLCPP_WARN_STREAM(logger_,
RCLCPP_WARN_STREAM(logger_, "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_ illegal: " << gmsl_trigger_fps_); "Start HardwareTrigger by soc-trigger-source. gmsl_trigger_fps_ illegal: "
<< gmsl_trigger_fps_);
} }
} }
void OBCameraNode::stopGmslTrigger() { void OBCameraNode::stopGmslTrigger() { closeSocSyncPwmTrigger(); }
closeSocSyncPwmTrigger();
}
void OBCameraNode::setupDefaultImageFormat() { void OBCameraNode::setupDefaultImageFormat() {
format_[DEPTH] = OB_FORMAT_Y16; format_[DEPTH] = OB_FORMAT_Y16;
@@ -1732,16 +1733,16 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
auto pid = device_info->getPid(); auto pid = device_info->getPid();
auto color_frame = frame_set->getFrame(OB_FRAME_COLOR); auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
if (isGemini335PID(pid)) { if (isGemini335PID(pid)) {
bool depth_aligned = false;
depth_frame = processDepthFrameFilter(depth_frame); depth_frame = processDepthFrameFilter(depth_frame);
if(depth_frame) if (depth_frame) {
{ frame_set->pushFrame(depth_frame);} frame_set->pushFrame(depth_frame);
}
if (depth_registration_ && align_filter_ && depth_frame && color_frame) { if (depth_registration_ && align_filter_ && depth_frame && color_frame) {
if (auto new_frame = align_filter_->process(frame_set)) { if (auto new_frame = align_filter_->process(frame_set)) {
auto new_frame_set = new_frame->as<ob::FrameSet>(); auto new_frame_set = new_frame->as<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get()); CHECK_NOTNULL(new_frame_set.get());
frame_set = new_frame_set; frame_set = new_frame_set;
depth_aligned = true; }
} else { } else {
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame"); RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
return; return;
@@ -1751,9 +1752,6 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
"Depth registration is disabled or align filter is null or depth frame is " "Depth registration is disabled or align filter is null or depth frame is "
"null or color frame is null"); "null or color frame is null");
} }
if (depth_registration_ && align_filter_ && !depth_aligned) {
return;
}
} }
if (enable_stream_[COLOR] && color_frame) { if (enable_stream_[COLOR] && color_frame) {
@@ -1796,13 +1794,16 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
} }
} }
} }
} catch (const ob::Error &e) { }
catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage()); RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage());
} catch (const std::exception &e) { }
catch (const std::exception &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what()); RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.what());
} catch (...) { }
catch (...) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: unknown error"); RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: unknown error");
} }
} }
void OBCameraNode::onNewColorFrameCallback() { void OBCameraNode::onNewColorFrameCallback() {
@@ -2051,6 +2052,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
} else { } else {
memcpy(image.data, video_frame->getData(), video_frame->getDataSize()); memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
} }
if (stream_index == DEPTH) { if (stream_index == DEPTH) {
auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale(); auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
image = image * depth_scale; image = image * depth_scale;