diff --git a/cmvr-es/service/grpc/include/grpc_camera_service.h b/cmvr-es/service/grpc/include/grpc_camera_service.h index 5ccf401f..d9bc1782 100644 --- a/cmvr-es/service/grpc/include/grpc_camera_service.h +++ b/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( - CameraStreamLowLatencyConfig stream_config = {}); + gRPCCameraServiceImpl(); ~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* stream) override; grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; private: device::DeviceManager& dmgr_; - CameraStreamLowLatencyConfig stream_config_; //双向流读写线程 std::shared_ptr read_thread_ = nullptr; diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index a86ef651..7d3125bd 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -5,12 +5,7 @@ #include "../include/grpc_camera_service.h" -#include -#include -#include -#include #include -#include 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 -// 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 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 camera_; - bool active_{false}; -}; -} - -gRPCCameraServiceImpl::gRPCCameraServiceImpl( - CameraStreamLowLatencyConfig stream_config) - : dmgr_(DeviceManager::getInstance()), - stream_config_(stream_config) {} +gRPCCameraServiceImpl::gRPCCameraServiceImpl(): dmgr_(DeviceManager::getInstance()) {} 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()) { - return failResponse(response, "Camera returned an empty RGB image: " + dev_id); - } + response->mutable_header()->set_success(true); 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; - // clients convert it once when constructing an RGB image. + // color 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(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; - } - 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(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(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()); + CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopRecording): success, id=" << dev_id; 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)) { - return grpc::Status::OK; - } + stream->Read(&request); 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(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)) && - !frame_data.depthFrame.empty()) { + dev->getEncodedFrame(frame_data,index); + 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_width(frame_data.depth_width > 0 ? frame_data.depth_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_codec(frame_data.codec); + response.mutable_depth_frame()->set_width(frame_data.width); + 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_cx(frame_data.intrinsics.cx); - response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy); + 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] (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)) { - return grpc::Status::OK; - } + stream->Read(&request); 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(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,34 +459,33 @@ 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)) && - !frame_data.rgbFrame.empty() && - !frame_data.depthFrame.empty()) { + dev->getEncodedFrame(frame_data,index); + if (!frame_data.rgbFrame.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_width(frame_data.depth_width > 0 ? frame_data.depth_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_codec(frame_data.codec); + response.mutable_depth_frame()->set_width(frame_data.width); + 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_cx(frame_data.intrinsics.cx); - response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy); + 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; @@ -631,11 +493,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con } } 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; - response.mutable_header()->set_success(false); response.mutable_header()->set_error_message(e.what()); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); stream->Write(response); @@ -646,13 +508,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte try { //读取首次传递的数据,获取设备id api::GetRGBImageStreamCommand_Request request; - if (!stream->Read(&request)) { - return grpc::Status::OK; - } + stream->Read(&request); 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(); + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start,id=" << dev_id; const auto dev = dmgr_.getDevice(dev_id); if (!dev) { api::GetRGBImageStreamCommand_Feedback response; @@ -662,259 +520,57 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte 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 client_eof_requested{false}; - std::atomic request_stream_closed{false}; - std::atomic 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(frame_age_ns) / 1'000'000.0; - const double write_ms = static_cast(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; - }; + int nFrameCount = 0; + dev->startStreaming(); + CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start streaming success, id=" << dev_id; + size_t last_sent_index = std::numeric_limits::max(); 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(); + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; 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( - std::chrono::duration_cast( - 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(descriptor->width)); - response.mutable_color_frame()->set_height(static_cast(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); + 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.set_seq_no(static_cast(std::min( - frame.sequence, - static_cast(std::numeric_limits::max())))); + 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); + response.mutable_intrinsics()->set_cx(frame_data.intrinsics.cx); + response.mutable_intrinsics()->set_cy(frame_data.intrinsics.cy); + for (int i = 0; i < 5 ; i++) { + response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]); + } + + response.set_seq_no(nFrameCount++); - 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::steady_clock::now() - write_started).count() / 1000.0; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; break; } - last_write_duration = std::chrono::duration_cast( - 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); - } + last_sent_index = next_index; } - 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(); + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id; + dev->stopStreaming(); return grpc::Status::OK; } - catch (const exception &e) { + catch (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);