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

refactor(camera): unify camera devices

Browse Source
This commit is contained in:
lgv 2026-06-30 16:04:59 +08:00
parent 80c8969ce9
commit ca19449545
19 changed files with 1181 additions and 695 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
cmvr-es/config/devices/camera/camera.pb.txt
View File

@ -3,14 +3,22 @@ camera {
id: "cam1"
realsense {
serialNumber: "243122074587"
camera_mode: CAMERA_MODE_VIDEO
capture {
width: 640
height: 480
fps: 30
stream_mode: STREAM_MODE_RGBD
}
encoder {
width: 640
height: 480
fps: 30
codec: "H265"
camera_mode: CAMERA_MODE_VIDEO
stream_mode: STREAM_MODE_RGBD
align_mode: ALIGN_MODE_COLOR
enable_stream_timestamp: true
buffer_size: 30
}
align_mode: ALIGN_MODE_COLOR
sync: false
}
}
@ -19,16 +27,22 @@ camera {
id: "right_hand_cam"
realsense {
serialNumber: "243122072252"
camera_mode: CAMERA_MODE_VIDEO
capture {
width: 1280
height: 720
encode_width: 640
encode_height: 360
fps: 30
stream_mode: STREAM_MODE_RGB
}
encoder {
width: 640
height: 360
fps: 30
codec: "H264"
camera_mode: CAMERA_MODE_VIDEO
stream_mode: STREAM_MODE_RGB
align_mode: ALIGN_MODE_COLOR
enable_stream_timestamp: true
buffer_size: 30
}
align_mode: ALIGN_MODE_COLOR
sync: false
}
}
@ -37,47 +51,98 @@ camera {
id: "cam3"
realsense {
serialNumber: "243122075614"
camera_mode: CAMERA_MODE_VIDEO
capture {
width: 640
height: 480
fps: 30
stream_mode: STREAM_MODE_RGBD
}
encoder {
width: 640
height: 480
fps: 30
codec: "H265"
camera_mode: CAMERA_MODE_VIDEO
stream_mode: STREAM_MODE_RGBD
align_mode: ALIGN_MODE_COLOR
enable_stream_timestamp: true
buffer_size: 30
}
align_mode: ALIGN_MODE_COLOR
sync: false
}
}
cameras {
id: "mujoco_hand_cam"
mujoco {
world_id: "mujoco_world"
camera_name: "hand_cam"
render {
width: 1280
height: 720
fps: 30
stream_mode: STREAM_MODE_RGBD
}
encoder {
width: 1280
height: 720
fps: 30
codec: "H264"
enable_stream_timestamp: true
buffer_size: 30
}
consume_new_frame_only: false
viewer_pip {
enable: true
left: -10
bottom: 10
width: 320
height: 180
}
}
}
cameras {
id: "left_eye_cam"
uvc {
usb: "/dev/uvc_left_camera"
camera_mode: CAMERA_MODE_VIDEO
capture {
width: 640
height: 480
fps: 30
stream_mode: STREAM_MODE_RGB
}
encoder {
width: 640
height: 480
fps: 30
codec: "H265"
camera_mode: CAMERA_MODE_VIDEO
stream_mode: STREAM_MODE_RGB
enable_stream_timestamp: true
buffer_size: 30
}
}
}
cameras {
id: "cam5"
realsense {
serialNumber: "243122070435"
camera_mode: CAMERA_MODE_VIDEO
capture {
width: 1280
height: 720
encode_width: 640
encode_height: 360
fps: 30
stream_mode: STREAM_MODE_RGB
}
encoder {
width: 640
height: 360
fps: 30
codec: "H264"
camera_mode: CAMERA_MODE_VIDEO
stream_mode: STREAM_MODE_RGB
align_mode: ALIGN_MODE_COLOR
enable_stream_timestamp: true
buffer_size: 30
}
align_mode: ALIGN_MODE_COLOR
sync: false
}
}

3
cmvr-es/devices/camera/CMakeLists.txt
View File

@ -1,4 +1,5 @@
#add_subdirectory(mechmind)
add_subdirectory(common)
add_subdirectory(uvc_camera)
add_subdirectory(realsense_camera)
add_subdirectory(mujoco_camera)
@ -11,6 +12,8 @@ target_link_libraries(camera
INTERFACE
cmvr_es::device::uvc_camera
cmvr_es::device::realsense_camera
cmvr_es::device::mujoco_camera
cmvr_es::device::camera_stream_encoder
cmvr_es::proto
)

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

@ -8,6 +8,8 @@
#include "cmvr/config/camera_config/camera_config.pb.h"
namespace cmvr::device {
enum CameraMode {PHOTO_MODE, VIDEO_MODE};
struct Rs2Intrinsics
{
float cx;

5
cmvr-es/devices/camera/camera_factory.h
View File

@ -8,6 +8,7 @@
#include "cmvr/config/camera_config/camera_config.pb.h"
#include "common/base/logging/logger.h"
#include "devices/camera/abstract_camera.h"
#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
#include "devices/camera/realsense_camera/include/realsense_camera.h"
#include "devices/camera/uvc_camera/include/uvc_camera.h"
@ -40,6 +41,10 @@ public:
return nullptr;
}
case config::CameraDeviceConfig::kMujoco:
return std::make_shared<MujocoCamera>(
backendWithId_(cfg.id(), cfg.mujoco()));
case config::CameraDeviceConfig::BACKEND_NOT_SET:
default:
{

18
cmvr-es/devices/camera/common/CMakeLists.txt Normal file
View File

@ -0,0 +1,18 @@
add_library(camera_stream_encoder SHARED src/camera_stream_encoder.cpp)
target_include_directories(camera_stream_encoder PUBLIC ${CMAKE_SOURCE_DIR}/cmvr-es)
target_link_libraries(camera_stream_encoder
PUBLIC
opencv_core
opencv_imgproc
avcodec
avformat
avutil
swscale
swresample
)
add_library(cmvr_es::device::camera_stream_encoder ALIAS camera_stream_encoder)
install(TARGETS camera_stream_encoder LIBRARY DESTINATION lib)

47
cmvr-es/devices/camera/common/include/camera_stream_encoder.h Normal file
View File

@ -0,0 +1,47 @@
#pragma once
#include <memory>
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
namespace cmvr::device {
struct FfmpegEncoderInfo {
std::string codec_name;
int width = 0;
int height = 0;
int fps = 0;
int64_t frame_pts = 0;
bool bRunning = false;
AVCodecContext* codec_context = nullptr;
AVFrame* frame = nullptr;
AVPacket* packet = nullptr;
SwsContext* sws_context = nullptr;
~FfmpegEncoderInfo();
};
struct CameraStreamEncodeOptions {
bool draw_timestamp = false;
};
class CameraStreamEncoder {
public:
static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const std::string& codec_name,
int width,
int height,
int fps);
static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const cv::Mat& frame,
std::vector<uint8_t>& encoded_frame,
bool& is_key,
const CameraStreamEncodeOptions& options = {});
};
} // namespace cmvr::device

297
cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp Normal file
View File

