From 472809dea52abd5eb9322e02fcaf4e0bdf29dc3e Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Wed, 26 Aug 2026 14:38:21 +0800 Subject: [PATCH] feat(camera): support RealSense depth streaming --- cmvr-es/devices/camera/abstract_camera.h | 3 + .../realsense_camera/src/realsense_camera.cpp | 99 ++++++++++++------- .../service/grpc/src/grpc_camera_service.cpp | 27 +++-- 3 files changed, 88 insertions(+), 41 deletions(-) diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index a3082602..7bf2d637 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/cmvr-es/devices/camera/abstract_camera.h @@ -39,6 +39,9 @@ namespace cmvr::device { 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; 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 e9db35ff..dd827b3a 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; @@ -180,8 +182,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!"; @@ -253,16 +256,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(); @@ -270,7 +277,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(); @@ -749,15 +756,17 @@ void RealsenseCamera::streaming_worker_() { frames = get_frameset(true); rs2::frame color_frame = frames.get_color_frame(); rs2::frame depth_frame = frames.get_depth_frame(); - if (!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 && !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; } @@ -774,43 +783,63 @@ 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_; - success = CameraStreamEncoder::encode(rgbEncoder_, - rgb_to_encode, - frame_data.rgbFrame, - frame_data.bKey, - encode_options); + if (needs_depth && frame_data.depthFrame.empty()) + success = false; // 深度图编码 // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey); if (success) { const uint64_t sequence = frame_sequence++; frame_data.stream_epoch = stream_epoch; frame_data.sequence = sequence; - frame_data.source_timestamp = static_cast(color_frame.get_timestamp()); - frame_data.source_frame_number = color_frame.get_frame_number(); + 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( @@ -821,9 +850,9 @@ void RealsenseCamera::streaming_worker_() { 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 08bc739c..92b7d99e 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -12,6 +12,7 @@ #include #include #include +#include using namespace std; using namespace cmvr::service; @@ -219,6 +220,13 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + // OpenCV/RealSense expose color frames as BGR. The gRPC contract uses + // RGB byte order so clients can construct RGB888 images directly. + 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); @@ -338,6 +346,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); // color image + 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); + } if (color_image.type() == CV_8UC3) { response->mutable_color_frame()->set_type(api::FrameData::U8C3); } @@ -520,11 +533,12 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con !frame_data.depthFrame.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.fy); @@ -606,11 +620,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.fy);