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

fix(camera): decouple MuJoCo rendering and camera reads

Browse Source
This commit is contained in:
lgv 2026-09-04 11:07:01 +08:00
parent 724da3d000
commit 708c585f28
5 changed files with 336 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -83,8 +83,8 @@ camera {
stream_mode: STREAM_MODE_RGBD
}
encoder {
width: 1280
height: 720
width: 480
height: 320
fps: 30
codec: "H264"
enable_stream_timestamp: true

2
cmvr-es/config/logger/logger.pb.txt
View File

@ -30,7 +30,7 @@ logger {
max_file_size_mb: 100
flush_interval_seconds: 1
format {
show_time: false
show_time: true
show_level: true
show_thread_id: false
show_source_location: true

2
cmvr-es/devices/arm/aubo_arm/CMakeLists.txt
View File

@ -1,5 +1,5 @@
add_library(aubo_arm SHARED
aubo_arm.cpp
src/aubo_arm.cpp
)
target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

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

@ -5,10 +5,13 @@
#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>
@ -57,6 +60,11 @@ private:
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,
@ -65,6 +73,14 @@ private:
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_;
@ -78,6 +94,7 @@ private:
mjrContext context_{};
bool scene_initialized_{false};
bool context_initialized_{false};
mjData* render_data_{nullptr};
int camera_id_{-1};
int width_{640};
int height_{480};
@ -92,6 +109,13 @@ private:
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

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

@ -106,10 +106,6 @@ bool MujocoCamera::init()
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);
@ -125,37 +121,132 @@ bool MujocoCamera::init()
bool MujocoCamera::start()
{
if (!state_.is_initialized) {
bool initialized = false;
{
std::lock_guard<std::mutex> lock(mtx_);
initialized = state_.is_initialized;
}
if (!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;
}
bool use_external_frames = false;
{
std::lock_guard<std::mutex> lock(mtx_);
state_.is_streaming = true;
state_.is_opened = true;
use_external_frames = static_cast<bool>(fetch_rgbd_fn_);
}
std::thread stale_thread;
{
std::lock_guard<std::mutex> lock(cache_mtx_);
if (render_thread_running_) {
return true;
}
latest_frame_ = CachedFrame{};
last_frame_id_ = 0;
has_last_frame_id_ = false;
render_stop_requested_ = false;
render_thread_running_ = true;
}
{
std::lock_guard<std::mutex> lock(mtx_);
stale_thread = std::move(render_thread_);
}
if (stale_thread.joinable()) {
stale_thread.join();
}
try {
std::lock_guard<std::mutex> lock(mtx_);
render_thread_ = std::thread(&MujocoCamera::renderLoop_, this);
} catch (const std::exception& e) {
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_running_ = false;
render_stop_requested_ = true;
}
cache_cv_.notify_all();
setError_("[MujocoCamera] failed to start render thread: " + std::string(e.what()));
return false;
}
// External PiP callbacks are not ready until the viewer enters its render
// loop, so let their polling thread warm up asynchronously.
if (use_external_frames) {
return true;
}
std::unique_lock<std::mutex> cache_lock(cache_mtx_);
const bool ready = cache_cv_.wait_for(
cache_lock,
std::chrono::seconds(5),
[this] { return latest_frame_.valid || !render_thread_running_ || render_stop_requested_; });
const bool has_frame = latest_frame_.valid;
cache_lock.unlock();
if (!ready || !has_frame) {
stop();
if (ready) {
setError_("[MujocoCamera] render thread stopped before producing a frame");
} else {
setError_("[MujocoCamera] timed out waiting for the first rendered frame");
}
return false;
}
return true;
}
bool MujocoCamera::stop()
{
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_stop_requested_ = true;
}
cache_cv_.notify_all();
std::thread thread_to_join;
{
std::lock_guard<std::mutex> lock(mtx_);
state_.is_streaming = false;
state_.is_opened = false;
destroyOffscreen_();
thread_to_join = std::move(render_thread_);
}
if (thread_to_join.joinable()) {
thread_to_join.join();
}
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_running_ = false;
latest_frame_ = CachedFrame{};
}
cache_cv_.notify_all();
return true;
}
void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn)
{
bool render_thread_active = false;
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_active = render_thread_running_;
}
if (render_thread_active) {
stop();
}
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);
@ -203,9 +294,10 @@ void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics&
bool MujocoCamera::startStreaming()
{
if (!state_.is_initialized && !init()) {
if (!start()) {
return false;
}
std::lock_guard<std::mutex> lock(mtx_);
streaming_ = true;
state_.is_streaming = true;
@ -273,14 +365,29 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
{
FetchRgbdFn fetch_rgbd_fn;
bool consume_new_frame_only = false;
bool render_thread_active = false;
{
std::lock_guard<std::mutex> lock(mtx_);
if (fetch_rgbd_fn_) {
fetch_rgbd_fn = fetch_rgbd_fn_;
consume_new_frame_only = consume_new_frame_only_;
}
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_active = render_thread_running_;
}
// Keep compatibility with callback-only cameras that have not been
// started. Once start() owns a polling thread, reads are cache-only.
if (fetch_rgbd_fn && !render_thread_active) {
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)) {
if (!fetch_rgbd_fn(rgb_raw, depth_raw, width, height, frame_id)) {
return false;
}
if (width <= 0 || height <= 0) {
@ -292,11 +399,14 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
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_) {
{
std::lock_guard<std::mutex> lock(cache_mtx_);
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);
@ -310,7 +420,31 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
return true;
}
return renderOffscreen_(color, depth, intrinsics);
return fetchCached_(color, depth, intrinsics, consume_new_frame_only);
}
bool MujocoCamera::fetchCached_(cv::Mat& color,
cv::Mat& depth,
Rs2Intrinsics& intrinsics,
const bool consume_new_frame_only)
{
std::lock_guard<std::mutex> lock(cache_mtx_);
if (!latest_frame_.valid || latest_frame_.color.empty()) {
return false;
}
if (consume_new_frame_only && has_last_frame_id_ &&
latest_frame_.frame_id == last_frame_id_) {
return false;
}
// cv::Mat copies are reference-counted; keep the cache immutable while the
// consumer reads the published frame and avoid a full image copy per poll.
color = latest_frame_.color;
depth = latest_frame_.depth;
intrinsics = latest_frame_.intrinsics;
last_frame_id_ = latest_frame_.frame_id;
has_last_frame_id_ = true;
return !color.empty();
}
bool MujocoCamera::initOffscreen_()
@ -345,12 +479,27 @@ bool MujocoCamera::initOffscreen_()
return false;
}
mjModel* model = nullptr;
{
std::lock_guard<std::mutex> world_lock(world->mutex());
const mjModel* model = world->model();
model = world->model();
if (model == nullptr) {
setError_("[MujocoCamera] world model is null");
return false;
}
// MuJoCo clips rendering to the model's offscreen buffer. Make sure
// the buffer is large enough before creating this camera's context;
// otherwise a larger requested frame is only partially populated.
model->vis.global.offwidth = std::max(model->vis.global.offwidth, width_);
model->vis.global.offheight = std::max(model->vis.global.offheight, height_);
}
render_data_ = mj_makeData(model);
if (render_data_ == nullptr) {
setError_("[MujocoCamera] failed to allocate render data");
return false;
}
mjv_makeScene(model, &scene_, kMaxGeom);
scene_initialized_ = true;
mjr_makeContext(model, &context_, mjFONTSCALE_150);
@ -360,9 +509,116 @@ bool MujocoCamera::initOffscreen_()
setError_("[MujocoCamera] MuJoCo offscreen buffer is not available");
return false;
}
if (context_.offWidth < width_ || context_.offHeight < height_) {
setError_("[MujocoCamera] offscreen buffer is smaller than requested frame: " +
std::to_string(context_.offWidth) + "x" +
std::to_string(context_.offHeight) + " < " +
std::to_string(width_) + "x" + std::to_string(height_));
return false;
}
return true;
}
void MujocoCamera::renderLoop_()
{
FetchRgbdFn external_fetch;
{
std::lock_guard<std::mutex> lock(mtx_);
external_fetch = fetch_rgbd_fn_;
}
const bool use_external_frames = static_cast<bool>(external_fetch);
if (!use_external_frames && !initOffscreen_()) {
destroyOffscreen_();
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_running_ = false;
}
cache_cv_.notify_all();
return;
}
const int fps = positiveOrDefault(config_.render().fps(), 30);
const auto period = std::chrono::duration<double>(1.0 / static_cast<double>(fps));
const auto period_ticks = std::chrono::duration_cast<std::chrono::steady_clock::duration>(period);
auto next_tick = std::chrono::steady_clock::now();
while (true) {
{
std::lock_guard<std::mutex> lock(cache_mtx_);
if (render_stop_requested_) {
break;
}
}
cv::Mat color;
cv::Mat depth;
Rs2Intrinsics intrinsics{};
uint64_t external_frame_id = 0;
bool got_frame = false;
if (use_external_frames) {
std::vector<unsigned char> rgb_raw;
std::vector<float> depth_raw;
int width = 0;
int height = 0;
if (external_fetch(rgb_raw, depth_raw, width, height, external_frame_id) &&
width > 0 && height > 0 &&
static_cast<int>(rgb_raw.size()) == width * height * 3 &&
(depth_raw.empty() || static_cast<int>(depth_raw.size()) == width * height)) {
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();
}
fillIntrinsics(width, height, intrinsics);
got_frame = !color.empty();
}
} else {
got_frame = renderOffscreen_(color, depth, intrinsics);
}
if (got_frame) {
{
std::lock_guard<std::mutex> lock(cache_mtx_);
latest_frame_.color = std::move(color);
latest_frame_.depth = std::move(depth);
latest_frame_.intrinsics = intrinsics;
latest_frame_.frame_id = use_external_frames && external_frame_id != 0
? external_frame_id
: latest_frame_.frame_id + 1;
latest_frame_.valid = true;
}
{
std::lock_guard<std::mutex> lock(mtx_);
clear_error_();
}
cache_cv_.notify_all();
}
next_tick += period_ticks;
std::unique_lock<std::mutex> lock(cache_mtx_);
if (cache_cv_.wait_until(lock, next_tick, [this] { return render_stop_requested_; })) {
break;
}
const auto now = std::chrono::steady_clock::now();
if (next_tick < now) {
next_tick = now + period_ticks;
}
}
if (!use_external_frames) {
destroyOffscreen_();
}
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_running_ = false;
}
cache_cv_.notify_all();
}
void MujocoCamera::destroyOffscreen_()
{
if (window_ != nullptr) {
@ -376,6 +632,10 @@ void MujocoCamera::destroyOffscreen_()
mjv_freeScene(&scene_);
scene_initialized_ = false;
}
if (render_data_ != nullptr) {
mj_deleteData(render_data_);
render_data_ = nullptr;
}
if (window_ != nullptr) {
glfwDestroyWindow(window_);
window_ = nullptr;
@ -388,7 +648,8 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
setError_("[MujocoCamera] camera is not initialized: " + id_);
return false;
}
if (!initOffscreen_()) {
if (window_ == nullptr || !context_initialized_ || !scene_initialized_) {
setError_("[MujocoCamera] offscreen renderer is not initialized: " + id_);
return false;
}
@ -402,15 +663,27 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
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_);
mjModel* model = nullptr;
{
std::lock_guard<std::mutex> world_lock(world->mutex());
mjModel* model = world->model();
mjData* data = world->data();
if (model == nullptr || data == nullptr) {
std::unique_lock<std::mutex> world_lock(world->mutex(), std::try_to_lock);
if (!world_lock.owns_lock()) {
// Never make the simulation wait for a camera frame. The next
// scheduled capture will use a newer state if this one is busy.
return false;
}
model = world->model();
const mjData* data = world->data();
if (model == nullptr || data == nullptr || render_data_ == nullptr) {
setError_("[MujocoCamera] world model/data is null");
return false;
}
// Keep the world lock limited to the state copy. GPU rendering runs on
// the camera thread using its private data snapshot.
mjv_copyData(render_data_, model, data);
}
{
camera_.type = mjCAMERA_FIXED;
camera_.fixedcamid = camera_id_;
camera_.trackbodyid = -1;
@ -421,7 +694,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
viewport.width = width_;
viewport.height = height_;
mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
mjv_updateScene(model, render_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_);
@ -429,13 +702,12 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
}
cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data());
color = rgb_mat.clone();
// mjr_readPixels returns RGB, while the rest of the camera API exposes
// OpenCV-compatible BGR frames (as UVC and RealSense do).
cv::cvtColor(rgb_mat, color, cv::COLOR_RGB2BGR);
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;
}