SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(camera): restore RealSense depth streaming

Browse Source
This commit is contained in:
linbo 2026-08-28 10:16:40 +08:00
parent aa69aa78ef
commit d1b65507d7
3 changed files with 89 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cmvr-es/devices/camera/abstract_camera.h
View File

@ -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;

63
cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
View File

@ -5,6 +5,8 @@
#include "../include/realsense_camera.h"
#include <cstring>
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 (stream_mode_ == RGBD_MODE) {
if (align_mode_ == "color") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
}
else if (align_mode_ == "depth") {
align_ = std::make_shared<rs2::align>(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();
@ -747,17 +754,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;
}
@ -773,19 +782,32 @@ void RealsenseCamera::streaming_worker_() {
}
// 保存图像数据
{
cv::Mat temp(cv::Size(width_, height_), CV_8UC3, const_cast<void*>(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<void*>(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<void*>(depth_frame.get_data()));
cv::Mat temp(cv::Size(depth_frame.get_width(), depth_frame.get_height()),
CV_16UC1, const_cast<void*>(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图像编码
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_)) {
@ -803,14 +825,17 @@ void RealsenseCamera::streaming_worker_() {
frame_data.rgbFrame,
frame_data.bKey,
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;
frame_data.source_timestamp = static_cast<uint64_t>(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<uint64_t>(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<std::chrono::nanoseconds>(
frame_start_time.time_since_epoch()).count();
frame_data.capture_utc_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
@ -821,9 +846,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);
}
// 计算从帧开始到现在的总耗时

28
cmvr-es/service/grpc/src/grpc_camera_service.cpp
View File

@ -12,6 +12,7 @@
#include <cstdint>
#include <limits>
#include <thread>
#include <utility>
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());
// 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);
@ -337,8 +345,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) {
@ -520,11 +532,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 +619,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);