@ -0,0 +1,297 @@
#include "devices/camera/common/include/camera_stream_encoder.h"
#include <chrono>
#include <ctime>
#include <iomanip>
#include <sstream>
#include <libavutil/opt.h>
#include <opencv2/imgproc.hpp>
#include "common/base/logging/logger.h"
namespace cmvr::device {
namespace {
std::string getCurrentTimeString()
{
const auto now = std::chrono::system_clock::now();
const auto now_sec = std::chrono::time_point_cast<std::chrono::seconds>(now);
const std::time_t now_time = std::chrono::system_clock::to_time_t(now_sec);
const std::tm* tm_ptr = std::localtime(&now_time);
const auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - now_sec).count();
std::stringstream ss;
ss << std::put_time(tm_ptr, "%Y-%m-%d %H:%M:%S")
<< "." << std::setw(3) << std::setfill('0') << ms;
return ss.str();
}
void drawTimeStamp(cv::Mat& image)
{
if (image.empty()) {
return;
}
const std::string time_str = getCurrentTimeString();
constexpr int font_face = cv::FONT_HERSHEY_SIMPLEX;
constexpr double font_scale = 0.8;
constexpr int thickness = 2;
int baseline = 0;
const cv::Size text_size = cv::getTextSize(time_str, font_face, font_scale, thickness, &baseline);
const cv::Point text_pos(image.cols - text_size.width - 10, text_size.height + 10);
cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), thickness + 2);
cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness);
}
const AVCodec* findEncoder(const std::string& codec_name)
{
if (codec_name == "h264" || codec_name == "H264") {
const AVCodec* codec = avcodec_find_encoder_by_name("libx264");
return codec ? codec : avcodec_find_encoder(AV_CODEC_ID_H264);
}
if (codec_name == "h265" || codec_name == "HEVC" || codec_name == "H265") {
const AVCodec* codec = avcodec_find_encoder_by_name("libx265");
return codec ? codec : avcodec_find_encoder(AV_CODEC_ID_HEVC);
}
return nullptr;
}
AVPixelFormat sourcePixelFormat(const cv::Mat& frame)
{
if (frame.channels() == 3) {
return AV_PIX_FMT_BGR24;
}
if (frame.channels() == 4) {
return AV_PIX_FMT_BGRA;
}
if (frame.channels() == 1) {
return AV_PIX_FMT_GRAY8;
}
return AV_PIX_FMT_NONE;
}
} // namespace
FfmpegEncoderInfo::~FfmpegEncoderInfo()
{
if (frame) {
av_frame_free(&frame);
frame = nullptr;
}
if (packet) {
av_packet_free(&packet);
packet = nullptr;
}
if (codec_context) {
avcodec_close(codec_context);
avcodec_free_context(&codec_context);
codec_context = nullptr;
}
if (sws_context) {
sws_freeContext(sws_context);
sws_context = nullptr;
}
}
bool CameraStreamEncoder::init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const std::string& codec_name,
const int width,
const int height,
const int fps)
{
encoder = std::make_shared<FfmpegEncoderInfo>();
encoder->codec_name = codec_name;
encoder->width = width;
encoder->height = height;
encoder->fps = fps;
const AVCodec* codec = findEncoder(codec_name);
if (!codec) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to find encoder: " << codec_name;
return false;
}
encoder->codec_context = avcodec_alloc_context3(codec);
if (!encoder->codec_context) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to allocate codec context";
return false;
}
AVCodecContext* ctx = encoder->codec_context;
ctx->codec_type = AVMEDIA_TYPE_VIDEO;
ctx->width = (width + 1) & ~1;
ctx->height = (height + 1) & ~1;
ctx->time_base = {1, fps};
ctx->framerate = {fps, 1};
ctx->max_b_frames = 0;
ctx->gop_size = 10;
if (codec->id == AV_CODEC_ID_H264) {
av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
av_opt_set(ctx->priv_data, "profile", "baseline", 0);
av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
av_opt_set(ctx->priv_data, "annexb", "1", 0);
} else if (codec->id == AV_CODEC_ID_HEVC) {
av_opt_set(ctx->priv_data, "x265-params",
"keyint=10:min-keyint=10:no-open-gop=1:bframes=0:rc-lookahead=0:log-level=none",
0);
av_opt_set_int(ctx->priv_data, "keyint", 10, 0);
av_opt_set_int(ctx->priv_data, "min-keyint", 10, 0);
av_opt_set(ctx->priv_data, "no-open-gop", "1", 0);
}
const AVPixelFormat* pix_fmts = codec->pix_fmts;
if (!pix_fmts) {
ctx->pix_fmt = AV_PIX_FMT_YUV420P;
} else {
ctx->pix_fmt = pix_fmts[0];
}
const int open_ret = avcodec_open2(ctx, codec, nullptr);
if (open_ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(open_ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to open " << codec_name << " encoder: " << errbuf;
return false;
}
encoder->frame = av_frame_alloc();
if (!encoder->frame) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to allocate AVFrame";
return false;
}
encoder->frame->format = ctx->pix_fmt;
encoder->frame->width = ctx->width;
encoder->frame->height = ctx->height;
if (av_frame_get_buffer(encoder->frame, 0) < 0) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to allocate AVFrame buffer";
av_frame_free(&encoder->frame);
return false;
}
encoder->packet = av_packet_alloc();
if (!encoder->packet) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to allocate AVPacket";
return false;
}
CMVR_LOG(INFO) << "[CameraStreamEncoder] initialized " << codec_name
<< " encoder, size=" << width << "x" << height
<< ", fps=" << fps;
return true;
}
bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const cv::Mat& frame,
std::vector<uint8_t>& encoded_frame,
bool& is_key,
const CameraStreamEncodeOptions& options)
{
encoded_frame.clear();
is_key = false;
if (!encoder || !encoder->codec_context || !encoder->frame || !encoder->packet) {
return false;
}
if (frame.cols != encoder->width || frame.rows != encoder->height) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] frame size mismatch"
<< ", frame=" << frame.cols << "x" << frame.rows
<< ", encoder=" << encoder->width << "x" << encoder->height;
return false;
}
cv::Mat frame_to_encode = frame;
if (options.draw_timestamp) {
frame_to_encode = frame.clone();
drawTimeStamp(frame_to_encode);
}
const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode);
if (src_pix_fmt == AV_PIX_FMT_NONE) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] unsupported channels: " << frame_to_encode.channels();
return false;
}
if (encoder->sws_context) {
sws_freeContext(encoder->sws_context);
}
encoder->sws_context = sws_getContext(frame_to_encode.cols,
frame_to_encode.rows,
src_pix_fmt,
encoder->codec_context->width,
encoder->codec_context->height,
encoder->codec_context->pix_fmt,
SWS_BILINEAR,
nullptr,
nullptr,
nullptr);
if (!encoder->sws_context) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to create SwsContext";
return false;
}
const uint8_t* src_data[AV_NUM_DATA_POINTERS] = {frame_to_encode.data};
int src_linesize[AV_NUM_DATA_POINTERS] = {static_cast<int>(frame_to_encode.step)};
const int scale_ret = sws_scale(encoder->sws_context,
src_data,
src_linesize,
0,
frame_to_encode.rows,
encoder->frame->data,
encoder->frame->linesize);
if (scale_ret < 0) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] error scaling frame";
return false;
}
encoder->frame->pts = encoder->frame_pts++;
int ret = avcodec_send_frame(encoder->codec_context, encoder->frame);
if (ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "[CameraStreamEncoder] error sending frame to encoder: " << errbuf;
return false;
}
while (true) {
ret = avcodec_receive_packet(encoder->codec_context, encoder->packet);
if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
break;
}
if (ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "[CameraStreamEncoder] error receiving packet from encoder: " << errbuf;
break;
}
if (encoder->packet->flags & AV_PKT_FLAG_KEY) {
is_key = true;
}
encoded_frame.reserve(encoded_frame.size() + encoder->packet->size);
encoded_frame.insert(encoded_frame.end(),
encoder->packet->data,
encoder->packet->data + encoder->packet->size);
av_packet_unref(encoder->packet);
}
if (encoded_frame.size() < 4) {
return false;
}
const bool has_start_code =
(encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 1) ||
(encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 0 && encoded_frame[3] == 1);
if (!has_start_code) {
CMVR_LOG(ERROR) << "[CameraStreamEncoder] invalid frame: no NALU start code";
return false;
}
return true;
}
} // namespace cmvr::device

24
cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt
View File

