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

Merge branch 'master' of code.orbbec.com.cn:OrbbecSDK/OrbbecSDK_ROS2

Browse Source
This commit is contained in:
Joe Dong
2023-12-28 15:00:55 +08:00
parent 2cf72ca8b2 62157b82a0
commit 952f7d7d3a
2 changed files with 12 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+11 -13
View File
@@ -305,7 +305,7 @@ void OBCameraNode::startStreams() {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline");
throw std::runtime_error("Failed to start pipeline");
}
if (enable_stream_[COLOR]) {
if (enable_stream_[COLOR] && !colorFrameThread_) {
colorFrameThread_ = std::make_shared<std::thread>([this]() { onNewColorFrameCallback(); });
}
if (enable_frame_sync_) {
@@ -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();
}