i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-12 23:39:50 +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/include/orbbec_camera/ob_camera_node.h
+1 -1
View File
@@ -262,7 +262,7 @@ class OBCameraNode {
void switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request, void switchIRCameraCallback(const std::shared_ptr<SetString::Request>& request,
std::shared_ptr<SetString::Response>& response); std::shared_ptr<SetString::Response>& response);
void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set, bool isColorPointCloud); void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set); void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
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 { try {
if (isColorPointCloud) { if (depth_registration_ || enable_colored_point_cloud_) {
if (depth_registration_ || enable_colored_point_cloud_) { if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) { publishColoredPointCloud(frame_set);
publishColoredPointCloud(frame_set);
}
} }
} }
if (!isColorPointCloud) { if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) { publishDepthPointCloud(frame_set);
publishDepthPointCloud(frame_set);
}
} }
} catch (const ob::Error &e) { } catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, e.getMessage()); RCLCPP_ERROR_STREAM(logger_, e.getMessage());
@@ -929,8 +925,10 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet> &fr
colorFrameQueue_.push(frame_set); colorFrameQueue_.push(frame_set);
colorFrameCV_.notify_all(); colorFrameCV_.notify_all();
} }
else {
publishPointCloud(frame_set);
}
publishPointCloud(frame_set, false);
for (const auto &stream_index : IMAGE_STREAMS) { for (const auto &stream_index : IMAGE_STREAMS) {
if (enable_stream_[stream_index]) { if (enable_stream_[stream_index]) {
auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first); 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(); std::shared_ptr<ob::FrameSet> frameSet = colorFrameQueue_.front();
is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_); is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_);
publishPointCloud(frameSet, true); publishPointCloud(frameSet);
onNewFrameCallback(frameSet->colorFrame(), IMAGE_STREAMS.at(2)); onNewFrameCallback(frameSet->colorFrame(), IMAGE_STREAMS.at(2));
colorFrameQueue_.pop(); colorFrameQueue_.pop();
} }