@ -4,8 +4,30 @@ add_library(mujoco_camera SHARED src/mujoco_camera.cpp)
target_include_directories(mujoco_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(mujoco_camera PUBLIC ${OpenCV_LIBS})
target_link_libraries(mujoco_camera
PUBLIC
${OpenCV_LIBS}
cmvr_es::proto
cmvr_es::device::camera_stream_encoder
cmvr_es::mujoco_world
cmvr_es::device::motor_manager
glfw
mujoco
)
add_library(cmvr_es::device::mujoco_camera ALIAS mujoco_camera)
add_executable(mujoco_camera_test
src/mujoco_camera_test.cpp
)
target_link_libraries(mujoco_camera_test
PRIVATE
cmvr_es::device::mujoco_camera
cmvr_es::mujoco_world
gtest
gtest_main
pthread
)
install(TARGETS mujoco_camera LIBRARY DESTINATION lib)

51
cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h
View File

@ -6,10 +6,19 @@
#include <cstdint>
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#include <mujoco/mujoco.h>
#include "cmvr/config/camera_config/camera_config.pb.h"
#include "devices/camera/abstract_camera.h"
#include "devices/camera/common/include/camera_stream_encoder.h"
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
struct GLFWwindow;
namespace cmvr::device {
@ -22,27 +31,67 @@ public:
uint64_t& frame_id)>;
explicit MujocoCamera(FetchRgbdFn fetch_rgbd_fn);
~MujocoCamera() override = default;
explicit MujocoCamera(config::MujocoCameraConfig config);
~MujocoCamera() override;
std::string typeName() const override { return "MujocoCamera"; }
const config::MujocoCameraConfig& config() const { return config_; }
bool init() override;
bool start() override;
bool stop() override;
void setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn);
void setFovyDeg(double fovy_deg);
void setConsumeNewFrameOnly(bool enable);
void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
void getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
bool startStreaming() override;
void stopStreaming() override;
bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
private:
bool fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
void fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsics) const;
bool initOffscreen_();
void destroyOffscreen_();
bool renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
bool ensureEncoder_(int width, int height, int fps);
void setError_(const std::string& error);
static void flipRgbAndDepth_(std::vector<unsigned char>& rgb,
std::vector<float>& depth,
int width,
int height);
static void linearizeDepth_(const mjModel* model, std::vector<float>& depth);
private:
FetchRgbdFn fetch_rgbd_fn_;
mutable std::mutex mtx_;
config::MujocoCameraConfig config_;
std::weak_ptr<simulate::MujocoWorld> world_;
GLFWwindow* window_{nullptr};
mjvCamera camera_{};
mjvOption option_{};
mjvPerturb perturb_{};
mjvScene scene_{};
mjrContext context_{};
bool scene_initialized_{false};
bool context_initialized_{false};
int camera_id_{-1};
int width_{640};
int height_{480};
int encode_width_{640};
int encode_height_{480};
std::string codec_{"H264"};
bool enable_stream_timestamp_{false};
double fovy_deg_{60.0};
bool consume_new_frame_only_{true};
uint64_t last_frame_id_{0};
bool has_last_frame_id_{false};
size_t stream_frame_index_{0};
bool streaming_{false};
std::shared_ptr<FfmpegEncoderInfo> rgb_encoder_;
};
} // namespace cmvr::device

475
cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp
View File

