use lgv direct camera grpc service

cmvr-es/service/grpc/include/grpc_camera_service.h

@@ -14,8 +14,7 @@ namespace cmvr::service {
 
     class gRPCCameraServiceImpl final: public api::CameraService::Service {
     public:
-        explicit gRPCCameraServiceImpl(
+        gRPCCameraServiceImpl();
-            CameraStreamLowLatencyConfig stream_config = {});
         ~gRPCCameraServiceImpl() override = default;
         grpc::Status GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) override;
         grpc::Status StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) override;
@@ -25,13 +24,11 @@ namespace cmvr::service {
         grpc::Status GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) override;
         grpc::Status StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) override;
         grpc::Status StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) override;
-        grpc::Status ControlPtz(grpc::ServerContext* context, const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response) override;
         grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream) override;
         grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream) override;
         grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream) override;
     private:
         device::DeviceManager& dmgr_;
-        CameraStreamLowLatencyConfig stream_config_;
 
         //双向流读写线程
         std::shared_ptr<std::thread> read_thread_ = nullptr;

cmvr-es/service/grpc/src/grpc_camera_service.cpp

@@ -5,12 +5,7 @@
 
 #include "../include/grpc_camera_service.h"
 
-#include <algorithm>
-#include <atomic>
-#include <chrono>
-#include <cstdint>
 #include <limits>
-#include <thread>
 
 using namespace std;
 using namespace cmvr::service;
@@ -25,88 +20,9 @@ grpc::Status failResponse(ResponseT* response, const std::string& message) {
     setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
     return grpc::Status::OK;
 }
-
-bool toPtzCommand(cmvr::api::ControlPtzCommand_Command command, PtzCommand& out)
-{
-    switch (command) {
-        case cmvr::api::ControlPtzCommand_Command_TILT_UP:
-            out = PtzCommand::TiltUp;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_TILT_DOWN:
-            out = PtzCommand::TiltDown;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_PAN_LEFT:
-            out = PtzCommand::PanLeft;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_PAN_RIGHT:
-            out = PtzCommand::PanRight;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_UP_LEFT:
-            out = PtzCommand::UpLeft;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_UP_RIGHT:
-            out = PtzCommand::UpRight;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_DOWN_LEFT:
-            out = PtzCommand::DownLeft;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_DOWN_RIGHT:
-            out = PtzCommand::DownRight;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_ZOOM_IN:
-            out = PtzCommand::ZoomIn;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_ZOOM_OUT:
-            out = PtzCommand::ZoomOut;
-            return true;
-        case cmvr::api::ControlPtzCommand_Command_PAN_AUTO:
-            out = PtzCommand::PanAuto;
-            return true;
-        default:
-            return false;
-    }
 }
 
-// The legacy depth/RGBD RPCs acquire the camera's shared producer directly
+gRPCCameraServiceImpl::gRPCCameraServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
-// instead of going through MediaSourceHub.  Keep that lease exception-safe:
-// cancellation, a failed Write(), or any conversion error must release exactly
-// the one startStreaming() reference acquired by this call.
-class CameraStreamingLease final {
-public:
-    explicit CameraStreamingLease(std::shared_ptr<AbstractCamera> camera)
-        : camera_(std::move(camera)) {
-        active_ = camera_ && camera_->startStreaming();
-    }
-
-    ~CameraStreamingLease() {
-        if (!active_ || !camera_) {
-            return;
-        }
-        try {
-            camera_->stopStreaming();
-        } catch (const std::exception& e) {
-            CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease: "
-                            << e.what();
-        } catch (...) {
-            CMVR_LOG(ERROR) << "[gRPCCameraServiceImpl] failed to release camera stream lease";
-        }
-    }
-
-    CameraStreamingLease(const CameraStreamingLease&) = delete;
-    CameraStreamingLease& operator=(const CameraStreamingLease&) = delete;
-
-    explicit operator bool() const noexcept { return active_; }
-
-private:
-    std::shared_ptr<AbstractCamera> camera_;
-    bool active_{false};
-};
-}
-
-gRPCCameraServiceImpl::gRPCCameraServiceImpl(
-    CameraStreamLowLatencyConfig stream_config)
-    : dmgr_(DeviceManager::getInstance()),
-      stream_config_(stream_config) {}
 
 grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
     const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response)
