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

Merge branch 'fix/depth-frame-race'

Browse Source
This commit is contained in:
slz
2026-08-31 13:40:13 +08:00
parent 3812e0f5ad 9372c2c6f8
commit a231099234
2 changed files with 14 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
-1
View File
@@ -540,7 +540,6 @@ class OBCameraNode {
bool ordered_pc_ = false;
bool enable_depth_scale_ = true;
std::shared_ptr<ob::Frame> depth_frame_ = nullptr;
std::string device_preset_ = "Default";
// filter switch
bool enable_decimation_filter_ = false;
orbbec_camera/src/ob_camera_node.cpp
+14 -22
View File
@@ -1589,17 +1589,12 @@ void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_
}
void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
(void)frame_set;
if (!depth_cloud_pub_ || depth_cloud_pub_->get_subscription_count() == 0 ||
!enable_point_cloud_ || !depth_frame_) {
!enable_point_cloud_) {
return;
}
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
if (!depth_frame_) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
}
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
auto depth_frame = frame_set->depthFrame();
if (!depth_frame) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
@@ -1692,17 +1687,12 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
if (!depth_registration_cloud_pub_ ||
depth_registration_cloud_pub_->get_subscription_count() == 0 ||
!enable_colored_point_cloud_ || !depth_frame_) {
!enable_colored_point_cloud_) {
return;
}
CHECK_NOTNULL(depth_frame_.get());
std::lock_guard<decltype(point_cloud_mutex_)> point_cloud_msg_lock(point_cloud_mutex_);
if (!depth_frame_) {
RCLCPP_ERROR_STREAM(logger_, "depth frame is null");
return;
}
auto depth_frame = depth_frame_->as<ob::DepthFrame>();
auto depth_frame = frame_set->depthFrame();
auto color_frame = frame_set->colorFrame();
if (!depth_frame || !color_frame) {
return;
@@ -1892,22 +1882,27 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
publishStaticTransforms();
tf_published_ = true;
}
depth_frame_ = frame_set->getFrame(OB_FRAME_DEPTH);
auto depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
auto device_info = device_->getDeviceInfo();
CHECK_NOTNULL(device_info.get());
auto pid = device_info->pid();
auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
has_first_color_frame_ = has_first_color_frame_ || color_frame;
if (isGemini335PID(pid) && depth_frame_) {
depth_frame_ = processDepthFrameFilter(depth_frame_);
if (depth_registration_ && align_filter_ && depth_frame_ && has_first_color_frame_) {
if (isGemini335PID(pid) && depth_frame) {
depth_frame = processDepthFrameFilter(depth_frame);
if (depth_frame) {
ob::FrameHelper::pushFrame(frame_set, OB_FRAME_DEPTH, depth_frame);
}
if (depth_registration_ && align_filter_ && depth_frame && has_first_color_frame_) {
auto new_frame = align_filter_->process(frame_set);
if (new_frame) {
auto new_frame_set = new_frame->as<ob::FrameSet>();
CHECK_NOTNULL(new_frame_set.get());
depth_frame_ = new_frame_set->getFrame(OB_FRAME_DEPTH);
frame_set = new_frame_set;
color_frame = frame_set->getFrame(OB_FRAME_COLOR);
} else {
RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
return;
}
} else {
RCLCPP_DEBUG(logger_,
@@ -1937,9 +1932,6 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
if (frame == nullptr) {
continue;
}
if (stream_index == DEPTH) {
frame = depth_frame_;
}
onNewFrameCallback(frame, stream_index);
}
}