refactor(camera): unify camera devices

cmvr-es/config/devices/camera/camera.pb.txt

@@ -3,14 +3,22 @@ camera {
     id: "cam1"
     realsense {
       serialNumber: "243122074587"
-      width: 640
-      height: 480
-      fps: 30
-      codec: "H265"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGBD
+      capture {
+        width: 640
+        height: 480
+        fps: 30
+        stream_mode: STREAM_MODE_RGBD
+      }
+      encoder {
+        width: 640
+        height: 480
+        fps: 30
+        codec: "H265"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
       align_mode: ALIGN_MODE_COLOR
-      buffer_size: 30
       sync: false
     }
   }
@@ -19,16 +27,22 @@ camera {
     id: "right_hand_cam"
     realsense {
       serialNumber: "243122072252"
-      width: 1280
-      height: 720
-      encode_width: 640
-      encode_height: 360
-      fps: 30
-      codec: "H264"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGB
+      capture {
+        width: 1280
+        height: 720
+        fps: 30
+        stream_mode: STREAM_MODE_RGB
+      }
+      encoder {
+        width: 640
+        height: 360
+        fps: 30
+        codec: "H264"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
       align_mode: ALIGN_MODE_COLOR
-      buffer_size: 30
       sync: false
     }
   }
@@ -37,48 +51,99 @@ camera {
     id: "cam3"
     realsense {
       serialNumber: "243122075614"
-      width: 640
-      height: 480
-      fps: 30
-      codec: "H265"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGBD
+      capture {
+        width: 640
+        height: 480
+        fps: 30
+        stream_mode: STREAM_MODE_RGBD
+      }
+      encoder {
+        width: 640
+        height: 480
+        fps: 30
+        codec: "H265"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
       align_mode: ALIGN_MODE_COLOR
-      buffer_size: 30
       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"
-      width: 640
-      height: 480
-      fps: 30
-      codec: "H265"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGB
+      capture {
-      buffer_size: 30
+        width: 640
+        height: 480
+        fps: 30
+        stream_mode: STREAM_MODE_RGB
+      }
+      encoder {
+        width: 640
+        height: 480
+        fps: 30
+        codec: "H265"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
     }
   }
 
-
+  cameras {
-    cameras {
+    id: "cam5"
-      id: "cam5"
+    realsense {
-      realsense {
+      serialNumber: "243122070435"
-        serialNumber: "243122070435"
+      camera_mode: CAMERA_MODE_VIDEO
+      capture {
         width: 1280
         height: 720
-        encode_width: 640
+        fps: 30
-        encode_height: 360
+        stream_mode: STREAM_MODE_RGB
+      }
+      encoder {
+        width: 640
+        height: 360
         fps: 30
         codec: "H264"
-        camera_mode: CAMERA_MODE_VIDEO
+        enable_stream_timestamp: true
-        stream_mode: STREAM_MODE_RGB
-        align_mode: ALIGN_MODE_COLOR
         buffer_size: 30
-        sync: false
       }
+      align_mode: ALIGN_MODE_COLOR
+      sync: false
     }
+  }
 }

cmvr-es/devices/camera/CMakeLists.txt

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

cmvr-es/devices/camera/abstract_camera.h

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

cmvr-es/devices/camera/camera_factory.h

@@ -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:
             {

cmvr-es/devices/camera/common/CMakeLists.txt

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

cmvr-es/devices/camera/common/include/camera_stream_encoder.h

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

cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp

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

cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt

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

cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h

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

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp

@@ -1,87 +1,460 @@
-//
+#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
-// Created by lgv on 2026/2/26.
-//
-
-#include "../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()
-    std::lock_guard<std::mutex> lock(mtx_);
+{
-    if (!fetch_rgbd_fn_) return false;
+    if (!state_.is_initialized && !init()) {
-
-    std::vector<unsigned char> rgb_raw;
-    std::vector<float> depth_raw;
-    int width = 0;
-    int height = 0;
-    uint64_t frame_id = 0;
-    if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
         return false;
     }
-
+    std::lock_guard<std::mutex> lock(mtx_);
-    if (width <= 0 || height <= 0) return false;
+    streaming_ = true;
-    if ((int)rgb_raw.size() != width * height * 3) return false;
+    state_.is_streaming = true;
-    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_) {
-            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();
-
-    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 {
+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;
+        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 (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());
+        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;
+    }
+
+    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

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp

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

cmvr-es/devices/camera/realsense_camera/CMakeLists.txt

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

cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h

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

cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp

@@ -76,12 +76,15 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
         state_.error_message = "empty device serial number";
         return;
     }
-    fps_ = camera_.fps();
+    const auto& capture = camera_.capture();
-    width_ = camera_.width();
+    const auto& encoder = camera_.encoder();
-    height_ = camera_.height();
+    fps_ = capture.fps();
-    encode_width_ = camera_.encode_width() > 0 ? camera_.encode_width() : width_;
+    width_ = capture.width();
-    encode_height_ = camera_.encode_height() > 0 ? camera_.encode_height() : height_;
+    height_ = capture.height();
-    buffer_size_ = camera_.buffer_size();
+    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);

cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp

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

cmvr-es/devices/camera/uvc_camera/CMakeLists.txt

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

cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h

@@ -7,17 +7,9 @@
 
 #include "common/base/ring_buffer.h"
 #include "camera/abstract_camera.h"
-
+#include "devices/camera/common/include/camera_stream_encoder.h"
-//使用ffmpeg来编码保存视频文件
-#define USE_FFMPEG_ENCODER 1
-
-#if USE_FFMPEG_ENCODER
-#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
-#endif
 
 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_;

cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp

@@ -20,12 +20,15 @@ UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
         state_.error_message = "empty device serial number";
         return;
     }
-    fps_ = camera_.fps();
+    const auto& capture = camera_.capture();
-    width_ = camera_.width();
+    const auto& encoder = camera_.encoder();
-    height_ = camera_.height();
+    fps_ = capture.fps();
-    encode_width_ = camera_.encode_width() > 0 ? camera_.encode_width() : width_;
+    width_ = capture.width();
-    encode_height_ = camera_.encode_height() > 0 ? camera_.encode_height() : height_;
+    height_ = capture.height();
-    buffer_size_ = camera_.buffer_size();
+    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);

protos/cmvr/config/camera_config/camera_config.proto

@@ -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;
+    CameraMode camera_mode = 3;
-    int32 height = 4;
+    CameraCaptureConfig capture = 4;
-    int32 fps = 5;
+    CameraEncoderConfig encoder = 5;
-    string codec = 6;
+    AlignMode align_mode = 6;
-    CameraMode camera_mode = 7;
+    bool sync = 7;
-    StreamMode stream_mode = 8;
-    AlignMode align_mode = 9;
-    int32 buffer_size = 10;
-    bool sync = 11;
-    int32 encode_width = 13;
-    int32 encode_height = 14;
 }
 
 // UVC摄像头
 message UVCCameraConfig {
-    reserved 10;
-
     string id = 1;
     string usb = 2;  // 或者叫 device_path
-    int32 width = 3;
+    CameraMode camera_mode = 3;
-    int32 height = 4;
+    CameraCaptureConfig capture = 4;
-    int32 fps = 5;
+    CameraEncoderConfig encoder = 5;
-    string codec = 6;
-    CameraMode camera_mode = 7;
-    StreamMode stream_mode = 8;
-    int32 buffer_size = 9;
-    int32 encode_width = 11;
-    int32 encode_height = 12;
 }
 
 // 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;
     }
 }