@@ -131,6 +47,14 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
         response->mutable_state()->set_fps(state.fps);
         response->mutable_state()->set_width(state.width);
         response->mutable_state()->set_height(state.height);
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetStatus): success, id=" << dev_id
+                        << ", initialized=" << state.is_initialized
+                        << ", opened=" << state.is_opened
+                        << ", streaming=" << state.is_streaming
+                        << ", recording=" << state.is_recording
+                        << ", error=" << state.is_error
+                        << ", size=" << state.width << "x" << state.height
+                        << ", fps=" << state.fps;
         return grpc::Status::OK;
     }
     catch(const exception &e) {
@@ -156,6 +80,7 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context,
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartCamera): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const exception &e) {
@@ -181,6 +106,7 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context,
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopCamera): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -204,9 +130,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
         }
         Rs2Intrinsics intrinsics = {0};
         dev->getRGBImage(image,intrinsics);
-        if (image.empty()) {
+        response->mutable_header()->set_success(true);
-            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);
@@ -236,6 +160,10 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
         response->mutable_color_frame()->set_height(image.rows);
         response->mutable_color_frame()->set_width(image.cols);
         response->mutable_color_frame()->set_codec("none");
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImage): success, id=" << dev_id
+                        << ", size=" << image.cols << "x" << image.rows
+                        << ", cv_type=" << image.type()
+                        << ", bytes=" << image.total() * image.elemSize();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -259,9 +187,6 @@ 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);
@@ -295,6 +220,10 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
         response->mutable_depth_frame()->set_height(image.rows);
         response->mutable_depth_frame()->set_width(image.cols);
         response->mutable_depth_frame()->set_codec("none");
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImage): success, id=" << dev_id
+                        << ", size=" << image.cols << "x" << image.rows
+                        << ", cv_type=" << image.type()
+                        << ", bytes=" << image.total() * image.elemSize();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -318,12 +247,6 @@ 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);
@@ -336,8 +259,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
 
-        // color image is serialized in the existing OpenCV BGR byte order;
+        // color image
-        // clients convert it once when constructing an RGB image.
         if (color_image.type() == CV_8UC3) {
             response->mutable_color_frame()->set_type(api::FrameData::U8C3);
         }
@@ -366,14 +288,16 @@ 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);
         response->mutable_depth_frame()->set_width(depth_image.cols);
         response->mutable_depth_frame()->set_codec("none");
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImages): success, id=" << dev_id
+                        << ", color_size=" << color_image.cols << "x" << color_image.rows
+                        << ", color_type=" << color_image.type()
+                        << ", depth_size=" << depth_image.cols << "x" << depth_image.rows
+                        << ", depth_type=" << depth_image.type();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -397,6 +321,8 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context,
         dev->startRecording(request->video_path());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartRecording): success, id=" << dev_id
+                        << ", path=" << request->video_path();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -420,50 +346,7 @@ grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context,
         dev->stopRecording();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopRecording): success, id=" << dev_id;
