From 791ecdf66f7babc44f6af7059c02362e181aa896 Mon Sep 17 00:00:00 2001 From: neverorfrog <97flavio.maiorana@gmail.com> Date: Tue, 2 Dec 2025 23:42:41 +0100 Subject: [PATCH 1/4] camera fix --- CMakeLists.txt | 4 +- include/AppWindow.h | 3 +- include/CircusApplication.h | 2 +- include/MujocoContext.h | 46 +++++++- include/RobotManager.h | 19 ++- include/SimulationThread.h | 14 ++- include/SimulationViewport.h | 2 +- include/robots/BoosterK1.h | 4 +- include/robots/BoosterT1.h | 4 +- include/robots/Robot.h | 2 +- include/sensors/Camera.h | 216 +++++++++++++++++++++++++++++++++-- include/sensors/Sensor.h | 23 +++- include/utils/ThreadPool.h | 119 +++++++++++++++++++ pixi.toml | 2 +- src/AppWindow.cpp | 11 +- src/MujocoContext.cpp | 53 ++++++++- src/SceneParser.cpp | 2 +- src/SimulationThread.cpp | 30 +++-- src/SimulationViewport.cpp | 2 +- 19 files changed, 508 insertions(+), 50 deletions(-) create mode 100644 include/utils/ThreadPool.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d0bd4d..e0a35d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ option(BUILD_PYTHON_BINDINGS "Build python bindings" ON) # ============================= Dependencies ================================================== find_package(Eigen3 REQUIRED) find_package(OpenGL REQUIRED) +find_library(EGL_LIBRARY NAMES EGL REQUIRED) find_package(Qt6 REQUIRED COMPONENTS Core Widgets OpenGLWidgets) # Optional deps (linked only if found) @@ -167,6 +168,7 @@ set(_EXTRA_LIBS Qt6::Widgets Qt6::OpenGLWidgets OpenGL::GL + ${EGL_LIBRARY} ${_PUGI_TGT} CURL::libcurl nlohmann_json::nlohmann_json @@ -301,4 +303,4 @@ if(BUILD_PYTHON_BINDINGS AND BINDINGS_SRC) ) endif() -# =============================================================================================== +# =============================================================================================== \ No newline at end of file diff --git a/include/AppWindow.h b/include/AppWindow.h index 590b804..ee8aff6 100644 --- a/include/AppWindow.h +++ b/include/AppWindow.h @@ -8,6 +8,7 @@ #include "MujocoContext.h" #include "SimulationThread.h" #include "SimulationViewport.h" + namespace spqr { class AppWindow : public QMainWindow { @@ -29,4 +30,4 @@ class AppWindow : public QMainWindow { std::unique_ptr sim; }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/CircusApplication.h b/include/CircusApplication.h index ee3bde4..6fb8994 100644 --- a/include/CircusApplication.h +++ b/include/CircusApplication.h @@ -9,4 +9,4 @@ class CircusApplication : public QApplication { ~CircusApplication(); }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/MujocoContext.h b/include/MujocoContext.h index 798521d..56a4713 100644 --- a/include/MujocoContext.h +++ b/include/MujocoContext.h @@ -1,28 +1,68 @@ #pragma once +#include #include +#include #include +#include #include namespace spqr { +// Shared EGL display for all cameras (owned by MujocoContext) +struct SharedEGLDisplay { + EGLDisplay eglDisplay = EGL_NO_DISPLAY; + + ~SharedEGLDisplay(); + void cleanup(); +}; + +// Per-camera rendering context (owned by each Camera instance) +struct CameraContext { + EGLDisplay eglDisplay = EGL_NO_DISPLAY; // Borrowed from SharedEGLDisplay + EGLContext eglContext = EGL_NO_CONTEXT; + EGLSurface eglSurface = EGL_NO_SURFACE; + mjrContext mjContext{}; + mjvOption opt{}; +}; + struct MujocoContext { mjModel* model = nullptr; - mjData* data = nullptr; + mjData* data = nullptr; // Live simulation data (written by simulation thread) + mjData* dataSnapshot = nullptr; // Read-only snapshot for sensors (read by worker threads) + + // Viewport rendering (used by Qt) mjrContext ctx{}; mjvCamera cam{}; mjvOption opt{}; mjvScene scene{}; + // Shared EGL display for camera rendering + SharedEGLDisplay sharedEGL; + + // Thread synchronization for snapshot updates + mutable std::mutex snapshotMutex; + MujocoContext(const std::string& xmlString); ~MujocoContext(); + // Update the snapshot with current simulation state + // Called by simulation thread after mj_step + void updateSnapshot(); + + // Get thread-safe read access to snapshot + mjData* getSnapshot() const { + return dataSnapshot; + } + // Copying could potentially lead to freeing the model or data twice. // Deleting the copy constructors prevents this. MujocoContext(const MujocoContext&) = delete; MujocoContext& operator=(const MujocoContext&) = delete; - MujocoContext& operator=(MujocoContext&& other) noexcept; + + private: + bool initializeSharedEGL(); }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/RobotManager.h b/include/RobotManager.h index 0400fc8..48b84d6 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -22,6 +22,7 @@ #include "robots/BoosterK1.h" #include "robots/BoosterT1.h" #include "robots/Robot.h" +#include "utils/ThreadPool.h" #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { @@ -53,9 +54,18 @@ class RobotManager { } void update() { - std::lock_guard lock(mutex_); - for (std::shared_ptr r : robots_) { - r->update(); + std::vector> futures; + + { + std::lock_guard lock(mutex_); + for (std::shared_ptr r : robots_) { + futures.push_back(threadPool_.enqueue([r]() { r->update(); })); + } + } + + // Wait for all robot updates to complete + for (auto& future : futures) { + future.wait(); } } @@ -231,6 +241,7 @@ class RobotManager { mutable std::mutex mutex_; std::vector> robots_; + ThreadPool threadPool_{std::thread::hardware_concurrency()}; using RobotCreator = std::function(const std::string&, const std::string&, uint8_t, const Eigen::Vector3d&, const Eigen::Vector3d&, @@ -246,4 +257,4 @@ class RobotManager { }}}; }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/SimulationThread.h b/include/SimulationThread.h index f90fb0c..78a1667 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -3,20 +3,26 @@ #include #include +#include +#include + +#include "MujocoContext.h" +#include "RobotManager.h" namespace spqr { class SimulationThread : public QThread { Q_OBJECT public: - SimulationThread(const mjModel* model, mjData* data); + SimulationThread(MujocoContext* mujContext); + ~SimulationThread() override; + void run() override; void stop(); private: - const mjModel* model_; - mjData* data_; + MujocoContext* mujContext_; std::atomic running_; }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/SimulationViewport.h b/include/SimulationViewport.h index 24934e2..0dea25f 100644 --- a/include/SimulationViewport.h +++ b/include/SimulationViewport.h @@ -57,4 +57,4 @@ class SimulationViewport : public QOpenGLWindow { int logicalWidth = initialWindowWidth, logicalHeight = initialWindowHeight; }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index e8e0d22..51d8878 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -98,10 +98,8 @@ class BoosterK1 : public Robot { pose->update(); imu->update(); joints->update(); - /* cameras[0]->update(); cameras[1]->update(); - */ } ~BoosterK1() = default; @@ -110,4 +108,4 @@ class BoosterK1 : public Robot { std::map joint_map; }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index dc6e706..0f31916 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -111,10 +111,8 @@ class BoosterT1 : public Robot { pose->update(); imu->update(); joints->update(); - /* cameras[0]->update(); cameras[1]->update(); - */ } ~BoosterT1() = default; @@ -123,4 +121,4 @@ class BoosterT1 : public Robot { std::map joint_map; }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/robots/Robot.h b/include/robots/Robot.h index f319591..e3fc715 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -49,4 +49,4 @@ class Robot { msgpack::zone buffer_zone_; }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/sensors/Camera.h b/include/sensors/Camera.h index 4e65b84..afc13f3 100644 --- a/include/sensors/Camera.h +++ b/include/sensors/Camera.h @@ -1,7 +1,14 @@ +// Include/Camera.h #pragma once + #include +#include +#include +#include #include +#include +#include #include #include "MujocoContext.h" @@ -11,35 +18,224 @@ namespace spqr { class Camera : public Sensor { public: - Camera(MujocoContext* mujContext, const char* cameraName) : mujContext(mujContext) { + Camera(MujocoContext* mujContext, const char* cameraName) + : Sensor(30.0f), mujContext(mujContext), mujModel(mujContext->model) { + // Find the camera in the MuJoCo model cam.type = mjCAMERA_FIXED; - cam.fixedcamid = mj_name2id(mujContext->model, mjOBJ_CAMERA, cameraName); + cam.fixedcamid = mj_name2id(mujModel, mjOBJ_CAMERA, cameraName); - if (cam.fixedcamid < 0) + if (cam.fixedcamid < 0) { throw std::runtime_error(std::string("Camera not found: ") + cameraName); + } - w = mujContext->model->cam_resolution[2 * cam.fixedcamid + 0]; - h = mujContext->model->cam_resolution[2 * cam.fixedcamid + 1]; + // Get camera resolution from model + w = mujModel->cam_resolution[cam.fixedcamid * 2]; + h = mujModel->cam_resolution[cam.fixedcamid * 2 + 1]; + // Allocate image buffer (RGB, 3 bytes per pixel) image.resize(w * h * 3); + // Create a scene for this camera's viewpoint + mjv_defaultScene(&scene); + mjv_makeScene(mujModel, &scene, 1000); + + // EGL context will be initialized lazily on first doUpdate() call + // because it must be created on the thread that will use it + } + + ~Camera() { + cleanupCameraContext(); + mjv_freeScene(&scene); } + // Called when sensor needs to update (respects frequency limiting) void doUpdate() override { + // Lazy-initialize EGL context on first call (must be on worker thread) + if (!contextInitialized) { + if (!initializeCameraContext()) { + std::cerr << "Failed to initialize camera context on worker thread" << std::endl; + return; + } + contextInitialized = true; + } + + // Make this camera's EGL context current + if (!eglMakeCurrent(cameraContext.eglDisplay, cameraContext.eglSurface, cameraContext.eglSurface, + cameraContext.eglContext)) { + std::cerr << "Failed to make camera EGL context current" << std::endl; + return; + } + + // 1. Define the viewport (rendering area) mjrRect viewport = {0, 0, w, h}; - mjr_render(viewport, &mujContext->scene, &mujContext->ctx); - mjr_readPixels(image.data(), nullptr, viewport, &mujContext->ctx); + + // 2. Update the scene from this camera's viewpoint using snapshot data + mjData* snapshot = mujContext->getSnapshot(); + mjv_updateScene(mujModel, snapshot, &cameraContext.opt, nullptr, &cam, mjCAT_ALL, &scene); + + // 3. Render the scene to the framebuffer + mjr_render(viewport, &scene, &cameraContext.mjContext); + + // 4. Read pixels from GPU to CPU + std::vector temp(w * h * 3); + mjr_readPixels(temp.data(), nullptr, viewport, &cameraContext.mjContext); + + // 5. Flip image vertically (OpenGL origin is bottom-left, we want top-left) + flipImageVertically(temp.data(), w, h, 3); + + // 6. Copy to the image buffer (with lock, for thread safety) + { + std::lock_guard lock(imageMutex); + image = std::move(temp); + } + + // Unbind context to allow other cameras to render + eglMakeCurrent(cameraContext.eglDisplay, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT); } + // Called when sending message to Docker container msgpack::object doSerialize(msgpack::zone& z) override { + std::lock_guard lock(imageMutex); std::vector img_copy(image.begin(), image.end()); return msgpack::object(img_copy, z); } + // Called from GUI thread to display camera feed + void copyImageTo(std::vector& buffer) const { + std::lock_guard lock(imageMutex); + buffer = image; + } + + int getWidth() const { + return w; + } + int getHeight() const { + return h; + } + private: - int w, h; + void flipImageVertically(uint8_t* data, int width, int height, int channels) { + int row_size = width * channels; + std::vector temp_row(row_size); + + for (int y = 0; y < height / 2; ++y) { + uint8_t* top = data + y * row_size; + uint8_t* bottom = data + (height - 1 - y) * row_size; + + std::memcpy(temp_row.data(), top, row_size); + std::memcpy(top, bottom, row_size); + std::memcpy(bottom, temp_row.data(), row_size); + } + } + + bool initializeCameraContext() { + // Reuse the EGL display from MujocoContext instead of creating a new one + cameraContext.eglDisplay = mujContext->sharedEGL.eglDisplay; + if (cameraContext.eglDisplay == EGL_NO_DISPLAY) { + std::cerr << "Failed to get EGL display from MujocoContext" << std::endl; + return false; + } + + // Skip eglInitialize - already done by MujocoContext + + const EGLint configAttribs[] = {EGL_SURFACE_TYPE, + EGL_PBUFFER_BIT, + EGL_RENDERABLE_TYPE, + EGL_OPENGL_BIT, + EGL_RED_SIZE, + 8, + EGL_GREEN_SIZE, + 8, + EGL_BLUE_SIZE, + 8, + EGL_DEPTH_SIZE, + 24, + EGL_NONE}; + + EGLConfig eglConfig; + EGLint numConfigs; + if (!eglChooseConfig(cameraContext.eglDisplay, configAttribs, &eglConfig, 1, &numConfigs)) { + std::cerr << "Failed to choose EGL config for camera" << std::endl; + return false; + } + + // eglBindAPI is per-thread, so each camera thread must call it + if (!eglBindAPI(EGL_OPENGL_API)) { + std::cerr << "Failed to bind OpenGL API for camera" << std::endl; + return false; + } + + const EGLint contextAttribs[] + = {EGL_CONTEXT_MAJOR_VERSION, 2, EGL_CONTEXT_MINOR_VERSION, 0, EGL_NONE}; + + cameraContext.eglContext + = eglCreateContext(cameraContext.eglDisplay, eglConfig, EGL_NO_CONTEXT, contextAttribs); + if (cameraContext.eglContext == EGL_NO_CONTEXT) { + std::cerr << "Failed to create EGL context for camera" << std::endl; + return false; + } + + const EGLint pbufferAttribs[] = {EGL_WIDTH, w, EGL_HEIGHT, h, EGL_NONE}; + + cameraContext.eglSurface + = eglCreatePbufferSurface(cameraContext.eglDisplay, eglConfig, pbufferAttribs); + if (cameraContext.eglSurface == EGL_NO_SURFACE) { + std::cerr << "Failed to create EGL pbuffer surface for camera" << std::endl; + return false; + } + + // Make current temporarily to initialize mjrContext + if (!eglMakeCurrent(cameraContext.eglDisplay, cameraContext.eglSurface, cameraContext.eglSurface, + cameraContext.eglContext)) { + std::cerr << "Failed to make EGL context current for camera initialization" << std::endl; + return false; + } + + mjr_defaultContext(&cameraContext.mjContext); + mjr_makeContext(mujModel, &cameraContext.mjContext, mjFONTSCALE_100); + mjv_defaultOption(&cameraContext.opt); + + // Unbind + eglMakeCurrent(cameraContext.eglDisplay, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT); + + return true; + } + + void cleanupCameraContext() { + if (cameraContext.eglDisplay != EGL_NO_DISPLAY) { + if (cameraContext.eglContext != EGL_NO_CONTEXT) { + eglMakeCurrent(cameraContext.eglDisplay, cameraContext.eglSurface, cameraContext.eglSurface, + cameraContext.eglContext); + mjr_freeContext(&cameraContext.mjContext); + eglMakeCurrent(cameraContext.eglDisplay, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT); + eglDestroyContext(cameraContext.eglDisplay, cameraContext.eglContext); + } + + if (cameraContext.eglSurface != EGL_NO_SURFACE) { + eglDestroySurface(cameraContext.eglDisplay, cameraContext.eglSurface); + } + + // DON'T call eglTerminate() - MujocoContext owns the display + } + } + + // MuJoCo data (read-only, shared across sensors) MujocoContext* mujContext; - std::vector image; + mjModel* mujModel; + + // Per-camera rendering context + CameraContext cameraContext{}; + bool contextInitialized = false; + + // Camera configuration + int w, h; mjvCamera cam{}; + + // Per-camera scene (each camera has its own viewpoint) + mjvScene scene{}; + + // Image buffer (written by doUpdate, read by doSerialize/copyImageTo) + std::vector image; + mutable std::mutex imageMutex; }; -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/include/sensors/Sensor.h b/include/sensors/Sensor.h index 013dd1c..61ef0c2 100644 --- a/include/sensors/Sensor.h +++ b/include/sensors/Sensor.h @@ -1,16 +1,35 @@ #pragma once +#include #include #include +#include #include class Sensor { public: + Sensor(std::optional frequency = std::nullopt) { + if (frequency.has_value()) { + dt = std::chrono::duration(1.0 / frequency.value()); + } else { + dt = std::chrono::duration(0); // Always update + } + lastUpdateTime = std::chrono::steady_clock::now(); + } + virtual ~Sensor() = default; virtual void update() final { + auto now = std::chrono::steady_clock::now(); + auto elapsed = now - lastUpdateTime; + + if (elapsed < dt) { + return; // Skip this update + } + std::unique_lock lock(mtx_); doUpdate(); + lastUpdateTime = now; } virtual msgpack::object serialize(msgpack::zone& z) final { @@ -24,4 +43,6 @@ class Sensor { private: mutable std::shared_mutex mtx_; -}; + std::chrono::duration dt; + std::chrono::steady_clock::time_point lastUpdateTime{}; +}; \ No newline at end of file diff --git a/include/utils/ThreadPool.h b/include/utils/ThreadPool.h new file mode 100644 index 0000000..3496b54 --- /dev/null +++ b/include/utils/ThreadPool.h @@ -0,0 +1,119 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace spqr { + +class ThreadPool { + public: + explicit ThreadPool(size_t numThreads) : stop_(false) { + for (size_t i = 0; i < numThreads; ++i) { + workers_.emplace_back([this] { + while (true) { + std::function task; + + { + std::unique_lock lock(queueMutex_); + condition_.wait(lock, [this] { return stop_ || !tasks_.empty(); }); + + if (stop_ && tasks_.empty()) { + return; + } + + task = std::move(tasks_.front()); + tasks_.pop(); + } + + task(); + } + }); + } + } + + ~ThreadPool() { + { + std::unique_lock lock(queueMutex_); + stop_ = true; + } + + condition_.notify_all(); + + for (std::thread& worker : workers_) { + if (worker.joinable()) { + worker.join(); + } + } + } + + // Delete copy/move constructors and assignment operators + ThreadPool(const ThreadPool&) = delete; + ThreadPool& operator=(const ThreadPool&) = delete; + ThreadPool(ThreadPool&&) = delete; + ThreadPool& operator=(ThreadPool&&) = delete; + + // Enqueue a task and return a future for the result + template + auto enqueue(F&& f, Args&&... args) -> std::future::type> { + using return_type = typename std::invoke_result::type; + + auto task = std::make_shared>( + std::bind(std::forward(f), std::forward(args)...)); + + std::future result = task->get_future(); + + { + std::unique_lock lock(queueMutex_); + + if (stop_) { + throw std::runtime_error("Cannot enqueue on stopped ThreadPool"); + } + + tasks_.emplace([task]() { (*task)(); }); + } + + condition_.notify_one(); + return result; + } + + // Wait for all currently enqueued tasks to complete + void waitAll() { + std::vector> futures; + + { + std::unique_lock lock(queueMutex_); + // Snapshot current task count + size_t taskCount = tasks_.size(); + + // Create dummy tasks that we can wait on + for (size_t i = 0; i < taskCount; ++i) { + auto task = std::make_shared>([]() {}); + futures.push_back(task->get_future()); + tasks_.emplace([task]() { (*task)(); }); + } + } + + condition_.notify_all(); + + // Wait for all futures + for (auto& future : futures) { + future.wait(); + } + } + + private: + std::vector workers_; + std::queue> tasks_; + + std::mutex queueMutex_; + std::condition_variable condition_; + bool stop_; +}; + +} // namespace spqr \ No newline at end of file diff --git a/pixi.toml b/pixi.toml index 2fa5ee9..61878d2 100644 --- a/pixi.toml +++ b/pixi.toml @@ -44,4 +44,4 @@ test = "python -c 'import circuspy; circuspy.test()'" [activation] # Add DYLD_LIBRARY_PATH on macOS so the MuJoCo dylib is found when running binaries directly -env = { DYLD_LIBRARY_PATH = "$DYLD_LIBRARY_PATH:$CONDA_PREFIX/lib", PATH = "$CONDA_PREFIX/bin:$PATH"} +env = { DYLD_LIBRARY_PATH = "$DYLD_LIBRARY_PATH:$CONDA_PREFIX/lib", PATH = "$CONDA_PREFIX/bin:$PATH"} \ No newline at end of file diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index ac4a0a6..2cde9e7 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -77,10 +77,17 @@ void AppWindow::loadScene(const QString& yamlFile) { RobotManager::instance().startContainers(); RobotManager::instance().bindMujoco(mujContext.get()); + // Update layout + if (viewportContainer) { + mainLayout->removeWidget(viewportContainer); + viewportContainer->deleteLater(); + } + viewportContainer = QWidget::createWindowContainer(viewport.get()); mainLayout->addWidget(viewportContainer); - sim = std::make_unique(mujContext->model, mujContext->data); + sim = std::make_unique(mujContext.get()); + sim->start(); } catch (const std::exception& e) { QMessageBox::critical(this, "Error loading scene", e.what()); @@ -101,4 +108,4 @@ AppWindow::~AppWindow() { TeamManager::instance().clear(); RobotManager::instance().stopCommunicationServer(); } -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/src/MujocoContext.cpp b/src/MujocoContext.cpp index 7a24b6c..5025aef 100644 --- a/src/MujocoContext.cpp +++ b/src/MujocoContext.cpp @@ -2,15 +2,17 @@ #include #include +#include #include #include namespace spqr { + MujocoContext::MujocoContext(const std::string& xmlString) { char error[1024] = {0}; std::filesystem::current_path(PROJECT_ROOT); - std::unique_ptr > spec( + std::unique_ptr> spec( mj_parseXMLString(xmlString.c_str(), nullptr, error, sizeof(error)), [](mjSpec* s) { mj_deleteSpec(s); }); @@ -24,13 +26,22 @@ MujocoContext::MujocoContext(const std::string& xmlString) { } data = mj_makeData(model); + dataSnapshot = mj_makeData(model); // Create snapshot copy + // Viewport rendering setup mjv_defaultOption(&opt); mjv_defaultCamera(&cam); mjv_makeScene(model, &scene, 10000); + + // Camera rendering setup + if (!initializeSharedEGL()) { + throw std::runtime_error("Failed to initialize shared EGL context"); + } } MujocoContext::~MujocoContext() { + if (dataSnapshot) + mj_deleteData(dataSnapshot); if (data) mj_deleteData(data); if (model) @@ -40,6 +51,8 @@ MujocoContext::~MujocoContext() { MujocoContext& MujocoContext::operator=(MujocoContext&& other) noexcept { if (this != &other) { + if (dataSnapshot) + mj_deleteData(dataSnapshot); if (data) mj_deleteData(data); if (model) @@ -48,13 +61,49 @@ MujocoContext& MujocoContext::operator=(MujocoContext&& other) noexcept { model = other.model; data = other.data; + dataSnapshot = other.dataSnapshot; cam = other.cam; opt = other.opt; scene = other.scene; + sharedEGL = std::move(other.sharedEGL); other.model = nullptr; other.data = nullptr; + other.dataSnapshot = nullptr; } return *this; } -} // namespace spqr + +bool MujocoContext::initializeSharedEGL() { + sharedEGL.eglDisplay = eglGetDisplay(EGL_DEFAULT_DISPLAY); + if (sharedEGL.eglDisplay == EGL_NO_DISPLAY) { + std::cerr << "Failed to get EGL display" << std::endl; + return false; + } + + EGLint major, minor; + if (!eglInitialize(sharedEGL.eglDisplay, &major, &minor)) { + std::cerr << "Failed to initialize EGL" << std::endl; + return false; + } + + return true; +} + +SharedEGLDisplay::~SharedEGLDisplay() { + cleanup(); +} + +void SharedEGLDisplay::cleanup() { + if (eglDisplay != EGL_NO_DISPLAY) { + eglTerminate(eglDisplay); + eglDisplay = EGL_NO_DISPLAY; + } +} + +void MujocoContext::updateSnapshot() { + std::lock_guard lock(snapshotMutex); + mj_copyData(dataSnapshot, model, data); +} + +} // namespace spqr \ No newline at end of file diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index 3747459..95a889c 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -262,4 +262,4 @@ void SceneParser::buildRobotInstance(const shared_ptr& robotSpec, xml_nod } } -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 1459d80..e7cedee 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -1,24 +1,34 @@ #include "SimulationThread.h" -#include - -#include "RobotManager.h" - namespace spqr { -SimulationThread::SimulationThread(const mjModel* model, mjData* data) - : model_(model), data_(data), running_(true) {} +SimulationThread::SimulationThread(MujocoContext* mujContext) : mujContext_(mujContext), running_(false) {} + +SimulationThread::~SimulationThread() { + if (running_) { + stop(); + } +} void SimulationThread::run() { - if (!model_) + if (!mujContext_ || !mujContext_->model) throw std::runtime_error("Cannot start simulation without mujoco model"); - double sim_dt = model_->opt.timestep; + running_ = true; + + double sim_dt = mujContext_->model->opt.timestep; using clock = std::chrono::steady_clock; auto next_step_time = clock::now(); + while (running_) { - mj_step(model_, data_); + // Step the simulation (updates live data) + mj_step(mujContext_->model, mujContext_->data); + + // Create snapshot for sensors to use + mujContext_->updateSnapshot(); + + // Update all robots (which update their sensors using the snapshot) RobotManager::instance().update(); next_step_time += std::chrono::duration_cast(std::chrono::duration(sim_dt)); @@ -34,4 +44,4 @@ void SimulationThread::stop() { wait(); } -} // namespace spqr +} // namespace spqr \ No newline at end of file diff --git a/src/SimulationViewport.cpp b/src/SimulationViewport.cpp index a21867b..2ad44f3 100644 --- a/src/SimulationViewport.cpp +++ b/src/SimulationViewport.cpp @@ -135,4 +135,4 @@ int SimulationViewport::selectBody(float relx, float rely) const { return bodyid; } -} // namespace spqr +} // namespace spqr \ No newline at end of file From b75a118533f74dd93e4226d7b5edcb40d6008435 Mon Sep 17 00:00:00 2001 From: neverorfrog <97flavio.maiorana@gmail.com> Date: Tue, 2 Dec 2025 23:43:35 +0100 Subject: [PATCH 2/4] clang format --- CMakeLists.txt | 2 +- include/AppWindow.h | 2 +- include/CircusApplication.h | 2 +- include/MujocoContext.h | 2 +- include/RobotManager.h | 2 +- include/SimulationThread.h | 2 +- include/SimulationViewport.h | 2 +- include/robots/BoosterK1.h | 2 +- include/robots/BoosterT1.h | 2 +- include/robots/Robot.h | 2 +- include/sensors/Camera.h | 2 +- include/sensors/Sensor.h | 2 +- include/utils/ThreadPool.h | 2 +- pixi.toml | 2 +- resources/meshes/ball/ball.obj | 2 +- src/AppWindow.cpp | 2 +- src/MujocoContext.cpp | 2 +- src/SceneParser.cpp | 2 +- src/SimulationThread.cpp | 2 +- src/SimulationViewport.cpp | 2 +- 20 files changed, 20 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e0a35d4..08482ce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -303,4 +303,4 @@ if(BUILD_PYTHON_BINDINGS AND BINDINGS_SRC) ) endif() -# =============================================================================================== \ No newline at end of file +# =============================================================================================== diff --git a/include/AppWindow.h b/include/AppWindow.h index ee8aff6..e0b2f6c 100644 --- a/include/AppWindow.h +++ b/include/AppWindow.h @@ -30,4 +30,4 @@ class AppWindow : public QMainWindow { std::unique_ptr sim; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/CircusApplication.h b/include/CircusApplication.h index 6fb8994..ee3bde4 100644 --- a/include/CircusApplication.h +++ b/include/CircusApplication.h @@ -9,4 +9,4 @@ class CircusApplication : public QApplication { ~CircusApplication(); }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/MujocoContext.h b/include/MujocoContext.h index 56a4713..eb211e0 100644 --- a/include/MujocoContext.h +++ b/include/MujocoContext.h @@ -65,4 +65,4 @@ struct MujocoContext { private: bool initializeSharedEGL(); }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/RobotManager.h b/include/RobotManager.h index 48b84d6..50c01fc 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -257,4 +257,4 @@ class RobotManager { }}}; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 78a1667..a4df2cb 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -25,4 +25,4 @@ class SimulationThread : public QThread { std::atomic running_; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/SimulationViewport.h b/include/SimulationViewport.h index 0dea25f..24934e2 100644 --- a/include/SimulationViewport.h +++ b/include/SimulationViewport.h @@ -57,4 +57,4 @@ class SimulationViewport : public QOpenGLWindow { int logicalWidth = initialWindowWidth, logicalHeight = initialWindowHeight; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 51d8878..bf87e31 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -108,4 +108,4 @@ class BoosterK1 : public Robot { std::map joint_map; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 0f31916..0e22e92 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -121,4 +121,4 @@ class BoosterT1 : public Robot { std::map joint_map; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/robots/Robot.h b/include/robots/Robot.h index e3fc715..f319591 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -49,4 +49,4 @@ class Robot { msgpack::zone buffer_zone_; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/sensors/Camera.h b/include/sensors/Camera.h index afc13f3..2fbb436 100644 --- a/include/sensors/Camera.h +++ b/include/sensors/Camera.h @@ -238,4 +238,4 @@ class Camera : public Sensor { mutable std::mutex imageMutex; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/include/sensors/Sensor.h b/include/sensors/Sensor.h index 61ef0c2..cae8a30 100644 --- a/include/sensors/Sensor.h +++ b/include/sensors/Sensor.h @@ -45,4 +45,4 @@ class Sensor { mutable std::shared_mutex mtx_; std::chrono::duration dt; std::chrono::steady_clock::time_point lastUpdateTime{}; -}; \ No newline at end of file +}; diff --git a/include/utils/ThreadPool.h b/include/utils/ThreadPool.h index 3496b54..41702a6 100644 --- a/include/utils/ThreadPool.h +++ b/include/utils/ThreadPool.h @@ -116,4 +116,4 @@ class ThreadPool { bool stop_; }; -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/pixi.toml b/pixi.toml index 61878d2..2fa5ee9 100644 --- a/pixi.toml +++ b/pixi.toml @@ -44,4 +44,4 @@ test = "python -c 'import circuspy; circuspy.test()'" [activation] # Add DYLD_LIBRARY_PATH on macOS so the MuJoCo dylib is found when running binaries directly -env = { DYLD_LIBRARY_PATH = "$DYLD_LIBRARY_PATH:$CONDA_PREFIX/lib", PATH = "$CONDA_PREFIX/bin:$PATH"} \ No newline at end of file +env = { DYLD_LIBRARY_PATH = "$DYLD_LIBRARY_PATH:$CONDA_PREFIX/lib", PATH = "$CONDA_PREFIX/bin:$PATH"} diff --git a/resources/meshes/ball/ball.obj b/resources/meshes/ball/ball.obj index adccca1..0a35801 100644 --- a/resources/meshes/ball/ball.obj +++ b/resources/meshes/ball/ball.obj @@ -17601,4 +17601,4 @@ f 4281/258/258 4156/3472/3265 4158/4544/4250 4282/2069/1964 f 4283/2425/2295 4160/4280/4016 4159/2765/2614 4284/2764/2613 f 4285/3084/2912 4162/3083/2911 4164/3042/2873 4286/3041/2872 f 267/2277/299 269/4365/295 2031/4368/4086 2035/2274/2153 -f 567/524/522 2460/1464/1410 2432/2175/2066 478/525/523 \ No newline at end of file +f 567/524/522 2460/1464/1410 2432/2175/2066 478/525/523 diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 2cde9e7..4ec23f1 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -108,4 +108,4 @@ AppWindow::~AppWindow() { TeamManager::instance().clear(); RobotManager::instance().stopCommunicationServer(); } -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/src/MujocoContext.cpp b/src/MujocoContext.cpp index 5025aef..430e871 100644 --- a/src/MujocoContext.cpp +++ b/src/MujocoContext.cpp @@ -106,4 +106,4 @@ void MujocoContext::updateSnapshot() { mj_copyData(dataSnapshot, model, data); } -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index 95a889c..3747459 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -262,4 +262,4 @@ void SceneParser::buildRobotInstance(const shared_ptr& robotSpec, xml_nod } } -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index e7cedee..75fcc9c 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -44,4 +44,4 @@ void SimulationThread::stop() { wait(); } -} // namespace spqr \ No newline at end of file +} // namespace spqr diff --git a/src/SimulationViewport.cpp b/src/SimulationViewport.cpp index 2ad44f3..a21867b 100644 --- a/src/SimulationViewport.cpp +++ b/src/SimulationViewport.cpp @@ -135,4 +135,4 @@ int SimulationViewport::selectBody(float relx, float rely) const { return bodyid; } -} // namespace spqr \ No newline at end of file +} // namespace spqr From 7a459f2596ef372f8f8acef9dcc9b47a8fe340c3 Mon Sep 17 00:00:00 2001 From: neverorfrog <97flavio.maiorana@gmail.com> Date: Mon, 8 Dec 2025 17:43:25 +0100 Subject: [PATCH 3/4] fix of reviews --- include/MujocoContext.h | 31 +++++++++++++++++++++++++++++-- include/sensors/Camera.h | 15 ++++++++++----- include/utils/ThreadPool.h | 25 ------------------------- src/AppWindow.cpp | 6 ------ src/MujocoContext.cpp | 16 ++++++++++++++++ 5 files changed, 55 insertions(+), 38 deletions(-) diff --git a/include/MujocoContext.h b/include/MujocoContext.h index eb211e0..04d6a2e 100644 --- a/include/MujocoContext.h +++ b/include/MujocoContext.h @@ -10,6 +10,32 @@ namespace spqr { +// RAII guard that holds a lock while accessing the snapshot +class SnapshotGuard { + public: + SnapshotGuard(std::mutex& mutex, mjData* data) : lock_(mutex), data_(data) {} + + // Get raw pointer to snapshot data + mjData* get() const { + return data_; + } + + // Allow pointer-like access + mjData* operator->() const { + return data_; + } + + // Prevent copying and moving (lock_guard is non-copyable and non-movable) + SnapshotGuard(const SnapshotGuard&) = delete; + SnapshotGuard& operator=(const SnapshotGuard&) = delete; + SnapshotGuard(SnapshotGuard&&) = delete; + SnapshotGuard& operator=(SnapshotGuard&&) = delete; + + private: + std::lock_guard lock_; + mjData* data_; +}; + // Shared EGL display for all cameras (owned by MujocoContext) struct SharedEGLDisplay { EGLDisplay eglDisplay = EGL_NO_DISPLAY; @@ -52,8 +78,9 @@ struct MujocoContext { void updateSnapshot(); // Get thread-safe read access to snapshot - mjData* getSnapshot() const { - return dataSnapshot; + // Returns a guard that holds the lock while the snapshot is being accessed + SnapshotGuard getSnapshot() const { + return SnapshotGuard(snapshotMutex, dataSnapshot); } // Copying could potentially lead to freeing the model or data twice. diff --git a/include/sensors/Camera.h b/include/sensors/Camera.h index 2fbb436..5d338e2 100644 --- a/include/sensors/Camera.h +++ b/include/sensors/Camera.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -18,7 +19,7 @@ namespace spqr { class Camera : public Sensor { public: - Camera(MujocoContext* mujContext, const char* cameraName) + Camera(MujocoContext* mujContext, const char* cameraName, int maxgeom = 100) : Sensor(30.0f), mujContext(mujContext), mujModel(mujContext->model) { // Find the camera in the MuJoCo model cam.type = mjCAMERA_FIXED; @@ -35,8 +36,9 @@ class Camera : public Sensor { // Allocate image buffer (RGB, 3 bytes per pixel) image.resize(w * h * 3); // Create a scene for this camera's viewpoint + // maxgeom determines memory usage: ~240 KB per 1000 geoms mjv_defaultScene(&scene); - mjv_makeScene(mujModel, &scene, 1000); + mjv_makeScene(mujModel, &scene, maxgeom); // EGL context will be initialized lazily on first doUpdate() call // because it must be created on the thread that will use it @@ -69,8 +71,11 @@ class Camera : public Sensor { mjrRect viewport = {0, 0, w, h}; // 2. Update the scene from this camera's viewpoint using snapshot data - mjData* snapshot = mujContext->getSnapshot(); - mjv_updateScene(mujModel, snapshot, &cameraContext.opt, nullptr, &cam, mjCAT_ALL, &scene); + // getSnapshot() returns a SnapshotGuard that holds the lock during access + { + auto snapshotGuard = mujContext->getSnapshot(); + mjv_updateScene(mujModel, snapshotGuard.get(), &cameraContext.opt, nullptr, &cam, mjCAT_ALL, &scene); + } // Lock released here // 3. Render the scene to the framebuffer mjr_render(viewport, &scene, &cameraContext.mjContext); @@ -131,7 +136,7 @@ class Camera : public Sensor { // Reuse the EGL display from MujocoContext instead of creating a new one cameraContext.eglDisplay = mujContext->sharedEGL.eglDisplay; if (cameraContext.eglDisplay == EGL_NO_DISPLAY) { - std::cerr << "Failed to get EGL display from MujocoContext" << std::endl; + throw std::runtime_error("EGL display not initialized in MujocoContext"); return false; } diff --git a/include/utils/ThreadPool.h b/include/utils/ThreadPool.h index 41702a6..ce6b30a 100644 --- a/include/utils/ThreadPool.h +++ b/include/utils/ThreadPool.h @@ -82,31 +82,6 @@ class ThreadPool { return result; } - // Wait for all currently enqueued tasks to complete - void waitAll() { - std::vector> futures; - - { - std::unique_lock lock(queueMutex_); - // Snapshot current task count - size_t taskCount = tasks_.size(); - - // Create dummy tasks that we can wait on - for (size_t i = 0; i < taskCount; ++i) { - auto task = std::make_shared>([]() {}); - futures.push_back(task->get_future()); - tasks_.emplace([task]() { (*task)(); }); - } - } - - condition_.notify_all(); - - // Wait for all futures - for (auto& future : futures) { - future.wait(); - } - } - private: std::vector workers_; std::queue> tasks_; diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 4ec23f1..d07b498 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -77,12 +77,6 @@ void AppWindow::loadScene(const QString& yamlFile) { RobotManager::instance().startContainers(); RobotManager::instance().bindMujoco(mujContext.get()); - // Update layout - if (viewportContainer) { - mainLayout->removeWidget(viewportContainer); - viewportContainer->deleteLater(); - } - viewportContainer = QWidget::createWindowContainer(viewport.get()); mainLayout->addWidget(viewportContainer); diff --git a/src/MujocoContext.cpp b/src/MujocoContext.cpp index 430e871..8fa00c3 100644 --- a/src/MujocoContext.cpp +++ b/src/MujocoContext.cpp @@ -1,5 +1,6 @@ #include "MujocoContext.h" +#include #include #include #include @@ -8,10 +9,25 @@ namespace spqr { +// Custom warning handler to filter out harmless GL_INVALID_OPERATION warnings from mjr_makeContext +static void mujocoWarningHandler(const char* msg) { + // Filter out the specific OpenGL warning that's harmless + if (strstr(msg, "OpenGL error 0x502 in or before mjr_makeContext") != nullptr) { + // Silently ignore this specific warning - it's caused by Qt/EGL context switching + // and doesn't affect functionality (see: https://github.com/google-deepmind/mujoco/issues/2393) + return; + } + // Print all other warnings normally + std::cerr << "MuJoCo Warning: " << msg << std::endl; +} + MujocoContext::MujocoContext(const std::string& xmlString) { char error[1024] = {0}; std::filesystem::current_path(PROJECT_ROOT); + // Install custom warning handler to filter harmless OpenGL warnings + mju_user_warning = mujocoWarningHandler; + std::unique_ptr> spec( mj_parseXMLString(xmlString.c_str(), nullptr, error, sizeof(error)), [](mjSpec* s) { mj_deleteSpec(s); }); From 2559fc8d077b426182213918c2ad60bd6bf3a049 Mon Sep 17 00:00:00 2001 From: neverorfrog <97flavio.maiorana@gmail.com> Date: Mon, 8 Dec 2025 17:45:57 +0100 Subject: [PATCH 4/4] clang format --- include/sensors/Camera.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/sensors/Camera.h b/include/sensors/Camera.h index 5d338e2..7aa27d2 100644 --- a/include/sensors/Camera.h +++ b/include/sensors/Camera.h @@ -74,7 +74,8 @@ class Camera : public Sensor { // getSnapshot() returns a SnapshotGuard that holds the lock during access { auto snapshotGuard = mujContext->getSnapshot(); - mjv_updateScene(mujModel, snapshotGuard.get(), &cameraContext.opt, nullptr, &cam, mjCAT_ALL, &scene); + mjv_updateScene(mujModel, snapshotGuard.get(), &cameraContext.opt, nullptr, &cam, mjCAT_ALL, + &scene); } // Lock released here // 3. Render the scene to the framebuffer