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

fix(camera): handle UVC snapshots in video mode

Browse Source
This commit is contained in:
linbo 2026-08-11 17:19:01 +08:00
parent 187e8042b8
commit b0efc341a9
2 changed files with 81 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp
View File

@ -68,6 +68,11 @@ bool UVCCamera::init() {
clear_error_();
state_.is_initialized = false;
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
}
stream_frame_buffer_ = std::make_shared<SPMCRingBuffer<StreamFrameData>>(buffer_size_);
stream_frame_buffer_->clear();
if (cap_.isOpened()) {
@ -135,6 +140,12 @@ bool UVCCamera::start() {
CMVR_LOG(WARNING) << "[RealsenseCamera] (start): camera already started";
return true;
}
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
}
cap_.open(serial_, cv::CAP_V4L2);
this_thread::sleep_for(chrono::milliseconds(100));
@ -186,16 +197,22 @@ bool UVCCamera::stop() {
}
if (mode_ == VIDEO_MODE){
state_.is_streaming = false;
if (stream_thread_->joinable()) {
stream_thread_->join();
if (stream_thread_) {
if (stream_thread_->joinable()) {
stream_thread_->join();
}
stream_thread_.reset();
stream_thread_ = nullptr;
}
is_streaming_running = false;
}
if (cap_.isOpened()) {
cap_.release();
}
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
}
state_.is_opened = false;
return true;
}
@ -236,6 +253,39 @@ void UVCCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
color.release();
return;
}
{
std::lock_guard frame_lock(frame_mutex_);
if (!latest_frame_.empty()) {
latest_frame_.copyTo(color);
}
}
const bool capture_worker_requested = state_.is_streaming || state_.is_recording;
if (color.empty() && !capture_worker_requested) {
if (stream_thread_) {
if (stream_thread_->joinable()) {
stream_thread_->join();
}
stream_thread_.reset();
is_streaming_running = false;
}
if (!cap_.read(color) || color.empty()) {
state_.is_error = true;
state_.error_message = "read color image failed";
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
color.release();
return;
}
}
if (color.empty()) {
state_.is_error = true;
state_.error_message = "no video frame available";
CMVR_LOG(ERROR) << "[UVCCamera] (getRGBImage): " << state_.error_message;
return;
}
}
}
@ -510,6 +560,11 @@ void UVCCamera::streaming_worker_() {
}
const auto capture_end_time = std::chrono::steady_clock::now();
{
std::lock_guard frame_lock(frame_mutex_);
frame.copyTo(latest_frame_);
}
// 以下为编码部分，用于流模式
StreamFrameData frame_data;
// 保存图像数据
@ -622,6 +677,10 @@ void UVCCamera::streaming_worker_() {
}
is_streaming_running = false;
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
}
// 线程结束时清空队列
stream_frame_buffer_->clear();
recordingIndex_ = 0;
@ -633,6 +692,10 @@ void UVCCamera::streaming_worker_() {
stream_frame_buffer_->clear();
// 确保线程状态正确更新
is_streaming_running = false;
{
std::lock_guard frame_lock(frame_mutex_);
latest_frame_.release();
}
state_.is_error = true;
state_.error_message = e.what();
CMVR_LOG(ERROR) << "[UVCCamera]streaming_worker_ error:" << state_.error_message;

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

@ -205,7 +205,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
}
Rs2Intrinsics intrinsics = {0};
dev->getRGBImage(image,intrinsics);
response->mutable_header()->set_success(true);
if (image.empty()) {
return failResponse(response, "Camera returned an empty RGB image: " + dev_id);
}
response->mutable_intrinsics()->set_fx(intrinsics.fx);
response->mutable_intrinsics()->set_fy(intrinsics.fy);
@ -258,6 +260,9 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
}
Rs2Intrinsics intrinsics = {0};
dev->getDepthImage(image,intrinsics);
if (image.empty()) {
return failResponse(response, "Camera returned an empty depth image: " + dev_id);
}
response->mutable_header()->set_success(true);
response->mutable_intrinsics()->set_fx(intrinsics.fx);
@ -314,6 +319,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
}
Rs2Intrinsics intrinsics = {0};
dev->getRGBDImages(color_image,depth_image, intrinsics);
if (color_image.empty()) {
return failResponse(response, "Camera returned an empty RGB image: " + dev_id);
}
if (depth_image.empty()) {
return failResponse(response, "Camera returned an empty depth image: " + dev_id);
}
response->mutable_header()->set_success(true);
response->mutable_intrinsics()->set_fx(intrinsics.fx);
@ -355,6 +366,9 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
else if (depth_image.type() == CV_32FC1) {
response->mutable_depth_frame()->set_type(api::FrameData::F32C1);
}
else {
return failResponse(response, "unsupported depth image type");
}
response->mutable_depth_frame()->set_data(
reinterpret_cast<const char*>(depth_image.data), depth_image.total() * depth_image.elemSize());
response->mutable_depth_frame()->set_height(depth_image.rows);