@ -1,87 +1,460 @@
//
// Created by lgv on 2026/2/26.
//
#include "../include/mujoco_camera.h"
#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <limits>
#include <utility>
#include <GLFW/glfw3.h>
#include <opencv2/imgproc.hpp>
#include "common/base/logging/logger.h"
#include "devices/motor/manager/include/motor_manager.h"
namespace cmvr::device {
namespace {
constexpr int kDefaultWidth = 640;
constexpr int kDefaultHeight = 480;
constexpr int kMaxGeom = 100000;
int positiveOrDefault(const int value, const int fallback)
{
return value > 0 ? value : fallback;
}
} // namespace
MujocoCamera::MujocoCamera(FetchRgbdFn fetch_rgbd_fn)
: fetch_rgbd_fn_(std::move(fetch_rgbd_fn)) {}
: fetch_rgbd_fn_(std::move(fetch_rgbd_fn))
{
id_ = "mujoco_camera";
}
void MujocoCamera::setFovyDeg(double fovy_deg) {
MujocoCamera::MujocoCamera(config::MujocoCameraConfig config)
: config_(config)
{
id_ = config_.id();
const auto& render = config_.render();
const auto& encoder = config_.encoder();
width_ = positiveOrDefault(render.width(), kDefaultWidth);
height_ = positiveOrDefault(render.height(), kDefaultHeight);
encode_width_ = positiveOrDefault(encoder.width(), width_);
encode_height_ = positiveOrDefault(encoder.height(), height_);
codec_ = encoder.codec().empty() ? "H264" : encoder.codec();
enable_stream_timestamp_ = encoder.enable_stream_timestamp();
consume_new_frame_only_ = config_.consume_new_frame_only();
}
MujocoCamera::~MujocoCamera()
{
stop();
}
bool MujocoCamera::init()
{
std::lock_guard<std::mutex> lock(mtx_);
if (fetch_rgbd_fn_) {
state_.is_initialized = true;
state_.is_opened = true;
state_.fps = positiveOrDefault(config_.render().fps(), 30);
state_.width = width_;
state_.height = height_;
return true;
}
if (id_.empty()) {
setError_("[MujocoCamera] id is empty");
return false;
}
if (config_.world_id().empty()) {
setError_("[MujocoCamera] world_id is empty: " + id_);
return false;
}
if (config_.camera_name().empty()) {
setError_("[MujocoCamera] camera_name is empty: " + id_);
return false;
}
auto world = simulate::MujocoWorldDevice::worldFor(config_.world_id());
if (!world) {
world = MotorManager::mujocoWorldFor(config_.world_id());
}
if (!world) {
setError_("[MujocoCamera] MuJoCo world not found: " + config_.world_id());
return false;
}
if (!world->isLoaded()) {
setError_("[MujocoCamera] MuJoCo world is not loaded: " + config_.world_id());
return false;
}
world_ = world;
{
std::lock_guard<std::mutex> world_lock(world->mutex());
const mjModel* model = world->model();
if (model == nullptr) {
setError_("[MujocoCamera] world model is null: " + config_.world_id());
return false;
}
camera_id_ = mj_name2id(model, mjOBJ_CAMERA, config_.camera_name().c_str());
if (camera_id_ < 0) {
setError_("[MujocoCamera] camera not found in MuJoCo model: " + config_.camera_name());
return false;
}
fovy_deg_ = model->cam_fovy[camera_id_];
}
if (!initOffscreen_()) {
return false;
}
state_.is_initialized = true;
state_.is_opened = true;
state_.fps = positiveOrDefault(config_.render().fps(), 30);
state_.width = width_;
state_.height = height_;
clear_error_();
CMVR_LOG(INFO) << "[MujocoCamera] initialized, id=" << id_
<< ", world_id=" << config_.world_id()
<< ", camera=" << config_.camera_name()
<< ", size=" << width_ << "x" << height_;
return true;
}
bool MujocoCamera::start()
{
if (!state_.is_initialized) {
if (!init()) {
return false;
}
}
std::lock_guard<std::mutex> lock(mtx_);
auto world = world_.lock();
if (world && !world->isRunning() && !world->start()) {
setError_("[MujocoCamera] failed to start MuJoCo world: " + world->lastError());
return false;
}
state_.is_streaming = true;
state_.is_opened = true;
return true;
}
bool MujocoCamera::stop()
{
std::lock_guard<std::mutex> lock(mtx_);
state_.is_streaming = false;
state_.is_opened = false;
destroyOffscreen_();
return true;
}
void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn)
{
std::lock_guard<std::mutex> lock(mtx_);
fetch_rgbd_fn_ = std::move(fetch_rgbd_fn);
if (fetch_rgbd_fn_) {
destroyOffscreen_();
state_.is_initialized = true;
state_.is_opened = true;
state_.fps = positiveOrDefault(config_.render().fps(), 30);
state_.width = width_;
state_.height = height_;
clear_error_();
}
}
void MujocoCamera::setFovyDeg(const double fovy_deg)
{
std::lock_guard<std::mutex> lock(mtx_);
fovy_deg_ = fovy_deg;
}
void MujocoCamera::setConsumeNewFrameOnly(bool enable) {
void MujocoCamera::setConsumeNewFrameOnly(const bool enable)
{
std::lock_guard<std::mutex> lock(mtx_);
consume_new_frame_only_ = enable;
}
void MujocoCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
void MujocoCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
{
cv::Mat depth;
if (!fetch(color, depth, intrinsics)) {
color.release();
}
}
void MujocoCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
void MujocoCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics)
{
cv::Mat color;
if (!fetch(color, depth, intrinsics)) {
depth.release();
}
}
void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
{
if (!fetch(color, depth, intrinsics)) {
color.release();
depth.release();
}
}
bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
bool MujocoCamera::startStreaming()
{
if (!state_.is_initialized && !init()) {
return false;
}
std::lock_guard<std::mutex> lock(mtx_);
if (!fetch_rgbd_fn_) return false;
streaming_ = true;
state_.is_streaming = true;
return true;
}
void MujocoCamera::stopStreaming()
{
std::lock_guard<std::mutex> lock(mtx_);
streaming_ = false;
state_.is_streaming = false;
stream_frame_index_ = 0;
rgb_encoder_.reset();
}
bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index)
{
if (!streaming_) {
return false;
}
cv::Mat color;
cv::Mat depth;
Rs2Intrinsics intrinsics{};
if (!fetch(color, depth, intrinsics) || color.empty()) {
return false;
}
cv::Mat color_to_encode = color;
if (encode_width_ > 0 && encode_height_ > 0 &&
(color.cols != encode_width_ || color.rows != encode_height_)) {
cv::resize(color, color_to_encode, cv::Size(encode_width_, encode_height_), 0.0, 0.0, cv::INTER_LINEAR);
}
const int fps = positiveOrDefault(config_.encoder().fps(), positiveOrDefault(config_.render().fps(), 30));
if (!ensureEncoder_(color_to_encode.cols, color_to_encode.rows, fps)) {
return false;
}
frame_data.rgbImage = color.clone();
CameraStreamEncodeOptions encode_options;
encode_options.draw_timestamp = enable_stream_timestamp_;
if (!CameraStreamEncoder::encode(rgb_encoder_,
color_to_encode,
frame_data.rgbFrame,
frame_data.bKey,
encode_options)) {
return false;
}
if (!depth.empty()) {
frame_data.depthImage = depth.clone();
const auto* depth_begin = reinterpret_cast<const uint8_t*>(depth.data);
const auto* depth_end = depth_begin + depth.total() * depth.elemSize();
frame_data.depthFrame.assign(depth_begin, depth_end);
}
frame_data.intrinsics = intrinsics;
frame_data.width = color_to_encode.cols;
frame_data.height = color_to_encode.rows;
frame_data.fps = fps;
frame_data.codec = codec_;
frame_data.depthKey = true;
next_index = ++stream_frame_index_;
return true;
}
bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
{
std::lock_guard<std::mutex> lock(mtx_);
if (fetch_rgbd_fn_) {
std::vector<unsigned char> rgb_raw;
std::vector<float> depth_raw;
int width = 0;
int height = 0;
uint64_t frame_id = 0;
std::uint64_t frame_id = 0;
if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
return false;
}
if (width <= 0 || height <= 0) return false;
if ((int)rgb_raw.size() != width * height * 3) return false;
if (!depth_raw.empty() && (int)depth_raw.size() != width * height) return false;
if (consume_new_frame_only_) {
if (has_last_frame_id_ && frame_id == last_frame_id_) {
if (width <= 0 || height <= 0) {
return false;
}
if (static_cast<int>(rgb_raw.size()) != width * height * 3) {
return false;
}
if (!depth_raw.empty() && static_cast<int>(depth_raw.size()) != width * height) {
return false;
}
if (consume_new_frame_only_ && has_last_frame_id_ && frame_id == last_frame_id_) {
return false;
}
last_frame_id_ = frame_id;
has_last_frame_id_ = true;
cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
color = rgb.clone();
cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
if (!depth_raw.empty()) {
cv::Mat dep(height, width, CV_32FC1, depth_raw.data());
depth = dep.clone();
} else {
depth.release();
}
fillIntrinsics(width, height, intrinsics);
return true;
}
void MujocoCamera::fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsics) const {
return renderOffscreen_(color, depth, intrinsics);
}
bool MujocoCamera::initOffscreen_()
{
if (window_ != nullptr && context_initialized_ && scene_initialized_) {
return true;
}
if (!glfwInit()) {
setError_("[MujocoCamera] glfwInit failed");
return false;
}
glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
glfwWindowHint(GLFW_DOUBLEBUFFER, GLFW_FALSE);
window_ = glfwCreateWindow(width_, height_, ("mujoco_camera_" + id_).c_str(), nullptr, nullptr);
if (window_ == nullptr) {
setError_("[MujocoCamera] glfwCreateWindow failed");
return false;
}
glfwMakeContextCurrent(window_);
mjv_defaultCamera(&camera_);
mjv_defaultOption(&option_);
mjv_defaultPerturb(&perturb_);
mjv_defaultScene(&scene_);
mjr_defaultContext(&context_);
auto world = world_.lock();
if (!world) {
setError_("[MujocoCamera] world expired");
return false;
}
std::lock_guard<std::mutex> world_lock(world->mutex());
const mjModel* model = world->model();
if (model == nullptr) {
setError_("[MujocoCamera] world model is null");
return false;
}
mjv_makeScene(model, &scene_, kMaxGeom);
scene_initialized_ = true;
mjr_makeContext(model, &context_, mjFONTSCALE_150);
context_initialized_ = true;
mjr_setBuffer(mjFB_OFFSCREEN, &context_);
if (context_.currentBuffer != mjFB_OFFSCREEN) {
setError_("[MujocoCamera] MuJoCo offscreen buffer is not available");
return false;
}
return true;
}
void MujocoCamera::destroyOffscreen_()
{
if (window_ != nullptr) {
glfwMakeContextCurrent(window_);
}
if (context_initialized_) {
mjr_freeContext(&context_);
context_initialized_ = false;
}
if (scene_initialized_) {
mjv_freeScene(&scene_);
scene_initialized_ = false;
}
if (window_ != nullptr) {
glfwDestroyWindow(window_);
window_ = nullptr;
}
}
bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
{
if (!state_.is_initialized) {
setError_("[MujocoCamera] camera is not initialized: " + id_);
return false;
}
if (!initOffscreen_()) {
return false;
}
auto world = world_.lock();
if (!world) {
setError_("[MujocoCamera] world expired");
return false;
}
glfwMakeContextCurrent(window_);
std::vector<unsigned char> rgb(static_cast<std::size_t>(width_) * height_ * 3);
std::vector<float> depth_raw(static_cast<std::size_t>(width_) * height_);
{
std::lock_guard<std::mutex> world_lock(world->mutex());
mjModel* model = world->model();
mjData* data = world->data();
if (model == nullptr || data == nullptr) {
setError_("[MujocoCamera] world model/data is null");
return false;
}
camera_.type = mjCAMERA_FIXED;
camera_.fixedcamid = camera_id_;
camera_.trackbodyid = -1;
mjrRect viewport;
viewport.left = 0;
viewport.bottom = 0;
viewport.width = width_;
viewport.height = height_;
mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
mjr_render(viewport, &scene_, &context_);
mjr_readPixels(rgb.data(), depth_raw.data(), viewport, &context_);
flipRgbAndDepth_(rgb, depth_raw, width_, height_);
linearizeDepth_(model, depth_raw);
}
cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data());
color = rgb_mat.clone();
cv::Mat depth_mat(height_, width_, CV_32FC1, depth_raw.data());
depth = depth_mat.clone();
fillIntrinsics(width_, height_, intrinsics);
++last_frame_id_;
has_last_frame_id_ = true;
clear_error_();
return true;
}
bool MujocoCamera::ensureEncoder_(const int width, const int height, const int fps)
{
if (rgb_encoder_ &&
rgb_encoder_->width == width &&
rgb_encoder_->height == height &&
rgb_encoder_->fps == fps &&
rgb_encoder_->codec_name == codec_) {
return true;
}
rgb_encoder_.reset();
return CameraStreamEncoder::init(rgb_encoder_, codec_, width, height, fps);
}
void MujocoCamera::fillIntrinsics(const int width, const int height, Rs2Intrinsics& intrinsics) const
{
const double fovy = fovy_deg_ * M_PI / 180.0;
const double fy = (height * 0.5) / std::tan(fovy * 0.5);
const double fx = fy;
@ -95,4 +468,56 @@ void MujocoCamera::fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsi
}
}
void MujocoCamera::setError_(const std::string& error)
{
state_.is_error = true;
state_.error_message = error;
CMVR_LOG(ERROR) << error;
}
void MujocoCamera::flipRgbAndDepth_(std::vector<unsigned char>& rgb,
std::vector<float>& depth,
const int width,
const int height)
{
for (int row = 0; row < height / 2; ++row) {
auto* rgb_top = rgb.data() + 3 * width * row;
auto* rgb_bottom = rgb.data() + 3 * width * (height - 1 - row);
std::swap_ranges(rgb_top, rgb_top + 3 * width, rgb_bottom);
auto* depth_top = depth.data() + width * row;
auto* depth_bottom = depth.data() + width * (height - 1 - row);
std::swap_ranges(depth_top, depth_top + width, depth_bottom);
}
}
void MujocoCamera::linearizeDepth_(const mjModel* model, std::vector<float>& depth)
{
if (model == nullptr) {
return;
}
const double znear = static_cast<double>(model->vis.map.znear) *
static_cast<double>(model->stat.extent);
const double zfar = static_cast<double>(model->vis.map.zfar) *
static_cast<double>(model->stat.extent);
if (znear <= 0.0 || zfar <= znear) {
return;
}
const double two_nf = 2.0 * znear * zfar;
const double f_plus_n = zfar + znear;
const double f_minus_n = zfar - znear;
for (float& item : depth) {
if (!std::isfinite(item) || item <= 0.0f || item >= 1.0f) {
item = std::numeric_limits<float>::infinity();
continue;
}
const double z_ndc = 2.0 * static_cast<double>(item) - 1.0;
const double denom = f_plus_n - z_ndc * f_minus_n;
item = denom <= 1e-12
? std::numeric_limits<float>::infinity()
: static_cast<float>(two_nf / denom);
}
}
} // namespace cmvr::device

