122 lines
3.9 KiB
C++
122 lines
3.9 KiB
C++
//
|
|
// Created by lgv on 2026/2/26.
|
|
//
|
|
|
|
#pragma once
|
|
|
|
#include <cstdint>
|
|
#include <condition_variable>
|
|
#include <chrono>
|
|
#include <functional>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <string>
|
|
#include <thread>
|
|
#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 {
|
|
|
|
class MujocoCamera final : public AbstractCamera {
|
|
public:
|
|
using FetchRgbdFn = std::function<bool(std::vector<unsigned char>& rgb,
|
|
std::vector<float>& depth,
|
|
int& width,
|
|
int& height,
|
|
uint64_t& frame_id)>;
|
|
|
|
explicit MujocoCamera(FetchRgbdFn fetch_rgbd_fn);
|
|
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);
|
|
void renderLoop_();
|
|
bool fetchCached_(cv::Mat& color,
|
|
cv::Mat& depth,
|
|
Rs2Intrinsics& intrinsics,
|
|
bool consume_new_frame_only);
|
|
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);
|
|
|
|
struct CachedFrame {
|
|
cv::Mat color;
|
|
cv::Mat depth;
|
|
Rs2Intrinsics intrinsics{};
|
|
uint64_t frame_id{0};
|
|
bool valid{false};
|
|
};
|
|
|
|
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};
|
|
mjData* render_data_{nullptr};
|
|
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_;
|
|
|
|
mutable std::mutex cache_mtx_;
|
|
std::condition_variable cache_cv_;
|
|
CachedFrame latest_frame_;
|
|
std::thread render_thread_;
|
|
bool render_thread_running_{false};
|
|
bool render_stop_requested_{false};
|
|
};
|
|
|
|
} // namespace cmvr::device
|