-    }
-    catch (exception &e) {
-        response->mutable_header()->set_success(false);
-        response->mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
-        return grpc::Status::OK;
-    }
-}
-
-grpc::Status gRPCCameraServiceImpl::ControlPtz(grpc::ServerContext* context,
-    const api::ControlPtzCommand_Request* request, api::ControlPtzCommand_Feedback* response)
-{
-    try {
-        string dev_id = request->header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (ControlPtz): id=" << dev_id
-                       << ", command=" << request->command()
-                       << ", action=" << request->action()
-                       << ", speed=" << request->speed();
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            return failResponse(response, "Camera device not found: " + dev_id);
-        }
-
-        PtzCommand command{};
-        if (!toPtzCommand(request->command(), command)) {
-            return failResponse(response, "Invalid PTZ command");
-        }
-
-        if (request->action() != api::ControlPtzCommand_Action_START &&
-            request->action() != api::ControlPtzCommand_Action_STOP) {
-            return failResponse(response, "Invalid PTZ action");
-        }
-        const bool stop = request->action() == api::ControlPtzCommand_Action_STOP;
-        if (!dev->controlPtz(command, stop, static_cast<int>(request->speed()))) {
-            CameraState state{};
-            dev->getState(state);
-            const std::string error_message =
-                state.error_message.empty() ? "Failed to control PTZ: " + dev_id : state.error_message;
-            return failResponse(response, error_message);
-        }
-
-        response->mutable_header()->set_success(true);
-        setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -480,11 +363,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
     try {
         //读取首次传递的数据，获取设备id
         api::GetDepthImageStreamCommand_Request request;
-        if (!stream->Read(&request)) {
+        stream->Read(&request);
-            return grpc::Status::OK;
-        }
         string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start,id=" << dev_id;
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
         const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
         if (!dev) {
             api::GetDepthImageStreamCommand_Feedback response;
@@ -494,17 +375,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
             stream->Write(response);
             return grpc::Status::OK;
         }
-        CameraStreamingLease stream_lease(dev);
-        if (!stream_lease) {
-            api::GetDepthImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-
         int nFrameCount = 0;
+        dev->startStreaming();
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start streaming success, id=" << dev_id;
         size_t index = 0;
         while (true)
         {
@@ -516,39 +389,39 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
 
             api::GetDepthImageStreamCommand_Feedback response;
             cmvr::device::StreamFrameData frame_data;
-            if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
+            dev->getEncodedFrame(frame_data,index);
-                !frame_data.depthFrame.empty()) {
+            if (!frame_data.rgbFrame.empty()) {
                 response.mutable_header()->set_success(true);
-                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-                response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
                 response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
                 response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
-                response.mutable_depth_frame()->set_codec("none");
+                response.mutable_depth_frame()->set_codec(frame_data.codec);
-                response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
+                response.mutable_depth_frame()->set_width(frame_data.width);
-                response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
+                response.mutable_depth_frame()->set_height(frame_data.height);
 
                 response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
+                response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx);
+                response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx);
-                response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy);
+                response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx);
                 for (int i = 0; i < 5 ; i++) {
                     response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
                 }
 
                 response.set_seq_no(nFrameCount++);
 
+                grpc::WriteOptions options;
+                options.set_last_message();
                 if (!stream->Write(response)) {
                     CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
                     break;
                 }
             }
         }
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetDepthImageStream): end,id=" << dev_id;
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id;
+        dev->stopStreaming();
         return grpc::Status::OK;
     }
