From a477daaf30184f56ac4886fcc131bbf4be8bbc16 Mon Sep 17 00:00:00 2001 From: Jacopo Tedeschi Date: Fri, 19 Sep 2025 17:47:04 +0200 Subject: [PATCH 1/7] Debug Drawings --- CMakeLists.txt | 4 +- Include/AppWindow.h | 4 ++ Include/DebugDrawings.h | 102 ++++++++++++++++++++++++++++ Include/Robot.h | 3 + Include/SimulationViewport.h | 1 + Include/Team.h | 2 +- Src/AppWindow.cpp | 6 ++ Src/DebugDrawings.cpp | 125 +++++++++++++++++++++++++++++++++++ Src/SimulationViewport.cpp | 3 + 9 files changed, 247 insertions(+), 3 deletions(-) create mode 100644 Include/DebugDrawings.h create mode 100644 Src/DebugDrawings.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index dda5cc4..e691da2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,9 +40,9 @@ find_package(nlohmann_json REQUIRED) # ============================ CIRCUS LIBRARY ================================================== set(CMAKE_AUTOMOC ON) -file(GLOB_RECURSE SOURCES ${CMAKE_SOURCE_DIR}/Src/*.cpp) +file(GLOB SOURCES CONFIGURE_DEPENDS ${CMAKE_SOURCE_DIR}/Src/*.cpp) list(REMOVE_ITEM SOURCES ${CMAKE_SOURCE_DIR}/Src/Bindings.cpp) -file(GLOB_RECURSE HEADERS ${CMAKE_SOURCE_DIR}/Include/*.h) +file(GLOB HEADERS CONFIGURE_DEPENDS ${CMAKE_SOURCE_DIR}/Include/*.h) add_library(${LIBRARY} STATIC ${SOURCES}) target_sources(${LIBRARY} PRIVATE ${HEADERS}) # Just used for IDE discovering headers diff --git a/Include/AppWindow.h b/Include/AppWindow.h index 8df6600..f044799 100644 --- a/Include/AppWindow.h +++ b/Include/AppWindow.h @@ -9,6 +9,8 @@ #include "MujocoContext.h" #include "SimulationThread.h" #include "SimulationViewport.h" +#include "DebugDrawings.h" + namespace spqr { class AppWindow : public QMainWindow { @@ -28,6 +30,8 @@ class AppWindow : public QMainWindow { std::unique_ptr mujContext; std::unique_ptr viewport; std::unique_ptr sim; + + DebugDrawings* debugDrawings; }; } // namespace spqr diff --git a/Include/DebugDrawings.h b/Include/DebugDrawings.h new file mode 100644 index 0000000..e1c7d96 --- /dev/null +++ b/Include/DebugDrawings.h @@ -0,0 +1,102 @@ +#pragma once + +#include +#include + +namespace spqr +{ + +class DebugDrawings +{ + +public: + + static void drawAllPrimitives(); + static void init(mjvScene* s) noexcept; + static void drawDebugDrawings(); + + // ---------------- regular geom types (mjtGeom) + /** + * @brief drawCircle: initialize a mjGEOM_CIRCLE in mujoco + * @param idLocal: id of the 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(int idLocal, const double center[3], const double radius, const float color[4]); + + /** + * @brief drawSphere: initialize a mjGEOM_SPHERE in mujoco + * @param idLocal: id of the 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(int idLocal, const double center[3], const double radius, const float color[4]); + + /** + * @brief drawCylinder: initialize a mjGEOM_SPHERE in mujoco + * @param idLocal: id of the 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(int idLocal, const double center[3], const double radius, const double length, const float color[4]); + + // ---------------- rendering-only geom types (mjtGeom) + + /** + * @brief drawArrow: initialize a mjGEOM_ARROW in mujoco + * @param idLocal: id of the geom object + * @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(int idLocal, const double start[3], const double end[3], const double thickness, const float color[4]); + + /** + * @brief drawLine: initialize a mjGEOM_LINE in mujoco + * @param idLocal: id of the 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(int idLocal, const double start[3], const double end[3], const double thickness, const float color[4]); + +private: + + inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug drawings + inline static std::vector geomsVector; // vector that stores the debug drawings to be drawn + inline static std::vector idsVector; // vector that stores the ids of the debug drawings to be drawn + + /** + * @brief drawRegularGeom: draw a regular geometric primitive + * @param type: type of the geometric primitive + * @param size: size of the geometric primitive + * @param pos: position of the geometric primitive + * @param color: RGBA color of the geometric primitive + */ + static void drawRegularGeom(mjtGeom type, const double size[3], const double pos[3], const float color[4]); + + /** + * @brief drawRenderOnlyGeom: draw a rendering-only geometric primitive + * @param type: type of the geometric primitive + * @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 + */ + static void drawRenderOnlyGeom(mjtGeom type, const double size[3], const double start[3], const double end[3], const double width, const float color[4]); + + /** + * @brief move the idLocal draw + */ + static bool moveGeom(int idLocal, const double center[3]); + static bool moveGeom(int idLocal, const double start[3], const double end[3]); +}; + +} // namespace spqr diff --git a/Include/Robot.h b/Include/Robot.h index 87e1cfe..afae2ad 100644 --- a/Include/Robot.h +++ b/Include/Robot.h @@ -9,6 +9,9 @@ #include #include "Container.h" +#include +#include + namespace spqr { struct Team; // Forward declaration diff --git a/Include/SimulationViewport.h b/Include/SimulationViewport.h index d769c5a..5c23f04 100644 --- a/Include/SimulationViewport.h +++ b/Include/SimulationViewport.h @@ -9,6 +9,7 @@ #include "Constants.h" #include "MujocoContext.h" +#include "DebugDrawings.h" namespace spqr { diff --git a/Include/Team.h b/Include/Team.h index 4743c25..5c447cd 100644 --- a/Include/Team.h +++ b/Include/Team.h @@ -7,7 +7,7 @@ namespace spqr { -struct Team { +struct Team{ std::string name; std::vector> robots; }; diff --git a/Src/AppWindow.cpp b/Src/AppWindow.cpp index 2206e3b..842310b 100644 --- a/Src/AppWindow.cpp +++ b/Src/AppWindow.cpp @@ -10,6 +10,9 @@ #include "MujocoContext.h" #include "Robot.h" #include "SceneParser.h" +#include "Robot.h" +#include "Container.h" + namespace spqr { AppWindow::AppWindow(int& argc, char** argv) { @@ -74,6 +77,9 @@ void AppWindow::loadScene(const QString& xml) { sim = std::make_unique(mujContext->model, mujContext->data); sim->start(); + + DebugDrawings::init(&(mujContext->scene)); + } catch (const std::exception& e) { QMessageBox::critical(this, "Error loading scene", e.what()); } diff --git a/Src/DebugDrawings.cpp b/Src/DebugDrawings.cpp new file mode 100644 index 0000000..95a14c0 --- /dev/null +++ b/Src/DebugDrawings.cpp @@ -0,0 +1,125 @@ +#include "DebugDrawings.h" + +namespace spqr +{ + +void DebugDrawings::init(mjvScene* s) noexcept { + ptrDebugDrawingsScene = s; +} + +void DebugDrawings::drawDebugDrawings() +{ + if (!ptrDebugDrawingsScene) return; + + for (int i = 0; i < geomsVector.size(); i++) { + ptrDebugDrawingsScene->geoms[ptrDebugDrawingsScene->ngeom] = geomsVector[i]; + ptrDebugDrawingsScene->ngeom += 1; + } +} + +void DebugDrawings::drawRegularGeom(mjtGeom type, const double size[3], const double pos[3], const float color[4]) +{ + mjvGeom geom; + mjv_initGeom(&geom, type, size, pos, NULL, color); + + geomsVector.push_back(geom); +} + +void DebugDrawings::drawRenderOnlyGeom(mjtGeom type, const double size[3], const double start[3], const double end[3], const double width, const float color[4]) +{ + mjvGeom geom; + mjv_initGeom(&geom, type, size, start, NULL, color); + mjv_connector(&geom, type, width, start, end); + + geomsVector.push_back(geom); +} + +bool DebugDrawings::moveGeom(int idLocal, const double center[3]) +{ + auto it = std::find(idsVector.begin(), idsVector.end(), idLocal); + if(it != idsVector.end()){ + const size_t idx = static_cast(std::distance(idsVector.begin(), it)); + + mjvGeom& g = geomsVector[idx]; + + g.pos[0] = center[0]; + g.pos[1] = center[1]; + g.pos[2] = center[2]; + return true; + } + else + { + idsVector.push_back(idLocal); + return false; + } +} + +bool DebugDrawings::moveGeom(int idLocal, const double start[3], const double end[3]) +{ + auto it = std::find(idsVector.begin(), idsVector.end(), idLocal); + if(it != idsVector.end()){ + const size_t idx = static_cast(std::distance(idsVector.begin(), it)); + + mjvGeom& g = geomsVector[idx]; + + mjv_connector(&g, g.type, g.size[0], start, end); + + return true; + } + else + { + idsVector.push_back(idLocal); + return false; + } +} + +void DebugDrawings::drawSphere(int idLocal, const double center[3], const double radius, const float color[4]) +{ + + if( !moveGeom(idLocal, center) ) + { + double size[3] = { radius, 0.0, 0.0 }; + drawRegularGeom(mjGEOM_SPHERE, size, center, color); + } + +} + +void DebugDrawings::drawCircle(int idLocal, const double center[3], const double radius, const float color[4]) +{ + if( !moveGeom(idLocal, center) ) + { + double size[3] = { radius, 0.0, 0.0 }; + drawRegularGeom(mjGEOM_CYLINDER, size, center, color); + } +} + +void DebugDrawings::drawCylinder(int idLocal, const double center[3], double radius, double length, const float color[4]) +{ + if( !moveGeom(idLocal, center) ) + { + double size[3] = { radius, length, 0.0 }; + drawRegularGeom(mjGEOM_CYLINDER, size, center, color); + } +} + +void DebugDrawings::drawArrow(int idLocal, const double start[3], const double end[3], const double thickness, const float color[4]) +{ + if( !moveGeom(idLocal, start, end) ) + { + mjtGeom geomType = mjGEOM_ARROW; + double size[3] = {1.0, 1.0, 1.0}; + double endDoubled[3] = {end[0]*2, end[1], end[2]}; + drawRenderOnlyGeom(geomType, size, start, endDoubled, thickness/100, color); + } +} + +void DebugDrawings::drawLine(int idLocal, const double start[3], const double end[3], const double thickness, const float color[4]) +{ + if( !moveGeom(idLocal, start, end) ) + { + mjtGeom geomType = mjGEOM_LINE; + drawRenderOnlyGeom(geomType, NULL, start, end, thickness, color); + } +} + +} // namespace spqr diff --git a/Src/SimulationViewport.cpp b/Src/SimulationViewport.cpp index 984376d..98417fa 100644 --- a/Src/SimulationViewport.cpp +++ b/Src/SimulationViewport.cpp @@ -38,6 +38,9 @@ void SimulationViewport::paintGL() { mjrRect viewport = {0, 0, width, height}; mjr_setBuffer(mjFB_WINDOW, &context); + + DebugDrawings::drawDebugDrawings(); + mjr_render(viewport, scene, &context); } From 87cf172a2490f15b14bb076d5f1481ef40f76ca8 Mon Sep 17 00:00:00 2001 From: Jacopo Tedeschi Date: Fri, 19 Sep 2025 17:49:36 +0200 Subject: [PATCH 2/7] pre-commit --- Include/AppWindow.h | 2 +- Include/DebugDrawings.h | 40 ++++++++------- Include/Robot.h | 2 - Include/SimulationViewport.h | 2 +- Include/Team.h | 2 +- Src/AppWindow.cpp | 2 - Src/DebugDrawings.cpp | 96 +++++++++++++++--------------------- 7 files changed, 65 insertions(+), 81 deletions(-) diff --git a/Include/AppWindow.h b/Include/AppWindow.h index f044799..d61388b 100644 --- a/Include/AppWindow.h +++ b/Include/AppWindow.h @@ -6,10 +6,10 @@ #include #include +#include "DebugDrawings.h" #include "MujocoContext.h" #include "SimulationThread.h" #include "SimulationViewport.h" -#include "DebugDrawings.h" namespace spqr { diff --git a/Include/DebugDrawings.h b/Include/DebugDrawings.h index e1c7d96..24133b8 100644 --- a/Include/DebugDrawings.h +++ b/Include/DebugDrawings.h @@ -1,18 +1,15 @@ #pragma once #include -#include - -namespace spqr -{ -class DebugDrawings -{ +#include -public: +namespace spqr { +class DebugDrawings { + public: static void drawAllPrimitives(); - static void init(mjvScene* s) noexcept; + static void init(mjvScene* s) noexcept; static void drawDebugDrawings(); // ---------------- regular geom types (mjtGeom) @@ -42,7 +39,8 @@ class DebugDrawings * @param length: half length of the cylinder * @param color: RGBA color of the geom */ - static void drawCylinder(int idLocal, const double center[3], const double radius, const double length, const float color[4]); + static void drawCylinder(int idLocal, const double center[3], const double radius, const double length, + const float color[4]); // ---------------- rendering-only geom types (mjtGeom) @@ -54,7 +52,8 @@ class DebugDrawings * @param thickness: thickness of the arrow * @param color: RGBA color of the geom */ - static void drawArrow(int idLocal, const double start[3], const double end[3], const double thickness, const float color[4]); + static void drawArrow(int idLocal, const double start[3], const double end[3], const double thickness, + const float color[4]); /** * @brief drawLine: initialize a mjGEOM_LINE in mujoco @@ -64,13 +63,14 @@ class DebugDrawings * @param thickness: thickness of the line * @param color: RGBA color of the geom */ - static void drawLine(int idLocal, const double start[3], const double end[3], const double thickness, const float color[4]); - -private: + static void drawLine(int idLocal, const double start[3], const double end[3], const double thickness, + const float color[4]); - inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug drawings - inline static std::vector geomsVector; // vector that stores the debug drawings to be drawn - inline static std::vector idsVector; // vector that stores the ids of the debug drawings to be drawn + private: + inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug + // drawings + inline static std::vector geomsVector; // vector that stores the debug drawings to be drawn + inline static std::vector idsVector; // vector that stores the ids of the debug drawings to be drawn /** * @brief drawRegularGeom: draw a regular geometric primitive @@ -79,7 +79,8 @@ class DebugDrawings * @param pos: position of the geometric primitive * @param color: RGBA color of the geometric primitive */ - static void drawRegularGeom(mjtGeom type, const double size[3], const double pos[3], const float color[4]); + static void drawRegularGeom(mjtGeom type, const double size[3], const double pos[3], + const float color[4]); /** * @brief drawRenderOnlyGeom: draw a rendering-only geometric primitive @@ -90,7 +91,8 @@ class DebugDrawings * @param width: width of the geometric primitive * @param color: RGBA color of the geometric primitive */ - static void drawRenderOnlyGeom(mjtGeom type, const double size[3], const double start[3], const double end[3], const double width, const float color[4]); + static void drawRenderOnlyGeom(mjtGeom type, const double size[3], const double start[3], + const double end[3], const double width, const float color[4]); /** * @brief move the idLocal draw @@ -99,4 +101,4 @@ class DebugDrawings static bool moveGeom(int idLocal, const double start[3], const double end[3]); }; -} // namespace spqr +} // namespace spqr diff --git a/Include/Robot.h b/Include/Robot.h index afae2ad..e540098 100644 --- a/Include/Robot.h +++ b/Include/Robot.h @@ -9,8 +9,6 @@ #include #include "Container.h" -#include -#include namespace spqr { diff --git a/Include/SimulationViewport.h b/Include/SimulationViewport.h index 5c23f04..ee98b08 100644 --- a/Include/SimulationViewport.h +++ b/Include/SimulationViewport.h @@ -8,8 +8,8 @@ #include #include "Constants.h" -#include "MujocoContext.h" #include "DebugDrawings.h" +#include "MujocoContext.h" namespace spqr { diff --git a/Include/Team.h b/Include/Team.h index 5c447cd..4743c25 100644 --- a/Include/Team.h +++ b/Include/Team.h @@ -7,7 +7,7 @@ namespace spqr { -struct Team{ +struct Team { std::string name; std::vector> robots; }; diff --git a/Src/AppWindow.cpp b/Src/AppWindow.cpp index 842310b..e9e393a 100644 --- a/Src/AppWindow.cpp +++ b/Src/AppWindow.cpp @@ -10,8 +10,6 @@ #include "MujocoContext.h" #include "Robot.h" #include "SceneParser.h" -#include "Robot.h" -#include "Container.h" namespace spqr { diff --git a/Src/DebugDrawings.cpp b/Src/DebugDrawings.cpp index 95a14c0..f3d051f 100644 --- a/Src/DebugDrawings.cpp +++ b/Src/DebugDrawings.cpp @@ -1,15 +1,14 @@ #include "DebugDrawings.h" -namespace spqr -{ +namespace spqr { void DebugDrawings::init(mjvScene* s) noexcept { ptrDebugDrawingsScene = s; } -void DebugDrawings::drawDebugDrawings() -{ - if (!ptrDebugDrawingsScene) return; +void DebugDrawings::drawDebugDrawings() { + if (!ptrDebugDrawingsScene) + return; for (int i = 0; i < geomsVector.size(); i++) { ptrDebugDrawingsScene->geoms[ptrDebugDrawingsScene->ngeom] = geomsVector[i]; @@ -17,16 +16,16 @@ void DebugDrawings::drawDebugDrawings() } } -void DebugDrawings::drawRegularGeom(mjtGeom type, const double size[3], const double pos[3], const float color[4]) -{ +void DebugDrawings::drawRegularGeom(mjtGeom type, const double size[3], const double pos[3], + const float color[4]) { mjvGeom geom; mjv_initGeom(&geom, type, size, pos, NULL, color); geomsVector.push_back(geom); } -void DebugDrawings::drawRenderOnlyGeom(mjtGeom type, const double size[3], const double start[3], const double end[3], const double width, const float color[4]) -{ +void DebugDrawings::drawRenderOnlyGeom(mjtGeom type, const double size[3], const double start[3], + const double end[3], const double width, const float color[4]) { mjvGeom geom; mjv_initGeom(&geom, type, size, start, NULL, color); mjv_connector(&geom, type, width, start, end); @@ -34,92 +33,79 @@ void DebugDrawings::drawRenderOnlyGeom(mjtGeom type, const double size[3], const geomsVector.push_back(geom); } -bool DebugDrawings::moveGeom(int idLocal, const double center[3]) -{ +bool DebugDrawings::moveGeom(int idLocal, const double center[3]) { auto it = std::find(idsVector.begin(), idsVector.end(), idLocal); - if(it != idsVector.end()){ + if (it != idsVector.end()) { const size_t idx = static_cast(std::distance(idsVector.begin(), it)); - + mjvGeom& g = geomsVector[idx]; - g.pos[0] = center[0]; - g.pos[1] = center[1]; - g.pos[2] = center[2]; + g.pos[0] = center[0]; + g.pos[1] = center[1]; + g.pos[2] = center[2]; return true; - } - else - { + } else { idsVector.push_back(idLocal); return false; } } -bool DebugDrawings::moveGeom(int idLocal, const double start[3], const double end[3]) -{ +bool DebugDrawings::moveGeom(int idLocal, const double start[3], const double end[3]) { auto it = std::find(idsVector.begin(), idsVector.end(), idLocal); - if(it != idsVector.end()){ + if (it != idsVector.end()) { const size_t idx = static_cast(std::distance(idsVector.begin(), it)); - + mjvGeom& g = geomsVector[idx]; mjv_connector(&g, g.type, g.size[0], start, end); return true; - } - else - { + } else { idsVector.push_back(idLocal); return false; } } -void DebugDrawings::drawSphere(int idLocal, const double center[3], const double radius, const float color[4]) -{ - - if( !moveGeom(idLocal, center) ) - { - double size[3] = { radius, 0.0, 0.0 }; +void DebugDrawings::drawSphere(int idLocal, const double center[3], const double radius, + const float color[4]) { + if (!moveGeom(idLocal, center)) { + double size[3] = {radius, 0.0, 0.0}; drawRegularGeom(mjGEOM_SPHERE, size, center, color); - } - + } } -void DebugDrawings::drawCircle(int idLocal, const double center[3], const double radius, const float color[4]) -{ - if( !moveGeom(idLocal, center) ) - { - double size[3] = { radius, 0.0, 0.0 }; +void DebugDrawings::drawCircle(int idLocal, const double center[3], const double radius, + const float color[4]) { + if (!moveGeom(idLocal, center)) { + double size[3] = {radius, 0.0, 0.0}; drawRegularGeom(mjGEOM_CYLINDER, size, center, color); } } -void DebugDrawings::drawCylinder(int idLocal, const double center[3], double radius, double length, const float color[4]) -{ - if( !moveGeom(idLocal, center) ) - { - double size[3] = { radius, length, 0.0 }; +void DebugDrawings::drawCylinder(int idLocal, const double center[3], double radius, double length, + const float color[4]) { + if (!moveGeom(idLocal, center)) { + double size[3] = {radius, length, 0.0}; drawRegularGeom(mjGEOM_CYLINDER, size, center, color); } } -void DebugDrawings::drawArrow(int idLocal, const double start[3], const double end[3], const double thickness, const float color[4]) -{ - if( !moveGeom(idLocal, start, end) ) - { +void DebugDrawings::drawArrow(int idLocal, const double start[3], const double end[3], const double thickness, + const float color[4]) { + if (!moveGeom(idLocal, start, end)) { mjtGeom geomType = mjGEOM_ARROW; double size[3] = {1.0, 1.0, 1.0}; - double endDoubled[3] = {end[0]*2, end[1], end[2]}; - drawRenderOnlyGeom(geomType, size, start, endDoubled, thickness/100, color); + double endDoubled[3] = {end[0] * 2, end[1], end[2]}; + drawRenderOnlyGeom(geomType, size, start, endDoubled, thickness / 100, color); } } -void DebugDrawings::drawLine(int idLocal, const double start[3], const double end[3], const double thickness, const float color[4]) -{ - if( !moveGeom(idLocal, start, end) ) - { +void DebugDrawings::drawLine(int idLocal, const double start[3], const double end[3], const double thickness, + const float color[4]) { + if (!moveGeom(idLocal, start, end)) { mjtGeom geomType = mjGEOM_LINE; drawRenderOnlyGeom(geomType, NULL, start, end, thickness, color); } } -} // namespace spqr +} // namespace spqr From 1c70fe0d6f7c79530b74c1b36d3a5c102f6cbd14 Mon Sep 17 00:00:00 2001 From: Jacopo Tedeschi Date: Sat, 4 Oct 2025 19:27:28 +0200 Subject: [PATCH 3/7] added robustness --- Include/DebugDrawings.h | 50 ++++++++++---- Src/AppWindow.cpp | 2 + Src/DebugDrawings.cpp | 141 ++++++++++++++++++++++++++-------------- 3 files changed, 130 insertions(+), 63 deletions(-) diff --git a/Include/DebugDrawings.h b/Include/DebugDrawings.h index 24133b8..973dd10 100644 --- a/Include/DebugDrawings.h +++ b/Include/DebugDrawings.h @@ -2,10 +2,11 @@ #include +#include #include +#include namespace spqr { - class DebugDrawings { public: static void drawAllPrimitives(); @@ -20,7 +21,7 @@ class DebugDrawings { * @param radius: radius of the circle * @param color: RGBA color of the geom */ - static void drawCircle(int idLocal, const double center[3], const double radius, const float color[4]); + static void drawCircle(mjString idLocal, const double center[3], const double radius, const float color[4]); /** * @brief drawSphere: initialize a mjGEOM_SPHERE in mujoco @@ -29,7 +30,7 @@ class DebugDrawings { * @param radius: radius of the sphere * @param color: RGBA color of the geom */ - static void drawSphere(int idLocal, const double center[3], const double radius, const float color[4]); + static void drawSphere(mjString idLocal, const double center[3], const double radius, const float color[4]); /** * @brief drawCylinder: initialize a mjGEOM_SPHERE in mujoco @@ -39,7 +40,7 @@ class DebugDrawings { * @param length: half length of the cylinder * @param color: RGBA color of the geom */ - static void drawCylinder(int idLocal, const double center[3], const double radius, const double length, + static void drawCylinder(mjString idLocal, const double center[3], const double radius, const double length, const float color[4]); // ---------------- rendering-only geom types (mjtGeom) @@ -52,7 +53,7 @@ class DebugDrawings { * @param thickness: thickness of the arrow * @param color: RGBA color of the geom */ - static void drawArrow(int idLocal, const double start[3], const double end[3], const double thickness, + static void drawArrow(mjString idLocal, const double start[3], const double end[3], const double thickness, const float color[4]); /** @@ -63,14 +64,28 @@ class DebugDrawings { * @param thickness: thickness of the line * @param color: RGBA color of the geom */ - static void drawLine(int idLocal, const double start[3], const double end[3], const double thickness, + static void drawLine(mjString idLocal, const double start[3], const double end[3], const double thickness, const float color[4]); private: - inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug - // drawings - inline static std::vector geomsVector; // vector that stores the debug drawings to be drawn - inline static std::vector idsVector; // vector that stores the ids of the debug drawings to be drawn + enum class drawGeomType { + Sphere, + Cylinder, + Circle, + Arrow, + Line, + }; + + struct GeomData{ + mjvGeom geom; + drawGeomType drawType; + }; + + inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug drawings + inline static std::map mapIdGeom; + + //inline static std::vector geomsVector; // vector that stores the debug drawings to be drawn + //inline static std::vector idsVector; // vector that stores the ids of the debug drawings to be drawn /** * @brief drawRegularGeom: draw a regular geometric primitive @@ -79,7 +94,7 @@ class DebugDrawings { * @param pos: position of the geometric primitive * @param color: RGBA color of the geometric primitive */ - static void drawRegularGeom(mjtGeom type, const double size[3], const double pos[3], + static void drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double pos[3], const float color[4]); /** @@ -91,14 +106,21 @@ class DebugDrawings { * @param width: width of the geometric primitive * @param color: RGBA color of the geometric primitive */ - static void drawRenderOnlyGeom(mjtGeom type, const double size[3], const double start[3], + static void drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double start[3], const double end[3], const double width, const float color[4]); + + /** + * @brief check is the mjv_Geom already exist + */ + + static mjvGeom* isGeomExist(mjString idLocal, drawGeomType type); + /** * @brief move the idLocal draw */ - static bool moveGeom(int idLocal, const double center[3]); - static bool moveGeom(int idLocal, const double start[3], const double end[3]); + static void moveGeom(mjvGeom* geom, const double center[3]); + static void moveGeom(mjvGeom* geom, const double start[3], const double end[3]); }; } // namespace spqr diff --git a/Src/AppWindow.cpp b/Src/AppWindow.cpp index e9e393a..133fba1 100644 --- a/Src/AppWindow.cpp +++ b/Src/AppWindow.cpp @@ -16,6 +16,7 @@ namespace spqr { AppWindow::AppWindow(int& argc, char** argv) { std::signal(SIGTERM, signalHandler); std::signal(SIGINT, signalHandler); + std::signal(SIGSEGV, signalHandler); resize(spqr::initialWindowWidth, spqr::initialWindowHeight); setWindowTitle(spqr::appName); @@ -35,6 +36,7 @@ AppWindow::AppWindow(int& argc, char** argv) { QString fileArg = QString::fromLocal8Bit(argv[1]); loadScene(fileArg); } + DebugDrawings::init(&mujContext->scene); }; void AppWindow::openScene() { diff --git a/Src/DebugDrawings.cpp b/Src/DebugDrawings.cpp index f3d051f..f7a9e3e 100644 --- a/Src/DebugDrawings.cpp +++ b/Src/DebugDrawings.cpp @@ -10,101 +10,144 @@ void DebugDrawings::drawDebugDrawings() { if (!ptrDebugDrawingsScene) return; - for (int i = 0; i < geomsVector.size(); i++) { - ptrDebugDrawingsScene->geoms[ptrDebugDrawingsScene->ngeom] = geomsVector[i]; + for (const auto& pair : mapIdGeom){ + + if( ptrDebugDrawingsScene->ngeom == ptrDebugDrawingsScene->maxgeom) + break; + + ptrDebugDrawingsScene->geoms[ptrDebugDrawingsScene->ngeom] = pair.second.geom; ptrDebugDrawingsScene->ngeom += 1; } } -void DebugDrawings::drawRegularGeom(mjtGeom type, const double size[3], const double pos[3], +void DebugDrawings::drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double pos[3], const float color[4]) { mjvGeom geom; mjv_initGeom(&geom, type, size, pos, NULL, color); - geomsVector.push_back(geom); + mjString extendedId = idLocal + "_" + std::to_string(static_cast(geomType)); + mapIdGeom[extendedId] = { .geom = geom, .drawType = geomType }; } -void DebugDrawings::drawRenderOnlyGeom(mjtGeom type, const double size[3], const double start[3], +void DebugDrawings::drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double start[3], const double end[3], const double width, const float color[4]) { mjvGeom geom; mjv_initGeom(&geom, type, size, start, NULL, color); mjv_connector(&geom, type, width, start, end); - geomsVector.push_back(geom); + mjString extendedId = idLocal + "_" + std::to_string(static_cast(geomType)); + mapIdGeom[extendedId] = { .geom = geom, .drawType = geomType }; } -bool DebugDrawings::moveGeom(int idLocal, const double center[3]) { - auto it = std::find(idsVector.begin(), idsVector.end(), idLocal); - if (it != idsVector.end()) { - const size_t idx = static_cast(std::distance(idsVector.begin(), it)); - - mjvGeom& g = geomsVector[idx]; - - g.pos[0] = center[0]; - g.pos[1] = center[1]; - g.pos[2] = center[2]; - return true; - } else { - idsVector.push_back(idLocal); - return false; - } +mjvGeom* DebugDrawings::isGeomExist(mjString idLocal, drawGeomType drawType){ + + mjString extendedId = idLocal + "_" + std::to_string(static_cast(drawType)); + auto it = mapIdGeom.find(extendedId); + + if( it == mapIdGeom.end() ) + return nullptr; + else if((it->second).drawType == drawType) + return &((it->second).geom); + else + return nullptr; + } -bool DebugDrawings::moveGeom(int idLocal, const double start[3], const double end[3]) { - auto it = std::find(idsVector.begin(), idsVector.end(), idLocal); - if (it != idsVector.end()) { - const size_t idx = static_cast(std::distance(idsVector.begin(), it)); +void DebugDrawings::moveGeom(mjvGeom* g, const double center[3]) { - mjvGeom& g = geomsVector[idx]; + g->pos[0] = center[0]; + g->pos[1] = center[1]; + g->pos[2] = center[2]; +} - mjv_connector(&g, g.type, g.size[0], start, end); +void DebugDrawings::moveGeom(mjvGeom* g, const double start[3], const double end[3]) { - return true; - } else { - idsVector.push_back(idLocal); - return false; - } + mjv_connector(g, g->type, g->size[0], start, end); } -void DebugDrawings::drawSphere(int idLocal, const double center[3], const double radius, +void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const double radius, const float color[4]) { - if (!moveGeom(idLocal, center)) { + + mjtGeom geomType = mjGEOM_SPHERE; + drawGeomType drawType = drawGeomType::Sphere; + + mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + + if ( foundGeom != nullptr ) { + moveGeom(foundGeom, center); + } + else + { double size[3] = {radius, 0.0, 0.0}; - drawRegularGeom(mjGEOM_SPHERE, size, center, color); + drawRegularGeom(idLocal, geomType, drawType, size, center, color); } } -void DebugDrawings::drawCircle(int idLocal, const double center[3], const double radius, +void DebugDrawings::drawCircle(mjString idLocal, const double center[3], const double radius, const float color[4]) { - if (!moveGeom(idLocal, center)) { + mjtGeom geomType = mjGEOM_CYLINDER; + drawGeomType drawType = drawGeomType::Circle; + + mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + + if ( foundGeom != nullptr ) { + moveGeom(foundGeom, center); + } + else + { double size[3] = {radius, 0.0, 0.0}; - drawRegularGeom(mjGEOM_CYLINDER, size, center, color); + drawRegularGeom(idLocal, geomType, drawType, size, center, color); } } -void DebugDrawings::drawCylinder(int idLocal, const double center[3], double radius, double length, +void DebugDrawings::drawCylinder(mjString idLocal, const double center[3], double radius, double length, const float color[4]) { - if (!moveGeom(idLocal, center)) { + mjtGeom geomType = mjGEOM_CYLINDER; + drawGeomType drawType = drawGeomType::Cylinder; + + mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + + if ( foundGeom != nullptr ) { + moveGeom(foundGeom, center); + } + else + { double size[3] = {radius, length, 0.0}; - drawRegularGeom(mjGEOM_CYLINDER, size, center, color); + drawRegularGeom(idLocal, geomType, drawType, size, center, color); } } -void DebugDrawings::drawArrow(int idLocal, const double start[3], const double end[3], const double thickness, +void DebugDrawings::drawArrow(mjString idLocal, const double start[3], const double end[3], const double thickness, const float color[4]) { - if (!moveGeom(idLocal, start, end)) { - mjtGeom geomType = mjGEOM_ARROW; + mjtGeom geomType = mjGEOM_ARROW; + drawGeomType drawType = drawGeomType::Arrow; + + mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + + if ( foundGeom != nullptr ) { + moveGeom(foundGeom, start, end); + } + else + { double size[3] = {1.0, 1.0, 1.0}; double endDoubled[3] = {end[0] * 2, end[1], end[2]}; - drawRenderOnlyGeom(geomType, size, start, endDoubled, thickness / 100, color); + drawRenderOnlyGeom(idLocal, geomType, drawType, size, start, endDoubled, thickness / 100, color); } } -void DebugDrawings::drawLine(int idLocal, const double start[3], const double end[3], const double thickness, +void DebugDrawings::drawLine(mjString idLocal, const double start[3], const double end[3], const double thickness, const float color[4]) { - if (!moveGeom(idLocal, start, end)) { - mjtGeom geomType = mjGEOM_LINE; - drawRenderOnlyGeom(geomType, NULL, start, end, thickness, color); + mjtGeom geomType = mjGEOM_LINE; + drawGeomType drawType = drawGeomType::Line; + + mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + + if ( foundGeom != nullptr ) { + moveGeom(foundGeom, start, end); + } + else + { + drawRenderOnlyGeom(idLocal, geomType, drawType, NULL, start, end, thickness, color); } } From d8acd4fa5b009849469ca03808b3f8465c289454 Mon Sep 17 00:00:00 2001 From: Jacopo Tedeschi Date: Sat, 4 Oct 2025 20:09:57 +0200 Subject: [PATCH 4/7] enhancement --- Include/DebugDrawings.h | 20 ++++++++++---------- Src/DebugDrawings.cpp | 42 +++++++++++++++++++++++++---------------- 2 files changed, 36 insertions(+), 26 deletions(-) diff --git a/Include/DebugDrawings.h b/Include/DebugDrawings.h index 973dd10..1ac8c3b 100644 --- a/Include/DebugDrawings.h +++ b/Include/DebugDrawings.h @@ -4,7 +4,7 @@ #include #include -#include +#include namespace spqr { class DebugDrawings { @@ -21,7 +21,7 @@ class DebugDrawings { * @param radius: radius of the circle * @param color: RGBA color of the geom */ - static void drawCircle(mjString idLocal, const double center[3], const double radius, const float color[4]); + static void drawCircle(mjString idLocal, const double center[3], const double radius, QColor color); /** * @brief drawSphere: initialize a mjGEOM_SPHERE in mujoco @@ -30,7 +30,7 @@ class DebugDrawings { * @param radius: radius of the sphere * @param color: RGBA color of the geom */ - static void drawSphere(mjString idLocal, const double center[3], const double radius, const float color[4]); + static void drawSphere(mjString idLocal, const double center[3], const double radius, QColor color); /** * @brief drawCylinder: initialize a mjGEOM_SPHERE in mujoco @@ -41,7 +41,7 @@ class DebugDrawings { * @param color: RGBA color of the geom */ static void drawCylinder(mjString idLocal, const double center[3], const double radius, const double length, - const float color[4]); + QColor color); // ---------------- rendering-only geom types (mjtGeom) @@ -54,7 +54,7 @@ class DebugDrawings { * @param color: RGBA color of the geom */ static void drawArrow(mjString idLocal, const double start[3], const double end[3], const double thickness, - const float color[4]); + QColor color); /** * @brief drawLine: initialize a mjGEOM_LINE in mujoco @@ -65,7 +65,7 @@ class DebugDrawings { * @param color: RGBA color of the geom */ static void drawLine(mjString idLocal, const double start[3], const double end[3], const double thickness, - const float color[4]); + QColor color); private: enum class drawGeomType { @@ -95,7 +95,7 @@ class DebugDrawings { * @param color: RGBA color of the geometric primitive */ static void drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double pos[3], - const float color[4]); + QColor color); /** * @brief drawRenderOnlyGeom: draw a rendering-only geometric primitive @@ -107,7 +107,7 @@ class DebugDrawings { * @param color: RGBA color of the geometric primitive */ static void drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double start[3], - const double end[3], const double width, const float color[4]); + const double end[3], const double width, QColor color); /** @@ -119,8 +119,8 @@ class DebugDrawings { /** * @brief move the idLocal draw */ - static void moveGeom(mjvGeom* geom, const double center[3]); - static void moveGeom(mjvGeom* geom, const double start[3], const double end[3]); + 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 diff --git a/Src/DebugDrawings.cpp b/Src/DebugDrawings.cpp index f7a9e3e..be5b202 100644 --- a/Src/DebugDrawings.cpp +++ b/Src/DebugDrawings.cpp @@ -21,18 +21,22 @@ void DebugDrawings::drawDebugDrawings() { } void DebugDrawings::drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double pos[3], - const float color[4]) { + QColor color) { mjvGeom geom; - mjv_initGeom(&geom, type, size, pos, NULL, color); + const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; + + mjv_initGeom(&geom, type, size, pos, NULL, colorRGBA); mjString extendedId = idLocal + "_" + std::to_string(static_cast(geomType)); mapIdGeom[extendedId] = { .geom = geom, .drawType = geomType }; } void DebugDrawings::drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double start[3], - const double end[3], const double width, const float color[4]) { + const double end[3], const double width, QColor color) { mjvGeom geom; - mjv_initGeom(&geom, type, size, start, NULL, color); + const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; + + mjv_initGeom(&geom, type, size, start, NULL, colorRGBA); mjv_connector(&geom, type, width, start, end); mjString extendedId = idLocal + "_" + std::to_string(static_cast(geomType)); @@ -53,20 +57,26 @@ mjvGeom* DebugDrawings::isGeomExist(mjString idLocal, drawGeomType drawType){ } -void DebugDrawings::moveGeom(mjvGeom* g, const double center[3]) { +void DebugDrawings::moveGeom(mjvGeom* g, const double center[3], QColor color) { g->pos[0] = center[0]; g->pos[1] = center[1]; g->pos[2] = center[2]; + + const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; + for(int i = 0; i < 4; ++i){ g->rgba[i] = colorRGBA[i]; } } -void DebugDrawings::moveGeom(mjvGeom* g, const double start[3], const double end[3]) { +void DebugDrawings::moveGeom(mjvGeom* g, const double start[3], const double end[3], QColor color) { + + const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; + for(int i = 0; i < 4; ++i){ g->rgba[i] = colorRGBA[i]; } mjv_connector(g, g->type, g->size[0], start, end); } void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const double radius, - const float color[4]) { + QColor color) { mjtGeom geomType = mjGEOM_SPHERE; drawGeomType drawType = drawGeomType::Sphere; @@ -74,7 +84,7 @@ void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const d mjvGeom* foundGeom = isGeomExist(idLocal, drawType); if ( foundGeom != nullptr ) { - moveGeom(foundGeom, center); + moveGeom(foundGeom, center, color); } else { @@ -84,14 +94,14 @@ void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const d } void DebugDrawings::drawCircle(mjString idLocal, const double center[3], const double radius, - const float color[4]) { + QColor color) { mjtGeom geomType = mjGEOM_CYLINDER; drawGeomType drawType = drawGeomType::Circle; mjvGeom* foundGeom = isGeomExist(idLocal, drawType); if ( foundGeom != nullptr ) { - moveGeom(foundGeom, center); + moveGeom(foundGeom, center, color); } else { @@ -101,14 +111,14 @@ void DebugDrawings::drawCircle(mjString idLocal, const double center[3], const d } void DebugDrawings::drawCylinder(mjString idLocal, const double center[3], double radius, double length, - const float color[4]) { + QColor color) { mjtGeom geomType = mjGEOM_CYLINDER; drawGeomType drawType = drawGeomType::Cylinder; mjvGeom* foundGeom = isGeomExist(idLocal, drawType); if ( foundGeom != nullptr ) { - moveGeom(foundGeom, center); + moveGeom(foundGeom, center, color); } else { @@ -118,14 +128,14 @@ void DebugDrawings::drawCylinder(mjString idLocal, const double center[3], doubl } void DebugDrawings::drawArrow(mjString idLocal, const double start[3], const double end[3], const double thickness, - const float color[4]) { + QColor color) { mjtGeom geomType = mjGEOM_ARROW; drawGeomType drawType = drawGeomType::Arrow; mjvGeom* foundGeom = isGeomExist(idLocal, drawType); if ( foundGeom != nullptr ) { - moveGeom(foundGeom, start, end); + moveGeom(foundGeom, start, end, color); } else { @@ -136,14 +146,14 @@ void DebugDrawings::drawArrow(mjString idLocal, const double start[3], const dou } void DebugDrawings::drawLine(mjString idLocal, const double start[3], const double end[3], const double thickness, - const float color[4]) { + QColor color) { mjtGeom geomType = mjGEOM_LINE; drawGeomType drawType = drawGeomType::Line; mjvGeom* foundGeom = isGeomExist(idLocal, drawType); if ( foundGeom != nullptr ) { - moveGeom(foundGeom, start, end); + moveGeom(foundGeom, start, end, color); } else { From f8a56ff4e2ec5cb05bf9fa56eca6fe13b50684f0 Mon Sep 17 00:00:00 2001 From: Jacopo Tedeschi Date: Sat, 4 Oct 2025 20:13:51 +0200 Subject: [PATCH 5/7] pre-commit after enhancement --- Include/DebugDrawings.h | 46 +++++++++++----------- Src/DebugDrawings.cpp | 87 ++++++++++++++++++----------------------- 2 files changed, 61 insertions(+), 72 deletions(-) diff --git a/Include/DebugDrawings.h b/Include/DebugDrawings.h index 1ac8c3b..c782bf6 100644 --- a/Include/DebugDrawings.h +++ b/Include/DebugDrawings.h @@ -2,9 +2,9 @@ #include -#include -#include #include +#include +#include namespace spqr { class DebugDrawings { @@ -40,8 +40,8 @@ class DebugDrawings { * @param length: half length of the cylinder * @param color: RGBA color of the geom */ - static void drawCylinder(mjString idLocal, const double center[3], const double radius, const double length, - QColor color); + static void drawCylinder(mjString idLocal, const double center[3], const double radius, + const double length, QColor color); // ---------------- rendering-only geom types (mjtGeom) @@ -53,8 +53,8 @@ class DebugDrawings { * @param thickness: thickness of the arrow * @param color: RGBA color of the geom */ - static void drawArrow(mjString idLocal, const double start[3], const double end[3], const double thickness, - QColor color); + static void drawArrow(mjString idLocal, const double start[3], const double end[3], + const double thickness, QColor color); /** * @brief drawLine: initialize a mjGEOM_LINE in mujoco @@ -69,23 +69,25 @@ class DebugDrawings { private: enum class drawGeomType { - Sphere, - Cylinder, - Circle, - Arrow, - Line, + Sphere, + Cylinder, + Circle, + Arrow, + Line, }; - struct GeomData{ + struct GeomData { mjvGeom geom; drawGeomType drawType; }; - inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug drawings - inline static std::map mapIdGeom; + inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug + // drawings + inline static std::map mapIdGeom; - //inline static std::vector geomsVector; // vector that stores the debug drawings to be drawn - //inline static std::vector idsVector; // vector that stores the ids of the debug drawings to be drawn + // inline static std::vector geomsVector; // vector that stores the debug drawings to be drawn + // inline static std::vector idsVector; // vector that stores the ids of the debug drawings to be + // drawn /** * @brief drawRegularGeom: draw a regular geometric primitive @@ -94,8 +96,8 @@ class DebugDrawings { * @param pos: position of the geometric primitive * @param color: RGBA color of the geometric primitive */ - static void drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double pos[3], - QColor color); + static void drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], + const double pos[3], QColor color); /** * @brief drawRenderOnlyGeom: draw a rendering-only geometric primitive @@ -106,14 +108,14 @@ class DebugDrawings { * @param width: width of the geometric primitive * @param color: RGBA color of the geometric primitive */ - static void drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double start[3], - const double end[3], const double width, QColor color); + static void drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, + 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 */ - + static mjvGeom* isGeomExist(mjString idLocal, drawGeomType type); /** diff --git a/Src/DebugDrawings.cpp b/Src/DebugDrawings.cpp index be5b202..5af4372 100644 --- a/Src/DebugDrawings.cpp +++ b/Src/DebugDrawings.cpp @@ -10,29 +10,29 @@ void DebugDrawings::drawDebugDrawings() { if (!ptrDebugDrawingsScene) return; - for (const auto& pair : mapIdGeom){ - - if( ptrDebugDrawingsScene->ngeom == ptrDebugDrawingsScene->maxgeom) + for (const auto& pair : mapIdGeom) { + if (ptrDebugDrawingsScene->ngeom == ptrDebugDrawingsScene->maxgeom) break; - + ptrDebugDrawingsScene->geoms[ptrDebugDrawingsScene->ngeom] = pair.second.geom; ptrDebugDrawingsScene->ngeom += 1; } } -void DebugDrawings::drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double pos[3], - QColor color) { +void DebugDrawings::drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, + const double size[3], const double pos[3], QColor color) { mjvGeom geom; const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; mjv_initGeom(&geom, type, size, pos, NULL, colorRGBA); mjString extendedId = idLocal + "_" + std::to_string(static_cast(geomType)); - mapIdGeom[extendedId] = { .geom = geom, .drawType = geomType }; + mapIdGeom[extendedId] = {.geom = geom, .drawType = geomType}; } -void DebugDrawings::drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], const double start[3], - const double end[3], const double width, QColor color) { +void DebugDrawings::drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, + const double size[3], const double start[3], const double end[3], + const double width, QColor color) { mjvGeom geom; const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; @@ -40,71 +40,64 @@ void DebugDrawings::drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomT mjv_connector(&geom, type, width, start, end); mjString extendedId = idLocal + "_" + std::to_string(static_cast(geomType)); - mapIdGeom[extendedId] = { .geom = geom, .drawType = geomType }; + mapIdGeom[extendedId] = {.geom = geom, .drawType = geomType}; } -mjvGeom* DebugDrawings::isGeomExist(mjString idLocal, drawGeomType drawType){ - +mjvGeom* DebugDrawings::isGeomExist(mjString idLocal, drawGeomType drawType) { mjString extendedId = idLocal + "_" + std::to_string(static_cast(drawType)); auto it = mapIdGeom.find(extendedId); - - if( it == mapIdGeom.end() ) + + if (it == mapIdGeom.end()) return nullptr; - else if((it->second).drawType == drawType) + else if ((it->second).drawType == drawType) return &((it->second).geom); else return nullptr; - } void DebugDrawings::moveGeom(mjvGeom* g, const double center[3], QColor color) { - g->pos[0] = center[0]; g->pos[1] = center[1]; g->pos[2] = center[2]; const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; - for(int i = 0; i < 4; ++i){ g->rgba[i] = colorRGBA[i]; } + for (int i = 0; i < 4; ++i) { + g->rgba[i] = colorRGBA[i]; + } } void DebugDrawings::moveGeom(mjvGeom* g, const double start[3], const double end[3], QColor color) { - const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; - for(int i = 0; i < 4; ++i){ g->rgba[i] = colorRGBA[i]; } + for (int i = 0; i < 4; ++i) { + g->rgba[i] = colorRGBA[i]; + } mjv_connector(g, g->type, g->size[0], start, end); } -void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const double radius, - QColor color) { - - mjtGeom geomType = mjGEOM_SPHERE; +void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const double radius, QColor color) { + mjtGeom geomType = mjGEOM_SPHERE; drawGeomType drawType = drawGeomType::Sphere; mjvGeom* foundGeom = isGeomExist(idLocal, drawType); - if ( foundGeom != nullptr ) { + if (foundGeom != nullptr) { moveGeom(foundGeom, center, color); - } - else - { + } else { double size[3] = {radius, 0.0, 0.0}; drawRegularGeom(idLocal, geomType, drawType, size, center, color); } } -void DebugDrawings::drawCircle(mjString idLocal, const double center[3], const double radius, - QColor color) { +void DebugDrawings::drawCircle(mjString idLocal, const double center[3], const double radius, QColor color) { mjtGeom geomType = mjGEOM_CYLINDER; drawGeomType drawType = drawGeomType::Circle; mjvGeom* foundGeom = isGeomExist(idLocal, drawType); - if ( foundGeom != nullptr ) { + if (foundGeom != nullptr) { moveGeom(foundGeom, center, color); - } - else - { + } else { double size[3] = {radius, 0.0, 0.0}; drawRegularGeom(idLocal, geomType, drawType, size, center, color); } @@ -117,46 +110,40 @@ void DebugDrawings::drawCylinder(mjString idLocal, const double center[3], doubl mjvGeom* foundGeom = isGeomExist(idLocal, drawType); - if ( foundGeom != nullptr ) { + if (foundGeom != nullptr) { moveGeom(foundGeom, center, color); - } - else - { + } else { double size[3] = {radius, length, 0.0}; drawRegularGeom(idLocal, geomType, drawType, size, center, color); } } -void DebugDrawings::drawArrow(mjString idLocal, const double start[3], const double end[3], const double thickness, - QColor color) { +void DebugDrawings::drawArrow(mjString idLocal, const double start[3], const double end[3], + const double thickness, QColor color) { mjtGeom geomType = mjGEOM_ARROW; drawGeomType drawType = drawGeomType::Arrow; mjvGeom* foundGeom = isGeomExist(idLocal, drawType); - if ( foundGeom != nullptr ) { + if (foundGeom != nullptr) { moveGeom(foundGeom, start, end, color); - } - else - { + } else { double size[3] = {1.0, 1.0, 1.0}; double endDoubled[3] = {end[0] * 2, end[1], end[2]}; drawRenderOnlyGeom(idLocal, geomType, drawType, size, start, endDoubled, thickness / 100, color); } } -void DebugDrawings::drawLine(mjString idLocal, const double start[3], const double end[3], const double thickness, - QColor color) { +void DebugDrawings::drawLine(mjString idLocal, const double start[3], const double end[3], + const double thickness, QColor color) { mjtGeom geomType = mjGEOM_LINE; drawGeomType drawType = drawGeomType::Line; mjvGeom* foundGeom = isGeomExist(idLocal, drawType); - if ( foundGeom != nullptr ) { + if (foundGeom != nullptr) { moveGeom(foundGeom, start, end, color); - } - else - { + } else { drawRenderOnlyGeom(idLocal, geomType, drawType, NULL, start, end, thickness, color); } } From d140e129f82ed0f3ce561de869585cd169752ad8 Mon Sep 17 00:00:00 2001 From: Jacopo Tedeschi Date: Sat, 8 Nov 2025 12:31:45 +0100 Subject: [PATCH 6/7] change names for readability --- CMakeLists.txt | 4 +-- Include/DebugDrawings.h | 43 ++++++++++++++++---------------- Src/AppWindow.cpp | 1 - Src/DebugDrawings.cpp | 54 ++++++++++++++++++++--------------------- 4 files changed, 51 insertions(+), 51 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e691da2..e8f8130 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,9 +40,9 @@ find_package(nlohmann_json REQUIRED) # ============================ CIRCUS LIBRARY ================================================== set(CMAKE_AUTOMOC ON) -file(GLOB SOURCES CONFIGURE_DEPENDS ${CMAKE_SOURCE_DIR}/Src/*.cpp) +file(GLOB_RECURSE SOURCES CONFIGURE_DEPENDS ${CMAKE_SOURCE_DIR}/Src/*.cpp) list(REMOVE_ITEM SOURCES ${CMAKE_SOURCE_DIR}/Src/Bindings.cpp) -file(GLOB HEADERS CONFIGURE_DEPENDS ${CMAKE_SOURCE_DIR}/Include/*.h) +file(GLOB_RECURSE HEADERS CONFIGURE_DEPENDS ${CMAKE_SOURCE_DIR}/Include/*.h) add_library(${LIBRARY} STATIC ${SOURCES}) target_sources(${LIBRARY} PRIVATE ${HEADERS}) # Just used for IDE discovering headers diff --git a/Include/DebugDrawings.h b/Include/DebugDrawings.h index c782bf6..1ba0b90 100644 --- a/Include/DebugDrawings.h +++ b/Include/DebugDrawings.h @@ -4,6 +4,7 @@ #include #include +#include #include namespace spqr { @@ -16,7 +17,7 @@ class DebugDrawings { // ---------------- regular geom types (mjtGeom) /** * @brief drawCircle: initialize a mjGEOM_CIRCLE in mujoco - * @param idLocal: id of the drawing + * @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 @@ -25,7 +26,7 @@ class DebugDrawings { /** * @brief drawSphere: initialize a mjGEOM_SPHERE in mujoco - * @param idLocal: id of the drawing + * @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 @@ -34,7 +35,7 @@ class DebugDrawings { /** * @brief drawCylinder: initialize a mjGEOM_SPHERE in mujoco - * @param idLocal: id of the drawing + * @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 @@ -47,7 +48,7 @@ class DebugDrawings { /** * @brief drawArrow: initialize a mjGEOM_ARROW in mujoco - * @param idLocal: id of the geom object + * @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 @@ -58,7 +59,7 @@ class DebugDrawings { /** * @brief drawLine: initialize a mjGEOM_LINE in mujoco - * @param idLocal: id of the drawing + * @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 @@ -78,48 +79,48 @@ class DebugDrawings { struct GeomData { mjvGeom geom; - drawGeomType drawType; + drawGeomType customType; }; inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug // drawings inline static std::map mapIdGeom; - // inline static std::vector geomsVector; // vector that stores the debug drawings to be drawn - // inline static std::vector idsVector; // vector that stores the ids of the debug drawings to be - // drawn - /** - * @brief drawRegularGeom: draw a regular geometric primitive - * @param type: type of the geometric primitive + * @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: RGBA color of the geometric primitive + * @param color: color of the primitive (RGBA), can use QColorConstants. */ - static void drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, const double size[3], - const double pos[3], QColor color); + 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 geometric primitive - * @param type: type of the geometric primitive + * @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 + * @param color: RGBA color of the geometric primitive, can be a QColorConstants */ - static void drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, + 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 + * @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 idLocal draw + * @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); diff --git a/Src/AppWindow.cpp b/Src/AppWindow.cpp index 133fba1..a558225 100644 --- a/Src/AppWindow.cpp +++ b/Src/AppWindow.cpp @@ -36,7 +36,6 @@ AppWindow::AppWindow(int& argc, char** argv) { QString fileArg = QString::fromLocal8Bit(argv[1]); loadScene(fileArg); } - DebugDrawings::init(&mujContext->scene); }; void AppWindow::openScene() { diff --git a/Src/DebugDrawings.cpp b/Src/DebugDrawings.cpp index 5af4372..5e418ec 100644 --- a/Src/DebugDrawings.cpp +++ b/Src/DebugDrawings.cpp @@ -19,37 +19,37 @@ void DebugDrawings::drawDebugDrawings() { } } -void DebugDrawings::drawRegularGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, +void DebugDrawings::drawRegularGeom(mjString idLocal, mjtGeom mujocoType, drawGeomType customType, const double size[3], const double pos[3], QColor color) { mjvGeom geom; const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; - mjv_initGeom(&geom, type, size, pos, NULL, colorRGBA); + mjv_initGeom(&geom, mujocoType, size, pos, NULL, colorRGBA); - mjString extendedId = idLocal + "_" + std::to_string(static_cast(geomType)); - mapIdGeom[extendedId] = {.geom = geom, .drawType = geomType}; + mjString extendedId = idLocal + "_" + std::to_string(static_cast(customType)); + mapIdGeom[extendedId] = {.geom = geom, .customType = customType}; } -void DebugDrawings::drawRenderOnlyGeom(mjString idLocal, mjtGeom type, drawGeomType geomType, +void DebugDrawings::drawRenderOnlyGeom(mjString idLocal, mjtGeom mujocoType, drawGeomType customType, const double size[3], const double start[3], const double end[3], const double width, QColor color) { mjvGeom geom; const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; - mjv_initGeom(&geom, type, size, start, NULL, colorRGBA); - mjv_connector(&geom, type, width, start, end); + mjv_initGeom(&geom, mujocoType, size, start, NULL, colorRGBA); + mjv_connector(&geom, mujocoType, width, start, end); - mjString extendedId = idLocal + "_" + std::to_string(static_cast(geomType)); - mapIdGeom[extendedId] = {.geom = geom, .drawType = geomType}; + mjString extendedId = idLocal + "_" + std::to_string(static_cast(customType)); + mapIdGeom[extendedId] = {.geom = geom, .customType = customType}; } -mjvGeom* DebugDrawings::isGeomExist(mjString idLocal, drawGeomType drawType) { - mjString extendedId = idLocal + "_" + std::to_string(static_cast(drawType)); +mjvGeom* DebugDrawings::isGeomExist(mjString idLocal, drawGeomType customType) { + mjString extendedId = idLocal + "_" + std::to_string(static_cast(customType)); auto it = mapIdGeom.find(extendedId); if (it == mapIdGeom.end()) return nullptr; - else if ((it->second).drawType == drawType) + else if ((it->second).customType == customType) return &((it->second).geom); else return nullptr; @@ -77,74 +77,74 @@ void DebugDrawings::moveGeom(mjvGeom* g, const double start[3], const double end void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const double radius, QColor color) { mjtGeom geomType = mjGEOM_SPHERE; - drawGeomType drawType = drawGeomType::Sphere; + drawGeomType customType = drawGeomType::Sphere; - mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + mjvGeom* foundGeom = isGeomExist(idLocal, customType); if (foundGeom != nullptr) { moveGeom(foundGeom, center, color); } else { double size[3] = {radius, 0.0, 0.0}; - drawRegularGeom(idLocal, geomType, drawType, size, center, color); + drawRegularGeom(idLocal, geomType, customType, size, center, color); } } void DebugDrawings::drawCircle(mjString idLocal, const double center[3], const double radius, QColor color) { mjtGeom geomType = mjGEOM_CYLINDER; - drawGeomType drawType = drawGeomType::Circle; + drawGeomType customType = drawGeomType::Circle; - mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + mjvGeom* foundGeom = isGeomExist(idLocal, customType); if (foundGeom != nullptr) { moveGeom(foundGeom, center, color); } else { double size[3] = {radius, 0.0, 0.0}; - drawRegularGeom(idLocal, geomType, drawType, size, center, color); + drawRegularGeom(idLocal, geomType, customType, size, center, color); } } void DebugDrawings::drawCylinder(mjString idLocal, const double center[3], double radius, double length, QColor color) { mjtGeom geomType = mjGEOM_CYLINDER; - drawGeomType drawType = drawGeomType::Cylinder; + drawGeomType customType = drawGeomType::Cylinder; - mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + mjvGeom* foundGeom = isGeomExist(idLocal, customType); if (foundGeom != nullptr) { moveGeom(foundGeom, center, color); } else { double size[3] = {radius, length, 0.0}; - drawRegularGeom(idLocal, geomType, drawType, size, center, color); + drawRegularGeom(idLocal, geomType, customType, size, center, color); } } void DebugDrawings::drawArrow(mjString idLocal, const double start[3], const double end[3], const double thickness, QColor color) { mjtGeom geomType = mjGEOM_ARROW; - drawGeomType drawType = drawGeomType::Arrow; + drawGeomType customType = drawGeomType::Arrow; - mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + mjvGeom* foundGeom = isGeomExist(idLocal, customType); if (foundGeom != nullptr) { moveGeom(foundGeom, start, end, color); } else { double size[3] = {1.0, 1.0, 1.0}; double endDoubled[3] = {end[0] * 2, end[1], end[2]}; - drawRenderOnlyGeom(idLocal, geomType, drawType, size, start, endDoubled, thickness / 100, color); + drawRenderOnlyGeom(idLocal, geomType, customType, size, start, endDoubled, thickness / 100, color); } } void DebugDrawings::drawLine(mjString idLocal, const double start[3], const double end[3], const double thickness, QColor color) { mjtGeom geomType = mjGEOM_LINE; - drawGeomType drawType = drawGeomType::Line; + drawGeomType customType = drawGeomType::Line; - mjvGeom* foundGeom = isGeomExist(idLocal, drawType); + mjvGeom* foundGeom = isGeomExist(idLocal, customType); if (foundGeom != nullptr) { moveGeom(foundGeom, start, end, color); } else { - drawRenderOnlyGeom(idLocal, geomType, drawType, NULL, start, end, thickness, color); + drawRenderOnlyGeom(idLocal, geomType, customType, NULL, start, end, thickness, color); } } From 933cfa7bc166ddf93d72f454ed7dd9d7ef3af872 Mon Sep 17 00:00:00 2001 From: Jacopo Tedeschi Date: Mon, 15 Dec 2025 12:29:08 +0100 Subject: [PATCH 7/7] changed to socket communication --- Include/Robot.h | 71 -------------------- MUJOCO_LOG.TXT | 1 - include/DebugDrawings.h | 62 ++++++++++++++---- include/RobotManager.h | 10 ++- include/robots/Robot.h | 38 +++++++++++ resources/config/framework_config.yaml | 4 +- resources/meshes/ball/ball.obj | 2 +- src/AppWindow.cpp | 1 - src/DebugDrawings.cpp | 91 +++++++++++++++++++++++--- 9 files changed, 181 insertions(+), 99 deletions(-) delete mode 100644 Include/Robot.h diff --git a/Include/Robot.h b/Include/Robot.h deleted file mode 100644 index e540098..0000000 --- a/Include/Robot.h +++ /dev/null @@ -1,71 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include -#include - -#include "Container.h" - -namespace spqr { - -struct Team; // Forward declaration - -struct Robot { - std::string name; - std::string type; - uint8_t number; - Eigen::Vector3d position; - Eigen::Vector3d orientation; // Euler angles - std::unique_ptr container; - std::shared_ptr team; -}; - -class RobotManager { - public: - // Singleton class - static RobotManager& instance() { - static RobotManager mgr; - return mgr; - } - - void registerRobot(std::shared_ptr robot) { - std::lock_guard lock(mutex_); - robots_.push_back(std::move(robot)); - } - - std::vector> getRobots() const { - std::lock_guard lock(mutex_); - return robots_; - } - - size_t count() const { - std::lock_guard lock(mutex_); - return robots_.size(); - } - - void clear() { - std::lock_guard lock(mutex_); - for (std::shared_ptr r : robots_) { - // Drop ownership first - r->container.reset(); - r->team.reset(); - } - robots_.clear(); - } - - private: - RobotManager() = default; - ~RobotManager() = default; - - RobotManager(const RobotManager&) = delete; - RobotManager& operator=(const RobotManager&) = delete; - - mutable std::mutex mutex_; - std::vector> robots_; -}; - -} // namespace spqr diff --git a/MUJOCO_LOG.TXT b/MUJOCO_LOG.TXT index 2b4675a..2f21e36 100644 --- a/MUJOCO_LOG.TXT +++ b/MUJOCO_LOG.TXT @@ -1,3 +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. - diff --git a/include/DebugDrawings.h b/include/DebugDrawings.h index 1ba0b90..6339024 100644 --- a/include/DebugDrawings.h +++ b/include/DebugDrawings.h @@ -6,67 +6,72 @@ #include #include #include +#include namespace spqr { class DebugDrawings { public: - static void drawAllPrimitives(); static void init(mjvScene* s) noexcept; static void drawDebugDrawings(); + static void processDebugMessage(std::map); + static void removeGeom(const std::string& idLocal); + // ---------------- regular geom types (mjtGeom) /** - * @brief drawCircle: initialize a mjGEOM_CIRCLE in mujoco + * @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(mjString idLocal, const double center[3], const double radius, QColor color); + static void drawCircle(const std::map& data_map); /** - * @brief drawSphere: initialize a mjGEOM_SPHERE in mujoco + * @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(mjString idLocal, const double center[3], const double radius, QColor color); + static void drawSphere(const std::map& data_map); /** - * @brief drawCylinder: initialize a mjGEOM_SPHERE in mujoco + * @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(mjString idLocal, const double center[3], const double radius, - const double length, QColor color); + static void drawCylinder(const std::map& data_map); // ---------------- rendering-only geom types (mjtGeom) /** - * @brief drawArrow: initialize a mjGEOM_ARROW in mujoco + * @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(mjString idLocal, const double start[3], const double end[3], - const double thickness, QColor color); + static void drawArrow(const std::map& data_map); /** - * @brief drawLine: initialize a mjGEOM_LINE in mujoco + * @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(mjString idLocal, const double start[3], const double end[3], const double thickness, - QColor color); + static void drawLine(const std::map& data_map); private: enum class drawGeomType { @@ -75,8 +80,37 @@ class DebugDrawings { 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; diff --git a/include/RobotManager.h b/include/RobotManager.h index 0400fc8..86a28c1 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -18,6 +18,7 @@ #include #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>(); 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(); @@ -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); diff --git a/include/robots/Robot.h b/include/robots/Robot.h index f319591..3b174e4 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -21,6 +21,29 @@ namespace spqr { struct Team; // Forward declaration +struct DebugMessage { + std::string idLocal; + std::string drawGeomType; + + std::array center + = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + std::array start + = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + std::array end + = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + + double radius = -1; + double thickness = -1; + double length = -1; + + std::array 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& message) = 0; virtual std::map sendMessage() = 0; + std::map sendDebugMessage(DebugMessage debugMessage) { + buffer_zone_.clear(); + std::map 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; diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 8cc022c..64cdbd6 100644 --- a/resources/config/framework_config.yaml +++ b/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" 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 5cf761c..237e02f 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -10,7 +10,6 @@ #include "RobotManager.h" #include "SceneParser.h" - namespace spqr { AppWindow::AppWindow(int& argc, char** argv) { diff --git a/src/DebugDrawings.cpp b/src/DebugDrawings.cpp index 5e418ec..e3cd814 100644 --- a/src/DebugDrawings.cpp +++ b/src/DebugDrawings.cpp @@ -6,6 +6,39 @@ void DebugDrawings::init(mjvScene* s) noexcept { ptrDebugDrawingsScene = s; } +void DebugDrawings::processDebugMessage(std::map data_map) { + std::string idLocal = data_map["idLocal"].as(); + std::string drawGeomTypeStr = data_map["drawGeomType"].as(); + + // Find debug figure + DebugDrawings::drawGeomType drawGeomType; + bool found = false; + for (int i = 0; i < static_cast(DebugDrawings::drawGeomType::COUNT); i++) { + if (DebugDrawings::toString(static_cast(i)) == drawGeomTypeStr) { + drawGeomType = static_cast(i); + found = true; + break; + } + } + + if (found == false) + throw std::runtime_error("Unknown drawGeomType: " + drawGeomTypeStr); + + if (drawGeomType == DebugDrawings::drawGeomType::Sphere) { + DebugDrawings::drawSphere(data_map); + } else if (drawGeomType == DebugDrawings::drawGeomType::Circle) { + DebugDrawings::drawCircle(data_map); + } else if (drawGeomType == DebugDrawings::drawGeomType::Cylinder) { + DebugDrawings::drawCylinder(data_map); + } else if (drawGeomType == DebugDrawings::drawGeomType::Arrow) { + DebugDrawings::drawArrow(data_map); + } else if (drawGeomType == DebugDrawings::drawGeomType::Line) { + DebugDrawings::drawLine(data_map); + } +} + +static void removeGeom(const std::string& idLocal) {} + void DebugDrawings::drawDebugDrawings() { if (!ptrDebugDrawingsScene) return; @@ -75,7 +108,15 @@ void DebugDrawings::moveGeom(mjvGeom* g, const double start[3], const double end mjv_connector(g, g->type, g->size[0], start, end); } -void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const double radius, QColor color) { +void DebugDrawings::drawSphere(const std::map& data_map) { + const auto centerArray = data_map.at("center").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double center[3] = {centerArray[0], centerArray[1], centerArray[2]}; + const double radius = data_map.at("radius").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + mjtGeom geomType = mjGEOM_SPHERE; drawGeomType customType = drawGeomType::Sphere; @@ -89,7 +130,15 @@ void DebugDrawings::drawSphere(mjString idLocal, const double center[3], const d } } -void DebugDrawings::drawCircle(mjString idLocal, const double center[3], const double radius, QColor color) { +void DebugDrawings::drawCircle(const std::map& data_map) { + const auto centerArray = data_map.at("center").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double center[3] = {centerArray[0], centerArray[1], centerArray[2]}; + const double radius = data_map.at("radius").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + mjtGeom geomType = mjGEOM_CYLINDER; drawGeomType customType = drawGeomType::Circle; @@ -103,8 +152,16 @@ void DebugDrawings::drawCircle(mjString idLocal, const double center[3], const d } } -void DebugDrawings::drawCylinder(mjString idLocal, const double center[3], double radius, double length, - QColor color) { +void DebugDrawings::drawCylinder(const std::map& data_map) { + const auto centerArray = data_map.at("center").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double center[3] = {centerArray[0], centerArray[1], centerArray[2]}; + const double radius = data_map.at("radius").as(); + const double length = data_map.at("length").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + mjtGeom geomType = mjGEOM_CYLINDER; drawGeomType customType = drawGeomType::Cylinder; @@ -118,8 +175,17 @@ void DebugDrawings::drawCylinder(mjString idLocal, const double center[3], doubl } } -void DebugDrawings::drawArrow(mjString idLocal, const double start[3], const double end[3], - const double thickness, QColor color) { +void DebugDrawings::drawArrow(const std::map& data_map) { + const auto startArray = data_map.at("start").as>(); + const auto endArray = data_map.at("end").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double start[3] = {startArray[0], startArray[1], startArray[2]}; + const double end[3] = {endArray[0], endArray[1], endArray[2]}; + const double thickness = data_map.at("thickness").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + mjtGeom geomType = mjGEOM_ARROW; drawGeomType customType = drawGeomType::Arrow; @@ -134,8 +200,17 @@ void DebugDrawings::drawArrow(mjString idLocal, const double start[3], const dou } } -void DebugDrawings::drawLine(mjString idLocal, const double start[3], const double end[3], - const double thickness, QColor color) { +void DebugDrawings::drawLine(const std::map& data_map) { + const auto startArray = data_map.at("start").as>(); + const auto endArray = data_map.at("end").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double start[3] = {startArray[0], startArray[1], startArray[2]}; + const double end[3] = {endArray[0], endArray[1], endArray[2]}; + const double thickness = data_map.at("thickness").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + mjtGeom geomType = mjGEOM_LINE; drawGeomType customType = drawGeomType::Line;