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

Using a new thread to process Color data.

Browse Source
This commit is contained in:
lixiaobin
2023-11-02 20:01:48 +08:00
parent b78910b895
commit 548f244087
2 changed files with 66 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+57 -10
View File
@@ -91,6 +91,12 @@ void OBCameraNode::clean() {
if (tf_thread_ && tf_thread_->joinable()) {
tf_thread_->join();
}
if (colorFrameThread_ && colorFrameThread_->joinable()) {
colorFrameCV_.notify_all();
colorFrameThread_->join();
}
RCLCPP_WARN_STREAM(logger_, "stop streams");
stopStreams();
stopIMU();
@@ -280,6 +286,9 @@ void OBCameraNode::startStreams() {
RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline");
throw std::runtime_error("Failed to start pipeline");
}
if (enable_stream_[COLOR]) {
colorFrameThread_ = std::make_shared<std::thread>([this]() { noNewColorFrameCallback(); });
}
if (enable_frame_sync_) {
pipeline_->enableFrameSync();
}
@@ -567,15 +576,20 @@ void OBCameraNode::setupPublishers() {
}
}
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set, bool isColorPointCloud) {
try {
if (depth_registration_ || enable_colored_point_cloud_) {
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
publishColoredPointCloud(frame_set);
if (isColorPointCloud) {
if (depth_registration_ || enable_colored_point_cloud_) {
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
publishColoredPointCloud(frame_set);
}
}
}
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set);
if (!isColorPointCloud) {
if (enable_point_cloud_ && frame_set->depthFrame() != nullptr) {
publishDepthPointCloud(frame_set);
}
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, e.getMessage());
@@ -799,25 +813,37 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet> &fr
tf_published_ = true;
}
is_color_frame_decoded_ = decodeColorFrameToBuffer(frame_set->colorFrame(), rgb_buffer_);
publishPointCloud(frame_set);
//is_color_frame_decoded_ = decodeColorFrameToBuffer(frame_set->colorFrame(), rgb_buffer_);
//publishPointCloud(frame_set);
std::shared_ptr<ob::ColorFrame> colorFrame = frame_set->colorFrame();
if (enable_stream_[COLOR] && colorFrame){
std::lock_guard<std::mutex> colorLock(colorFrameMtx_);
colorFrameQueue_.push(frame_set);
colorFrameCV_.notify_all();
}
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);
if (frame_type == OB_FRAME_COLOR) {
continue;
}
auto frame = frame_set->getFrame(frame_type);
if (frame == nullptr) {
continue;
}
std::shared_ptr<ob::Frame> irFrame = decodeIRMJPGFrame(frame);
if(irFrame) {
if (irFrame) {
onNewFrameCallback(irFrame, stream_index);
} else {
onNewFrameCallback(frame, stream_index);
}
}
}
} catch (const ob::Error &e) {
RCLCPP_ERROR_STREAM(logger_, "onNewFrameSetCallback error: " << e.getMessage());
} catch (const std::exception &e) {
@@ -827,6 +853,27 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet> &fr
}
}
void OBCameraNode::noNewColorFrameCallback() {
while (enable_stream_[COLOR] && rclcpp::ok() && is_running_.load()) {
std::unique_lock<std::mutex> lock(colorFrameMtx_);
colorFrameCV_.wait(lock, [this]() { return !colorFrameQueue_.empty() || !(is_running_.load()); });
if(!rclcpp::ok() || !is_running_.load()) {
break;
}
std::shared_ptr<ob::FrameSet> frameSet = colorFrameQueue_.front();
is_color_frame_decoded_ = decodeColorFrameToBuffer(frameSet->colorFrame(), rgb_buffer_);
publishPointCloud(frameSet, true);
if (is_color_frame_decoded_) {
onNewFrameCallback(frameSet->colorFrame(), IMAGE_STREAMS.at(2));
}
colorFrameQueue_.pop();
}
RCLCPP_INFO_STREAM(logger_, "Color frame thread exit!");
}
std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
const std::shared_ptr<ob::Frame> &frame) {
if (frame == nullptr) {