fix(camera): decouple MuJoCo rendering and camera reads

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

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

cmvr-es/config/logger/logger.pb.txt

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

cmvr-es/devices/arm/aubo_arm/CMakeLists.txt

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

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

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

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

@@ -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;
@@ -272,15 +364,30 @@ 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());
+        std::unique_lock<std::mutex> world_lock(world->mutex(), std::try_to_lock);
-        mjModel* model = world->model();
+        if (!world_lock.owns_lock()) {
-        mjData* data = world->data();
+            // Never make the simulation wait for a camera frame. The next
-        if (model == nullptr || data == nullptr) {
+            // 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;
 }