Added Debug drawings

MUJOCO_LOG.TXT

@@ -0,0 +1,2 @@
+Tue Dec  9 14:28:38 2025
+WARNING: Nan, Inf or huge value in QACC at DOF 91. The simulation is unstable. Time = 80.6200.

include/AppWindow.h

@@ -5,9 +5,11 @@
 #include <QVBoxLayout>
 #include <memory>
 
+#include "DebugDrawings.h"
 #include "MujocoContext.h"
 #include "SimulationThread.h"
 #include "SimulationViewport.h"
+
 namespace spqr {
 
 class AppWindow : public QMainWindow {
@@ -27,6 +29,8 @@ class AppWindow : public QMainWindow {
     std::unique_ptr<MujocoContext> mujContext;
     std::unique_ptr<SimulationViewport> viewport;
     std::unique_ptr<SimulationThread> sim;
+
+    DebugDrawings* debugDrawings;
 };
 
 }  // namespace spqr

include/DebugDrawings.h

@@ -0,0 +1,163 @@
+#pragma once
+
+#include <mujoco/mujoco.h>
+
+#include <QColor>
+#include <algorithm>
+#include <iostream>
+#include <map>
+#include <msgpack.hpp>
+
+namespace spqr {
+class DebugDrawings {
+   public:
+    static void init(mjvScene* s) noexcept;
+    static void drawDebugDrawings();
+
+    static void processDebugMessage(std::map<std::string, msgpack::object>);
+    static void removeGeom(const std::string& idLocal);
+
+    // ---------------- regular geom types (mjtGeom)
+    /**
+     * @brief drawCircle: initialize a mjGEOM_CIRCLE in mujoco processing a predefined message from the socker
+     * with the framework
+     * @param idLocal: identifier of the debug drawing
+     * @param center: position of the center of the geom
+     * @param radius: radius of the circle
+     * @param color: RGBA color of the geom
+     */
+    static void drawCircle(const std::map<std::string, msgpack::object>& data_map);
+
+    /**
+     * @brief drawSphere: initialize a mjGEOM_SPHERE in mujoco processing a predefined message from the socker
+     * with the framework
+     * @param idLocal: identifier of the debug drawing
+     * @param center: position of the center of the geom
+     * @param radius: radius of the sphere
+     * @param color: RGBA color of the geom
+     */
+    static void drawSphere(const std::map<std::string, msgpack::object>& data_map);
+
+    /**
+     * @brief drawCylinder: initialize a mjGEOM_SPHERE in mujoco processing a predefined message from the
+     * socker with the framework
+     * @param idLocal: identifier of the debug drawing
+     * @param center: position of the center of the geom
+     * @param radius: radius of the cylinder
+     * @param length: half length of the cylinder
+     * @param color: RGBA color of the geom
+     */
+    static void drawCylinder(const std::map<std::string, msgpack::object>& data_map);
+
+    // ---------------- rendering-only geom types (mjtGeom)
+
+    /**
+     * @brief drawArrow: initialize a mjGEOM_ARROW in mujoco processing a predefined message from the socker
+     * with the framework
+     * @param idLocal: identifier of the debug drawing
+     * @param start: position of the start point of the arrow
+     * @param end: position of the end point of the arrow
+     * @param thickness: thickness of the arrow
+     * @param color: RGBA color of the geom
+     */
+    static void drawArrow(const std::map<std::string, msgpack::object>& data_map);
+
+    /**
+     * @brief drawLine: initialize a mjGEOM_LINE in mujoco processing a predefined message from the socker
+     * with the framework
+     * @param idLocal: identifier of the debug drawing
+     * @param start: position of the start point of the line
+     * @param end: position of the end point of the line
+     * @param thickness: thickness of the line
+     * @param color: RGBA color of the geom
+     */
+    static void drawLine(const std::map<std::string, msgpack::object>& data_map);
+
+   private:
+    enum class drawGeomType {
+        Sphere,
+        Cylinder,
+        Circle,
+        Arrow,
+        Line,
+
+        COUNT,  // keep this last
+    };
+
+    static const char* toString(drawGeomType t) {
+        switch (t) {
+            case drawGeomType::Sphere:
+                return "Sphere";
+            case drawGeomType::Cylinder:
+                return "Cylinder";
+            case drawGeomType::Circle:
+                return "Circle";
+            case drawGeomType::Arrow:
+                return "Arrow";
+            case drawGeomType::Line:
+                return "Line";
+        }
+        return "Unknown";
+    }
+
+    constexpr bool isRegularGeom(drawGeomType t) {
+        switch (t) {
+            case drawGeomType::Sphere:
+            case drawGeomType::Cylinder:
+            case drawGeomType::Circle:
+                return true;
+            default:
+                return false;
+        }
+    }
+
+    struct GeomData {
+        mjvGeom geom;
+        drawGeomType customType;
+    };
+
+    inline static mjvScene* ptrDebugDrawingsScene;  // pointer to the mujoco scene where to draw the debug
+                                                    // drawings
+    inline static std::map<mjString, GeomData> mapIdGeom;
+
+    /**
+     * @brief drawRegularGeom: draw a 'regular'-defined geometric primitive
+     * @param idLocal: identifier of the debug drawing
+     * @param mujocoType: standard MuJoCo geometric type (mjtGeom)
+     * @param customType: custom geometric type used to select the appropriate mjvGeom
+     * @param size: size of the geometric primitive
+     * @param pos: position of the geometric primitive
+     * @param color: color of the primitive (RGBA), can use QColorConstants.
+     */
+    static void drawRegularGeom(mjString idLocal, mjtGeom mujocoType, drawGeomType customType,
+                                const double size[3], const double pos[3], QColor color);
+
+    /**
+     * @brief drawRenderOnlyGeom: draw a 'rendering-only'-defined geometric primitive
+     * @param idLocal: identifier of the debug drawing
+     * @param mujocoType: standard MuJoCo geometric type (mjtGeom)
+     * @param customType: custom geometric type used to select the appropriate mjvGeom
+     * @param size: size of the geometric primitive
+     * @param start: start position of the geometric primitive
+     * @param end: end position of the geometric primitive
+     * @param width: width of the geometric primitive
+     * @param color: RGBA color of the geometric primitive, can be a QColorConstants
+     */
+    static void drawRenderOnlyGeom(mjString idLocal, mjtGeom mujocoType, drawGeomType customType,
+                                   const double size[3], const double start[3], const double end[3],
+                                   const double width, QColor color);
+
+    /**
+     * @brief check is the mjv_Geom already exist. If true return the right mjvGeom into mjvScene->geoms
+     */
+
+    static mjvGeom* isGeomExist(mjString idLocal, drawGeomType type);
+
+    /**
+     * @brief move the mjvGeom object
+     */
+    static void moveGeom(mjvGeom* geom, const double center[3], QColor color);
+    static void moveGeom(mjvGeom* geom, const double start[3], const double end[3], QColor color);
+};
+
+}  // namespace spqr

include/RobotManager.h

@@ -18,6 +18,7 @@
 #include <vector>
 
 #include "Constants.h"
+#include "DebugDrawings.h"
 #include "MujocoContext.h"
 #include "robots/BoosterK1.h"
 #include "robots/BoosterT1.h"
@@ -149,6 +150,7 @@ class RobotManager {
     RobotManager& operator=(const RobotManager&) = delete;
 
     void _serverInternal(int port) {
+        std::cout << "Starting RobotManager communication server on port " << port << std::endl;
         int server_fd = socket(AF_INET, SOCK_STREAM, 0);
         if (server_fd < 0)
             throw std::runtime_error("Failed to create socket");
@@ -201,7 +203,9 @@ class RobotManager {
                         msgpack::object_handle oh = msgpack::unpack(buffer, n);
                         auto data_map = oh.get().as<std::map<std::string, msgpack::object>>();
                         auto it = data_map.find("robot_name");
-                        if (it == data_map.end())
+                        auto it_debug = data_map.find("drawGeomType");
+
+                        if (it == data_map.end() && it_debug == data_map.end())
                             continue;
 
                         std::string messageRecipient = it->second.as<std::string>();
@@ -210,6 +214,10 @@ class RobotManager {
                         for (auto& r : robots_) {
                             if (r->name == messageRecipient) {
                                 r->receiveMessage(data_map);
+
+                                if (it_debug != data_map.end())
+                                    DebugDrawings::processDebugMessage(data_map);
+
                                 auto answ = r->sendMessage();
                                 msgpack::sbuffer sbuf;
                                 msgpack::pack(sbuf, answ);

include/SimulationViewport.h

@@ -8,6 +8,7 @@
 #include <QWheelEvent>
 
 #include "Constants.h"
+#include "DebugDrawings.h"
 #include "MujocoContext.h"
 namespace spqr {
 

include/robots/Robot.h

@@ -21,6 +21,29 @@ namespace spqr {
 
 struct Team;  // Forward declaration
 
+struct DebugMessage {
+    std::string idLocal;
+    std::string drawGeomType;
+
+    std::array<double, 3> center
+        = {std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
+           std::numeric_limits<double>::quiet_NaN()};
+    std::array<double, 3> start
+        = {std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
+           std::numeric_limits<double>::quiet_NaN()};
+    std::array<double, 3> end
+        = {std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(),
+           std::numeric_limits<double>::quiet_NaN()};
+
+    double radius = -1;
+    double thickness = -1;
+    double length = -1;
+
+    std::array<double, 4> color = {1.0, 1.0, 1.0, 1.0};
+
+    bool remove = false;
+};
+
 class Robot {
    public:
     Robot(const std::string& name, const std::string& type, uint8_t number,
@@ -38,6 +61,21 @@ class Robot {
     virtual void receiveMessage(const std::map<std::string, msgpack::object>& message) = 0;
     virtual std::map<std::string, msgpack::object> sendMessage() = 0;
 
+    std::map<std::string, msgpack::object> sendDebugMessage(DebugMessage debugMessage) {
+        buffer_zone_.clear();
+        std::map<std::string, msgpack::object> msg;
+        msg["robot_name"] = msgpack::object(name, buffer_zone_);
+        msg["idLocal"] = msgpack::object(debugMessage.idLocal, buffer_zone_);
+        msg["drawGeomType"] = msgpack::object(debugMessage.drawGeomType, buffer_zone_);
+        msg["center"] = msgpack::object(debugMessage.center, buffer_zone_);
+        msg["radius"] = msgpack::object(debugMessage.radius, buffer_zone_);
+        msg["start"] = msgpack::object(debugMessage.start, buffer_zone_);
+        msg["end"] = msgpack::object(debugMessage.end, buffer_zone_);
+        msg["color"] = msgpack::object(debugMessage.color, buffer_zone_);
+
+        return msg;
+    }
+
     std::string name;
     std::string type;
     uint8_t number;

resources/config/framework_config.yaml

@@ -1,4 +1,4 @@
 image: ubuntu:22.04
 volumes:
-  - "/home/daniaffch/Dev/spqrbooster2026/src/SimBridge/bridge/build:/app/BridgeSubscriber"
-  - "/home/daniaffch/Dev/spqrbooster2026/src/SimBridge/fake_framework/build:/app/fake_framework"
+  - "/home/ubuntu/Robocup/spqrbooster2026/src/SimBridge/bridge/build:/app/BridgeSubscriber"
+  - "/home/ubuntu/Robocup/spqrbooster2026/src/SimBridge/fake_framework/build:/app/fake_framework"

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
+f 567/524/522 2460/1464/1410 2432/2175/2066 478/525/523

src/AppWindow.cpp

@@ -16,6 +16,7 @@ AppWindow::AppWindow(int& argc, char** argv) {
     std::signal(SIGTERM, signalHandler);
     std::signal(SIGINT, signalHandler);
     std::signal(SIGSEGV, signalHandler);
+    std::signal(SIGSEGV, signalHandler);
     std::signal(SIGABRT, signalHandler);
 
     resize(spqr::initialWindowWidth, spqr::initialWindowHeight);
@@ -82,6 +83,9 @@ void AppWindow::loadScene(const QString& yamlFile) {
 
         sim = std::make_unique<SimulationThread>(mujContext->model, mujContext->data);
         sim->start();
+
+        DebugDrawings::init(&(mujContext->scene));
+
     } catch (const std::exception& e) {
         QMessageBox::critical(this, "Error loading scene", e.what());
     }