56
cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp Normal file
View File

@ -0,0 +1,56 @@
#include <gtest/gtest.h>
#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
namespace {
TEST(MujocoCameraTest, CapturesOffscreenRgbdFrame)
{
cmvr::config::MujocoWorldConfig world_config;
world_config.set_id("mujoco_camera_test_world");
world_config.set_model_path("model/xiaoyan_description/dual_arm.xml");
world_config.set_timestep_s(0.001);
world_config.set_realtime_factor(1.0);
world_config.set_require_actuator(false);
auto world_device = std::make_shared<cmvr::simulate::MujocoWorldDevice>(world_config);
ASSERT_TRUE(world_device->init());
ASSERT_TRUE(world_device->start());
cmvr::config::MujocoCameraConfig camera_config;
camera_config.set_id("mujoco_camera_test_cam");
camera_config.set_world_id(world_config.id());
camera_config.set_camera_name("hand_cam");
auto* render = camera_config.mutable_render();
render->set_width(320);
render->set_height(240);
render->set_fps(30);
render->set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
auto* encoder = camera_config.mutable_encoder();
encoder->set_width(320);
encoder->set_height(240);
encoder->set_fps(30);
encoder->set_codec("H264");
encoder->set_enable_stream_timestamp(true);
cmvr::device::MujocoCamera camera(camera_config);
ASSERT_TRUE(camera.init());
ASSERT_TRUE(camera.start());
cv::Mat color;
cv::Mat depth;
cmvr::device::Rs2Intrinsics intrinsics{};
camera.getRGBDImages(color, depth, intrinsics);
EXPECT_EQ(color.cols, 320);
EXPECT_EQ(color.rows, 240);
EXPECT_EQ(color.type(), CV_8UC3);
EXPECT_EQ(depth.cols, 320);
EXPECT_EQ(depth.rows, 240);
EXPECT_EQ(depth.type(), CV_32FC1);
EXPECT_GT(intrinsics.fx, 0.0f);
EXPECT_GT(intrinsics.fy, 0.0f);
}
} // namespace

1
cmvr-es/devices/camera/realsense_camera/CMakeLists.txt
View File

