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

Resolving the issue of abnormal simultaneous output of depth point cloud and color point cloud.

Browse Source
This commit is contained in:
lixiaobin
2023-12-28 11:02:20 +08:00
parent df21b7d46e
commit 62157b82a0
2 changed files with 11 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+10 -12
View File
@@ -672,20 +672,16 @@ void OBCameraNode::setupPublishers() {
}
}
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set, bool isColorPointCloud) {
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
try {
if (isColorPointCloud) {
if (depth_registration_ || enable_colored_point_cloud_) {
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
publishColoredPointCloud(frame_set);
}
if (depth_registration_ || enable_colored_point_cloud_) {
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
publishColoredPointCloud(frame_set);
}
}
if (!isColorPointCloud) {
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set);
}
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set);
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, e.getMessage());
@@ -929,8 +925,10 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet> &fr
colorFrameQueue_.push(frame_set);
colorFrameCV_.notify_all();
}
else {
publishPointCloud(frame_set);
}
publishPointCloud(frame_set, false);
for (const auto &stream_index : IMAGE_STREAMS) {
if (enable_stream_[stream_index]) {
auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first);
@@ -972,7 +970,7 @@ void OBCameraNode::onNewColorFrameCallback() {
std::shared_ptr<ob::FrameSet> frameSet = colorFrameQueue_.front();
is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_);
publishPointCloud(frameSet, true);
publishPointCloud(frameSet);
onNewFrameCallback(frameSet->colorFrame(), IMAGE_STREAMS.at(2));
colorFrameQueue_.pop();
}