fix(camera): recover from incomplete UVC frames

cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp

@@ -27,6 +27,80 @@ std::string ffmpeg_error_string(const int error_code)
     return error_buffer;
 }
 
+AVPixelFormat normalize_deprecated_yuv_format(const AVPixelFormat format)
+{
+    switch (format) {
+        case AV_PIX_FMT_YUVJ420P:
+            return AV_PIX_FMT_YUV420P;
+        case AV_PIX_FMT_YUVJ422P:
+            return AV_PIX_FMT_YUV422P;
+        case AV_PIX_FMT_YUVJ444P:
+            return AV_PIX_FMT_YUV444P;
+        case AV_PIX_FMT_YUVJ440P:
+            return AV_PIX_FMT_YUV440P;
+        default:
+            return format;
+    }
+}
+
+int swscale_colorspace(const AVColorSpace colorspace)
+{
+    switch (colorspace) {
+        case AVCOL_SPC_BT709:
+            return SWS_CS_ITU709;
+        case AVCOL_SPC_BT2020_NCL:
+        case AVCOL_SPC_BT2020_CL:
+            return SWS_CS_BT2020;
+        case AVCOL_SPC_SMPTE170M:
+        case AVCOL_SPC_BT470BG:
+            return SWS_CS_ITU601;
+        default:
+            return SWS_CS_DEFAULT;
+    }
+}
+
+bool is_sof_marker(const uint8_t marker)
+{
+    return marker >= 0xc0 && marker <= 0xcf
+        && marker != 0xc4 && marker != 0xc8 && marker != 0xcc;
+}
+
+bool is_complete_mjpeg_packet(const AVPacket* packet)
+{
+    if (!packet || !packet->data || packet->size < 4
+        || packet->data[0] != 0xff || packet->data[1] != 0xd8) {
+        return false;
+    }
+
+    bool found_sof = false;
+    for (int index = 2; index + 1 < packet->size; ++index) {
+        if (packet->data[index] != 0xff) {
+            continue;
+        }
+
+        int marker_index = index + 1;
+        while (marker_index < packet->size && packet->data[marker_index] == 0xff) {
+            ++marker_index;
+        }
+        if (marker_index >= packet->size) {
+            break;
+        }
+
+        const uint8_t marker = packet->data[marker_index];
+        if (marker == 0x00) {
+            index = marker_index;
+            continue;
+        }
+        if (is_sof_marker(marker)) {
+            found_sof = true;
+        } else if (marker == 0xd9) {
+            return found_sof;
+        }
+        index = marker_index;
+    }
+    return false;
+}
+
 } // namespace
 
 UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
@@ -386,6 +460,7 @@ void UVCCamera::capture_worker_()
 {
     AVPacket* input_packet = av_packet_alloc();
     AVFrame* decoded_frame = av_frame_alloc();
+    uint64_t discarded_frame_count = 0;
     if (!input_packet || !decoded_frame) {
         {
             std::lock_guard frame_lock(capture_frame_mutex_);
@@ -417,8 +492,30 @@ void UVCCamera::capture_worker_()
             continue;
         }
 
+        if (!is_complete_mjpeg_packet(input_packet)) {
+            av_packet_unref(input_packet);
+            ++discarded_frame_count;
+            if (discarded_frame_count == 1 || discarded_frame_count % 30 == 0) {
+                CMVR_LOG(WARNING) << "[UVCCamera] discarded incomplete MJPEG frame"
+                                  << ", device=" << serial_
+                                  << ", discarded=" << discarded_frame_count;
+            }
+            continue;
+        }
+
         result = avcodec_send_packet(capture_decoder_context_, input_packet);
         av_packet_unref(input_packet);
+        if (result == AVERROR_INVALIDDATA) {
+            ++discarded_frame_count;
+            if (discarded_frame_count == 1 || discarded_frame_count % 30 == 0) {
+                CMVR_LOG(WARNING) << "[UVCCamera] discarded invalid MJPEG frame"
+                                  << ", device=" << serial_
+                                  << ", discarded=" << discarded_frame_count;
+            }
+            avcodec_flush_buffers(capture_decoder_context_);
+            av_frame_unref(decoded_frame);
+            continue;
+        }
         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);
@@ -430,6 +527,17 @@ void UVCCamera::capture_worker_()
             if (result == AVERROR(EAGAIN) || result == AVERROR_EOF) {
                 break;
             }
+            if (result == AVERROR_INVALIDDATA) {
+                ++discarded_frame_count;
+                if (discarded_frame_count == 1 || discarded_frame_count % 30 == 0) {
+                    CMVR_LOG(WARNING) << "[UVCCamera] discarded undecodable MJPEG frame"
+                                      << ", device=" << serial_
+                                      << ", discarded=" << discarded_frame_count;
+                }
+                avcodec_flush_buffers(capture_decoder_context_);
+                av_frame_unref(decoded_frame);
+                break;
+            }
             if (result < 0) {
                 std::lock_guard frame_lock(capture_frame_mutex_);
                 capture_error_ = "failed to decode camera frame: " + ffmpeg_error_string(result);
@@ -437,11 +545,15 @@ void UVCCamera::capture_worker_()
                 break;
             }
 
+            const auto decoded_format = static_cast<AVPixelFormat>(decoded_frame->format);
+            const auto source_format = normalize_deprecated_yuv_format(decoded_format);
+            const bool source_full_range = decoded_frame->color_range == AVCOL_RANGE_JPEG
+                || source_format != decoded_format;
             capture_sws_context_ = sws_getCachedContext(
                 capture_sws_context_,
                 decoded_frame->width,
                 decoded_frame->height,
-                static_cast<AVPixelFormat>(decoded_frame->format),
+                source_format,
                 width_,
                 height_,
                 AV_PIX_FMT_BGR24,
@@ -456,6 +568,25 @@ void UVCCamera::capture_worker_()
                 break;
             }
 
+            const int* color_coefficients = sws_getCoefficients(
+                swscale_colorspace(decoded_frame->colorspace));
+            const int color_result = sws_setColorspaceDetails(
+                capture_sws_context_,
+                color_coefficients,
+                source_full_range ? 1 : 0,
+                color_coefficients,
+                1,
+                0,
+                1 << 16,
+                1 << 16);
+            if (color_result < 0) {
+                std::lock_guard frame_lock(capture_frame_mutex_);
+                capture_error_ = "failed to configure camera color conversion: "
+                    + ffmpeg_error_string(color_result);
+                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[] = {
@@ -1062,7 +1193,9 @@ bool UVCCamera::startStreaming()
 void UVCCamera::stopStreaming()
 {
     std::lock_guard lock(ctrl_mtx_);
-    stream_count_--;
+    if (stream_count_ > 0) {
+        --stream_count_;
+    }
     if (stream_count_ == 0)
     {
         // 当前已经没有正在使用的流了，编码采集线程状态修改