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 f3e3cefbf8
3 changed files with 92 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{}; Rs2Intrinsics intrinsics{};
int width = 0; int width = 0;
int height = 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; int fps = 0;
bool bKey = false; bool bKey = false;
bool depthKey = false; bool depthKey = false;

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

@ -5,6 +5,8 @@
#include "../include/realsense_camera.h" #include "../include/realsense_camera.h"
#include <cstring>
using namespace std; using namespace std;
using namespace cmvr::device; using namespace cmvr::device;
@ -180,8 +182,9 @@ bool RealsenseCamera::init() {
//初始化编码器 //初始化编码器
// 初始化RGB编码器（示例参数：640x480，30fps，H.264） // 初始化RGB编码器（示例参数：640x480，30fps，H.264）
if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) { if (stream_mode_ != DEPTH_MODE &&
CMVR_LOG(ERROR) << "[RealsenseCamera] (start): Failed to init RGB encoder!"; !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_initialized = false;
state_.is_error = true; state_.is_error = true;
state_.error_message = "Failed to init RGB encoder!"; state_.error_message = "Failed to init RGB encoder!";
@ -253,16 +256,20 @@ bool RealsenseCamera::start() {
intrinsics_ = Kd_; intrinsics_ = Kd_;
} }
if (align_mode_ == "color") { if (stream_mode_ == RGBD_MODE) {
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR); 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); else if (align_mode_ == "depth") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
}
} }
// 启动后做一次首帧探测，避免“start 成功但长期无帧”假阳性。 // 启动后做一次首帧探测，避免“start 成功但长期无帧”假阳性。
rs2::frameset probe = pipe_.wait_for_frames(kProbeTimeoutMs); 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"; last_error = "no color frame after start";
CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error; CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error;
pipe_.stop(); pipe_.stop();
@ -270,7 +277,7 @@ bool RealsenseCamera::start() {
std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs)); std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs));
continue; continue;
} }
if (stream_mode_ == RGBD_MODE && !probe.get_depth_frame()) { if (needs_depth && !probe.get_depth_frame()) {
last_error = "no depth frame after start"; last_error = "no depth frame after start";
CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error; CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error;
pipe_.stop(); pipe_.stop();
@ -747,17 +754,19 @@ void RealsenseCamera::streaming_worker_() {
rs2::frameset frames; rs2::frameset frames;
frames = get_frameset(true); frames = get_frameset(true);
rs2::frame color_frame = frames.get_color_frame(); rs2::video_frame color_frame = frames.get_color_frame();
rs2::frame depth_frame = frames.get_depth_frame(); rs2::depth_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_.is_error = true;
state_.error_message = "missing color frame"; state_.error_message = "missing color frame";
CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message;
break; break;
} }
if (stream_mode_ == RGBD_MODE && !depth_frame) { if (needs_depth && !depth_frame) {
state_.is_error = true; 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; CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message;
break; break;
} }
@ -774,43 +783,62 @@ 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) {
temp.copyTo(frame_data.rgbImage); // 执行深拷贝 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) { 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); // 执行深拷贝 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(); auto encode_start_time = std::chrono::high_resolution_clock::now();
// rgb图像编码 // rgb图像编码
cv::Mat rgb_to_encode = frame_data.rgbImage; success = true;
if (encode_width_ > 0 && encode_height_ > 0 && if (needs_color) {
(frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) { cv::Mat rgb_to_encode = frame_data.rgbImage;
cv::resize(frame_data.rgbImage, if (encode_width_ > 0 && encode_height_ > 0 &&
rgb_to_encode, (frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) {
cv::Size(encode_width_, encode_height_), cv::resize(frame_data.rgbImage,
0.0, rgb_to_encode,
0.0, cv::Size(encode_width_, encode_height_),
cv::INTER_LINEAR); 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; if (needs_depth && frame_data.depthFrame.empty())
encode_options.draw_timestamp = enable_stream_timestamp_; success = false;
success = CameraStreamEncoder::encode(rgbEncoder_,
rgb_to_encode,
frame_data.rgbFrame,
frame_data.bKey,
encode_options);
// 深度图编码
// success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);
if (success) { if (success) {
const uint64_t sequence = frame_sequence++; const uint64_t sequence = frame_sequence++;
frame_data.stream_epoch = stream_epoch; frame_data.stream_epoch = stream_epoch;
frame_data.sequence = sequence; frame_data.sequence = sequence;
frame_data.source_timestamp = static_cast<uint64_t>(color_frame.get_timestamp()); const rs2::frame source_frame = color_frame ? color_frame : depth_frame;
frame_data.source_frame_number = color_frame.get_frame_number(); 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_data.capture_monotonic_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
frame_start_time.time_since_epoch()).count(); frame_start_time.time_since_epoch()).count();
frame_data.capture_utc_ns = std::chrono::duration_cast<std::chrono::nanoseconds>( frame_data.capture_utc_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
@ -821,9 +849,9 @@ void RealsenseCamera::streaming_worker_() {
frame_data.time_base_den = fps_; frame_data.time_base_den = fps_;
frame_data.duration = 1; frame_data.duration = 1;
frame_data.fps = fps_; frame_data.fps = fps_;
frame_data.width = encode_width_; frame_data.width = needs_color ? encode_width_ : frame_data.depth_width;
frame_data.height = encode_height_; frame_data.height = needs_color ? encode_height_ : frame_data.depth_height;
frame_data.codec = codec_; frame_data.codec = needs_color ? codec_ : "none";
stream_frame_buffer_->push(frame_data); 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 <cstdint>
#include <limits> #include <limits>
#include <thread> #include <thread>
#include <utility>
using namespace std; using namespace std;
using namespace cmvr::service; using namespace cmvr::service;
@ -219,6 +220,13 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
response->mutable_header()->set_success(true); response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); 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(); auto imageType = image.type();
if (imageType == CV_8UC1) { if (imageType == CV_8UC1) {
response->mutable_color_frame()->set_type(api::FrameData::U8C1); 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); response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); 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) { 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); response->mutable_color_frame()->set_type(api::FrameData::U8C3);
} }
else if (color_image.type() == CV_16UC3) { else if (color_image.type() == CV_16UC3) {
@ -520,11 +532,12 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
!frame_data.depthFrame.empty()) { !frame_data.depthFrame.empty()) {
response.mutable_header()->set_success(true); response.mutable_header()->set_success(true);
setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); 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_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_is_key_frame(frame_data.depthKey);
response.mutable_depth_frame()->set_codec(frame_data.codec); response.mutable_depth_frame()->set_codec("none");
response.mutable_depth_frame()->set_width(frame_data.width); 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.height); 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_fx(frame_data.intrinsics.fx);
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); 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_width(frame_data.width);
response.mutable_color_frame()->set_height(frame_data.height); 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_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_is_key_frame(frame_data.depthKey);
response.mutable_depth_frame()->set_codec(frame_data.codec); response.mutable_depth_frame()->set_codec("none");
response.mutable_depth_frame()->set_width(frame_data.width); 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.height); 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_fx(frame_data.intrinsics.fx);
response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);