@ -5,6 +5,7 @@ target_include_directories(realsense_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命名空间别名 OK
# 链接 librealsense2
target_link_libraries(realsense_camera PUBLIC cmvr_es::device::camera_stream_encoder)
target_link_libraries(realsense_camera PRIVATE realsense2 opencv_core opencv_imgproc opencv_videoio cmvr_es::proto)
target_link_libraries(realsense_camera PRIVATE
realsense2

10
cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h
View File

@ -5,7 +5,9 @@
#ifndef REALSENSE_CAMERA_H
#define REALSENSE_CAMERA_H
#include "../../uvc_camera/include/uvc_camera.h"
#include "camera/abstract_camera.h"
#include "common/base/ring_buffer.h"
#include "devices/camera/common/include/camera_stream_encoder.h"
#include <librealsense2/rs.hpp>
#include <librealsense2/hpp/rs_internal.hpp>
@ -30,11 +32,6 @@ namespace cmvr::device{
void pauseRecording() override;
void resumeRecording() override;
// 初始化单个编码器的通用函数（复用逻辑，避免重复代码）
static bool initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const std::string& codec_name,
int width, int height, int fps);
static bool encodeFrameWithEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,const cv::Mat& frame,std::vector<uint8_t>& encoded_frame,bool& is_key);
void getEncodedFrame(StreamFrameData& frame_data, size_t& index) override;
bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
@ -58,6 +55,7 @@ namespace cmvr::device{
cv::VideoCapture cap_;
size_t buffer_size_;
std::string codec_;
bool enable_stream_timestamp_{false};
CameraMode mode_;
StreamMode stream_mode_;

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

@ -76,12 +76,15 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
state_.error_message = "empty device serial number";
return;
}
fps_ = camera_.fps();
width_ = camera_.width();
height_ = camera_.height();
encode_width_ = camera_.encode_width() > 0 ? camera_.encode_width() : width_;
encode_height_ = camera_.encode_height() > 0 ? camera_.encode_height() : height_;
buffer_size_ = camera_.buffer_size();
const auto& capture = camera_.capture();
const auto& encoder = camera_.encoder();
fps_ = capture.fps();
width_ = capture.width();
height_ = capture.height();
encode_width_ = encoder.width() > 0 ? encoder.width() : width_;
encode_height_ = encoder.height() > 0 ? encoder.height() : height_;
buffer_size_ = encoder.buffer_size() > 0 ? encoder.buffer_size() : 30;
enable_stream_timestamp_ = encoder.enable_stream_timestamp();
state_.fps = fps_;
state_.width = width_;
@ -100,7 +103,7 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
return;
}
auto stream_mode = camera_.stream_mode();
auto stream_mode = capture.stream_mode();
if (stream_mode == config::STREAM_MODE_RGB)
{
stream_mode_ = COLOR_MODE;
@ -126,7 +129,7 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
align_mode_ = "color";
}
codec_ = camera_.codec();
codec_ = encoder.codec().empty() ? "H264" : encoder.codec();
}
RealsenseCamera::~RealsenseCamera() {
@ -171,7 +174,7 @@ bool RealsenseCamera::init() {
//初始化编码器
// 初始化RGB编码器（示例参数：640x480，30fps，H.264）
if (!initSingleEncoder(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
CMVR_LOG(ERROR) << "[RealsenseCamera] (start): Failed to init RGB encoder!";
state_.is_initialized = false;
state_.is_error = true;
@ -767,7 +770,13 @@ void RealsenseCamera::streaming_worker_() {
0.0,
cv::INTER_LINEAR);
}
success = encodeFrameWithEncoder(rgbEncoder_, rgb_to_encode, frame_data.rgbFrame, frame_data.bKey);
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);
// 深度图编码
// success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);
if (success) {
@ -890,297 +899,6 @@ void RealsenseCamera::recording_worker_() {
state_.is_recording = false;
}
// 初始化单个编码器的通用函数
bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const std::string& codec_name,
int width, int height, int fps) {
// 1. 创建编码器实例（不变）
encoder = std::make_shared<FfmpegEncoderInfo>();
encoder->codec_name = codec_name;
encoder->width = width;
encoder->height = height;
encoder->fps = fps;
// 2. 查找编码器（不变）
const AVCodec* codec = nullptr;
if (codec_name == "h264" || codec_name == "H264") {
codec = avcodec_find_encoder_by_name("libx264");
if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_H264);
} else if (codec_name == "h265" || codec_name == "HEVC" || codec_name == "H265") {
codec = avcodec_find_encoder_by_name("libx265");
if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_HEVC);
}
if (!codec) {
CMVR_LOG(ERROR) << "Failed to find " << codec_name << " encoder!";
return false;
}
// 3. 初始化编码器上下文（不变）
encoder->codec_context = avcodec_alloc_context3(codec);
if (!encoder->codec_context) {
CMVR_LOG(ERROR) << "Failed to allocate codec context!";
return false;
}
// 4. 设置编码器参数（不变）
AVCodecContext* ctx = encoder->codec_context;
ctx->codec_type = AVMEDIA_TYPE_VIDEO;
ctx->width = (width + 1) & ~1; // 确保宽度为偶数
ctx->height = (height + 1) & ~1;// 确保高度为偶数
ctx->time_base = {1, fps}; // 时间基（1/fps）
ctx->framerate = {fps, 1}; // 帧率
ctx->max_b_frames = 0; // 禁用B帧（降低延迟）
ctx->gop_size = 10;//I帧间隔
// 5. 设置编码器私有参数
if (codec->id == AV_CODEC_ID_H264) {
av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
av_opt_set(ctx->priv_data, "profile", "baseline", 0);
av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
av_opt_set(ctx->priv_data, "annexb", "1", 0);
} else if (codec->id == AV_CODEC_ID_HEVC) {
// 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。
av_opt_set(ctx->priv_data, "x265-params", "log-level=none", 0);
// H.265 的强制设置
// 使用 x265-params 字符串设置所有参数
char x265_params[256];
snprintf(x265_params, sizeof(x265_params),
"keyint=%d:" // 关键帧间隔
"min-keyint=%d:" // 最小关键帧间隔
"no-open-gop=1:" // 禁用开放GOP
"bframes=0:" // 禁用B帧
"rc-lookahead=0:"
"log-level=none", // 日志级别
10, 10); // 设置keyint和min-keyint为10
av_opt_set(ctx->priv_data, "x265-params", x265_params, 0);
// 或者分开设置（如果支持）
av_opt_set_int(ctx->priv_data, "keyint", 10, 0);
av_opt_set_int(ctx->priv_data, "min-keyint", 10, 0);
av_opt_set(ctx->priv_data, "no-open-gop", "1", 0);
}
// 6. 设置像素格式（不变）
const enum AVPixelFormat* pix_fmts = codec->pix_fmts;
if (!pix_fmts) {
CMVR_LOG(INFO) << "Using default pixel format: YUV420P";
ctx->pix_fmt = AV_PIX_FMT_YUV420P;
} else {
ctx->pix_fmt = pix_fmts[0];
CMVR_LOG(INFO) << "Selected pixel format: " << av_get_pix_fmt_name(ctx->pix_fmt);
}
// 7. 打开编码器（不变）
int ret = avcodec_open2(ctx, codec, nullptr);
if (ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "Failed to open " << codec_name << " encoder: " << errbuf;
return false;
}
// 8. 分配AVFrame（不变）
encoder->frame = av_frame_alloc();
if (!encoder->frame) {
CMVR_LOG(ERROR) << "Failed to allocate AVFrame!";
return false;
}
encoder->frame->format = ctx->pix_fmt;
encoder->frame->width = ctx->width;
encoder->frame->height = ctx->height;
if (av_frame_get_buffer(encoder->frame, 0) < 0) {
CMVR_LOG(ERROR) << "Failed to allocate AVFrame buffer!";
av_frame_free(&encoder->frame);
return false;
}
// 9. 分配AVPacket（不变）
encoder->packet = av_packet_alloc();
if (!encoder->packet) {
CMVR_LOG(ERROR) << "Failed to allocate AVPacket!";
return false;
}
// 添加调试信息，确认设置生效
CMVR_LOG(INFO) << "Encoder settings:";
CMVR_LOG(INFO) << " GOP size: " << ctx->gop_size;
if (codec->id == AV_CODEC_ID_HEVC) {
char* params = nullptr;
if (av_opt_get(ctx->priv_data, "x265-params", 0, (uint8_t**)&params) >= 0) {
CMVR_LOG(INFO) << " x265-params: " << params;
av_free(params);
}
}
CMVR_LOG(INFO) << "Successfully initialized " << codec_name << " encoder ( "
<< width << "x" << height << "@" << fps << "fps )";
return true;
}
// 获取当前时间并格式化为字符串
std::string getCurrentTimeString() {
// 获取当前时间（精确到毫秒）
auto now = std::chrono::system_clock::now();
// 转换为秒级时间
auto now_sec = std::chrono::time_point_cast<std::chrono::seconds>(now);
std::time_t now_time = std::chrono::system_clock::to_time_t(now_sec);
std::tm* tm_ptr = std::localtime(&now_time);
// 计算毫秒部分
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
now - now_sec
).count();
// 格式化时间字符串
std::stringstream ss;
ss << std::put_time(tm_ptr, "%Y-%m-%d %H:%M:%S")
<< "." << std::setw(3) << std::setfill('0') << ms;
return ss.str();
}
// 在cv::Mat右上角绘制时间戳
void drawTimeStamp(cv::Mat& image) {
if (image.empty()) return;
std::string time_str = getCurrentTimeString();
cv::Point text_pos;
cv::Scalar text_color(255, 255, 255); // 白色文字
int font_face = cv::FONT_HERSHEY_SIMPLEX;
double font_scale = 0.8;
int thickness = 2;
// 计算文本尺寸，用于确定右上角位置
int baseline = 0;
cv::Size text_size = cv::getTextSize(time_str, font_face, font_scale, thickness, &baseline);
// 设置右上角坐标（留出10像素边距）
text_pos.x = image.cols - text_size.width - 10;
text_pos.y = text_size.height + 10;
// 绘制文字（先画黑色背景增加可读性）
cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), thickness + 2);
cv::putText(image, time_str, text_pos, font_face, font_scale, text_color, thickness);
}
bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder, const cv::Mat& frame,std::vector<uint8_t>& encoded_frame,bool& is_key) {
if (!encoder || !encoder->codec_context || !encoder->frame || !encoder->packet) {
return false;
}
// 确保输入帧尺寸匹配（不变）
if (frame.cols != encoder->width || frame.rows != encoder->height) {
CMVR_LOG(ERROR) << "Frame size does not match encoder dimensions";
return false;
}
cv::Mat dateImage;
frame.copyTo(dateImage);
drawTimeStamp(dateImage);
// 【修复3：设置递增的PTS，确保编码器正确处理I帧请求】
encoder->frame->pts = encoder->frame_pts++; // 分配唯一PTS
// 根据cv::Mat的类型设置源格式（不变）
AVPixelFormat src_pix_fmt;
if (dateImage.channels() == 3) {
src_pix_fmt = AV_PIX_FMT_BGR24; // OpenCV默认BGR
} else if (dateImage.channels() == 4) {
src_pix_fmt = AV_PIX_FMT_BGRA; // 4通道为BGRA
} else if (dateImage.channels() == 1) {
src_pix_fmt = AV_PIX_FMT_GRAY8; // 单通道灰度图
} else {
CMVR_LOG(ERROR) << "Unsupported number of channels: " << dateImage.channels();
return false;
}
// 【修复4：动态创建sws_context（匹配当前输入格式）】
if (encoder->sws_context) {
sws_freeContext(encoder->sws_context); // 释放旧上下文
}
encoder->sws_context = sws_getContext(
dateImage.cols, dateImage.rows, src_pix_fmt, // 输入格式由当前frame决定
encoder->codec_context->width, encoder->codec_context->height, encoder->codec_context->pix_fmt,
SWS_BILINEAR, nullptr, nullptr, nullptr
);
if (!encoder->sws_context) {
CMVR_LOG(ERROR) << "Failed to create SwsContext";
return false;
}
// 转换输入帧格式为编码器所需格式（不变）
const uint8_t* src_data[AV_NUM_DATA_POINTERS] = {dateImage.data};
int src_linesize[AV_NUM_DATA_POINTERS] = {static_cast<int>(dateImage.step)};
int ret = sws_scale(encoder->sws_context, src_data, src_linesize, 0, dateImage.rows,
encoder->frame->data, encoder->frame->linesize);
if (ret < 0) {
CMVR_LOG(ERROR) << "Error scaling frame";
return false;
}
// 发送帧到编码器（不变）
ret = avcodec_send_frame(encoder->codec_context, encoder->frame);
if (ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "Error sending frame to encoder: " << errbuf;
return false;
}
while (true)
{
// 接收编码后的数据（不变）
ret = avcodec_receive_packet(encoder->codec_context, encoder->packet);
if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
break;
} else if (ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "Error receiving packet from encoder: " << errbuf;
break;
}
// 调试：打印帧类型（I帧/P帧）
if (encoder->packet->flags & AV_PKT_FLAG_KEY) {
is_key = true;
//CMVR_LOG(INFO) << "Encoded I frame (size: " << encoder->packet->size << " bytes)";
} else {
is_key = false;
//CMVR_LOG(INFO) << "Encoded P frame (size: " << encoder->packet->size << " bytes)";
}
// 预留足够空间，避免多次内存分配
encoded_frame.reserve(encoded_frame.size() + encoder->packet->size);
// 复制数据包内容到输出向量
encoded_frame.insert(encoded_frame.end(),
encoder->packet->data,
encoder->packet->data + encoder->packet->size);
av_packet_unref(encoder->packet);
}
// 验证帧有效性（NALU起始码、元数据）
if (!encoded_frame.empty()) {
// 检查NALU起始码
bool has_start_code = false;
if ((encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 1) ||
(encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 0 && encoded_frame[3] == 1)) {
has_start_code = true;
}
if (!has_start_code) {
CMVR_LOG(ERROR) << "Invalid frame: no NALU start code!";
return false;
}
} else {
//CMVR_LOG(ERROR) << "Encoded frame is empty!";
return false;
}
return true;
}
void RealsenseCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) {
// 环形队列取数据的index由接口传入
auto frame = stream_frame_buffer_->pop(index);

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

@ -70,14 +70,20 @@ cmvr::config::RealSenseCameraConfig makeRsConfig() {
cmvr::config::RealSenseCameraConfig cfg;
cfg.set_id("realsense_health_check");
cfg.set_serialnumber(kRsSerialForHealthCheck);
cfg.set_width(kRsWidth);
cfg.set_height(kRsHeight);
cfg.set_fps(kRsFps);
cfg.set_codec("H265");
cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
auto* capture = cfg.mutable_capture();
capture->set_width(kRsWidth);
capture->set_height(kRsHeight);
capture->set_fps(kRsFps);
capture->set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
auto* encoder = cfg.mutable_encoder();
encoder->set_width(kRsWidth);
encoder->set_height(kRsHeight);
encoder->set_fps(kRsFps);
encoder->set_codec("H265");
encoder->set_enable_stream_timestamp(true);
encoder->set_buffer_size(30);
cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
cfg.set_buffer_size(30);
cfg.set_sync(true);
return cfg;
}

2
cmvr-es/devices/camera/uvc_camera/CMakeLists.txt
View File

@ -2,7 +2,7 @@ add_library(uvc_camera SHARED src/uvc_camera.cpp)
target_include_directories(uvc_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(uvc_camera PUBLIC glog opencv_core opencv_imgproc cmvr_es::proto)
target_link_libraries(uvc_camera PUBLIC glog opencv_core opencv_imgproc cmvr_es::proto cmvr_es::device::camera_stream_encoder)
add_library(cmvr_es::device::uvc_camera ALIAS uvc_camera)
install(TARGETS uvc_camera LIBRARY DESTINATION lib)

55
cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h
View File

@ -7,17 +7,9 @@
#include "common/base/ring_buffer.h"
#include "camera/abstract_camera.h"
//使用ffmpeg来编码保存视频文件
#define USE_FFMPEG_ENCODER 1
#if USE_FFMPEG_ENCODER
#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
#endif
#include "devices/camera/common/include/camera_stream_encoder.h"
namespace cmvr::device {
enum CameraMode {PHOTO_MODE, VIDEO_MODE};
struct ImageData
{
@ -27,45 +19,6 @@ namespace cmvr::device {
cv::Mat depthImage;
};
struct FfmpegEncoderInfo {
std::string codec_name; // 编码器名称（如"h264"、"hevc"）
int width = 0; // 图像宽度
int height = 0; // 图像高度
int fps = 0; // 帧率
int64_t frame_pts = 0;
bool bRunning = false; // 是否进行编码
AVCodecContext* codec_context = nullptr; // 编码器上下文
AVFrame* frame = nullptr; // 输入帧
AVPacket* packet = nullptr; // 输出包
SwsContext* sws_context = nullptr; // 图像转换上下文（如果需要格式转换）
// 析构函数：释放FFmpeg资源（核心！避免内存泄漏）
~FfmpegEncoderInfo() {
// 释放编码帧
if (frame) {
av_frame_free(&frame);
frame = nullptr;
}
// 释放编码包
if (packet) {
av_packet_free(&packet);
packet = nullptr;
}
// 释放编码器上下文
if (codec_context) {
avcodec_close(codec_context); // 关闭编码器
avcodec_free_context(&codec_context); // 释放上下文
codec_context = nullptr;
}
// 释放格式转换上下文
if (sws_context) {
sws_freeContext(sws_context);
sws_context = nullptr;
}
//std::cout << "FfmpegEncoderInfo resources released." << std::endl;
}
};
//USB摄像机
class UVCCamera final : public AbstractCamera {
public:
@ -83,11 +36,6 @@ namespace cmvr::device {
void stopRecording() override;
void pauseRecording() override;
void resumeRecording() override;
// 初始化单个编码器的通用函数（复用逻辑，避免重复代码）
static bool initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const std::string& codec_name,
int width, int height, int fps);
static bool encodeFrameWithEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,const cv::Mat& frame,std::vector<uint8_t>& encoded_frame,bool& is_key);
void getEncodedFrame(StreamFrameData& frame_data, size_t& index) override;
bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
@ -106,6 +54,7 @@ namespace cmvr::device {
cv::VideoCapture cap_;
size_t buffer_size_;
std::string codec_;
bool enable_stream_timestamp_{false};
CameraMode mode_;
// std::shared_ptr<cv::Mat> current_image_;

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

@ -20,12 +20,15 @@ UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
state_.error_message = "empty device serial number";
return;
}
fps_ = camera_.fps();
width_ = camera_.width();
height_ = camera_.height();
encode_width_ = camera_.encode_width() > 0 ? camera_.encode_width() : width_;
encode_height_ = camera_.encode_height() > 0 ? camera_.encode_height() : height_;
buffer_size_ = camera_.buffer_size();
const auto& capture = camera_.capture();
const auto& encoder = camera_.encoder();
fps_ = capture.fps();
width_ = capture.width();
height_ = capture.height();
encode_width_ = encoder.width() > 0 ? encoder.width() : width_;
encode_height_ = encoder.height() > 0 ? encoder.height() : height_;
buffer_size_ = encoder.buffer_size() > 0 ? encoder.buffer_size() : 30;
enable_stream_timestamp_ = encoder.enable_stream_timestamp();
state_.fps = fps_;
state_.width = width_;
@ -44,7 +47,7 @@ UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
return;
}
codec_ = camera_.codec();
codec_ = encoder.codec().empty() ? "H264" : encoder.codec();
}
UVCCamera::~UVCCamera() {
stop();
@ -100,7 +103,7 @@ bool UVCCamera::init() {
}
//初始化编码器
// 初始化RGB编码器（示例参数：640x480，30fps，H.264）
if (!initSingleEncoder(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
CMVR_LOG(ERROR) << "[UVCCamera] (start): Failed to init RGB encoder!";
state_.is_error = true;
state_.error_message = "Failed to init RGB encoder!";
@ -499,7 +502,13 @@ void UVCCamera::streaming_worker_() {
0.0,
cv::INTER_LINEAR);
}
success = encodeFrameWithEncoder(rgbEncoder_, rgb_to_encode, frame_data.rgbFrame, frame_data.bKey);
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 (success) {
frame_data.fps = fps_;
frame_data.width = encode_width_;
@ -621,216 +630,6 @@ void UVCCamera::recording_worker_() {
state_.is_recording = false;
}
// 初始化单个编码器的通用函数
bool UVCCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const std::string& codec_name,
int width, int height, int fps) {
// 1. 创建编码器实例（不变）
encoder = std::make_shared<FfmpegEncoderInfo>();
encoder->codec_name = codec_name;
encoder->width = width;
encoder->height = height;
encoder->fps = fps;
// 2. 查找编码器（不变）
const AVCodec* codec = nullptr;
if (codec_name == "h264" || codec_name == "H264") {
codec = avcodec_find_encoder_by_name("libx264");
if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_H264);
} else if (codec_name == "h265" || codec_name == "HEVC" || codec_name == "H265") {
codec = avcodec_find_encoder_by_name("libx265");
if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_HEVC);
}
if (!codec) {
CMVR_LOG(ERROR) << "Failed to find " << codec_name << " encoder!";
return false;
}
// 3. 初始化编码器上下文（不变）
encoder->codec_context = avcodec_alloc_context3(codec);
if (!encoder->codec_context) {
CMVR_LOG(ERROR) << "Failed to allocate codec context!";
return false;
}
// 4. 设置编码器参数（不变）
AVCodecContext* ctx = encoder->codec_context;
ctx->codec_type = AVMEDIA_TYPE_VIDEO;
ctx->width = (width + 1) & ~1; // 确保宽度为偶数
ctx->height = (height + 1) & ~1;// 确保高度为偶数
ctx->time_base = {1, fps}; // 时间基（1/fps）
ctx->framerate = {fps, 1}; // 帧率
ctx->max_b_frames = 0; // 禁用B帧（降低延迟）
ctx->gop_size = 10;//I帧间隔1，每一帧都是I帧
// 5. 设置编码器私有参数
if (codec->id == AV_CODEC_ID_H264) {
av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
av_opt_set(ctx->priv_data, "profile", "baseline", 0);
av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
av_opt_set(ctx->priv_data, "annexb", "1", 0);
} else if (codec->id == AV_CODEC_ID_HEVC) {
// 直接使用默认参数，不自定义
// av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
// av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
}
// 6. 设置像素格式（不变）
const enum AVPixelFormat* pix_fmts = codec->pix_fmts;
if (!pix_fmts) {
CMVR_LOG(INFO) << "Using default pixel format: YUV420P";
ctx->pix_fmt = AV_PIX_FMT_YUV420P;
} else {
ctx->pix_fmt = pix_fmts[0];
CMVR_LOG(INFO) << "Selected pixel format: " << av_get_pix_fmt_name(ctx->pix_fmt);
}
// 7. 打开编码器（不变）
int ret = avcodec_open2(ctx, codec, nullptr);
if (ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "Failed to open " << codec_name << " encoder: " << errbuf;
return false;
}
// 8. 分配AVFrame（不变）
encoder->frame = av_frame_alloc();
if (!encoder->frame) {
CMVR_LOG(ERROR) << "Failed to allocate AVFrame!";
return false;
}
encoder->frame->format = ctx->pix_fmt;
encoder->frame->width = ctx->width;
encoder->frame->height = ctx->height;
if (av_frame_get_buffer(encoder->frame, 0) < 0) {
CMVR_LOG(ERROR) << "Failed to allocate AVFrame buffer!";
av_frame_free(&encoder->frame);
return false;
}
// 9. 分配AVPacket（不变）
encoder->packet = av_packet_alloc();
if (!encoder->packet) {
CMVR_LOG(ERROR) << "Failed to allocate AVPacket!";
return false;
}
CMVR_LOG(INFO) << "Successfully initialized " << codec_name << " encoder ( "
<< width << "x" << height << "@" << fps << "fps )";
return true;
}
bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder, const cv::Mat& frame,std::vector<uint8_t>& encoded_frame,bool& is_key) {
if (!encoder || !encoder->codec_context || !encoder->frame || !encoder->packet) {
return false;
}
// 确保输入帧尺寸匹配（不变）
if (frame.cols != encoder->width || frame.rows != encoder->height) {
CMVR_LOG(ERROR) << "Frame size does not match encoder dimensions";
return false;
}
// 【修复3：设置递增的PTS，确保编码器正确处理I帧请求】
encoder->frame->pts = encoder->frame_pts++; // 分配唯一PTS
// 根据cv::Mat的类型设置源格式（不变）
AVPixelFormat src_pix_fmt;
if (frame.channels() == 3) {
src_pix_fmt = AV_PIX_FMT_BGR24; // OpenCV默认BGR
} else if (frame.channels() == 4) {
src_pix_fmt = AV_PIX_FMT_BGRA; // 4通道为BGRA
} else if (frame.channels() == 1) {
src_pix_fmt = AV_PIX_FMT_GRAY8; // 单通道灰度图
} else {
CMVR_LOG(ERROR) << "Unsupported number of channels: " << frame.channels();
return false;
}
// 【修复4：动态创建sws_context（匹配当前输入格式）】
if (encoder->sws_context) {
sws_freeContext(encoder->sws_context); // 释放旧上下文
}
encoder->sws_context = sws_getContext(
frame.cols, frame.rows, src_pix_fmt, // 输入格式由当前frame决定
encoder->codec_context->width, encoder->codec_context->height, encoder->codec_context->pix_fmt,
SWS_BILINEAR, nullptr, nullptr, nullptr
);
if (!encoder->sws_context) {
CMVR_LOG(ERROR) << "Failed to create SwsContext";
return false;
}
// 转换输入帧格式为编码器所需格式（不变）
const uint8_t* src_data[AV_NUM_DATA_POINTERS] = {frame.data};
int src_linesize[AV_NUM_DATA_POINTERS] = {static_cast<int>(frame.step)};
int ret = sws_scale(encoder->sws_context, src_data, src_linesize, 0, frame.rows,
encoder->frame->data, encoder->frame->linesize);
if (ret < 0) {
CMVR_LOG(ERROR) << "Error scaling frame";
return false;
}
// 发送帧到编码器（不变）
ret = avcodec_send_frame(encoder->codec_context, encoder->frame);
if (ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "Error sending frame to encoder: " << errbuf;
return false;
}
while (true)
{
// 接收编码后的数据（不变）
ret = avcodec_receive_packet(encoder->codec_context, encoder->packet);
if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
break;
} else if (ret < 0) {
char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
av_strerror(ret, errbuf, sizeof(errbuf));
CMVR_LOG(ERROR) << "Error receiving packet from encoder: " << errbuf;
break;
}
// 调试：打印帧类型（I帧/P帧）
if (encoder->packet->flags & AV_PKT_FLAG_KEY) {
is_key = true;
//CMVR_LOG(INFO) << "Encoded I frame (size: " << encoder->packet->size << " bytes)";
} else {
is_key = false;
//CMVR_LOG(INFO) << "Encoded P frame (size: " << encoder->packet->size << " bytes)";
}
// 预留足够空间，避免多次内存分配
encoded_frame.reserve(encoded_frame.size() + encoder->packet->size);
// 复制数据包内容到输出向量
encoded_frame.insert(encoded_frame.end(),
encoder->packet->data,
encoder->packet->data + encoder->packet->size);
av_packet_unref(encoder->packet);
}
// 验证帧有效性（NALU起始码、元数据）
if (!encoded_frame.empty()) {
// 检查NALU起始码
bool has_start_code = false;
if ((encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 1) ||
(encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 0 && encoded_frame[3] == 1)) {
has_start_code = true;
}
if (!has_start_code) {
CMVR_LOG(ERROR) << "Invalid frame: no NALU start code!";
return false;
}
} else {
//CMVR_LOG(ERROR) << "Encoded frame is empty!";
return false;
}
return true;
}
void UVCCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) {
// 环形队列取数据的index由接口传入
auto frame = stream_frame_buffer_->pop(index);

74
protos/cmvr/config/camera_config/camera_config.proto
View File

@ -19,40 +19,55 @@ enum AlignMode {
ALIGN_MODE_DEPTH = 1;
}
message CameraCaptureConfig {
int32 width = 1;
int32 height = 2;
int32 fps = 3;
StreamMode stream_mode = 4;
}
message CameraRenderConfig {
int32 width = 1;
int32 height = 2;
int32 fps = 3;
StreamMode stream_mode = 4;
}
message CameraEncoderConfig {
int32 width = 1;
int32 height = 2;
int32 fps = 3;
string codec = 4;
bool enable_stream_timestamp = 5;
int32 buffer_size = 6;
}
message MujocoViewerPipConfig {
bool enable = 1;
int32 left = 2;
int32 bottom = 3;
int32 width = 4;
int32 height = 5;
}
// RealSense摄像头
message RealSenseCameraConfig {
reserved 12;
string id = 1;
string serialNumber = 2;
int32 width = 3;
int32 height = 4;
int32 fps = 5;
string codec = 6;
CameraMode camera_mode = 7;
StreamMode stream_mode = 8;
AlignMode align_mode = 9;
int32 buffer_size = 10;
bool sync = 11;
int32 encode_width = 13;
int32 encode_height = 14;
CameraMode camera_mode = 3;
CameraCaptureConfig capture = 4;
CameraEncoderConfig encoder = 5;
AlignMode align_mode = 6;
bool sync = 7;
}
// UVC摄像头
message UVCCameraConfig {
reserved 10;
string id = 1;
string usb = 2; // 或者叫 device_path
int32 width = 3;
int32 height = 4;
int32 fps = 5;
string codec = 6;
CameraMode camera_mode = 7;
StreamMode stream_mode = 8;
int32 buffer_size = 9;
int32 encode_width = 11;
int32 encode_height = 12;
CameraMode camera_mode = 3;
CameraCaptureConfig capture = 4;
CameraEncoderConfig encoder = 5;
}
// MechMind工业相机
@ -65,6 +80,16 @@ message MechMindCameraConfig {
string image2d_type = 4;
}
message MujocoCameraConfig {
string id = 1;
string world_id = 2;
string camera_name = 3;
CameraRenderConfig render = 4;
CameraEncoderConfig encoder = 5;
bool consume_new_frame_only = 6;
MujocoViewerPipConfig viewer_pip = 7;
}
message CameraDeviceConfig {
string id = 1;
reserved 2;
@ -73,6 +98,7 @@ message CameraDeviceConfig {
UVCCameraConfig uvc = 10;
RealSenseCameraConfig realsense = 11;
MechMindCameraConfig mechmind = 12;
MujocoCameraConfig mujoco = 13;
}
}