diff --git a/cmvr-es/config/devices/camera/camera.pb.txt b/cmvr-es/config/devices/camera/camera.pb.txt index 05469ae0..f21959ce 100644 --- a/cmvr-es/config/devices/camera/camera.pb.txt +++ b/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 - buffer_size: 30 + capture { + 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 { - id: "cam5" - realsense { - serialNumber: "243122070435" + 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 - sync: false } + align_mode: ALIGN_MODE_COLOR + sync: false } + } } diff --git a/cmvr-es/devices/camera/CMakeLists.txt b/cmvr-es/devices/camera/CMakeLists.txt index 8013eb6a..e659f9ab 100644 --- a/cmvr-es/devices/camera/CMakeLists.txt +++ b/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 ) diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index 9a9bd728..a05281dc 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/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; diff --git a/cmvr-es/devices/camera/camera_factory.h b/cmvr-es/devices/camera/camera_factory.h index 600c0bb5..60a8f549 100644 --- a/cmvr-es/devices/camera/camera_factory.h +++ b/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( + backendWithId_(cfg.id(), cfg.mujoco())); + case config::CameraDeviceConfig::BACKEND_NOT_SET: default: { diff --git a/cmvr-es/devices/camera/common/CMakeLists.txt b/cmvr-es/devices/camera/common/CMakeLists.txt new file mode 100644 index 00000000..90ab01ef --- /dev/null +++ b/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) diff --git a/cmvr-es/devices/camera/common/include/camera_stream_encoder.h b/cmvr-es/devices/camera/common/include/camera_stream_encoder.h new file mode 100644 index 00000000..5001d23d --- /dev/null +++ b/cmvr-es/devices/camera/common/include/camera_stream_encoder.h @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include + +#include + +#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& encoder, + const std::string& codec_name, + int width, + int height, + int fps); + + static bool encode(std::shared_ptr& encoder, + const cv::Mat& frame, + std::vector& encoded_frame, + bool& is_key, + const CameraStreamEncodeOptions& options = {}); +}; + +} // namespace cmvr::device diff --git a/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp b/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp new file mode 100644 index 00000000..c08f9f37 --- /dev/null +++ b/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp @@ -0,0 +1,297 @@ +#include "devices/camera/common/include/camera_stream_encoder.h" + +#include +#include +#include +#include + +#include +#include + +#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(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(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& encoder, + const std::string& codec_name, + const int width, + const int height, + const int fps) +{ + encoder = std::make_shared(); + 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& encoder, + const cv::Mat& frame, + std::vector& 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(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 diff --git a/cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt b/cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt index a78953f4..232ee07b 100644 --- a/cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt +++ b/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) diff --git a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h index 220e9936..78419aa1 100644 --- a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h +++ b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h @@ -6,10 +6,19 @@ #include #include +#include #include +#include #include +#include + +#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& rgb, + std::vector& depth, + int width, + int height); + static void linearizeDepth_(const mjModel* model, std::vector& depth); private: FetchRgbdFn fetch_rgbd_fn_; mutable std::mutex mtx_; + config::MujocoCameraConfig config_; + std::weak_ptr 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 rgb_encoder_; }; } // namespace cmvr::device diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp index 05b44f91..3b02335b 100644 --- a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp +++ b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp @@ -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 #include +#include +#include +#include + +#include +#include + +#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 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 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 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 lock(mtx_); + state_.is_streaming = false; + state_.is_opened = false; + destroyOffscreen_(); + return true; +} + +void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn) +{ + std::lock_guard 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 lock(mtx_); fovy_deg_ = fovy_deg; } -void MujocoCamera::setConsumeNewFrameOnly(bool enable) { +void MujocoCamera::setConsumeNewFrameOnly(const bool enable) +{ std::lock_guard 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) { - std::lock_guard lock(mtx_); - if (!fetch_rgbd_fn_) return false; - - std::vector rgb_raw; - std::vector 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)) { +bool MujocoCamera::startStreaming() +{ + if (!state_.is_initialized && !init()) { 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_) { - 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); + std::lock_guard lock(mtx_); + streaming_ = true; + state_.is_streaming = true; return true; } -void MujocoCamera::fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsics) const { +void MujocoCamera::stopStreaming() +{ + std::lock_guard 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(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 lock(mtx_); + if (fetch_rgbd_fn_) { + std::vector rgb_raw; + std::vector 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(rgb_raw.size()) != width * height * 3) { + return false; + } + if (!depth_raw.empty() && static_cast(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 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 rgb(static_cast(width_) * height_ * 3); + std::vector depth_raw(static_cast(width_) * height_); + + { + std::lock_guard 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& rgb, + std::vector& 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& depth) +{ + if (model == nullptr) { + return; + } + const double znear = static_cast(model->vis.map.znear) * + static_cast(model->stat.extent); + const double zfar = static_cast(model->vis.map.zfar) * + static_cast(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::infinity(); + continue; + } + const double z_ndc = 2.0 * static_cast(item) - 1.0; + const double denom = f_plus_n - z_ndc * f_minus_n; + item = denom <= 1e-12 + ? std::numeric_limits::infinity() + : static_cast(two_nf / denom); + } +} + } // namespace cmvr::device diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp new file mode 100644 index 00000000..67a6c083 --- /dev/null +++ b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp @@ -0,0 +1,56 @@ +#include + +#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(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 diff --git a/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt b/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt index 0fd4da5f..ba376072 100644 --- a/cmvr-es/devices/camera/realsense_camera/CMakeLists.txt +++ b/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 diff --git a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h b/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h index 50a8eb7b..fcc50dfe 100644 --- a/cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h +++ b/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 #include @@ -30,11 +32,6 @@ namespace cmvr::device{ void pauseRecording() override; void resumeRecording() override; - // 初始化单个编码器的通用函数（复用逻辑，避免重复代码） - static bool initSingleEncoder(std::shared_ptr& encoder, - const std::string& codec_name, - int width, int height, int fps); - static bool encodeFrameWithEncoder(std::shared_ptr& encoder,const cv::Mat& frame,std::vector& 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_; diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index f5afa71d..6aad3be4 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/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(); - 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& encoder, - const std::string& codec_name, - int width, int height, int fps) { - // 1. 创建编码器实例（不变） - encoder = std::make_shared(); - 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**)¶ms) >= 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(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( - 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& encoder, const cv::Mat& frame,std::vector& 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(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); diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp index d7e939c6..d66d9495 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp +++ b/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; } diff --git a/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt b/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt index 77596fdf..0b36983e 100644 --- a/cmvr-es/devices/camera/uvc_camera/CMakeLists.txt +++ b/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) diff --git a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h b/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h index 9c035efa..651e0dbd 100644 --- a/cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h +++ b/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" - -//使用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& encoder, - const std::string& codec_name, - int width, int height, int fps); - static bool encodeFrameWithEncoder(std::shared_ptr& encoder,const cv::Mat& frame,std::vector& 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 current_image_; diff --git a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp index dd6e80f3..bccc0204 100644 --- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp +++ b/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(); - 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& encoder, - const std::string& codec_name, - int width, int height, int fps) { - // 1. 创建编码器实例（不变） - encoder = std::make_shared(); - 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& encoder, const cv::Mat& frame,std::vector& 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(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); diff --git a/protos/cmvr/config/camera_config/camera_config.proto b/protos/cmvr/config/camera_config/camera_config.proto index 153a7ecc..ce225f41 100644 --- a/protos/cmvr/config/camera_config/camera_config.proto +++ b/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; - 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; } }