i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-11 23:09:51 +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/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
+11 -13
View File
@@ -305,7 +305,7 @@ void OBCameraNode::startStreams() {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline"); RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline");
throw std::runtime_error("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(); }); colorFrameThread_ = std::make_shared<std::thread>([this]() { onNewColorFrameCallback(); });
} }
if (enable_frame_sync_) { 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 { 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();
} }