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

Update sdk to v2.2.8 and replace Repeat using clean IR video and speckle depth video with skip frame

Browse Source
This commit is contained in:
datean
2025-03-17 18:13:34 +08:00
parent 9caaa78c0a
commit 2c0fa34988
29 changed files with 242 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+52 -65
View File
@@ -542,14 +542,14 @@ void OBCameraNode::setupDepthPostProcessFilter() {
} else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: "
<< "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size);
RCLCPP_INFO_STREAM(
logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size);
params.disp_diff = noise_removal_filter_min_diff_;
params.max_size = noise_removal_filter_max_size_;
RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: "
<< "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size);
RCLCPP_INFO_STREAM(logger_,
"Set noise removal filter params: " << "disp_diff: " << params.disp_diff
<< ", max_size: " << params.max_size);
if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
noise_removal_filter->setFilterParams(params);
}
@@ -558,11 +558,11 @@ void OBCameraNode::setupDepthPostProcessFilter() {
hdr_merge_gain_2_ != -1) {
auto hdr_merge_filter = filter->as<ob::HdrMerge>();
hdr_merge_filter->enable(true);
RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: "
<< "exposure_1: " << hdr_merge_exposure_1_
<< ", gain_1: " << hdr_merge_gain_1_
<< ", exposure_2: " << hdr_merge_exposure_2_
<< ", gain_2: " << hdr_merge_gain_2_);
RCLCPP_INFO_STREAM(
logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
<< ", gain_1: " << hdr_merge_gain_1_
<< ", exposure_2: " << hdr_merge_exposure_2_
<< ", gain_2: " << hdr_merge_gain_2_);
auto config = OBHdrConfig();
config.enable = true;
config.exposure_1 = hdr_merge_exposure_1_;
@@ -1615,9 +1615,9 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
return;
}
if (depth_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) !=
if (depth_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_POWER_MODE) ==
interleave_skip_index_) {
RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", depth_frame->getType());
// RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", depth_frame->getType());
return;
}
@@ -2164,25 +2164,52 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
}
std::shared_ptr<ob::VideoFrame> video_frame;
if (frame->getType() == OB_FRAME_COLOR) {
if (interleave_frame_enable_ && interleave_skip_enable_) {
interleave_skip_color_index_++;
if (interleave_skip_color_index_ % 2 == 0) {
return;
}
updateStreamInfo(frame, color_stream_info_);
}
video_frame = frame->as<ob::ColorFrame>();
} else if (frame->getType() == OB_FRAME_DEPTH) {
video_frame = frame->as<ob::DepthFrame>();
if (buffer_depth_ == nullptr) {
buffer_depth_ =
std::shared_ptr<char[]>(new char[video_frame->getWidth() * video_frame->getHeight() * 2]);
if (interleave_frame_enable_ && interleave_skip_enable_) {
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_POWER_MODE) ==
interleave_skip_index_) {
return;
}
}
} else if (frame->getType() == OB_FRAME_IR || frame->getType() == OB_FRAME_IR_LEFT ||
frame->getType() == OB_FRAME_IR_RIGHT) {
video_frame = frame->as<ob::IRFrame>();
if (buffer_left_ir_ == nullptr) {
buffer_left_ir_ =
std::shared_ptr<char[]>(new char[video_frame->getWidth() * video_frame->getHeight()]);
}
if (buffer_right_ir_ == nullptr) {
buffer_right_ir_ =
std::shared_ptr<char[]>(new char[video_frame->getWidth() * video_frame->getHeight()]);
}
if (interleave_frame_enable_ && interleave_skip_enable_) {
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_LASER_POWER_MODE) !=
interleave_skip_index_) {
if (frame->getType() == OB_FRAME_IR_LEFT) {
if ((getFrameTimestampUs(frame) - left_last_time) > 37000) {
RCLCPP_INFO_STREAM(logger_,
"left this FrameTimestampUs: " << (getFrameTimestampUs(frame)));
RCLCPP_INFO_STREAM(logger_, "left last FrameTimestampUs: " << (left_last_time));
RCLCPP_INFO_STREAM(logger_, "left-between current and previous fram "
<< (getFrameTimestampUs(frame) - left_last_time));
}
left_last_time = getFrameTimestampUs(frame);
}
if (frame->getType() == OB_FRAME_IR_RIGHT) {
if ((getFrameTimestampUs(frame) - right_last_time) > 37000) {
RCLCPP_INFO_STREAM(logger_,
"right this FrameTimestampUs: " << (getFrameTimestampUs(frame)));
RCLCPP_INFO_STREAM(logger_, "right last FrameTimestampUs: " << (right_last_time));
RCLCPP_INFO_STREAM(logger_, "right-between current and previous fram "
<< (getFrameTimestampUs(frame) - right_last_time));
}
right_last_time = getFrameTimestampUs(frame);
}
return;
}
}
} else {
RCLCPP_ERROR(logger_, "Unsupported frame type: %d", frame->getType());
return;
@@ -2265,28 +2292,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
frame->format() != OB_FORMAT_RGBA && image_publishers_[COLOR]->get_subscription_count() > 0) {
memcpy(image.data, rgb_buffer_, video_frame->getWidth() * video_frame->getHeight() * 3);
} else if (frame->getType() == OB_FRAME_DEPTH) {
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) !=
interleave_skip_index_) {
memcpy(image.data, buffer_depth_.get(), video_frame->getDataSize());
} else {
memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
}
} else {
if (interleave_frame_enable_ && interleave_skip_enable_) {
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) ==
interleave_skip_index_) {
if (frame->getType() == OB_FRAME_IR_LEFT) {
memcpy(image.data, buffer_left_ir_.get(), video_frame->getDataSize());
} else if (frame->getType() == OB_FRAME_IR_RIGHT) {
memcpy(image.data, buffer_right_ir_.get(), video_frame->getDataSize());
}
} else {
memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
}
} else {
memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
}
memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
}
if (stream_index == DEPTH) {
auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
@@ -2304,26 +2311,6 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
CHECK(image_publishers_.count(stream_index) > 0);
saveImageToFile(stream_index, image, *image_msg);
image_publishers_[stream_index]->publish(std::move(image_msg));
if (frame->getType() == OB_FRAME_DEPTH) {
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) ==
interleave_skip_index_) {
memcpy(buffer_depth_.get(), video_frame->getData(), video_frame->getDataSize());
}
} else if (frame->getType() == OB_FRAME_IR || frame->getType() == OB_FRAME_IR_LEFT ||
frame->getType() == OB_FRAME_IR_RIGHT) {
if (interleave_frame_enable_ && interleave_skip_enable_) {
if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) !=
interleave_skip_index_) {
if (frame->getType() == OB_FRAME_IR_LEFT) {
memcpy(buffer_left_ir_.get(), video_frame->getData(), video_frame->getDataSize());
} else if (frame->getType() == OB_FRAME_IR_RIGHT) {
memcpy(buffer_right_ir_.get(), video_frame->getData(), video_frame->getDataSize());
}
}
}
}
if (stream_index == COLOR && enable_color_undistortion_ &&
color_undistortion_publisher_->get_subscription_count() > 0) {
auto undistorted_image = undistortImage(image, intrinsic, distortion);