From be2977211b91259727f49c491cf990d0c62b130c Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Fri, 28 Aug 2026 10:16:40 +0800 Subject: [PATCH] fix(camera): restore RealSense depth streaming --- cmvr-es/devices/camera/abstract_camera.h | 34 +++++- .../realsense_camera/src/realsense_camera.cpp | 106 +++++++++++++----- .../service/grpc/src/grpc_camera_service.cpp | 30 +++-- 3 files changed, 128 insertions(+), 42 deletions(-) diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index 525414a7..86ce2879 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/cmvr-es/devices/camera/abstract_camera.h @@ -33,12 +33,34 @@ namespace cmvr::device { std::vector depthFrame; //编码格式 std::string codec = ".h264"; - Rs2Intrinsics intrinsics; - int width; - int height; - int fps; - bool bKey; - bool depthKey; + Rs2Intrinsics intrinsics{}; + int width = 0; + int height = 0; + // Depth frames may use a different resolution from the encoded color frame. + int depth_width = 0; + int depth_height = 0; + int fps = 0; + bool bKey = false; + bool depthKey = false; + + // Protocol-neutral real-time metadata. The producer fills these values + // when a complete encoded access unit is published. + uint64_t stream_epoch = 0; + uint64_t sequence = 0; + // Opaque producer-native timing/counter values. Their units and epoch + // are source-defined; zero means that the source did not provide them. + uint64_t source_timestamp = 0; + uint64_t source_frame_number = 0; + int64_t capture_monotonic_ns = 0; + int64_t capture_utc_ns = 0; + int64_t pts = 0; + int64_t dts = 0; + int32_t time_base_num = 1; + int32_t time_base_den = 1; + int64_t duration = 0; + bool discontinuity = false; + uint32_t codec_config_generation = 0; + std::vector codec_config; }; class AbstractCamera : public AbstractDevice { public: diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index 71565a78..da7396cc 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -5,6 +5,8 @@ #include "../include/realsense_camera.h" +#include + using namespace std; using namespace cmvr::device; @@ -174,8 +176,9 @@ bool RealsenseCamera::init() { //初始化编码器 // 初始化RGB编码器（示例参数：640x480，30fps，H.264） - if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) { - CMVR_LOG(ERROR) << "[RealsenseCamera] (start): Failed to init RGB encoder!"; + if (stream_mode_ != DEPTH_MODE && + !CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) { + CMVR_LOG(ERROR) << "[RealsenseCamera] (init): Failed to init RGB encoder!"; state_.is_initialized = false; state_.is_error = true; state_.error_message = "Failed to init RGB encoder!"; @@ -247,16 +250,20 @@ bool RealsenseCamera::start() { intrinsics_ = Kd_; } - if (align_mode_ == "color") { - align_ = std::make_shared(RS2_STREAM_COLOR); - } - else if (align_mode_ == "depth") { - align_ = std::make_shared(RS2_STREAM_DEPTH); + if (stream_mode_ == RGBD_MODE) { + if (align_mode_ == "color") { + align_ = std::make_shared(RS2_STREAM_COLOR); + } + else if (align_mode_ == "depth") { + align_ = std::make_shared(RS2_STREAM_DEPTH); + } } // 启动后做一次首帧探测，避免“start 成功但长期无帧”假阳性。 rs2::frameset probe = pipe_.wait_for_frames(kProbeTimeoutMs); - if (!probe.get_color_frame()) { + const bool needs_color = stream_mode_ == COLOR_MODE || stream_mode_ == RGBD_MODE; + const bool needs_depth = stream_mode_ == DEPTH_MODE || stream_mode_ == RGBD_MODE; + if (needs_color && !probe.get_color_frame()) { last_error = "no color frame after start"; CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error; pipe_.stop(); @@ -264,7 +271,7 @@ bool RealsenseCamera::start() { std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs)); continue; } - if (stream_mode_ == RGBD_MODE && !probe.get_depth_frame()) { + if (needs_depth && !probe.get_depth_frame()) { last_error = "no depth frame after start"; CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error; pipe_.stop(); @@ -721,17 +728,19 @@ void RealsenseCamera::streaming_worker_() { rs2::frameset frames; frames = get_frameset(true); - rs2::frame color_frame = frames.get_color_frame(); - rs2::frame depth_frame = frames.get_depth_frame(); - if (!color_frame) { + rs2::video_frame color_frame = frames.get_color_frame(); + rs2::depth_frame depth_frame = frames.get_depth_frame(); + const bool needs_color = stream_mode_ == COLOR_MODE || stream_mode_ == RGBD_MODE; + const bool needs_depth = stream_mode_ == DEPTH_MODE || stream_mode_ == RGBD_MODE; + if (needs_color && !color_frame) { state_.is_error = true; state_.error_message = "missing color frame"; CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; break; } - if (stream_mode_ == RGBD_MODE && !depth_frame) { + if (needs_depth && !depth_frame) { state_.is_error = true; - state_.error_message = "missing depth frame in RGBD mode"; + state_.error_message = "missing depth frame"; CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; break; } @@ -747,28 +756,49 @@ void RealsenseCamera::streaming_worker_() { } // 保存图像数据 - { - cv::Mat temp(cv::Size(width_, height_), CV_8UC3, const_cast(color_frame.get_data()), cv::Mat::AUTO_STEP); + if (color_frame) { + cv::Mat temp(cv::Size(color_frame.get_width(), color_frame.get_height()), + CV_8UC3, const_cast(color_frame.get_data()), cv::Mat::AUTO_STEP); temp.copyTo(frame_data.rgbImage); // 执行深拷贝 } if (depth_frame) { - auto temp = cv::Mat(cv::Size(width_, height_), CV_16UC1, const_cast(depth_frame.get_data())); + cv::Mat temp(cv::Size(depth_frame.get_width(), depth_frame.get_height()), + CV_16UC1, const_cast(depth_frame.get_data()), cv::Mat::AUTO_STEP); temp.copyTo(frame_data.depthImage); // 执行深拷贝 } // 记录编码开始时间 + if (depth_frame) { + frame_data.depth_width = frame_data.depthImage.cols; + frame_data.depth_height = frame_data.depthImage.rows; + frame_data.depthKey = true; + const size_t depth_bytes = frame_data.depthImage.total() * frame_data.depthImage.elemSize(); + frame_data.depthFrame.resize(depth_bytes); + std::memcpy(frame_data.depthFrame.data(), frame_data.depthImage.data, depth_bytes); + } + auto encode_start_time = std::chrono::high_resolution_clock::now(); // rgb图像编码 - cv::Mat rgb_to_encode = frame_data.rgbImage; - if (encode_width_ > 0 && encode_height_ > 0 && - (frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) { - cv::resize(frame_data.rgbImage, - rgb_to_encode, - cv::Size(encode_width_, encode_height_), - 0.0, - 0.0, - cv::INTER_LINEAR); + success = true; + if (needs_color) { + cv::Mat rgb_to_encode = frame_data.rgbImage; + if (encode_width_ > 0 && encode_height_ > 0 && + (frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) { + cv::resize(frame_data.rgbImage, + rgb_to_encode, + cv::Size(encode_width_, encode_height_), + 0.0, + 0.0, + cv::INTER_LINEAR); + } + CameraStreamEncodeOptions encode_options; + encode_options.draw_timestamp = enable_stream_timestamp_; + success = CameraStreamEncoder::encode(rgbEncoder_, + rgb_to_encode, + frame_data.rgbFrame, + frame_data.bKey, + encode_options); } CameraStreamEncodeOptions encode_options; encode_options.draw_timestamp = enable_stream_timestamp_; @@ -787,11 +817,29 @@ void RealsenseCamera::streaming_worker_() { encode_options); // 深度图编码 // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey); + if (needs_depth && frame_data.depthFrame.empty()) + success = false; if (success) { + const uint64_t sequence = frame_sequence++; + frame_data.stream_epoch = stream_epoch; + frame_data.sequence = sequence; + const rs2::frame source_frame = color_frame ? color_frame : depth_frame; + frame_data.source_timestamp = source_frame + ? static_cast(source_frame.get_timestamp()) : 0; + frame_data.source_frame_number = source_frame ? source_frame.get_frame_number() : 0; + frame_data.capture_monotonic_ns = std::chrono::duration_cast( + frame_start_time.time_since_epoch()).count(); + frame_data.capture_utc_ns = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + frame_data.pts = static_cast(sequence); + frame_data.dts = frame_data.pts; + frame_data.time_base_num = 1; + frame_data.time_base_den = fps_; + frame_data.duration = 1; frame_data.fps = fps_; - frame_data.width = encode_width_; - frame_data.height = encode_height_; - frame_data.codec = codec_; + frame_data.width = needs_color ? encode_width_ : frame_data.depth_width; + frame_data.height = needs_color ? encode_height_ : frame_data.depth_height; + frame_data.codec = needs_color ? codec_ : "none"; stream_frame_buffer_->push(frame_data); } // 计算从帧开始到现在的总耗时 diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 7d3125bd..68cbd375 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -6,6 +6,8 @@ #include "../include/grpc_camera_service.h" #include +#include +#include using namespace std; using namespace cmvr::service; @@ -142,6 +144,13 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + // FrameData::U8C3 is defined as RGB byte order. OpenCV/RealSense + // color frames are BGR, so normalize the single-frame contract here. + if (image.type() == CV_8UC3) { + cv::Mat rgb_image; + cv::cvtColor(image, rgb_image, cv::COLOR_BGR2RGB); + image = std::move(rgb_image); + } auto imageType = image.type(); if (imageType == CV_8UC1) { response->mutable_color_frame()->set_type(api::FrameData::U8C1); @@ -259,8 +268,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - // color image + // FrameData::U8C3 is defined as RGB byte order. OpenCV/RealSense + // color frames are BGR, so normalize the single-frame contract here. if (color_image.type() == CV_8UC3) { + cv::Mat rgb_image; + cv::cvtColor(color_image, rgb_image, cv::COLOR_BGR2RGB); + color_image = std::move(rgb_image); response->mutable_color_frame()->set_type(api::FrameData::U8C3); } else if (color_image.type() == CV_16UC3) { @@ -392,11 +405,13 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con 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(frame_data.codec); - response.mutable_depth_frame()->set_width(frame_data.width); - response.mutable_depth_frame()->set_height(frame_data.height); + 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_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx); @@ -468,11 +483,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con 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(frame_data.codec); - response.mutable_depth_frame()->set_width(frame_data.width); - response.mutable_depth_frame()->set_height(frame_data.height); + 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_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx);