From b0efc341a9d3ea5f4b7b83ddabe380d3f2ac7d70 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Tue, 11 Aug 2026 17:19:01 +0800 Subject: [PATCH] fix(camera): handle UVC snapshots in video mode --- .../camera/uvc_camera/src/uvc_camera.cpp | 69 ++++++++++++++++++- .../service/grpc/src/grpc_camera_service.cpp | 16 ++++- 2 files changed, 81 insertions(+), 4 deletions(-) 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 52a1d121..4372b791 100644 --- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp +++ b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp @@ -68,6 +68,11 @@ bool UVCCamera::init() { clear_error_(); state_.is_initialized = false; + { + std::lock_guard frame_lock(frame_mutex_); + latest_frame_.release(); + } + stream_frame_buffer_ = std::make_shared>(buffer_size_); stream_frame_buffer_->clear(); if (cap_.isOpened()) { @@ -135,6 +140,12 @@ bool UVCCamera::start() { CMVR_LOG(WARNING) << "[RealsenseCamera] (start): camera already started"; return true; } + + { + std::lock_guard frame_lock(frame_mutex_); + latest_frame_.release(); + } + cap_.open(serial_, cv::CAP_V4L2); this_thread::sleep_for(chrono::milliseconds(100)); @@ -186,16 +197,22 @@ bool UVCCamera::stop() { } if (mode_ == VIDEO_MODE){ state_.is_streaming = false; - if (stream_thread_->joinable()) { - stream_thread_->join(); + if (stream_thread_) { + if (stream_thread_->joinable()) { + stream_thread_->join(); + } stream_thread_.reset(); - stream_thread_ = nullptr; } + is_streaming_running = false; } if (cap_.isOpened()) { cap_.release(); } + { + std::lock_guard frame_lock(frame_mutex_); + latest_frame_.release(); + } state_.is_opened = false; return true; } @@ -236,6 +253,39 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) color.release(); return; } + + { + std::lock_guard frame_lock(frame_mutex_); + if (!latest_frame_.empty()) { + latest_frame_.copyTo(color); + } + } + + const bool capture_worker_requested = state_.is_streaming || state_.is_recording; + if (color.empty() && !capture_worker_requested) { + 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 = "no video frame available"; + CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message; + return; + } } } @@ -510,6 +560,11 @@ void UVCCamera::streaming_worker_() { } const auto capture_end_time = std::chrono::steady_clock::now(); + { + std::lock_guard frame_lock(frame_mutex_); + frame.copyTo(latest_frame_); + } + // 以下为编码部分，用于流模式 StreamFrameData frame_data; // 保存图像数据 @@ -622,6 +677,10 @@ void UVCCamera::streaming_worker_() { } is_streaming_running = false; + { + std::lock_guard frame_lock(frame_mutex_); + latest_frame_.release(); + } // 线程结束时清空队列 stream_frame_buffer_->clear(); recordingIndex_ = 0; @@ -633,6 +692,10 @@ void UVCCamera::streaming_worker_() { stream_frame_buffer_->clear(); // 确保线程状态正确更新 is_streaming_running = false; + { + std::lock_guard frame_lock(frame_mutex_); + latest_frame_.release(); + } state_.is_error = true; state_.error_message = e.what(); CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message; diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 569279e8..08bc739c 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -205,7 +205,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, } Rs2Intrinsics intrinsics = {0}; dev->getRGBImage(image,intrinsics); - response->mutable_header()->set_success(true); + if (image.empty()) { + return failResponse(response, "Camera returned an empty RGB image: " + dev_id); + } response->mutable_intrinsics()->set_fx(intrinsics.fx); response->mutable_intrinsics()->set_fy(intrinsics.fy); @@ -258,6 +260,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, } Rs2Intrinsics intrinsics = {0}; dev->getDepthImage(image,intrinsics); + if (image.empty()) { + return failResponse(response, "Camera returned an empty depth image: " + dev_id); + } response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); @@ -314,6 +319,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, } Rs2Intrinsics intrinsics = {0}; dev->getRGBDImages(color_image,depth_image, intrinsics); + if (color_image.empty()) { + return failResponse(response, "Camera returned an empty RGB image: " + dev_id); + } + if (depth_image.empty()) { + return failResponse(response, "Camera returned an empty depth image: " + dev_id); + } response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); @@ -355,6 +366,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, else if (depth_image.type() == CV_32FC1) { response->mutable_depth_frame()->set_type(api::FrameData::F32C1); } + else { + return failResponse(response, "unsupported depth image type"); + } response->mutable_depth_frame()->set_data( reinterpret_cast(depth_image.data), depth_image.total() * depth_image.elemSize()); response->mutable_depth_frame()->set_height(depth_image.rows);