-    catch (const exception &e) {
+    catch (exception &e) {
         api::GetDepthImageStreamCommand_Feedback response;
-        response.mutable_header()->set_success(false);
         response.mutable_header()->set_error_message(e.what());
         setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
         stream->Write(response);
@@ -560,11 +433,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
     try {
         //读取首次传递的数据，获取设备id
         api::GetRGBDImagesStreamCommand_Request request;
-        if (!stream->Read(&request)) {
+        stream->Read(&request);
-            return grpc::Status::OK;
-        }
         string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start,id=" << dev_id;
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
         const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
         if (!dev) {
             api::GetRGBDImagesStreamCommand_Feedback response;
@@ -574,17 +445,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
             stream->Write(response);
             return grpc::Status::OK;
         }
-        CameraStreamingLease stream_lease(dev);
-        if (!stream_lease) {
-            api::GetRGBDImagesStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to start camera stream: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-
         int nFrameCount = 0;
+        dev->startStreaming();
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start streaming success, id=" << dev_id;
         size_t index = 0;
         while (true)
         {
@@ -596,23 +459,95 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
 
             api::GetRGBDImagesStreamCommand_Feedback response;
             cmvr::device::StreamFrameData frame_data;
-            if (dev->waitEncodedFrame(frame_data, index, std::chrono::milliseconds(100)) &&
+            dev->getEncodedFrame(frame_data,index);
-                !frame_data.rgbFrame.empty() &&
+            if (!frame_data.rgbFrame.empty()) {
-                !frame_data.depthFrame.empty()) {
                 response.mutable_header()->set_success(true);
-                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
                 response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
                 response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
                 response.mutable_color_frame()->set_codec(frame_data.codec);
                 response.mutable_color_frame()->set_width(frame_data.width);
                 response.mutable_color_frame()->set_height(frame_data.height);
 
-                response.mutable_depth_frame()->set_type(api::FrameData::U16C1);
                 response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size());
                 response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey);
-                response.mutable_depth_frame()->set_codec("none");
+                response.mutable_depth_frame()->set_codec(frame_data.codec);
-                response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width);
+                response.mutable_depth_frame()->set_width(frame_data.width);
-                response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height);
+                response.mutable_depth_frame()->set_height(frame_data.height);
+
+                response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
+                response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);
+                response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx);
+                response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx);
+                for (int i = 0; i < 5 ; i++) {
+                    response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]);
+                }
+
+                response.set_seq_no(nFrameCount++);
+
+                grpc::WriteOptions options;
+                options.set_last_message();
+                if (!stream->Write(response)) {
+                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
+                    break;
+                }
+            }
+        }
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id;
+        dev->stopStreaming();
+        return grpc::Status::OK;
+    }
+    catch (exception &e) {
+        api::GetRGBDImagesStreamCommand_Feedback response;
+        response.mutable_header()->set_error_message(e.what());
+        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+        stream->Write(response);
+        return grpc::Status::OK;
+    }
+}
+grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream){
+    try {
+        //读取首次传递的数据，获取设备id
+        api::GetRGBImageStreamCommand_Request request;
+        stream->Read(&request);
+        string dev_id = request.header().device_id();
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id;
+        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
+        if (!dev) {
+            api::GetRGBImageStreamCommand_Feedback response;
+            response.mutable_header()->set_success(false);
+            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
+            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
+            stream->Write(response);
+            return grpc::Status::OK;
+        }
+        int nFrameCount = 0;
+        dev->startStreaming();
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start streaming success, id=" << dev_id;
+        size_t last_sent_index = std::numeric_limits<size_t>::max();
+        while (true)
+        {
+            if (context->IsCancelled())
+            {
+                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id;
+                break;
+            }
+
+            api::GetRGBImageStreamCommand_Feedback response;
+            cmvr::device::StreamFrameData frame_data;
+            size_t next_index = last_sent_index;
+            if (!dev->getLatestEncodedFrame(frame_data, next_index) ||
+                frame_data.rgbFrame.empty() ||
+                next_index == last_sent_index) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(1));
+                continue;
+            }
+
+            response.mutable_header()->set_success(true);
+            response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size());
+            response.mutable_color_frame()->set_is_key_frame(frame_data.bKey);
+            response.mutable_color_frame()->set_codec(frame_data.codec);
+            response.mutable_color_frame()->set_width(frame_data.width);
+            response.mutable_color_frame()->set_height(frame_data.height);
 
             response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx);
             response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);
@@ -628,293 +563,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
                 CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id;
                 break;
             }
+            last_sent_index = next_index;
         }
-        }
+        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id;
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): end,id=" << dev_id;
+        dev->stopStreaming();
         return grpc::Status::OK;
     }
