refactor(camera): capture UVC frames with FFmpeg
This commit is contained in:
parent
bb6c627fad
commit
8850df3983
@ -2,7 +2,18 @@ add_library(uvc_camera SHARED src/uvc_camera.cpp)
|
||||
|
||||
target_include_directories(uvc_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
target_link_libraries(uvc_camera PUBLIC glog opencv_core opencv_imgproc cmvr_es::proto cmvr_es::device::camera_stream_encoder)
|
||||
target_link_libraries(uvc_camera PUBLIC
|
||||
glog
|
||||
opencv_core
|
||||
opencv_imgproc
|
||||
avcodec
|
||||
avdevice
|
||||
avformat
|
||||
avutil
|
||||
swscale
|
||||
cmvr_es::proto
|
||||
cmvr_es::device::camera_stream_encoder
|
||||
)
|
||||
|
||||
add_library(cmvr_es::device::uvc_camera ALIAS uvc_camera)
|
||||
install(TARGETS uvc_camera LIBRARY DESTINATION lib)
|
||||
|
||||
@ -5,6 +5,9 @@
|
||||
#ifndef CMVR_ES_UVC_CAMERA_H
|
||||
#define CMVR_ES_UVC_CAMERA_H
|
||||
|
||||
#include <atomic>
|
||||
#include <condition_variable>
|
||||
|
||||
#include "common/base/ring_buffer.h"
|
||||
#include "camera/abstract_camera.h"
|
||||
#include "devices/camera/common/include/camera_stream_encoder.h"
|
||||
@ -46,6 +49,14 @@ namespace cmvr::device {
|
||||
void streaming_worker_();
|
||||
void recording_worker_();
|
||||
void cleanup_recording_resources_();
|
||||
bool open_capture_();
|
||||
void close_capture_();
|
||||
void capture_worker_();
|
||||
bool wait_for_capture_frame_(cv::Mat& frame,
|
||||
uint64_t& last_sequence,
|
||||
int64_t& capture_monotonic_ns,
|
||||
int64_t& capture_utc_ns);
|
||||
static int interrupt_capture_(void* opaque);
|
||||
|
||||
int fps_;
|
||||
int width_;
|
||||
@ -53,7 +64,6 @@ namespace cmvr::device {
|
||||
int encode_width_;
|
||||
int encode_height_;
|
||||
std::string serial_;
|
||||
cv::VideoCapture cap_;
|
||||
size_t buffer_size_;
|
||||
std::string codec_;
|
||||
bool enable_stream_timestamp_{false};
|
||||
@ -64,12 +74,22 @@ namespace cmvr::device {
|
||||
std::string current_video_path_;
|
||||
|
||||
std::mutex ctrl_mtx_{};
|
||||
std::unique_ptr<cv::VideoWriter> video_writer_;
|
||||
std::shared_ptr<std::thread> stream_thread_;
|
||||
std::shared_ptr<std::thread> recording_thread_;
|
||||
std::shared_ptr<std::thread> capture_thread_;
|
||||
|
||||
|
||||
std::mutex capture_mutex_;
|
||||
AVFormatContext* capture_format_context_ = nullptr;
|
||||
AVCodecContext* capture_decoder_context_ = nullptr;
|
||||
SwsContext* capture_sws_context_ = nullptr;
|
||||
int capture_video_stream_index_ = -1;
|
||||
std::atomic<bool> capture_running_{false};
|
||||
std::mutex capture_frame_mutex_;
|
||||
std::condition_variable capture_frame_cv_;
|
||||
cv::Mat latest_capture_frame_;
|
||||
uint64_t latest_capture_sequence_ = 0;
|
||||
int64_t latest_capture_monotonic_ns_ = 0;
|
||||
int64_t latest_capture_utc_ns_ = 0;
|
||||
std::string capture_error_;
|
||||
|
||||
std::string output_path_;
|
||||
bool is_recording_ = false;
|
||||
|
||||
@ -4,14 +4,31 @@
|
||||
//
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
#include "../include/uvc_camera.h"
|
||||
|
||||
extern "C" {
|
||||
#include <libavdevice/avdevice.h>
|
||||
#include <libavutil/error.h>
|
||||
}
|
||||
|
||||
using namespace std;
|
||||
using namespace cmvr::device;
|
||||
|
||||
#define USE_LIST_IMAGE 1
|
||||
|
||||
namespace {
|
||||
|
||||
std::string ffmpeg_error_string(const int error_code)
|
||||
{
|
||||
char error_buffer[AV_ERROR_MAX_STRING_SIZE] = {};
|
||||
av_strerror(error_code, error_buffer, sizeof(error_buffer));
|
||||
return error_buffer;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
|
||||
{
|
||||
id_ = camera_.id();
|
||||
@ -53,6 +70,7 @@ UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
|
||||
}
|
||||
UVCCamera::~UVCCamera() {
|
||||
stop();
|
||||
close_capture_();
|
||||
if (stream_thread_ && stream_thread_->joinable())
|
||||
stream_thread_->join();
|
||||
if (recording_thread_ && recording_thread_->joinable())
|
||||
@ -72,58 +90,29 @@ bool UVCCamera::init() {
|
||||
|
||||
stream_frame_buffer_ = std::make_shared<SPMCRingBuffer<StreamFrameData>>(buffer_size_);
|
||||
stream_frame_buffer_->clear();
|
||||
if (cap_.isOpened()) {
|
||||
cap_.release();
|
||||
}
|
||||
|
||||
cap_.open(serial_, cv::CAP_V4L2);
|
||||
this_thread::sleep_for(chrono::milliseconds(100));
|
||||
|
||||
if (!cap_.isOpened()) {
|
||||
// Validate the requested V4L2 mode during initialization. start() reopens
|
||||
// the device and owns it for the lifetime of the camera session.
|
||||
if (!open_capture_()) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "Failed to open USB camera at index " + serial_;
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init)" << state_.error_message;
|
||||
state_.error_message = capture_error_;
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init): " << state_.error_message;
|
||||
return false;
|
||||
}
|
||||
close_capture_();
|
||||
|
||||
// 设置格式为MJPG
|
||||
cap_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
|
||||
if (!cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_)) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "set width failed";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init): set width failed";
|
||||
return false;
|
||||
}
|
||||
state_.width = width_;
|
||||
if (!cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_)) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "set height failed";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init): set height failed";
|
||||
return false;
|
||||
}
|
||||
state_.height = height_;
|
||||
if (!cap_.set(cv::CAP_PROP_FPS, fps_)) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "set fps failed";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init): set fps failed";
|
||||
return false;
|
||||
}
|
||||
if (!cap_.set(cv::CAP_PROP_BUFFERSIZE, 1)) {
|
||||
CMVR_LOG(WARNING) << "[UVCCamera] (init): failed to limit capture buffer size";
|
||||
}
|
||||
//初始化编码器
|
||||
// 初始化RGB编码器（示例参数：640x480，30fps，H.264）
|
||||
if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (start): Failed to init RGB encoder!";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init): Failed to init RGB encoder!";
|
||||
state_.is_error = true;
|
||||
state_.error_message = "Failed to init RGB encoder!";
|
||||
return false;
|
||||
}
|
||||
state_.fps = fps_;
|
||||
state_.width = width_;
|
||||
state_.height = height_;
|
||||
state_.is_initialized = true;
|
||||
state_.is_error = false;
|
||||
CMVR_LOG(INFO) << "[UVCCamera] (init): UVC camera initialized at index " << serial_; // 记录初始化成功日志
|
||||
CMVR_LOG(INFO) << "[UVCCamera] (init): UVC camera initialized at " << serial_;
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -141,42 +130,45 @@ bool UVCCamera::start() {
|
||||
return true;
|
||||
}
|
||||
|
||||
cap_.open(serial_, cv::CAP_V4L2);
|
||||
this_thread::sleep_for(chrono::milliseconds(100));
|
||||
|
||||
if (!cap_.isOpened()) {
|
||||
if (!open_capture_()) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "Failed to open USB camera at index " + serial_;
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init)" << state_.error_message;
|
||||
state_.error_message = capture_error_;
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (start): " << state_.error_message;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 设置格式为MJPG
|
||||
cap_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
try {
|
||||
capture_thread_ = std::make_shared<std::thread>(&UVCCamera::capture_worker_, this);
|
||||
} catch (const std::exception& e) {
|
||||
capture_error_ = std::string("failed to start capture thread: ") + e.what();
|
||||
close_capture_();
|
||||
state_.is_error = true;
|
||||
state_.error_message = capture_error_;
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (start): " << state_.error_message;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_)) {
|
||||
cv::Mat first_frame;
|
||||
uint64_t first_sequence = 0;
|
||||
int64_t first_capture_monotonic_ns = 0;
|
||||
int64_t first_capture_utc_ns = 0;
|
||||
if (!wait_for_capture_frame_(first_frame,
|
||||
first_sequence,
|
||||
first_capture_monotonic_ns,
|
||||
first_capture_utc_ns)) {
|
||||
std::string capture_error;
|
||||
{
|
||||
std::lock_guard capture_lock(capture_frame_mutex_);
|
||||
capture_error = capture_error_;
|
||||
}
|
||||
close_capture_();
|
||||
state_.is_error = true;
|
||||
state_.error_message = "set width failed";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init): set width failed";
|
||||
state_.error_message = capture_error.empty()
|
||||
? "timed out waiting for the first camera frame"
|
||||
: capture_error;
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (start): " << state_.error_message;
|
||||
return false;
|
||||
}
|
||||
state_.width = width_;
|
||||
if (!cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_)) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "set height failed";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init): set height failed";
|
||||
return false;
|
||||
}
|
||||
state_.height = height_;
|
||||
if (!cap_.set(cv::CAP_PROP_FPS, fps_)) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "set fps failed";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (init): set fps failed";
|
||||
return false;
|
||||
}
|
||||
if (!cap_.set(cv::CAP_PROP_BUFFERSIZE, 1)) {
|
||||
CMVR_LOG(WARNING) << "[UVCCamera] (start): failed to limit capture buffer size";
|
||||
}
|
||||
state_.is_opened = true;
|
||||
return true;
|
||||
}
|
||||
@ -204,9 +196,7 @@ bool UVCCamera::stop() {
|
||||
is_streaming_running = false;
|
||||
}
|
||||
|
||||
if (cap_.isOpened()) {
|
||||
cap_.release();
|
||||
}
|
||||
close_capture_();
|
||||
state_.is_opened = false;
|
||||
return true;
|
||||
}
|
||||
@ -230,39 +220,304 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t requested_sequence = 0;
|
||||
{
|
||||
std::lock_guard capture_lock(capture_mutex_);
|
||||
if (!state_.is_streaming && !state_.is_recording) {
|
||||
cap_.release();
|
||||
if (!cap_.open(serial_, cv::CAP_V4L2)) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "failed to reopen camera for snapshot";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
|
||||
state_.is_opened = false;
|
||||
return;
|
||||
}
|
||||
std::lock_guard capture_lock(capture_frame_mutex_);
|
||||
requested_sequence = latest_capture_sequence_;
|
||||
}
|
||||
|
||||
cap_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
|
||||
cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_);
|
||||
cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_);
|
||||
cap_.set(cv::CAP_PROP_FPS, fps_);
|
||||
cap_.set(cv::CAP_PROP_BUFFERSIZE, 1);
|
||||
|
||||
if (!cap_.grab()) {
|
||||
state_.is_error = true;
|
||||
state_.error_message = "failed to discard camera startup frame";
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
|
||||
return;
|
||||
}
|
||||
int64_t capture_monotonic_ns = 0;
|
||||
int64_t capture_utc_ns = 0;
|
||||
if (!wait_for_capture_frame_(color,
|
||||
requested_sequence,
|
||||
capture_monotonic_ns,
|
||||
capture_utc_ns) || color.empty()) {
|
||||
std::string capture_error;
|
||||
{
|
||||
std::lock_guard capture_lock(capture_frame_mutex_);
|
||||
capture_error = capture_error_;
|
||||
}
|
||||
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;
|
||||
state_.is_error = true;
|
||||
state_.error_message = capture_error.empty()
|
||||
? "timed out waiting for a fresh camera frame"
|
||||
: capture_error;
|
||||
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
|
||||
color.release();
|
||||
}
|
||||
}
|
||||
|
||||
int UVCCamera::interrupt_capture_(void* opaque)
|
||||
{
|
||||
const auto* camera = static_cast<const UVCCamera*>(opaque);
|
||||
return camera && !camera->capture_running_.load(std::memory_order_relaxed);
|
||||
}
|
||||
|
||||
bool UVCCamera::open_capture_()
|
||||
{
|
||||
close_capture_();
|
||||
|
||||
{
|
||||
std::lock_guard frame_lock(capture_frame_mutex_);
|
||||
latest_capture_frame_.release();
|
||||
latest_capture_sequence_ = 0;
|
||||
latest_capture_monotonic_ns_ = 0;
|
||||
latest_capture_utc_ns_ = 0;
|
||||
capture_error_.clear();
|
||||
}
|
||||
|
||||
auto* input_format = av_find_input_format("v4l2");
|
||||
if (!input_format) {
|
||||
capture_error_ = "FFmpeg v4l2 input format is unavailable";
|
||||
return false;
|
||||
}
|
||||
|
||||
capture_format_context_ = avformat_alloc_context();
|
||||
if (!capture_format_context_) {
|
||||
capture_error_ = "failed to allocate FFmpeg capture context";
|
||||
return false;
|
||||
}
|
||||
|
||||
capture_running_.store(true, std::memory_order_relaxed);
|
||||
capture_format_context_->interrupt_callback.callback = &UVCCamera::interrupt_capture_;
|
||||
capture_format_context_->interrupt_callback.opaque = this;
|
||||
capture_format_context_->flags |= AVFMT_FLAG_NOBUFFER;
|
||||
|
||||
AVDictionary* options = nullptr;
|
||||
const std::string video_size = std::to_string(width_) + "x" + std::to_string(height_);
|
||||
av_dict_set(&options, "input_format", "mjpeg", 0);
|
||||
av_dict_set(&options, "video_size", video_size.c_str(), 0);
|
||||
av_dict_set(&options, "framerate", std::to_string(fps_).c_str(), 0);
|
||||
|
||||
int result = avformat_open_input(
|
||||
&capture_format_context_, serial_.c_str(), input_format, &options);
|
||||
av_dict_free(&options);
|
||||
if (result < 0) {
|
||||
const std::string error =
|
||||
"failed to open " + serial_ + ": " + ffmpeg_error_string(result);
|
||||
close_capture_();
|
||||
capture_error_ = error;
|
||||
return false;
|
||||
}
|
||||
|
||||
result = avformat_find_stream_info(capture_format_context_, nullptr);
|
||||
if (result < 0) {
|
||||
const std::string error =
|
||||
"failed to read camera stream info: " + ffmpeg_error_string(result);
|
||||
close_capture_();
|
||||
capture_error_ = error;
|
||||
return false;
|
||||
}
|
||||
|
||||
capture_video_stream_index_ = av_find_best_stream(
|
||||
capture_format_context_, AVMEDIA_TYPE_VIDEO, -1, -1, nullptr, 0);
|
||||
if (capture_video_stream_index_ < 0) {
|
||||
const std::string error = "camera has no video stream: "
|
||||
+ ffmpeg_error_string(capture_video_stream_index_);
|
||||
close_capture_();
|
||||
capture_error_ = error;
|
||||
return false;
|
||||
}
|
||||
|
||||
AVStream* video_stream = capture_format_context_->streams[capture_video_stream_index_];
|
||||
const AVCodec* decoder = avcodec_find_decoder(video_stream->codecpar->codec_id);
|
||||
if (!decoder) {
|
||||
const std::string error = "FFmpeg decoder is unavailable for camera input";
|
||||
close_capture_();
|
||||
capture_error_ = error;
|
||||
return false;
|
||||
}
|
||||
|
||||
capture_decoder_context_ = avcodec_alloc_context3(decoder);
|
||||
if (!capture_decoder_context_) {
|
||||
const std::string error = "failed to allocate camera decoder context";
|
||||
close_capture_();
|
||||
capture_error_ = error;
|
||||
return false;
|
||||
}
|
||||
|
||||
result = avcodec_parameters_to_context(capture_decoder_context_, video_stream->codecpar);
|
||||
if (result >= 0) {
|
||||
result = avcodec_open2(capture_decoder_context_, decoder, nullptr);
|
||||
}
|
||||
if (result < 0) {
|
||||
const std::string error =
|
||||
"failed to open camera decoder: " + ffmpeg_error_string(result);
|
||||
close_capture_();
|
||||
capture_error_ = error;
|
||||
return false;
|
||||
}
|
||||
|
||||
CMVR_LOG(INFO) << "[UVCCamera] FFmpeg capture opened"
|
||||
<< ", device=" << serial_
|
||||
<< ", input_codec=" << avcodec_get_name(video_stream->codecpar->codec_id)
|
||||
<< ", width=" << capture_decoder_context_->width
|
||||
<< ", height=" << capture_decoder_context_->height
|
||||
<< ", requested_fps=" << fps_;
|
||||
return true;
|
||||
}
|
||||
|
||||
void UVCCamera::close_capture_()
|
||||
{
|
||||
capture_running_.store(false, std::memory_order_relaxed);
|
||||
capture_frame_cv_.notify_all();
|
||||
|
||||
if (capture_thread_) {
|
||||
if (capture_thread_->joinable()) {
|
||||
capture_thread_->join();
|
||||
}
|
||||
capture_thread_.reset();
|
||||
}
|
||||
|
||||
if (capture_sws_context_) {
|
||||
sws_freeContext(capture_sws_context_);
|
||||
capture_sws_context_ = nullptr;
|
||||
}
|
||||
if (capture_decoder_context_) {
|
||||
avcodec_free_context(&capture_decoder_context_);
|
||||
}
|
||||
if (capture_format_context_) {
|
||||
avformat_close_input(&capture_format_context_);
|
||||
}
|
||||
capture_video_stream_index_ = -1;
|
||||
}
|
||||
|
||||
void UVCCamera::capture_worker_()
|
||||
{
|
||||
AVPacket* input_packet = av_packet_alloc();
|
||||
AVFrame* decoded_frame = av_frame_alloc();
|
||||
if (!input_packet || !decoded_frame) {
|
||||
{
|
||||
std::lock_guard frame_lock(capture_frame_mutex_);
|
||||
capture_error_ = "failed to allocate FFmpeg capture frame resources";
|
||||
}
|
||||
capture_running_.store(false, std::memory_order_relaxed);
|
||||
capture_frame_cv_.notify_all();
|
||||
av_packet_free(&input_packet);
|
||||
av_frame_free(&decoded_frame);
|
||||
return;
|
||||
}
|
||||
|
||||
while (capture_running_.load(std::memory_order_relaxed)) {
|
||||
int result = av_read_frame(capture_format_context_, input_packet);
|
||||
if (result == AVERROR(EAGAIN)) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
continue;
|
||||
}
|
||||
if (result < 0) {
|
||||
if (capture_running_.load(std::memory_order_relaxed)) {
|
||||
std::lock_guard frame_lock(capture_frame_mutex_);
|
||||
capture_error_ = "failed to read camera packet: " + ffmpeg_error_string(result);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (input_packet->stream_index != capture_video_stream_index_) {
|
||||
av_packet_unref(input_packet);
|
||||
continue;
|
||||
}
|
||||
|
||||
result = avcodec_send_packet(capture_decoder_context_, input_packet);
|
||||
av_packet_unref(input_packet);
|
||||
if (result < 0 && result != AVERROR(EAGAIN)) {
|
||||
std::lock_guard frame_lock(capture_frame_mutex_);
|
||||
capture_error_ = "failed to submit camera packet: " + ffmpeg_error_string(result);
|
||||
break;
|
||||
}
|
||||
|
||||
while (capture_running_.load(std::memory_order_relaxed)) {
|
||||
result = avcodec_receive_frame(capture_decoder_context_, decoded_frame);
|
||||
if (result == AVERROR(EAGAIN) || result == AVERROR_EOF) {
|
||||
break;
|
||||
}
|
||||
if (result < 0) {
|
||||
std::lock_guard frame_lock(capture_frame_mutex_);
|
||||
capture_error_ = "failed to decode camera frame: " + ffmpeg_error_string(result);
|
||||
capture_running_.store(false, std::memory_order_relaxed);
|
||||
break;
|
||||
}
|
||||
|
||||
capture_sws_context_ = sws_getCachedContext(
|
||||
capture_sws_context_,
|
||||
decoded_frame->width,
|
||||
decoded_frame->height,
|
||||
static_cast<AVPixelFormat>(decoded_frame->format),
|
||||
width_,
|
||||
height_,
|
||||
AV_PIX_FMT_BGR24,
|
||||
SWS_BILINEAR,
|
||||
nullptr,
|
||||
nullptr,
|
||||
nullptr);
|
||||
if (!capture_sws_context_) {
|
||||
std::lock_guard frame_lock(capture_frame_mutex_);
|
||||
capture_error_ = "failed to create camera pixel conversion context";
|
||||
capture_running_.store(false, std::memory_order_relaxed);
|
||||
break;
|
||||
}
|
||||
|
||||
cv::Mat bgr_frame(height_, width_, CV_8UC3);
|
||||
uint8_t* destination_data[] = {bgr_frame.data, nullptr, nullptr, nullptr};
|
||||
int destination_linesize[] = {
|
||||
static_cast<int>(bgr_frame.step[0]), 0, 0, 0
|
||||
};
|
||||
const int converted_rows = sws_scale(
|
||||
capture_sws_context_,
|
||||
decoded_frame->data,
|
||||
decoded_frame->linesize,
|
||||
0,
|
||||
decoded_frame->height,
|
||||
destination_data,
|
||||
destination_linesize);
|
||||
if (converted_rows != height_) {
|
||||
std::lock_guard frame_lock(capture_frame_mutex_);
|
||||
capture_error_ = "camera pixel conversion returned an incomplete frame";
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto capture_monotonic = std::chrono::steady_clock::now();
|
||||
const auto capture_utc = std::chrono::system_clock::now();
|
||||
{
|
||||
std::lock_guard frame_lock(capture_frame_mutex_);
|
||||
latest_capture_frame_ = std::move(bgr_frame);
|
||||
++latest_capture_sequence_;
|
||||
latest_capture_monotonic_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
capture_monotonic.time_since_epoch()).count();
|
||||
latest_capture_utc_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
capture_utc.time_since_epoch()).count();
|
||||
capture_error_.clear();
|
||||
}
|
||||
capture_frame_cv_.notify_all();
|
||||
av_frame_unref(decoded_frame);
|
||||
}
|
||||
}
|
||||
|
||||
capture_running_.store(false, std::memory_order_relaxed);
|
||||
capture_frame_cv_.notify_all();
|
||||
av_packet_free(&input_packet);
|
||||
av_frame_free(&decoded_frame);
|
||||
}
|
||||
|
||||
bool UVCCamera::wait_for_capture_frame_(cv::Mat& frame,
|
||||
uint64_t& last_sequence,
|
||||
int64_t& capture_monotonic_ns,
|
||||
int64_t& capture_utc_ns)
|
||||
{
|
||||
std::unique_lock frame_lock(capture_frame_mutex_);
|
||||
const auto timeout = std::chrono::milliseconds(
|
||||
std::max(500, fps_ > 0 ? 3000 / fps_ : 500));
|
||||
const bool ready = capture_frame_cv_.wait_for(frame_lock, timeout, [&] {
|
||||
return latest_capture_sequence_ > last_sequence
|
||||
|| !capture_running_.load(std::memory_order_relaxed);
|
||||
});
|
||||
if (!ready || latest_capture_sequence_ <= last_sequence || latest_capture_frame_.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
frame = latest_capture_frame_;
|
||||
last_sequence = latest_capture_sequence_;
|
||||
capture_monotonic_ns = latest_capture_monotonic_ns_;
|
||||
capture_utc_ns = latest_capture_utc_ns_;
|
||||
return true;
|
||||
}
|
||||
|
||||
void UVCCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
|
||||
@ -504,6 +759,9 @@ void UVCCamera::streaming_worker_() {
|
||||
bool success = false;
|
||||
is_streaming_running = true;
|
||||
cv::Mat frame;
|
||||
uint64_t capture_sequence = 0;
|
||||
int64_t frame_capture_monotonic_ns = 0;
|
||||
int64_t frame_capture_utc_ns = 0;
|
||||
uint64_t frame_sequence = 0;
|
||||
const uint64_t stream_epoch = static_cast<uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
@ -523,14 +781,19 @@ void UVCCamera::streaming_worker_() {
|
||||
while (state_.is_streaming || state_.is_recording) {
|
||||
// 记录当前帧处理开始时间
|
||||
const auto frame_start_time = std::chrono::steady_clock::now();
|
||||
bool frame_read = false;
|
||||
{
|
||||
std::lock_guard capture_lock(capture_mutex_);
|
||||
frame_read = cap_.read(frame);
|
||||
}
|
||||
if (!frame_read || frame.empty()) {
|
||||
if (!wait_for_capture_frame_(frame,
|
||||
capture_sequence,
|
||||
frame_capture_monotonic_ns,
|
||||
frame_capture_utc_ns) || frame.empty()) {
|
||||
std::string capture_error;
|
||||
{
|
||||
std::lock_guard capture_lock(capture_frame_mutex_);
|
||||
capture_error = capture_error_;
|
||||
}
|
||||
state_.is_error = true;
|
||||
state_.error_message = "failed to read frame";
|
||||
state_.error_message = capture_error.empty()
|
||||
? "timed out waiting for camera frame"
|
||||
: capture_error;
|
||||
CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_: " << state_.error_message;
|
||||
break;
|
||||
}
|
||||
@ -540,7 +803,7 @@ void UVCCamera::streaming_worker_() {
|
||||
StreamFrameData frame_data;
|
||||
// 保存图像数据
|
||||
{
|
||||
frame.copyTo(frame_data.rgbImage); // 执行深拷贝
|
||||
frame_data.rgbImage = frame;
|
||||
}
|
||||
const auto copy_end_time = std::chrono::steady_clock::now();
|
||||
|
||||
@ -570,10 +833,9 @@ void UVCCamera::streaming_worker_() {
|
||||
const uint64_t sequence = frame_sequence++;
|
||||
frame_data.stream_epoch = stream_epoch;
|
||||
frame_data.sequence = sequence;
|
||||
frame_data.capture_monotonic_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
frame_start_time.time_since_epoch()).count();
|
||||
frame_data.capture_utc_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
frame_data.source_frame_number = capture_sequence;
|
||||
frame_data.capture_monotonic_ns = frame_capture_monotonic_ns;
|
||||
frame_data.capture_utc_ns = frame_capture_utc_ns;
|
||||
frame_data.pts = static_cast<int64_t>(sequence);
|
||||
frame_data.dts = frame_data.pts;
|
||||
frame_data.time_base_num = 1;
|
||||
@ -634,17 +896,6 @@ void UVCCamera::streaming_worker_() {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 计算从帧开始到现在的总耗时
|
||||
auto total_duration = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now() - frame_start_time
|
||||
).count();
|
||||
|
||||
// 计算需要休眠的时间(确保总耗时达到frame_interval)
|
||||
int sleep_time = frame_interval - total_duration;
|
||||
// 只有当需要休眠的时间为正数时才休眠
|
||||
if (sleep_time > 0) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
|
||||
}
|
||||
}
|
||||
|
||||
is_streaming_running = false;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user