diff --git a/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt b/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt index 0b36983e..6ba925bb 100644 --- a/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt +++ b/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt @@ -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) diff --git a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h index aa9a32e9..e429bbca 100644 --- a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h +++ b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h @@ -5,6 +5,9 @@ #ifndef CMVR_ES_UVC_CAMERA_H #define CMVR_ES_UVC_CAMERA_H +#include +#include + #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 video_writer_; std::shared_ptr stream_thread_; std::shared_ptr recording_thread_; + std::shared_ptr 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 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; diff --git a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp index 4086c473..1bad4278 100644 --- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp +++ b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp @@ -4,14 +4,31 @@ // #include +#include #include "../include/uvc_camera.h" +extern "C" { +#include +#include +} + 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>(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(&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(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(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(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( + capture_monotonic.time_since_epoch()).count(); + latest_capture_utc_ns_ = std::chrono::duration_cast( + 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( std::chrono::duration_cast( @@ -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( - frame_start_time.time_since_epoch()).count(); - frame_data.capture_utc_ns = std::chrono::duration_cast( - 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(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::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;