-    catch (const exception &e) {
+    catch (exception &e) {
-        api::GetRGBDImagesStreamCommand_Feedback response;
+        api::GetRGBImageStreamCommand_Feedback response;
-        response.mutable_header()->set_success(false);
-        response.mutable_header()->set_error_message(e.what());
-        setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-        stream->Write(response);
-        return grpc::Status::OK;
-    }
-}
-grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream){
-    try {
-        //读取首次传递的数据，获取设备id
-        api::GetRGBImageStreamCommand_Request request;
-        if (!stream->Read(&request)) {
-            return grpc::Status::OK;
-        }
-        string dev_id = request.header().device_id();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id
-                       << ", max_pending_frames=" << stream_config_.max_pending_frames
-                       << ", max_frame_age_ms=" << stream_config_.max_frame_age.count();
-        const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
-        if (!dev) {
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Camera device not found: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        auto& media_hub = cmvr::media::globalMediaSourceHub();
-        const std::string track_id = cmvr::media::cameraColorTrackId(dev_id);
-        if (!cmvr::media::ensureCameraMediaSource(media_hub, dev)) {
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to register camera media source: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-        auto subscription = media_hub.subscribe(
-            track_id,
-            cmvr::media::MediaSourceHub::StartPosition::NEXT_PUBLISHED,
-            [context] { return context->IsCancelled(); });
-        if (!subscription) {
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(false);
-            response.mutable_header()->set_error_message("Failed to subscribe camera media source: " + dev_id);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            stream->Write(response);
-            return grpc::Status::OK;
-        }
-
-        std::atomic<bool> client_eof_requested{false};
-        std::atomic<bool> request_stream_closed{false};
-        std::atomic<uint64_t> control_requests_read{1};
-        std::thread request_reader([&] {
-            api::GetRGBImageStreamCommand_Request control_request;
-            while (stream->Read(&control_request)) {
-                ++control_requests_read;
-                if (control_request.eof()) {
-                    client_eof_requested.store(true, std::memory_order_release);
-                    CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] client requested RGB stream EOF"
-                                   << ", id=" << dev_id
-                                   << ", peer=" << context->peer()
-                                   << ", control_requests=" << control_requests_read.load();
-                    break;
-                }
-            }
-            request_stream_closed.store(true, std::memory_order_release);
-        });
-        struct RequestReaderJoiner {
-            std::thread& thread;
-            ~RequestReaderJoiner() {
-                if (thread.joinable()) {
-                    thread.join();
-                }
-            }
-        } request_reader_joiner{request_reader};
-        const auto join_request_reader = [&] {
-            if (request_reader.joinable()) {
-                request_reader.join();
-            }
-        };
-
-        bool waiting_for_key_frame = true;
-        const char* exit_reason = "unknown";
-        auto last_key_frame_request = std::chrono::steady_clock::now();
-        auto last_latency_log = std::chrono::steady_clock::time_point{};
-        uint64_t discarded_since_log = 0;
-        std::chrono::microseconds last_write_duration{0};
-        media_hub.requestKeyFrame(track_id);
-        const auto request_key_frame_if_due = [&] {
-            const auto now = std::chrono::steady_clock::now();
-            if (now - last_key_frame_request >= std::chrono::milliseconds(500)) {
-                media_hub.requestKeyFrame(track_id);
-                last_key_frame_request = now;
-            }
-        };
-        const auto request_key_frame_now = [&] {
-            waiting_for_key_frame = true;
-            media_hub.requestKeyFrame(track_id);
-            last_key_frame_request = std::chrono::steady_clock::now();
-        };
-        const auto log_latency_event = [&](
-            const char* reason,
-            const uint64_t discarded,
-            const uint64_t frame_age_ns,
-            const std::chrono::microseconds write_duration) {
-            discarded_since_log += discarded;
-            const auto now = std::chrono::steady_clock::now();
-            if (last_latency_log != std::chrono::steady_clock::time_point{} &&
-                now - last_latency_log < std::chrono::seconds(1)) {
-                return;
-            }
-            const double frame_age_ms = static_cast<double>(frame_age_ns) / 1'000'000.0;
-            const double write_ms = static_cast<double>(write_duration.count()) / 1'000.0;
-            CMVR_LOG(WARNING) << "[gRPCCameraServiceImpl] low-latency camera stream event"
-                              << ", id=" << dev_id
-                              << ", reason=" << reason
-                              << ", discarded=" << discarded_since_log
-                              << ", age_ms=" << frame_age_ms
-                              << ", write_ms=" << write_ms
-                              << ", max_pending_frames="
-                              << stream_config_.max_pending_frames
-                              << ", max_frame_age_ms="
-                              << stream_config_.max_frame_age.count();
-            discarded_since_log = 0;
-            last_latency_log = now;
-        };
-        while (true)
-        {
-            if (client_eof_requested.load(std::memory_order_acquire)) {
-                exit_reason = "client_eof";
-                break;
-            }
-            if (context->IsCancelled())
-            {
-                exit_reason = "context_cancelled";
-                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled"
-                               << ", id=" << dev_id
-                               << ", peer=" << context->peer()
-                               << ", client_eof=" << client_eof_requested.load()
-                               << ", request_stream_closed=" << request_stream_closed.load();
-                break;
-            }
-
-            const auto read = subscription.waitRead(std::chrono::milliseconds(100));
-            if (!read || !read->value || read->value->empty()) {
-                if (!subscription.valid()) {
-                    exit_reason = "subscription_invalid";
-                    break;
-                }
-                if (waiting_for_key_frame) {
-                    request_key_frame_if_due();
-                }
-                continue;
-            }
-            const auto& frame = *read->value;
-            const auto descriptor = frame.descriptor;
-            if (!descriptor) {
-                continue;
-            }
-            const uint64_t now_ns = static_cast<uint64_t>(
-                std::chrono::duration_cast<std::chrono::nanoseconds>(
-                    std::chrono::steady_clock::now().time_since_epoch()).count());
-            const auto frame_age = cameraFrameAgeNs(frame.capture_time_ns, now_ns);
-            if (frame_age &&
-                cameraFrameExceedsAgeLimit(
-                    frame.capture_time_ns,
-                    now_ns,
-                    stream_config_.max_frame_age)) {
-                // This frame is already outside the latency budget. Flush all
-                // currently queued frames and wait for a fresh IDR; sending any
-                // P/B frame after an intentional gap would break decoder continuity.
-                const uint64_t discarded =
-                    1 + subscription.discardPendingIfExceeds(0);
-                request_key_frame_now();
-                log_latency_event(
-                    "stale_frame",
-                    discarded,
-                    *frame_age,
-                    last_write_duration);
-                continue;
-            }
-            const bool inter_frame_codec = descriptor->codec == cmvr::media::Codec::H264 ||
-                                           descriptor->codec == cmvr::media::Codec::H265;
-            if (!inter_frame_codec || descriptor->payload_format != cmvr::media::PayloadFormat::ANNEX_B) {
-                api::GetRGBImageStreamCommand_Feedback response;
-                response.mutable_header()->set_success(false);
-                response.mutable_header()->set_error_message(
-                    "Unsupported camera stream codec or payload format: " + dev_id);
-                setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-                stream->Write(response);
-                exit_reason = "unsupported_stream";
-                break;
-            }
-            if (read->dropped_since_last_read > 0 || read->generation_changed || frame.discontinuity) {
-                request_key_frame_now();
-            }
-            if (waiting_for_key_frame && !frame.key_frame) {
-                request_key_frame_if_due();
-                continue;
-            }
-            waiting_for_key_frame = false;
-
-            api::GetRGBImageStreamCommand_Feedback response;
-            response.mutable_header()->set_success(true);
-            setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
-            response.mutable_color_frame()->set_data(frame.data(), frame.size());
-            response.mutable_color_frame()->set_is_key_frame(frame.key_frame);
-            response.mutable_color_frame()->set_codec(
-                descriptor->codec == cmvr::media::Codec::H264 ? "h264" :
-                descriptor->codec == cmvr::media::Codec::H265 ? "h265" : "unknown");
-            response.mutable_color_frame()->set_width(static_cast<int32_t>(descriptor->width));
-            response.mutable_color_frame()->set_height(static_cast<int32_t>(descriptor->height));
-            response.mutable_color_frame()->set_capture_utc_ns(frame.capture_utc_ns);
-            response.mutable_color_frame()->set_source_sequence(frame.sequence);
-            response.mutable_color_frame()->set_pts(frame.pts);
-            response.mutable_color_frame()->set_dts(frame.dts);
-            response.mutable_color_frame()->set_source_fps(descriptor->nominal_rate);
-            response.mutable_color_frame()->set_source_timestamp(frame.source_timestamp);
-            response.mutable_color_frame()->set_source_frame_number(frame.source_frame_number);
-
-            response.mutable_intrinsics()->set_fx(descriptor->fx);
-            response.mutable_intrinsics()->set_fy(descriptor->fy);
-            response.mutable_intrinsics()->set_cx(descriptor->cx);
-            response.mutable_intrinsics()->set_cy(descriptor->cy);
-            for (const float coefficient : descriptor->distortion) {
-                response.mutable_intrinsics()->add_coeffs(coefficient);
-            }
-
-            response.set_seq_no(static_cast<int32_t>(std::min<uint64_t>(
-                frame.sequence,
-                static_cast<uint64_t>(std::numeric_limits<int32_t>::max()))));
-
-            const auto write_started = std::chrono::steady_clock::now();
-            if (!stream->Write(response)) {
-                exit_reason = "write_failed";
-                CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed"
-                               << ", id=" << dev_id
-                               << ", peer=" << context->peer()
-                               << ", context_cancelled=" << context->IsCancelled()
-                               << ", client_eof=" << client_eof_requested.load()
-                               << ", request_stream_closed=" << request_stream_closed.load()
-                               << ", control_requests=" << control_requests_read.load()
-                               << ", write_ms="
-                               << std::chrono::duration_cast<std::chrono::microseconds>(
-                                      std::chrono::steady_clock::now() - write_started).count() / 1000.0;
-                break;
-            }
-            last_write_duration = std::chrono::duration_cast<std::chrono::microseconds>(
-                std::chrono::steady_clock::now() - write_started);
-
-            // A successful synchronous Write may have been flow-controlled long
-            // enough for the source to outpace this consumer. Once the pending
-            // count crosses the configured trigger, discard the whole pending
-            // batch and require a fresh key frame before resuming.
-            const uint64_t discarded = subscription.discardPendingIfExceeds(
-                stream_config_.max_pending_frames);
-            if (discarded > 0) {
-                request_key_frame_now();
-                log_latency_event(
-                    "write_backpressure",
-                    discarded,
-                    frame_age.value_or(0),
-                    last_write_duration);
-            }
-        }
-        join_request_reader();
-        CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end"
-                       << ", id=" << dev_id
-                       << ", reason=" << exit_reason
-                       << ", peer=" << context->peer()
-                       << ", context_cancelled=" << context->IsCancelled()
-                       << ", client_eof=" << client_eof_requested.load()
-                       << ", request_stream_closed=" << request_stream_closed.load()
-                       << ", control_requests=" << control_requests_read.load();
-        return grpc::Status::OK;
-    }
-    catch (const exception &e) {
-        api::GetRGBImageStreamCommand_Feedback response;
-        response.mutable_header()->set_success(false);
         response.mutable_header()->set_error_message(e.what());
         setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
         stream->Write(response);