SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(camera): read UVC snapshots directly

Browse Source
This commit is contained in:
linbo 2026-08-12 09:48:37 +08:00
parent 8c2ed13f80
commit 2c5855a497
2 changed files with 17 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h
View File

@ -5,8 +5,6 @@
#ifndef CMVR_ES_UVC_CAMERA_H #ifndef CMVR_ES_UVC_CAMERA_H
#define CMVR_ES_UVC_CAMERA_H #define CMVR_ES_UVC_CAMERA_H
#include <condition_variable>
#include "common/base/ring_buffer.h" #include "common/base/ring_buffer.h"
#include "camera/abstract_camera.h" #include "camera/abstract_camera.h"
#include "devices/camera/common/include/camera_stream_encoder.h" #include "devices/camera/common/include/camera_stream_encoder.h"
@ -70,10 +68,7 @@ namespace cmvr::device {
std::shared_ptr<std::thread> recording_thread_; std::shared_ptr<std::thread> recording_thread_;
cv::Mat latest_frame_; std::mutex capture_mutex_;
std::mutex frame_mutex_;
std::condition_variable frame_condition_;
uint64_t latest_frame_sequence_{0};
std::string output_path_; std::string output_path_;
bool is_recording_ = false; bool is_recording_ = false;

112
cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp
View File

@ -68,12 +68,6 @@ bool UVCCamera::init() {
clear_error_(); clear_error_();
state_.is_initialized = false; state_.is_initialized = false;
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
latest_frame_sequence_ = 0;
}
stream_frame_buffer_ = std::make_shared<SPMCRingBuffer<StreamFrameData>>(buffer_size_); stream_frame_buffer_ = std::make_shared<SPMCRingBuffer<StreamFrameData>>(buffer_size_);
stream_frame_buffer_->clear(); stream_frame_buffer_->clear();
if (cap_.isOpened()) { if (cap_.isOpened()) {
@ -142,12 +136,6 @@ bool UVCCamera::start() {
return true; return true;
} }
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
latest_frame_sequence_ = 0;
}
cap_.open(serial_, cv::CAP_V4L2); cap_.open(serial_, cv::CAP_V4L2);
this_thread::sleep_for(chrono::milliseconds(100)); this_thread::sleep_for(chrono::milliseconds(100));
@ -211,11 +199,6 @@ bool UVCCamera::stop() {
if (cap_.isOpened()) { if (cap_.isOpened()) {
cap_.release(); cap_.release();
} }
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
latest_frame_sequence_ = 0;
}
state_.is_opened = false; state_.is_opened = false;
return true; return true;
} }
@ -232,15 +215,16 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
std::lock_guard lock(ctrl_mtx_); std::lock_guard lock(ctrl_mtx_);
clear_error_(); clear_error_();
color.release(); color.release();
if (mode_ == PHOTO_MODE) { if (!state_.is_opened) {
if (!state_.is_opened) { state_.is_error = true;
state_.is_error = true; state_.error_message = "camera not opened";
state_.error_message = "camera not opened"; CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; return;
color.release(); }
return;
} {
if (!cap_.read(color)) { std::lock_guard capture_lock(capture_mutex_);
if (!cap_.read(color) || color.empty()) {
state_.is_error = true; state_.is_error = true;
state_.error_message = "read color image failed"; state_.error_message = "read color image failed";
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
@ -248,60 +232,6 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
return; return;
} }
} }
else if (mode_ == VIDEO_MODE)
{
if (!state_.is_opened) {
state_.is_error = true;
state_.error_message = "camera not opened";
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
color.release();
return;
}
const bool capture_worker_requested = state_.is_streaming || state_.is_recording;
bool snapshot_timed_out = false;
if (capture_worker_requested) {
std::unique_lock frame_lock(frame_mutex_);
const uint64_t requested_after_sequence = latest_frame_sequence_;
const bool received_new_frame = frame_condition_.wait_for(
frame_lock,
std::chrono::seconds(2),
[this, requested_after_sequence]() {
return latest_frame_sequence_ != requested_after_sequence;
});
if (received_new_frame && !latest_frame_.empty()) {
latest_frame_.copyTo(color);
}
snapshot_timed_out = !received_new_frame;
}
else {
if (stream_thread_) {
if (stream_thread_->joinable()) {
stream_thread_->join();
}
stream_thread_.reset();
is_streaming_running = false;
}
if (!cap_.read(color) || color.empty()) {
state_.is_error = true;
state_.error_message = "read color image failed";
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
color.release();
return;
}
}
if (color.empty()) {
state_.is_error = true;
state_.error_message = snapshot_timed_out
? "timed out waiting for a new video frame"
: "no video frame available";
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
return;
}
}
} }
void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) { void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
@ -567,7 +497,12 @@ void UVCCamera::streaming_worker_() {
while (state_.is_streaming || state_.is_recording) { while (state_.is_streaming || state_.is_recording) {
// 记录当前帧处理开始时间 // 记录当前帧处理开始时间
const auto frame_start_time = std::chrono::steady_clock::now(); const auto frame_start_time = std::chrono::steady_clock::now();
if (!cap_.read(frame) || frame.empty()) { bool frame_read = false;
{
std::lock_guard capture_lock(capture_mutex_);
frame_read = cap_.read(frame);
}
if (!frame_read || frame.empty()) {
state_.is_error = true; state_.is_error = true;
state_.error_message = "failed to read frame"; state_.error_message = "failed to read frame";
CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message; CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message;
@ -575,13 +510,6 @@ void UVCCamera::streaming_worker_() {
} }
const auto capture_end_time = std::chrono::steady_clock::now(); const auto capture_end_time = std::chrono::steady_clock::now();
{
std::lock_guard frame_lock(frame_mutex_);
frame.copyTo(latest_frame_);
++latest_frame_sequence_;
}
frame_condition_.notify_all();
// 以下为编码部分，用于流模式 // 以下为编码部分，用于流模式
StreamFrameData frame_data; StreamFrameData frame_data;
// 保存图像数据 // 保存图像数据
@ -694,10 +622,6 @@ void UVCCamera::streaming_worker_() {
} }
is_streaming_running = false; is_streaming_running = false;
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
}
// 线程结束时清空队列 // 线程结束时清空队列
stream_frame_buffer_->clear(); stream_frame_buffer_->clear();
recordingIndex_ = 0; recordingIndex_ = 0;
@ -709,10 +633,6 @@ void UVCCamera::streaming_worker_() {
stream_frame_buffer_->clear(); stream_frame_buffer_->clear();
// 确保线程状态正确更新 // 确保线程状态正确更新
is_streaming_running = false; is_streaming_running = false;
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
}
state_.is_error = true; state_.is_error = true;
state_.error_message = e.what(); state_.error_message = e.what();
CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message; CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message;