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 108fa5d206
commit be2977211b
3 changed files with 128 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,12 +33,34 @@ namespace cmvr::device {
std::vector<uint8_t> 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<uint8_t> codec_config;
};
class AbstractCamera : public AbstractDevice {
public:

106
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;
@ -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::align>(RS2_STREAM_COLOR);
}
else if (align_mode_ == "depth") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
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();
@ -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<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图像编码
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<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>(
std::chrono::system_clock::now().time_since_epoch()).count();
frame_data.pts = static_cast<int64_t>(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);
}
// 计算从帧开始到现在的总耗时

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

@ -6,6 +6,8 @@
#include "../include/grpc_camera_service.h"
#include <limits>
#include <thread>
#include <utility>
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);