fix(camera): handle UVC snapshots in video mode

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<SPMCRingBuffer<StreamFrameData>>(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()) {
+            if (stream_thread_) {
-                stream_thread_->join();
+                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;

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<const char*>(depth_image.data), depth_image.total() * depth_image.elemSize());
         response->mutable_depth_frame()->set_height(depth_image.rows);