From 24b3a6d0302e1c939e5a0d3a1bab5d3af4e232d0 Mon Sep 17 00:00:00 2001 From: Flavio Date: Sun, 25 Jan 2026 18:46:40 +0100 Subject: [PATCH 01/56] oracle for fast simulation The oracle is modeled as a sensor attached to each robot. It must contains all the information simulator can provide, bypassing sensoring computation (e.g. position of the ball can be provided directly from the simulator, instead of doing ball detection from camera images, ...) --- include/robots/BoosterT1.h | 7 ++++ include/sensors/Oracle.h | 80 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 87 insertions(+) create mode 100644 include/sensors/Oracle.h diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index a641568..3b1a765 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -21,6 +21,8 @@ #include "sensors/Imu.h" #include "sensors/Joint.h" #include "sensors/Pose.h" +#include "sensors/Oracle.h" + #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { @@ -33,6 +35,7 @@ class BoosterT1 : public Robot { Imu* imu = nullptr; Joints* joints = nullptr; std::array cameras = {}; + Oracle* oracle = nullptr; BoosterT1(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, const Eigen::Vector3d& initOrientation, const std::tuple color, const std::shared_ptr& team) @@ -92,6 +95,8 @@ class BoosterT1 : public Robot { cameras[0] = new Camera(mujCtx, (name + "_left_cam").c_str()); cameras[1] = new Camera(mujCtx, (name + "_right_cam").c_str()); + + oracle = new Oracle(mujCtx->model, mujCtx->data, pose); } void receiveMessage(const std::map& message) override { @@ -124,6 +129,7 @@ class BoosterT1 : public Robot { msg["pose"] = pose->serialize(buffer_zone_); msg["imu"] = imu->serialize(buffer_zone_); msg["joints"] = joints->serialize(buffer_zone_); + msg["oracle"] = oracle->serialize(buffer_zone_); return msg; } @@ -144,6 +150,7 @@ class BoosterT1 : public Robot { joints->update(); cameras[0]->update(); cameras[1]->update(); + oracle->update(); } ~BoosterT1() = default; diff --git a/include/sensors/Oracle.h b/include/sensors/Oracle.h new file mode 100644 index 0000000..729f003 --- /dev/null +++ b/include/sensors/Oracle.h @@ -0,0 +1,80 @@ +#pragma once + +#include + +#include + +#include "sensors/Sensor.h" + +namespace spqr { + + +// L'oracle is defined as a sensor that provides the ground truth about the world. +// It should contain information about the position of the ball, robots, obstacles, ... +// It can contain also the information their velocities, ... +class Oracle : public Sensor { + public: + Oracle(mjModel* mujModel, mjData* mujData, Pose* robotPose) : mujModel(mujModel), mujData(mujData), robotPose(robotPose) { + + ballId = mj_name2id(mujModel, mjOBJ_BODY, "ball"); + ballAdr = mujModel->body_jntadr[ballId]; + + ballPosAdr = mujModel->jnt_qposadr[ballAdr]; + ballVelAdr = mujModel->jnt_dofadr[ballAdr]; + + + // AGGIUNGERE TUTTI I CAMPI CHE CI INTERESSANO SAPERE COME ORACLE + + } + + void doUpdate() override { + Eigen::Vector3d ballPosGlobal = Eigen::Vector3d(Eigen::Map(mujData->qpos + ballPosAdr)); + ballPosition = globalToLocalPosition(ballPosGlobal, robotPose->getTransformationMatrix()); + } + + msgpack::object doSerialize(msgpack::zone& z) override { + std::vector ball_pos = {ballPosition(0), ballPosition(1), ballPosition(2)}; + std::map oracle_data; + oracle_data["ball_position"] = msgpack::object(ball_pos, z); + return msgpack::object(oracle_data, z); + } + + Eigen::Vector3d getBallPosition() const { + return ballPosition; + } + + + private: + /** + * @brief Transform a global position to local position relative to a robot's pose + * + * This function can be used to convert any global world coordinates to local + * robot-centric coordinates using the transformation matrix. + * + * @param globalPosition The position in global world frame (x, y, z) + * @param robotTransformationMatrix The robot's 4x4 transformation matrix (world-to-robot) + * @return The position relative to the robot's local frame + */ + static Eigen::Vector3d globalToLocalPosition( + const Eigen::Vector3d& globalPosition, + const Eigen::Matrix4d& robotTransformationMatrix) { + + // The transformation matrix T converts from local to global: p_global = T * p_local + // So to get p_local: p_local = T^(-1) * p_global + Eigen::Vector4d globalPosHomogeneous(globalPosition(0), globalPosition(1), globalPosition(2), 1.0); + Eigen::Vector4d localPosHomogeneous = robotTransformationMatrix.inverse() * globalPosHomogeneous; + + return Eigen::Vector3d(localPosHomogeneous(0), localPosHomogeneous(1), localPosHomogeneous(2)); + } + + + Eigen::Vector3d ballPosition; + int ballId, ballAdr, ballPosAdr, ballVelAdr; + + mjModel* mujModel; + mjData* mujData; + Pose* robotPose; + + +}; +} // namespace spqr From 8377236ee42592dfe9d7afa5b83ee5e6bfef7c1d Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 26 Jan 2026 19:05:25 +0100 Subject: [PATCH 02/56] Add clean-containers cmd, CHANGE CONTAINER NAME --- clean-containers.sh | 18 ++++++++++++++++++ include/RobotManager.h | 2 +- pixi.toml | 1 + 3 files changed, 20 insertions(+), 1 deletion(-) create mode 100644 clean-containers.sh diff --git a/clean-containers.sh b/clean-containers.sh new file mode 100644 index 0000000..b5a45f7 --- /dev/null +++ b/clean-containers.sh @@ -0,0 +1,18 @@ +# https://stackoverflow.com/questions/56998486/stop-docker-containers-with-name-matching-a-pattern +# ...and some help from Gemini for the ifs + +RUNNING_IDS=$(docker ps --filter name="CIRCUS_*" --filter status=running -aq) +if [ -n "$RUNNING_IDS" ]; then + echo "Stopping circus containers" + docker stop $RUNNING_IDS +else + echo "Nothing to stop" +fi + +EXITED_IDS=$(docker ps --filter name="CIRCUS_*" --filter status=exited -aq) +if [ -n "$EXITED_IDS" ]; then + echo "Removing circus containers" + docker rm $EXITED_IDS +else + echo "Nothing to remove" +fi diff --git a/include/RobotManager.h b/include/RobotManager.h index 2f9d788..24de599 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -115,7 +115,7 @@ class RobotManager { } for (std::shared_ptr r : robots_) { - r->container = std::make_unique(r->name + "_container"); + r->container = std::make_unique("CIRCUS_" + r->name + "_container"); r->container->create(r->name, image, binds); r->container->start(); } diff --git a/pixi.toml b/pixi.toml index e339c16..0625b16 100644 --- a/pixi.toml +++ b/pixi.toml @@ -42,6 +42,7 @@ libegl-devel = "==1.7.0" [tasks] test = "python -c 'import circuspy; circuspy.test()'" +clean-containers = "bash clean-containers.sh" [activation] # Add DYLD_LIBRARY_PATH on macOS so the MuJoCo dylib is found when running binaries directly From 43359a2b39a4f0eb97f246d1293ac06927f3bb8f Mon Sep 17 00:00:00 2001 From: neverorfrog <97flavio.maiorana@gmail.com> Date: Thu, 29 Jan 2026 16:26:32 +0100 Subject: [PATCH 03/56] tentativi --- dockerfiles/booster.conf | 8 ++++++++ pixi.toml | 3 --- resources/config/framework_config.yaml | 1 + 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index dfea72d..e8d0dd3 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -16,3 +16,11 @@ autostart=true autorestart=true stdout_logfile=/var/log/simbridge.log stderr_logfile=/var/log/simbridge.err + +[program:maximus] +directory=/app/maximus +command=/app/maximus/bin/maximus_main +autostart=true +autorestart=true +stdout_logfile=/var/log/maximus.log +stderr_logfile=/var/log/maximus.err \ No newline at end of file diff --git a/pixi.toml b/pixi.toml index e339c16..7a95b99 100644 --- a/pixi.toml +++ b/pixi.toml @@ -40,9 +40,6 @@ libegl-devel = "==1.7.0" [package.target.osx-arm64.host-dependencies] -[tasks] -test = "python -c 'import circuspy; circuspy.test()'" - [activation] # Add DYLD_LIBRARY_PATH on macOS so the MuJoCo dylib is found when running binaries directly env = { DYLD_LIBRARY_PATH = "$DYLD_LIBRARY_PATH:$CONDA_PREFIX/lib", PATH = "$CONDA_PREFIX/bin:$PATH"} diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 8a52b32..8bf5d82 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -4,3 +4,4 @@ image: spqr:booster volumes: - "/tools/booster_motion:/app/booster_motion" - "/build:/app/bridge" + - ":/app/maximus" From e8c04ca76858d82fece5b7a4babf3404d856b1b1 Mon Sep 17 00:00:00 2001 From: Flavio Date: Thu, 29 Jan 2026 19:02:59 +0100 Subject: [PATCH 04/56] booster.conf correction and needed env variables added Added variables are necessary due to how the main.cpp and application.cpp files are --- dockerfiles/booster.conf | 2 +- dockerfiles/configs/bashrc | 5 +++++ dockerfiles/entrypoint.sh | 3 +++ resources/scenes/1v1.yaml | 2 +- 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index e8d0dd3..be81445 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -19,7 +19,7 @@ stderr_logfile=/var/log/simbridge.err [program:maximus] directory=/app/maximus -command=/app/maximus/bin/maximus_main +command=/app/maximus/.pixi/envs/default/bin/maximus_main autostart=true autorestart=true stdout_logfile=/var/log/maximus.log diff --git a/dockerfiles/configs/bashrc b/dockerfiles/configs/bashrc index b401216..572d127 100644 --- a/dockerfiles/configs/bashrc +++ b/dockerfiles/configs/bashrc @@ -5,6 +5,11 @@ source /opt/booster/sdk_ros2/install/setup.bash export LD_LIBRARY_PATH=/app/booster_motion/lib:/app/booster_motion/lib-usr-local:/app/booster_motion/lib-x86_64-linux-gnu:$LD_LIBRARY_PATH export FASTRTPS_DEFAULT_PROFILES_FILE=/app/booster_motion/fastdds_profile.xml +# Export environment variables for MAXIMUS framework +# (They should not be used, but jsut in case ...) +export SPQR_CONFIG_ROOT=/app/maximus/src/app/config +export SPQR_BEHAVIOR_TREE_PATH=/app/maximus/src/app/behaviors/trees + alias loco="/opt/booster/sdk/build/b1_loco_example_client 127.0.0.1" alias looc="loco" alias lcoo="loco" diff --git a/dockerfiles/entrypoint.sh b/dockerfiles/entrypoint.sh index a9cfb26..c3bbd99 100644 --- a/dockerfiles/entrypoint.sh +++ b/dockerfiles/entrypoint.sh @@ -4,6 +4,9 @@ export LD_LIBRARY_PATH=/app/booster_motion/lib:/app/booster_motion/lib-usr-local:/app/booster_motion/lib-x86_64-linux-gnu:$LD_LIBRARY_PATH export FASTRTPS_DEFAULT_PROFILES_FILE=/app/booster_motion/fastdds_profile.xml +# Export environment variables for MAXIMUS framework +export SPQR_CONFIG_ROOT=/app/maximus/src/app/config +export SPQR_BEHAVIOR_TREE_PATH=/app/maximus/src/app/behaviors/trees # Start supervisord to manage processes /usr/bin/supervisord -n -c /etc/supervisor/conf.d/booster.conf diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index 5db32d6..1150980 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -8,5 +8,5 @@ teams: blue: - type: Booster-T1 number: 3 - position: [1.0, 0.0, 0.68] + position: [1.0, 1.0, 0.68] orientation: [0.0, 0.0, 3.14] From 1d0dc7b301f643c5dfd65044d8bbc6c9acf0a591 Mon Sep 17 00:00:00 2001 From: neverorfrog <97flavio.maiorana@gmail.com> Date: Thu, 29 Jan 2026 23:12:55 +0100 Subject: [PATCH 05/56] installed config files into docker container --- dockerfiles/booster.conf | 2 +- dockerfiles/configs/bashrc | 5 ----- dockerfiles/entrypoint.sh | 4 ++-- resources/config/framework_config.yaml | 4 +++- 4 files changed, 6 insertions(+), 9 deletions(-) diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index be81445..e8d0dd3 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -19,7 +19,7 @@ stderr_logfile=/var/log/simbridge.err [program:maximus] directory=/app/maximus -command=/app/maximus/.pixi/envs/default/bin/maximus_main +command=/app/maximus/bin/maximus_main autostart=true autorestart=true stdout_logfile=/var/log/maximus.log diff --git a/dockerfiles/configs/bashrc b/dockerfiles/configs/bashrc index 572d127..b401216 100644 --- a/dockerfiles/configs/bashrc +++ b/dockerfiles/configs/bashrc @@ -5,11 +5,6 @@ source /opt/booster/sdk_ros2/install/setup.bash export LD_LIBRARY_PATH=/app/booster_motion/lib:/app/booster_motion/lib-usr-local:/app/booster_motion/lib-x86_64-linux-gnu:$LD_LIBRARY_PATH export FASTRTPS_DEFAULT_PROFILES_FILE=/app/booster_motion/fastdds_profile.xml -# Export environment variables for MAXIMUS framework -# (They should not be used, but jsut in case ...) -export SPQR_CONFIG_ROOT=/app/maximus/src/app/config -export SPQR_BEHAVIOR_TREE_PATH=/app/maximus/src/app/behaviors/trees - alias loco="/opt/booster/sdk/build/b1_loco_example_client 127.0.0.1" alias looc="loco" alias lcoo="loco" diff --git a/dockerfiles/entrypoint.sh b/dockerfiles/entrypoint.sh index c3bbd99..9e63089 100644 --- a/dockerfiles/entrypoint.sh +++ b/dockerfiles/entrypoint.sh @@ -5,8 +5,8 @@ export LD_LIBRARY_PATH=/app/booster_motion/lib:/app/booster_motion/lib-usr-local export FASTRTPS_DEFAULT_PROFILES_FILE=/app/booster_motion/fastdds_profile.xml # Export environment variables for MAXIMUS framework -export SPQR_CONFIG_ROOT=/app/maximus/src/app/config -export SPQR_BEHAVIOR_TREE_PATH=/app/maximus/src/app/behaviors/trees +export SPQR_CONFIG_ROOT=/app/maximus/config +export SPQR_BEHAVIOR_TREE_PATH=/app/maximus/behaviors # Start supervisord to manage processes /usr/bin/supervisord -n -c /etc/supervisor/conf.d/booster.conf diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 8bf5d82..153b671 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -4,4 +4,6 @@ image: spqr:booster volumes: - "/tools/booster_motion:/app/booster_motion" - "/build:/app/bridge" - - ":/app/maximus" + - "/.pixi/envs/default:/app/maximus" + - "/src/app/config:/app/maximus/config" + - "/src/app/behaviors/trees:/app/maximus/behaviors" From cf24883f1e5c8b25e9adac83537dc8c5262652ff Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Fri, 30 Jan 2026 17:49:24 +0100 Subject: [PATCH 06/56] Remove filter from clean-containers --- clean-containers.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clean-containers.sh b/clean-containers.sh index b5a45f7..023e5df 100644 --- a/clean-containers.sh +++ b/clean-containers.sh @@ -9,7 +9,7 @@ else echo "Nothing to stop" fi -EXITED_IDS=$(docker ps --filter name="CIRCUS_*" --filter status=exited -aq) +EXITED_IDS=$(docker ps --filter name="CIRCUS_*" -aq) if [ -n "$EXITED_IDS" ]; then echo "Removing circus containers" docker rm $EXITED_IDS From efd9334ebfcae5c8854e8d4f07c0c295021711b7 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Tue, 3 Feb 2026 18:12:32 +0100 Subject: [PATCH 07/56] Add some network utils to docker --- dockerfiles/Dockerfile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile index b57a800..fc4b90b 100644 --- a/dockerfiles/Dockerfile +++ b/dockerfiles/Dockerfile @@ -61,4 +61,7 @@ RUN mkdir /app COPY entrypoint.sh /app RUN chmod +x /app/entrypoint.sh +# used to test communication, can be removed at a later date if so desired +RUN apt install -y iputils-ping netcat net-tools + CMD ["/app/entrypoint.sh"] From 9034f393c423257e7479e72ac96c19670ce129f3 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Tue, 3 Feb 2026 18:12:45 +0100 Subject: [PATCH 08/56] Add log display aliases --- dockerfiles/configs/bashrc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/dockerfiles/configs/bashrc b/dockerfiles/configs/bashrc index b401216..521ab31 100644 --- a/dockerfiles/configs/bashrc +++ b/dockerfiles/configs/bashrc @@ -14,3 +14,10 @@ alias mp="loco" alias echocmd="ros2 topic echo /booster/ros2_k2_joint_cmd" alias echostate="ros2 topic echo /booster/ros2_k2_joint_states" alias echolow="ros2 topic echo /low_state" + +alias bmlog="cat /var/log/booster-motion.log" +alias bmerr="cat /var/log/booster-motion.err" +alias maxlog="cat /var/log/maximus.log" +alias maxerr="cat /var/log/maximus.err" +alias brilog="cat /var/log/simbridge.log" +alias brierr="cat /var/log/simbridge.err" From 5c87af8a4a6ac862d0d7c5cb36a4e37a5846954e Mon Sep 17 00:00:00 2001 From: Flavio Date: Wed, 11 Feb 2026 00:31:43 +0100 Subject: [PATCH 09/56] moved the timer of 6 seconds outside the bridge This is due to the fact that the bridge probably will run also on the robot. On the robot we are not interested on the timer, it is useful just for the simulation. So moving it outside the bridge guarantees an easier integration of the bridge in the real robot --- dockerfiles/Dockerfile | 4 ++++ dockerfiles/booster.conf | 25 ++++++++++++++++++++++--- dockerfiles/delayed_start.sh | 9 +++++++++ 3 files changed, 35 insertions(+), 3 deletions(-) create mode 100644 dockerfiles/delayed_start.sh diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile index fc4b90b..7fa1851 100644 --- a/dockerfiles/Dockerfile +++ b/dockerfiles/Dockerfile @@ -61,7 +61,11 @@ RUN mkdir /app COPY entrypoint.sh /app RUN chmod +x /app/entrypoint.sh +COPY delayed_start.sh /app +RUN chmod +x /app/delayed_start.sh + # used to test communication, can be removed at a later date if so desired RUN apt install -y iputils-ping netcat net-tools + CMD ["/app/entrypoint.sh"] diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index e8d0dd3..0860ec2 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -1,18 +1,37 @@ +[unix_http_server] +file=/var/run/supervisor.sock + +[supervisorctl] +serverurl=unix:///var/run/supervisor.sock + +[rpcinterface:supervisor] +supervisor.rpcinterface_factory = supervisor.rpcinterface:make_main_rpcinterface + [supervisord] nodaemon=true [program:booster-motion] directory=/app/booster_motion -command=/app/booster_motion/booster-motion -mode sim -config ./configs/config_isaac.lua # DON'T CHANGE +command=/app/booster_motion/booster-motion -mode sim -config ./configs/config_isaac.lua autostart=true autorestart=true +priority=1 stdout_logfile=/var/log/booster-motion.log stderr_logfile=/var/log/booster-motion.err +[program:delayed-starter] +command=/app/delayed_start.sh +autostart=true +autorestart=false +priority=2 +startsecs=0 +stdout_logfile=/var/log/delayed-starter.log +stderr_logfile=/var/log/delayed-starter.err + [program:simbridge] directory=/app/bridge command=/app/bridge/simbridge -autostart=true +autostart=false autorestart=true stdout_logfile=/var/log/simbridge.log stderr_logfile=/var/log/simbridge.err @@ -20,7 +39,7 @@ stderr_logfile=/var/log/simbridge.err [program:maximus] directory=/app/maximus command=/app/maximus/bin/maximus_main -autostart=true +autostart=false autorestart=true stdout_logfile=/var/log/maximus.log stderr_logfile=/var/log/maximus.err \ No newline at end of file diff --git a/dockerfiles/delayed_start.sh b/dockerfiles/delayed_start.sh new file mode 100644 index 0000000..e57e1fe --- /dev/null +++ b/dockerfiles/delayed_start.sh @@ -0,0 +1,9 @@ +#!/bin/bash +# Wait for 5 seconds +sleep 6 + +# Start simbridge and maximus in parallel +# This substitute the timer of 5 seconds inside the bridge. +# In this way the bridge (or maximus) does not need to have the +# check on the timer if the code is run on the robot or on simulator +supervisorctl -c /etc/supervisor/conf.d/booster.conf start simbridge maximus \ No newline at end of file From 729aa48d318ad76cb887b0f4377ded1965f4f76b Mon Sep 17 00:00:00 2001 From: Flavio Date: Sat, 14 Feb 2026 13:15:59 +0100 Subject: [PATCH 10/56] increased socket buffer size + TCP-like sneding message Adding just the ball_position to the message genereates a EAGAIN or EWOULDBLOCK error. So adding a tcp-like message when sending information helps to prevent this (on the other side of the socket, i.e. the simbridge), there is a corresponding receiving function that manage correctly this type of message --- include/RobotManager.h | 503 ++++++++++++++++++++++------------------- 1 file changed, 269 insertions(+), 234 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 24de599..4daa4d1 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -30,303 +30,338 @@ namespace spqr { struct Team; // Forward declaration class RobotManager { - public: - // Singleton class - static RobotManager& instance() { - static RobotManager mgr; - return mgr; + 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 update() { + std::lock_guard lock(mutex_); + for (std::shared_ptr r : robots_) { + r->update(); } - - 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_; + } + + void clear() { + std::lock_guard lock(mutex_); + for (std::shared_ptr r : robots_) { + // Drop ownership first + r->container.reset(); + r->team.reset(); } + robots_.clear(); + } - size_t count() const { - std::lock_guard lock(mutex_); - return robots_.size(); - } - - void update() { - std::lock_guard lock(mutex_); - for (std::shared_ptr r : robots_) { - r->update(); - } - } - - void clear() { - std::lock_guard lock(mutex_); - for (std::shared_ptr r : robots_) { - // Drop ownership first - r->container.reset(); - r->team.reset(); - } - robots_.clear(); - } - - void bindMujoco(MujocoContext* mujContext) { - for (std::shared_ptr r : robots_) - r->bindMujoco(mujContext); - } + void bindMujoco(MujocoContext* mujContext) { + for (std::shared_ptr r : robots_) + r->bindMujoco(mujContext); + } std::shared_ptr create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos, const Eigen::Vector3d& ori, std::tuple color, const std::shared_ptr team) { - auto it = robotFactory.find(type); - if (it != robotFactory.end()) + auto it = robotFactory.find(type); + if (it != robotFactory.end()) return it->second(name, type, number, pos, ori, color, team); - return nullptr; - } + return nullptr; + } - void startContainers() { - startCommunicationServer(frameworkCommunicationPort); + void startContainers() { + startCommunicationServer(frameworkCommunicationPort); - YAML::Node pathsRoot = loadYamlFile(pathsConfigPath); - YAML::Node configRoot = loadYamlFile(frameworkConfigPath); + YAML::Node pathsRoot = loadYamlFile(pathsConfigPath); + YAML::Node configRoot = loadYamlFile(frameworkConfigPath); - if (!configRoot["image"]) - throw std::runtime_error("Missing 'image' key in YAML file"); + if (!configRoot["image"]) + throw std::runtime_error("Missing 'image' key in YAML file"); - std::string image = tryString(configRoot["image"], "'image' must be a string: "); + std::string image = tryString(configRoot["image"], "'image' must be a string: "); - if (!configRoot["volumes"] || !configRoot["volumes"].IsSequence()) - throw std::runtime_error("'volumes' key missing or not a sequence"); + if (!configRoot["volumes"] || !configRoot["volumes"].IsSequence()) + throw std::runtime_error("'volumes' key missing or not a sequence"); - std::vector binds; - for (const auto& v : configRoot["volumes"]) { - std::string v2 = tryString(v, "Volume entry must be a string: "); - if (v2.starts_with("<")) { - int end = v2.find('>'); - std::string name = v2.substr(1, end - 1); + std::vector binds; + for (const auto& v : configRoot["volumes"]) { + std::string v2 = tryString(v, "Volume entry must be a string: "); + if (v2.starts_with("<")) { + int end = v2.find('>'); + std::string name = v2.substr(1, end - 1); - if (!pathsRoot[name]) { - throw std::runtime_error("Entry doesn't exist in path_constants: " + name); - } - - std::string name_str = tryString(pathsRoot[name], "path_constants entries must be strings: "); - v2.replace(0, end + 1, name_str); + if (!pathsRoot[name]) { + throw std::runtime_error("Entry doesn't exist in path_constants: " + name); } - binds.push_back(v2); - } - for (std::shared_ptr r : robots_) { - r->container = std::make_unique("CIRCUS_" + r->name + "_container"); - r->container->create(r->name, image, binds); - r->container->start(); + std::string name_str = tryString(pathsRoot[name], "path_constants entries must be strings: "); + v2.replace(0, end + 1, name_str); } + binds.push_back(v2); } - void startCommunicationServer(int port) { - if (serverRunning_) - throw std::runtime_error("Server already running"); - serverRunning_ = true; - serverThread_ = std::thread(&RobotManager::_serverInternal, this, port); + for (std::shared_ptr r : robots_) { + r->container = std::make_unique("CIRCUS_" + r->name + "_container"); + r->container->create(r->name, image, binds); + r->container->start(); } - - void stopCommunicationServer() { - if (!serverRunning_) - return; - - serverRunning_ = false; - - if (serverThread_.joinable()) - serverThread_.join(); + } + + void startCommunicationServer(int port) { + if (serverRunning_) + throw std::runtime_error("Server already running"); + serverRunning_ = true; + serverThread_ = std::thread(&RobotManager::_serverInternal, this, port); + } + + void stopCommunicationServer() { + if (!serverRunning_) + return; + + serverRunning_ = false; + + if (serverThread_.joinable()) + serverThread_.join(); + } + + void setAreAllRobotsReadyCallback(std::function cb) { + std::lock_guard lock(mutex_); + areAllRobotsReadyCallback_ = std::move(cb); + } + + private: + RobotManager() = default; + ~RobotManager() = default; + + RobotManager(const RobotManager&) = delete; + RobotManager& operator=(const RobotManager&) = delete; + + // Source - https://stackoverflow.com/a + // Posted by Arun, modified by community. See post 'Timeline' for change history + // Retrieved 2026-01-12, License - CC BY-SA 3.0 + // TCP Communication, it sends before the size of the message and then the message itself + ssize_t send_all(int fd, char *buf, size_t len) + { + // First, send the size of the message + uint32_t msg_size = htonl(len); + ssize_t bytes_sent = send(fd, &msg_size, sizeof(msg_size), 0); + if (bytes_sent != sizeof(msg_size)) { + return -1; } - void setAreAllRobotsReadyCallback(std::function cb) { - std::lock_guard lock(mutex_); - areAllRobotsReadyCallback_ = std::move(cb); + ssize_t total = 0; // how many bytes we've sent + size_t bytesleft = len; // how many we have left to send + ssize_t n = 0; + while(total < len) { + n = send(fd, buf+total, bytesleft, 0); + if (n == -1) { + /* print/log error details */ + return -1; + } + total += n; + bytesleft -= n; } - - private: - RobotManager() = default; - ~RobotManager() = default; - - RobotManager(const RobotManager&) = delete; - RobotManager& operator=(const RobotManager&) = delete; - - void _serverInternal(int port) { - int server_fd = socket(AF_INET, SOCK_STREAM, 0); - if (server_fd < 0) - throw std::runtime_error("Failed to create socket"); - - int opt = 1; - setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = INADDR_ANY; - address.sin_port = htons(port); - - if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) - throw std::runtime_error("Socket bind failed"); - if (listen(server_fd, robots_.size()) < 0) - throw std::runtime_error("Listen failed"); - - std::vector fds; - fds.push_back({server_fd, POLLIN, 0}); - - // Using a polling server. It isn't immediately intuitive, but it is efficient for this use case. - while (serverRunning_) { - // the poll blocks until a new connection arrives on server_fd or data arrives in one of the - // monitored fd or a socket closes or the timeout expires. - int ret = poll(fds.data(), fds.size(), 100); - if (ret <= 0) - continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is - // still true) - - for (size_t i = 0; i < fds.size(); ++i) { - // An event occured for the i-th fd - if (fds[i].revents & POLLIN) { - if (fds[i].fd == server_fd) { - // The only event for the server is someone knocking - int client_fd = accept(server_fd, nullptr, nullptr); - if (client_fd >= 0) { - fds.push_back({client_fd, POLLIN, 0}); - - // Receive initial message with robot name - char buffer[MAX_MSG_SIZE]; - int n = read(client_fd, buffer, sizeof(buffer) - 1); - - if (n <= 0) { - std::cerr << "Error reading the initial message.\n"; - // close(client_fd); - continue; - } - - // unpack of the MsgPack message - msgpack::object_handle oh = msgpack::unpack(buffer, n); - msgpack::object obj = oh.get(); - - // First message is the robot name as a string - if (obj.type != msgpack::type::STR) { - std::cerr << "First message must be a string. Ignore it...\n"; - continue; - } - - std::string robotName = obj.as(); - - // Send message with initial state - msgpack::sbuffer sbuf; - std::map answ; - bool answOk = false; - // Pack initial message - { - std::lock_guard lock(mutex_); - for (auto& r : robots_) { - if (r->name == robotName) { - r->isConnected = true; - answ = r->sendMessage(); - answOk = true; - break; - } - } - } - if (answOk) { - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - std::cout << "Connected Robot: " << robotName << "\n"; - std::cout << "Sending initial message to " << robotName << std::endl; - ssize_t bytes_sent = send(client_fd, sbuf.data(), sbuf.size(), 0); - if (bytes_sent <= 0) { - perror("Error in sending initial message"); - } - } - } - } - } else { - // The events for other fds indicate either an incoming message or a closed connection - // the read call disambiguates the two cases + return total; + } + + void _serverInternal(int port) { + int server_fd = socket(AF_INET, SOCK_STREAM, 0); + if (server_fd < 0) + throw std::runtime_error("Failed to create socket"); + + int opt = 1; + setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + int send_buf_size = 1 * 1024 * 1024; + if (setsockopt(server_fd, SOL_SOCKET, SO_SNDBUF, + &send_buf_size, sizeof(send_buf_size)) < 0) { + perror("setsockopt(SO_SNDBUF)"); + } + int recv_buf_size = 1 * 1024 * 1024; + if (setsockopt(server_fd, SOL_SOCKET, SO_RCVBUF, + &recv_buf_size, sizeof(recv_buf_size)) < 0) { + perror("setsockopt(SO_RCVBUF)"); + } + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(port); + + if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) + throw std::runtime_error("Socket bind failed"); + if (listen(server_fd, robots_.size()) < 0) + throw std::runtime_error("Listen failed"); + + std::vector fds; + fds.push_back({server_fd, POLLIN, 0}); + + // Using a polling server. It isn't immediately intuitive, but it is efficient for this use case. + while (serverRunning_) { + // the poll blocks until a new connection arrives on server_fd or data arrives in one of the + // monitored fd or a socket closes or the timeout expires. + int ret = poll(fds.data(), fds.size(), 100); + if (ret <= 0) + continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is + // still true) + + for (size_t i = 0; i < fds.size(); ++i) { + // An event occured for the i-th fd + if (fds[i].revents & POLLIN) { + if (fds[i].fd == server_fd) { + // The only event for the server is someone knocking + int client_fd = accept(server_fd, nullptr, nullptr); + if (client_fd >= 0) { + fds.push_back({client_fd, POLLIN, 0}); + + // Receive initial message with robot name char buffer[MAX_MSG_SIZE]; - int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); + int n = read(client_fd, buffer, sizeof(buffer) - 1); + if (n <= 0) { - close(fds[i].fd); - fds.erase(fds.begin() + i); - --i; + std::cerr << "Error reading the initial message.\n"; + // close(client_fd); continue; } + // unpack of the MsgPack message 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()) - continue; + msgpack::object obj = oh.get(); - std::string messageRecipient = it->second.as(); + // First message is the robot name as a string + if (obj.type != msgpack::type::STR) { + std::cerr << "First message must be a string. Ignore it...\n"; + continue; + } + std::string robotName = obj.as(); msgpack::sbuffer sbuf; std::map answ; bool answOk = false; + // Pack initial message { - std::unique_lock lock(mutex_); + std::lock_guard lock(mutex_); for (auto& r : robots_) { - if (r->name == messageRecipient) { - if (!r->isReady) { - r->isReady = true; - std::cout << "Robot ready: " << r->name << std::endl; - areAllRobotsReadyWrapper(); - } - r->receiveMessage(data_map); + if (r->name == robotName) { + r->isConnected = true; answ = r->sendMessage(); answOk = true; break; } } } - - if (answOk) { + if (answOk){ msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - ssize_t bytes_sent = send(fds[i].fd, sbuf.data(), sbuf.size(), 0); + if(sbuf.size() > 0) { + std::cout << "Connected Robot: " << robotName << "\n"; + std::cout << "Sending initial message to " << robotName << std::endl; + ssize_t bytes_sent = send_all(client_fd, sbuf.data(), sbuf.size()); if (bytes_sent <= 0) { - perror("Error in sending message"); + perror("Sending initial message"); } } } } - } - } - } + } else { + // The events for other fds indicate either an incoming message or a closed connection + // the read call disambiguates the two cases + char buffer[MAX_MSG_SIZE]; + int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); + if (n <= 0) { + close(fds[i].fd); + fds.erase(fds.begin() + i); + --i; + continue; + } - for (auto& fd : fds) - close(fd.fd); - } + 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()) + continue; + + std::string messageRecipient = it->second.as(); + msgpack::sbuffer sbuf; + std::map answ; + bool answOk = false; + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (r->name == messageRecipient) { + if (!r->isReady) { + r->isReady = true; + std::cout << "Robot ready: " << r->name << std::endl; + areAllRobotsReadyWrapper(); + } + r->receiveMessage(data_map); + answ = r->sendMessage(); + answOk = true; + break; + } + } + } - void areAllRobotsReadyWrapper() { - if (areAllRobotsReady() && areAllRobotsReadyCallback_) { - areAllRobotsReadyCallback_(); + if (answOk){ + msgpack::pack(sbuf, answ); + if(sbuf.size() > 0){ + ssize_t bytes_sent = send_all(fds[i].fd, sbuf.data(), sbuf.size()); + if (bytes_sent <= 0) { + perror("Sending message"); + } + } + } + } + } } } - bool areAllRobotsReady() const { - for (const auto& r : robots_) - if (!r->isReady) - return false; - std::cout << "All robots are ready!" << std::endl; - return true; - } - std::atomic serverRunning_ = false; - std::thread serverThread_; + for (auto& fd : fds) + close(fd.fd); + } - mutable std::mutex mutex_; - std::vector> robots_; - std::function areAllRobotsReadyCallback_; + void areAllRobotsReadyWrapper() { + if (areAllRobotsReady() && areAllRobotsReadyCallback_) { + areAllRobotsReadyCallback_(); + } + } + bool areAllRobotsReady() const { + for (const auto& r : robots_) + if (!r->isReady) + return false; + std::cout << "All robots are ready!" << std::endl; + return true; + } + + std::atomic serverRunning_ = false; + std::thread serverThread_; + + mutable std::mutex mutex_; + std::vector> robots_; + std::function areAllRobotsReadyCallback_; using RobotCreator = std::function(const std::string&, const std::string&, uint8_t, const Eigen::Vector3d&, const Eigen::Vector3d&, const std::tuple&, const std::shared_ptr&)>; - std::unordered_map robotFactory + std::unordered_map robotFactory = {{"Booster-K1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& color, auto&& team) { return std::make_shared(name, type, number, pos, ori, color, team); }}, {"Booster-T1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& color, auto&& team) { return std::make_shared(name, type, number, pos, ori, color, team); - }}}; + }}}; }; } // namespace spqr From da60fc427190f8b2a45fc0a6d92c906955780050 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 16 Feb 2026 13:47:55 +0100 Subject: [PATCH 11/56] Add YAML file output Didn't end up using it in the end, but why waste a good (?) function --- include/Utils.h | 3 +-- src/Utils.cpp | 13 +++++++++++++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/include/Utils.h b/include/Utils.h index 7fa6c6d..df9ca08 100644 --- a/include/Utils.h +++ b/include/Utils.h @@ -3,10 +3,9 @@ #include #include -// TODO creare Utils.cpp - namespace spqr { YAML::Node loadYamlFile(const char* path); +void writeYamlFile(const YAML::Node& root, const char* path); // untested probably // TODO templatizzare std::string tryString(YAML::Node node, std::string message); diff --git a/src/Utils.cpp b/src/Utils.cpp index 41803a5..3619d4b 100644 --- a/src/Utils.cpp +++ b/src/Utils.cpp @@ -1,4 +1,5 @@ #include "Utils.h" +#include namespace spqr { YAML::Node loadYamlFile(const char* path) { @@ -10,6 +11,18 @@ YAML::Node loadYamlFile(const char* path) { throw std::runtime_error("Failed to parse YAML file: " + std::string(e.what())); } } +void writeYamlFile(const YAML::Node& root, const char* path) { // untested probably + YAML::Emitter out; + out << root; + if (!out.good()) { + throw std::runtime_error("Failed to emit YAML: " + out.GetLastError()); + } + std::ofstream ofout(path); + ofout << out.c_str(); + if (!ofout.good()) { + throw std::runtime_error("Failed to write YAML file."); + } +} // TODO templatizzare std::string tryString(YAML::Node node, std::string message) { From 6b36a52caa7499f490bc9bb4f34e96d14de79825 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 16 Feb 2026 13:52:50 +0100 Subject: [PATCH 12/56] Move color name-to-rgb conversion, add colorName So the robot can save its color name for sending to the framework --- include/RobotManager.h | 19 +++++++++++-------- include/robots/BoosterK1.h | 4 ++-- include/robots/BoosterT1.h | 4 ++-- include/robots/Robot.h | 13 +++++++++++-- src/SceneParser.cpp | 9 +-------- 5 files changed, 27 insertions(+), 22 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 24de599..bf6001f 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -76,10 +76,10 @@ class RobotManager { } std::shared_ptr create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos, - const Eigen::Vector3d& ori, std::tuple color, const std::shared_ptr team) { + const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team) { auto it = robotFactory.find(type); if (it != robotFactory.end()) - return it->second(name, type, number, pos, ori, color, team); + return it->second(name, type, number, pos, ori, colorName, team); return nullptr; } @@ -319,14 +319,17 @@ class RobotManager { using RobotCreator = std::function(const std::string&, const std::string&, uint8_t, const Eigen::Vector3d&, const Eigen::Vector3d&, - const std::tuple&, const std::shared_ptr&)>; + const std::string&, const std::shared_ptr&)>; std::unordered_map robotFactory - = {{"Booster-K1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& color, - auto&& team) { return std::make_shared(name, type, number, pos, ori, color, team); }}, - {"Booster-T1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& color, auto&& team) { - return std::make_shared(name, type, number, pos, ori, color, team); - }}}; + = { + {"Booster-K1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& colorName, auto&& team) { + return std::make_shared(name, type, number, pos, ori, colorName, team); + }}, + {"Booster-T1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& colorName, auto&& team) { + return std::make_shared(name, type, number, pos, ori, colorName, team); + }} + }; }; } // namespace spqr diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 86fa51c..715c456 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -35,8 +35,8 @@ class BoosterK1 : public Robot { std::array cameras = {}; BoosterK1(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, - const Eigen::Vector3d& initOrientation, const std::tuple color, const std::shared_ptr& team) - : Robot(name, type, number, initPosition, initOrientation, color, team), + const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team) + : Robot(name, type, number, initPosition, initOrientation, colorName, team), joint_map{{JointValue::HEAD_YAW, name + "_AAHead_yaw"}, {JointValue::HEAD_PITCH, name + "_Head_pitch"}, {JointValue::SHOULDER_LEFT_PITCH, name + "_ALeft_Shoulder_Pitch"}, diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index a641568..6c0fc41 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -35,8 +35,8 @@ class BoosterT1 : public Robot { std::array cameras = {}; BoosterT1(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, - const Eigen::Vector3d& initOrientation, const std::tuple color, const std::shared_ptr& team) - : Robot(name, type, number, initPosition, initOrientation, color, team), + const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team) + : Robot(name, type, number, initPosition, initOrientation, colorName, team), joint_map{{JointValue::HEAD_YAW, name + "_AAHead_yaw"}, {JointValue::HEAD_PITCH, name + "_Head_pitch"}, {JointValue::SHOULDER_LEFT_PITCH, name + "_Left_Shoulder_Pitch"}, diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 24a6093..dfdcd6a 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -26,8 +26,16 @@ struct Team; // Forward declaration class Robot { public: Robot(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, - const Eigen::Vector3d& initOrientation, const std::tuple color, const std::shared_ptr& team) - : name(name), type(type), number(number), initPosition(initPosition), initOrientation(initOrientation), color(color), team(team) {} + const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team) + : name(name), type(type), number(number), initPosition(initPosition), initOrientation(initOrientation), colorName(colorName), team(team) { + if (colorName == "red") { + color = {130, 36, 51}; + } else if (colorName == "blue") { + color = {0, 103, 120}; + } else { + throw std::runtime_error("Team color currently unsupported: " + colorName); + } + } virtual ~Robot() = default; virtual void bindMujoco(MujocoContext* mujContext) = 0; virtual void update() = 0; @@ -40,6 +48,7 @@ class Robot { uint8_t number; Eigen::Vector3d initPosition; Eigen::Vector3d initOrientation; // Euler angles + std::string colorName; std::tuple color; std::unique_ptr container; std::shared_ptr team; diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index 6fea1a1..236b62b 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -120,14 +120,7 @@ SceneParser::SceneParser(const string& yamlPath) { ori[i] = robotNode["orientation"][i].as(); } - std::tuple teamColor = {255, 255, 255}; // Default to white - if (teamName == "red") { - teamColor = {130, 36, 51}; - } else if (teamName == "blue") { - teamColor = {0, 103, 120}; - } - - shared_ptr robot = RobotManager::instance().create(robotName, robotType, robotNumber, pos, ori, teamColor, teamSpec); + shared_ptr robot = RobotManager::instance().create(robotName, robotType, robotNumber, pos, ori, teamName, teamSpec); robotTypes.insert(robotType); teamSpec->robots.push_back(std::move(robot)); From 85979e748a8b3511aa46305ae3c570e812a85dd2 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 16 Feb 2026 13:54:51 +0100 Subject: [PATCH 13/56] Add team number --- include/Team.h | 1 + resources/scenes/1v1.yaml | 4 ++++ src/SceneParser.cpp | 6 +++++- 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/include/Team.h b/include/Team.h index 6f49838..9c66eac 100644 --- a/include/Team.h +++ b/include/Team.h @@ -10,6 +10,7 @@ namespace spqr { struct Team { std::string name; + int number; std::vector> robots; }; diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index 1150980..8e91e04 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -1,11 +1,15 @@ simulation_config: default teams: red: + number: 8 + players: - type: Booster-T1 number: 3 position: [-1.0, 0.0, 0.68] orientation: [0.0, 0.0, 0.0] blue: + number: 51 + players: - type: Booster-T1 number: 3 position: [1.0, 1.0, 0.68] diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index 236b62b..c9afd14 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -89,13 +89,17 @@ SceneParser::SceneParser(const string& yamlPath) { for (const auto& team : teamsNode) { const string& teamName = team.first.as(); - const YAML::Node& robotsNode = team.second; + const YAML::Node& theTeamNode = team.second; + + const int& teamNumber = theTeamNode["number"].as(); + const YAML::Node& robotsNode = theTeamNode["players"]; if (!robotsNode.IsSequence()) throw runtime_error("Each team must be a sequence of robots."); shared_ptr teamSpec = std::make_shared(); teamSpec->name = teamName; + teamSpec->number = teamNumber; uint8_t typeIndex = 0; for (const YAML::Node& robotNode : robotsNode) { From 9d3fffd5c8aa3eea1a82866e358e698fc9622c89 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 16 Feb 2026 14:07:52 +0100 Subject: [PATCH 14/56] Create implementation file for RobotManager Needed to make use of the forward declaration of Team in the next commit. --- include/RobotManager.h | 273 ++------------------------------------- src/RobotManager.cpp | 281 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 295 insertions(+), 259 deletions(-) create mode 100644 src/RobotManager.cpp diff --git a/include/RobotManager.h b/include/RobotManager.h index bf6001f..3a6a58a 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -17,9 +17,7 @@ #include #include -#include "Constants.h" #include "MujocoContext.h" -#include "Utils.h" #include "robots/BoosterK1.h" #include "robots/BoosterT1.h" #include "robots/Robot.h" @@ -37,111 +35,23 @@ class RobotManager { return mgr; } - void registerRobot(std::shared_ptr robot) { - std::lock_guard lock(mutex_); + void registerRobot(std::shared_ptr robot); + std::vector> getRobots() const; + size_t count() const; + void update(); + void clear(); - 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 update() { - std::lock_guard lock(mutex_); - for (std::shared_ptr r : robots_) { - r->update(); - } - } - - void clear() { - std::lock_guard lock(mutex_); - for (std::shared_ptr r : robots_) { - // Drop ownership first - r->container.reset(); - r->team.reset(); - } - robots_.clear(); - } - - void bindMujoco(MujocoContext* mujContext) { - for (std::shared_ptr r : robots_) - r->bindMujoco(mujContext); - } + void bindMujoco(MujocoContext* mujContext); std::shared_ptr create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos, - const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team) { - auto it = robotFactory.find(type); - if (it != robotFactory.end()) - return it->second(name, type, number, pos, ori, colorName, team); - return nullptr; - } + const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team); - void startContainers() { - startCommunicationServer(frameworkCommunicationPort); + void startContainers(); - YAML::Node pathsRoot = loadYamlFile(pathsConfigPath); - YAML::Node configRoot = loadYamlFile(frameworkConfigPath); + void startCommunicationServer(int port); + void stopCommunicationServer(); - if (!configRoot["image"]) - throw std::runtime_error("Missing 'image' key in YAML file"); - - std::string image = tryString(configRoot["image"], "'image' must be a string: "); - - if (!configRoot["volumes"] || !configRoot["volumes"].IsSequence()) - throw std::runtime_error("'volumes' key missing or not a sequence"); - - std::vector binds; - for (const auto& v : configRoot["volumes"]) { - std::string v2 = tryString(v, "Volume entry must be a string: "); - if (v2.starts_with("<")) { - int end = v2.find('>'); - std::string name = v2.substr(1, end - 1); - - if (!pathsRoot[name]) { - throw std::runtime_error("Entry doesn't exist in path_constants: " + name); - } - - std::string name_str = tryString(pathsRoot[name], "path_constants entries must be strings: "); - v2.replace(0, end + 1, name_str); - } - binds.push_back(v2); - } - - for (std::shared_ptr r : robots_) { - r->container = std::make_unique("CIRCUS_" + r->name + "_container"); - r->container->create(r->name, image, binds); - r->container->start(); - } - } - - void startCommunicationServer(int port) { - if (serverRunning_) - throw std::runtime_error("Server already running"); - serverRunning_ = true; - serverThread_ = std::thread(&RobotManager::_serverInternal, this, port); - } - - void stopCommunicationServer() { - if (!serverRunning_) - return; - - serverRunning_ = false; - - if (serverThread_.joinable()) - serverThread_.join(); - } - - void setAreAllRobotsReadyCallback(std::function cb) { - std::lock_guard lock(mutex_); - areAllRobotsReadyCallback_ = std::move(cb); - } + void setAreAllRobotsReadyCallback(std::function cb); private: RobotManager() = default; @@ -150,165 +60,10 @@ class RobotManager { RobotManager(const RobotManager&) = delete; RobotManager& operator=(const RobotManager&) = delete; - void _serverInternal(int port) { - int server_fd = socket(AF_INET, SOCK_STREAM, 0); - if (server_fd < 0) - throw std::runtime_error("Failed to create socket"); - - int opt = 1; - setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = INADDR_ANY; - address.sin_port = htons(port); - - if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) - throw std::runtime_error("Socket bind failed"); - if (listen(server_fd, robots_.size()) < 0) - throw std::runtime_error("Listen failed"); - - std::vector fds; - fds.push_back({server_fd, POLLIN, 0}); - - // Using a polling server. It isn't immediately intuitive, but it is efficient for this use case. - while (serverRunning_) { - // the poll blocks until a new connection arrives on server_fd or data arrives in one of the - // monitored fd or a socket closes or the timeout expires. - int ret = poll(fds.data(), fds.size(), 100); - if (ret <= 0) - continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is - // still true) - - for (size_t i = 0; i < fds.size(); ++i) { - // An event occured for the i-th fd - if (fds[i].revents & POLLIN) { - if (fds[i].fd == server_fd) { - // The only event for the server is someone knocking - int client_fd = accept(server_fd, nullptr, nullptr); - if (client_fd >= 0) { - fds.push_back({client_fd, POLLIN, 0}); - - // Receive initial message with robot name - char buffer[MAX_MSG_SIZE]; - int n = read(client_fd, buffer, sizeof(buffer) - 1); - - if (n <= 0) { - std::cerr << "Error reading the initial message.\n"; - // close(client_fd); - continue; - } - - // unpack of the MsgPack message - msgpack::object_handle oh = msgpack::unpack(buffer, n); - msgpack::object obj = oh.get(); + void _serverInternal(int port); - // First message is the robot name as a string - if (obj.type != msgpack::type::STR) { - std::cerr << "First message must be a string. Ignore it...\n"; - continue; - } - - std::string robotName = obj.as(); - - // Send message with initial state - msgpack::sbuffer sbuf; - std::map answ; - bool answOk = false; - // Pack initial message - { - std::lock_guard lock(mutex_); - for (auto& r : robots_) { - if (r->name == robotName) { - r->isConnected = true; - answ = r->sendMessage(); - answOk = true; - break; - } - } - } - if (answOk) { - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - std::cout << "Connected Robot: " << robotName << "\n"; - std::cout << "Sending initial message to " << robotName << std::endl; - ssize_t bytes_sent = send(client_fd, sbuf.data(), sbuf.size(), 0); - if (bytes_sent <= 0) { - perror("Error in sending initial message"); - } - } - } - } - } else { - // The events for other fds indicate either an incoming message or a closed connection - // the read call disambiguates the two cases - char buffer[MAX_MSG_SIZE]; - int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); - if (n <= 0) { - close(fds[i].fd); - fds.erase(fds.begin() + i); - --i; - continue; - } - - 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()) - continue; - - std::string messageRecipient = it->second.as(); - - msgpack::sbuffer sbuf; - std::map answ; - bool answOk = false; - { - std::unique_lock lock(mutex_); - for (auto& r : robots_) { - if (r->name == messageRecipient) { - if (!r->isReady) { - r->isReady = true; - std::cout << "Robot ready: " << r->name << std::endl; - areAllRobotsReadyWrapper(); - } - r->receiveMessage(data_map); - answ = r->sendMessage(); - answOk = true; - break; - } - } - } - - if (answOk) { - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - ssize_t bytes_sent = send(fds[i].fd, sbuf.data(), sbuf.size(), 0); - if (bytes_sent <= 0) { - perror("Error in sending message"); - } - } - } - } - } - } - } - - for (auto& fd : fds) - close(fd.fd); - } - - void areAllRobotsReadyWrapper() { - if (areAllRobotsReady() && areAllRobotsReadyCallback_) { - areAllRobotsReadyCallback_(); - } - } - bool areAllRobotsReady() const { - for (const auto& r : robots_) - if (!r->isReady) - return false; - std::cout << "All robots are ready!" << std::endl; - return true; - } + void areAllRobotsReadyWrapper(); + bool areAllRobotsReady() const; std::atomic serverRunning_ = false; std::thread serverThread_; diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp new file mode 100644 index 0000000..12aeff7 --- /dev/null +++ b/src/RobotManager.cpp @@ -0,0 +1,281 @@ +#include "RobotManager.h" + +#include "Team.h" // needed for the forward declaration in the .h +#include "Constants.h" +#include "Utils.h" + + +namespace spqr { + +void RobotManager::registerRobot(std::shared_ptr robot) { + std::lock_guard lock(mutex_); + + robots_.push_back(std::move(robot)); +} + +std::vector> RobotManager::getRobots() const { + std::lock_guard lock(mutex_); + return robots_; +} + +size_t RobotManager::count() const { + std::lock_guard lock(mutex_); + return robots_.size(); +} + +void RobotManager::update() { + std::lock_guard lock(mutex_); + for (std::shared_ptr r : robots_) { + r->update(); + } +} + +void RobotManager::clear() { + std::lock_guard lock(mutex_); + for (std::shared_ptr r : robots_) { + // Drop ownership first + r->container.reset(); + r->team.reset(); + } + robots_.clear(); +} + +void RobotManager::bindMujoco(MujocoContext* mujContext) { + for (std::shared_ptr r : robots_) + r->bindMujoco(mujContext); +} + +std::shared_ptr RobotManager::create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos, + const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team) { + auto it = robotFactory.find(type); + if (it != robotFactory.end()) + return it->second(name, type, number, pos, ori, colorName, team); + return nullptr; +} + +void RobotManager::startContainers() { + startCommunicationServer(frameworkCommunicationPort); + + YAML::Node pathsRoot = loadYamlFile(pathsConfigPath); + YAML::Node configRoot = loadYamlFile(frameworkConfigPath); + + if (!configRoot["image"]) + throw std::runtime_error("Missing 'image' key in YAML file"); + + std::string image = tryString(configRoot["image"], "'image' must be a string: "); + + if (!configRoot["volumes"] || !configRoot["volumes"].IsSequence()) + throw std::runtime_error("'volumes' key missing or not a sequence"); + + std::vector binds; + for (const auto& v : configRoot["volumes"]) { + std::string v2 = tryString(v, "Volume entry must be a string: "); + if (v2.starts_with("<")) { + int end = v2.find('>'); + std::string name = v2.substr(1, end - 1); + + if (!pathsRoot[name]) { + throw std::runtime_error("Entry doesn't exist in path_constants: " + name); + } + + std::string name_str = tryString(pathsRoot[name], "path_constants entries must be strings: "); + v2.replace(0, end + 1, name_str); + } + binds.push_back(v2); + } + + for (std::shared_ptr r : robots_) { + r->container = std::make_unique("CIRCUS_" + r->name + "_container"); + // r->container->create(r->name, r->team->number, r->number, r->colorName, image, binds); // this is for the next commit, don't mind it for now + r->container->create(r->name, image, binds); + r->container->start(); + } +} + +void RobotManager::startCommunicationServer(int port) { + if (serverRunning_) + throw std::runtime_error("Server already running"); + serverRunning_ = true; + serverThread_ = std::thread(&RobotManager::_serverInternal, this, port); +} + +void RobotManager::stopCommunicationServer() { + if (!serverRunning_) + return; + + serverRunning_ = false; + + if (serverThread_.joinable()) + serverThread_.join(); +} + + + + +void RobotManager::_serverInternal(int port) { + int server_fd = socket(AF_INET, SOCK_STREAM, 0); + if (server_fd < 0) + throw std::runtime_error("Failed to create socket"); + + int opt = 1; + setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(port); + + if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) + throw std::runtime_error("Socket bind failed"); + if (listen(server_fd, robots_.size()) < 0) + throw std::runtime_error("Listen failed"); + + std::vector fds; + fds.push_back({server_fd, POLLIN, 0}); + + // Using a polling server. It isn't immediately intuitive, but it is efficient for this use case. + while (serverRunning_) { + // the poll blocks until a new connection arrives on server_fd or data arrives in one of the + // monitored fd or a socket closes or the timeout expires. + int ret = poll(fds.data(), fds.size(), 100); + if (ret <= 0) + continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is + // still true) + + for (size_t i = 0; i < fds.size(); ++i) { + // An event occured for the i-th fd + if (fds[i].revents & POLLIN) { + if (fds[i].fd == server_fd) { + // The only event for the server is someone knocking + int client_fd = accept(server_fd, nullptr, nullptr); + if (client_fd >= 0) { + fds.push_back({client_fd, POLLIN, 0}); + + // Receive initial message with robot name + char buffer[MAX_MSG_SIZE]; + int n = read(client_fd, buffer, sizeof(buffer) - 1); + + if (n <= 0) { + std::cerr << "Error reading the initial message.\n"; + // close(client_fd); + continue; + } + + // unpack of the MsgPack message + msgpack::object_handle oh = msgpack::unpack(buffer, n); + msgpack::object obj = oh.get(); + + // First message is the robot name as a string + if (obj.type != msgpack::type::STR) { + std::cerr << "First message must be a string. Ignore it...\n"; + continue; + } + + std::string robotName = obj.as(); + + // Send message with initial state + msgpack::sbuffer sbuf; + std::map answ; + bool answOk = false; + // Pack initial message + { + std::lock_guard lock(mutex_); + for (auto& r : robots_) { + if (r->name == robotName) { + r->isConnected = true; + answ = r->sendMessage(); + answOk = true; + break; + } + } + } + if (answOk) { + msgpack::pack(sbuf, answ); + if (sbuf.size() > 0) { + std::cout << "Connected Robot: " << robotName << "\n"; + std::cout << "Sending initial message to " << robotName << std::endl; + ssize_t bytes_sent = send(client_fd, sbuf.data(), sbuf.size(), 0); + if (bytes_sent <= 0) { + perror("Error in sending initial message"); + } + } + } + } + } else { + // The events for other fds indicate either an incoming message or a closed connection + // the read call disambiguates the two cases + char buffer[MAX_MSG_SIZE]; + int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); + if (n <= 0) { + close(fds[i].fd); + fds.erase(fds.begin() + i); + --i; + continue; + } + + 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()) + continue; + + std::string messageRecipient = it->second.as(); + + msgpack::sbuffer sbuf; + std::map answ; + bool answOk = false; + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (r->name == messageRecipient) { + if (!r->isReady) { + r->isReady = true; + std::cout << "Robot ready: " << r->name << std::endl; + areAllRobotsReadyWrapper(); + } + r->receiveMessage(data_map); + answ = r->sendMessage(); + answOk = true; + break; + } + } + } + + if (answOk) { + msgpack::pack(sbuf, answ); + if (sbuf.size() > 0) { + ssize_t bytes_sent = send(fds[i].fd, sbuf.data(), sbuf.size(), 0); + if (bytes_sent <= 0) { + perror("Error in sending message"); + } + } + } + } + } + } + } + + for (auto& fd : fds) + close(fd.fd); +} + + + +void RobotManager::setAreAllRobotsReadyCallback(std::function cb) { + std::lock_guard lock(mutex_); + areAllRobotsReadyCallback_ = std::move(cb); +} +void RobotManager::areAllRobotsReadyWrapper() { + if (areAllRobotsReady() && areAllRobotsReadyCallback_) { + areAllRobotsReadyCallback_(); + } +} +bool RobotManager::areAllRobotsReady() const { + for (const auto& r : robots_) + if (!r->isReady) + return false; + std::cout << "All robots are ready!" << std::endl; + return true; +} + +} \ No newline at end of file From 710f94bea4beb7c5f8b5bde8754274488290eff0 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 16 Feb 2026 14:12:37 +0100 Subject: [PATCH 15/56] Send team number and player number to framework Via environment vars --- include/Container.h | 2 +- include/RobotManager.h | 1 - src/Container.cpp | 22 ++++++++++++++++++++-- src/RobotManager.cpp | 3 +-- 4 files changed, 22 insertions(+), 6 deletions(-) diff --git a/include/Container.h b/include/Container.h index 1a0860e..35b769b 100644 --- a/include/Container.h +++ b/include/Container.h @@ -14,7 +14,7 @@ class Container { Container(const std::string& name, const std::string& sockPath = "/var/run/docker.sock"); ~Container(); - void create(const std::string& robot_name, const std::string& image, const std::vector& binds); + void create(const std::string& robot_name, const int& team_number, const int& player_number, const std::string& team_color, const std::string& image, const std::vector& binds); void start(); void stop(); diff --git a/include/RobotManager.h b/include/RobotManager.h index 3a6a58a..e17532a 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -12,7 +12,6 @@ #include #include #include -#include #include #include #include diff --git a/src/Container.cpp b/src/Container.cpp index 2156239..d762dbb 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -1,9 +1,15 @@ #include "Container.h" #include +#include #include "Constants.h" +#include "Utils.h" +#include "robots/Robot.h" +#include +#include + #define POST "POST" #define GET "GET" #define DELETE "DELETE" @@ -63,7 +69,7 @@ Container::~Container() { curl_easy_cleanup(curl_handle); } -void Container::create(const std::string& robot_name, const std::string& image, const std::vector& binds) { +void Container::create(const std::string& robot_name, const int& team_number, const int& player_number, const std::string& team_color, const std::string& image, const std::vector& binds) { nlohmann::json payload; payload["Image"] = image; @@ -74,7 +80,14 @@ void Container::create(const std::string& robot_name, const std::string& image, {"Ulimits", nlohmann::json::array({{{"Name", "memlock"}, {"Soft", -1}, {"Hard", -1}}})}, {"Privileged", true}}; - payload["Env"] = {"ROBOT_NAME=" + robot_name, "SERVER_IP=172.17.0.1", "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort)}; + payload["Env"] = { + "ROBOT_NAME=" + robot_name, + "SERVER_IP=172.17.0.1", + "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort), + "TEAM_NUMBER=" + std::to_string(team_number), + "PLAYER_NUMBER=" + std::to_string(player_number), + "TEAM_COLOR=" + team_color + }; payload["Tty"] = true; payload["OpenStdin"] = true; @@ -88,6 +101,11 @@ void Container::create(const std::string& robot_name, const std::string& image, id = resp["Id"]; state = ContainerState::IDLE; + + // YAML::Node settingsRoot = loadYamlFile("/home/francesco/SPQRBOOSTER2026/FRAMEWORK/spqrbooster2026/src/app/config/settings.yaml"); + // settingsRoot["teamNumber"] = 51; + // settingsRoot["playerNumber"] = 3; + // // TODO write a tar } void Container::start() { diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 12aeff7..147017d 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -86,8 +86,7 @@ void RobotManager::startContainers() { for (std::shared_ptr r : robots_) { r->container = std::make_unique("CIRCUS_" + r->name + "_container"); - // r->container->create(r->name, r->team->number, r->number, r->colorName, image, binds); // this is for the next commit, don't mind it for now - r->container->create(r->name, image, binds); + r->container->create(r->name, r->team->number, r->number, r->colorName, image, binds); // this is for the next commit, don't mind it for now r->container->start(); } } From c3086ae55b46d0aae5cb38a63aeb19dcbfa61d27 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 16 Feb 2026 14:18:49 +0100 Subject: [PATCH 16/56] Make container.create prettier Had to solve one forward decaration earlier, why not another one --- include/Container.h | 5 ++++- src/Container.cpp | 15 +++++++++------ src/RobotManager.cpp | 2 +- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/include/Container.h b/include/Container.h index 35b769b..804b831 100644 --- a/include/Container.h +++ b/include/Container.h @@ -1,4 +1,5 @@ #pragma once +#include #include #include #include @@ -7,6 +8,8 @@ namespace spqr { +class Robot; // forwards declaration + enum class ContainerState { NONE, IDLE, RUNNING, REMOVED }; class Container { public: @@ -14,7 +17,7 @@ class Container { Container(const std::string& name, const std::string& sockPath = "/var/run/docker.sock"); ~Container(); - void create(const std::string& robot_name, const int& team_number, const int& player_number, const std::string& team_color, const std::string& image, const std::vector& binds); + void create(const std::shared_ptr& robot, const std::string& image, const std::vector& binds); void start(); void stop(); diff --git a/src/Container.cpp b/src/Container.cpp index d762dbb..c9bcad3 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -5,11 +5,14 @@ #include "Constants.h" -#include "Utils.h" #include "robots/Robot.h" #include #include +// for the forward declarations +#include "robots/Robot.h" +#include "Team.h" + #define POST "POST" #define GET "GET" #define DELETE "DELETE" @@ -69,7 +72,7 @@ Container::~Container() { curl_easy_cleanup(curl_handle); } -void Container::create(const std::string& robot_name, const int& team_number, const int& player_number, const std::string& team_color, const std::string& image, const std::vector& binds) { +void Container::create(const std::shared_ptr& robot, const std::string& image, const std::vector& binds) { nlohmann::json payload; payload["Image"] = image; @@ -81,12 +84,12 @@ void Container::create(const std::string& robot_name, const int& team_number, co {"Privileged", true}}; payload["Env"] = { - "ROBOT_NAME=" + robot_name, + "ROBOT_NAME=" + robot->name, "SERVER_IP=172.17.0.1", "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort), - "TEAM_NUMBER=" + std::to_string(team_number), - "PLAYER_NUMBER=" + std::to_string(player_number), - "TEAM_COLOR=" + team_color + "TEAM_NUMBER=" + std::to_string(robot->team->number), + "PLAYER_NUMBER=" + std::to_string(robot->number), + "TEAM_COLOR=" + robot->colorName }; payload["Tty"] = true; diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 147017d..3bd4c83 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -86,7 +86,7 @@ void RobotManager::startContainers() { for (std::shared_ptr r : robots_) { r->container = std::make_unique("CIRCUS_" + r->name + "_container"); - r->container->create(r->name, r->team->number, r->number, r->colorName, image, binds); // this is for the next commit, don't mind it for now + r->container->create(r, image, binds); // this is for the next commit, don't mind it for now r->container->start(); } } From b8c9c609ad7cabe7229332ae1aad2d5d8b05ae25 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 16 Feb 2026 14:32:29 +0100 Subject: [PATCH 17/56] Update 5v5 scene to new format --- resources/scenes/5v5.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/resources/scenes/5v5.yaml b/resources/scenes/5v5.yaml index 31a2ff7..78672a8 100644 --- a/resources/scenes/5v5.yaml +++ b/resources/scenes/5v5.yaml @@ -1,6 +1,8 @@ simulation_config: default teams: red: + number: 8 + players: - type: Booster-T1 number: 1 position: [-7.0, 0.0, 0.68] @@ -23,6 +25,8 @@ teams: orientation: [0.0, 0.0, 0.0] blue: + number: 51 + players: - type: Booster-T1 number: 1 position: [7.0, 0.0, 0.68] From f37d73b0191a6aa16297dcfaf0ce3f760677ad7f Mon Sep 17 00:00:00 2001 From: Flavio Date: Mon, 16 Feb 2026 18:19:17 +0100 Subject: [PATCH 18/56] change of booster.conf and framework_config to work with the new bridge (REMEMBER TO BUILD THE SIMBRIDGE) --- dockerfiles/booster.conf | 2 +- resources/config/framework_config.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index 0860ec2..bbb5588 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -30,7 +30,7 @@ stderr_logfile=/var/log/delayed-starter.err [program:simbridge] directory=/app/bridge -command=/app/bridge/simbridge +command=/app/bridge/bin/simbridge_node autostart=false autorestart=true stdout_logfile=/var/log/simbridge.log diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 153b671..61f2600 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -3,7 +3,7 @@ image: spqr:booster volumes: - "/tools/booster_motion:/app/booster_motion" - - "/build:/app/bridge" + - "/.pixi/envs/default:/app/bridge" - "/.pixi/envs/default:/app/maximus" - "/src/app/config:/app/maximus/config" - "/src/app/behaviors/trees:/app/maximus/behaviors" From 1a42fe0bf994b706cb9b8e69df83ac214ad945ac Mon Sep 17 00:00:00 2001 From: Flavio Date: Mon, 16 Feb 2026 18:20:04 +0100 Subject: [PATCH 19/56] work with new simbridge in branch "main"(REMEMBER TO BUILD SIMBRIDGE) --- dockerfiles/booster.conf | 2 +- resources/config/framework_config.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index 0860ec2..bbb5588 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -30,7 +30,7 @@ stderr_logfile=/var/log/delayed-starter.err [program:simbridge] directory=/app/bridge -command=/app/bridge/simbridge +command=/app/bridge/bin/simbridge_node autostart=false autorestart=true stdout_logfile=/var/log/simbridge.log diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 153b671..61f2600 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -3,7 +3,7 @@ image: spqr:booster volumes: - "/tools/booster_motion:/app/booster_motion" - - "/build:/app/bridge" + - "/.pixi/envs/default:/app/bridge" - "/.pixi/envs/default:/app/maximus" - "/src/app/config:/app/maximus/config" - "/src/app/behaviors/trees:/app/maximus/behaviors" From a89a5141b731fe9ae8056f76047d4c09224e42c6 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Tue, 17 Feb 2026 12:10:03 +0100 Subject: [PATCH 20/56] Move Docker REST API to its own file Paving the way for what's coming next (Team using the same code) --- include/Container.h | 8 +--- include/DockerREST.h | 103 +++++++++++++++++++++++++++++++++++++++++++ src/Container.cpp | 97 +++------------------------------------- src/RobotManager.cpp | 2 +- 4 files changed, 111 insertions(+), 99 deletions(-) create mode 100644 include/DockerREST.h diff --git a/include/Container.h b/include/Container.h index 804b831..9bd454e 100644 --- a/include/Container.h +++ b/include/Container.h @@ -4,7 +4,7 @@ #include #include -#include "curl/curl.h" +#include "DockerREST.h" namespace spqr { @@ -28,14 +28,10 @@ class Container { } private: - std::string request(const std::string& method, const std::string& endpoint, const long expected_response, - const nlohmann::json* body = nullptr); - std::string id; ContainerState state; std::string name; - std::string sockPath; - CURL* curl_handle; + CURLClient curlClient; }; } // namespace spqr diff --git a/include/DockerREST.h b/include/DockerREST.h new file mode 100644 index 0000000..51cad00 --- /dev/null +++ b/include/DockerREST.h @@ -0,0 +1,103 @@ +#pragma once + +#include +#include +#include +#include +#include "curl/curl.h" + +#define POST "POST" +#define GET "GET" +#define DELETE "DELETE" +#define PUT "PUT" + +/* +Endpoints and parameters: +https://docs.docker.com/reference/api/engine/version/v1.39/ +*/ + +#define CREATE_OK_RESPONSE 201 +#define START_OK_RESPONSE 204 +#define STOP_OK_RESPONSE 204 +#define DELETE_OK_RESPONSE 204 + +namespace spqr { + +inline std::string create_container_endpoint(const std::string& name) { + return "/containers/create?name=" + name; +} + +inline std::string start_container_endpoint(const std::string& id) { + return "/containers/" + id + "/start"; +} + +inline std::string stop_container_endpoint(const std::string& id) { + return "/containers/" + id + "/stop?t=0"; // TODO: forcing a SIGKILL trigger to 0 seconds as workaround. If the application + // is well behaved, this shouldn't be necessary +} + +inline std::string remove_container_endpoint(const std::string& id) { + return "/containers/" + id; +} + +class CURLClient { + public: + CURLClient(const std::string& sockPath) : sockPath(sockPath) { + curl_handle = curl_easy_init(); + if (!curl_handle) + throw std::runtime_error("Failed to init curl handle"); + } + ~CURLClient() { + if (curl_handle) + curl_easy_cleanup(curl_handle); + std::cout << "A CURLClient was destroyed!" << std::endl; + }; + + std::string request(const std::string& method, const std::string& endpoint, const long expected_response, const nlohmann::json* body = nullptr) { + curl_easy_reset(curl_handle); + std::string url = "http://localhost" + endpoint; + + curl_easy_setopt(curl_handle, CURLOPT_UNIX_SOCKET_PATH, sockPath.c_str()); + curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str()); + curl_easy_setopt(curl_handle, CURLOPT_CUSTOMREQUEST, method.c_str()); + + struct curl_slist* headers = nullptr; + headers = curl_slist_append(headers, "Content-Type: application/json"); + curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, headers); + + const auto write_callback = +[](char* ptr, size_t size, size_t nmemb, void* userdata) -> size_t { + std::string* resp = static_cast(userdata); + resp->append(ptr, size * nmemb); + return size * nmemb; + }; + + curl_easy_setopt(curl_handle, CURLOPT_WRITEFUNCTION, write_callback); + std::string response; + curl_easy_setopt(curl_handle, CURLOPT_WRITEDATA, &response); + + std::string postData; + if (body != nullptr) { + postData = body->dump(); + curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, postData.c_str()); + curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDSIZE, postData.size()); + } + + long response_code = 0; + CURLcode res = curl_easy_perform(curl_handle); + curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &response_code); + curl_slist_free_all(headers); + + if (res != CURLE_OK) + throw std::runtime_error(std::string("Curl error: ") + curl_easy_strerror(res)); + + if (expected_response && response_code != expected_response) + throw std::runtime_error("Docker API request to " + endpoint + " failed: HTTP " + std::to_string(response_code) + ". " + response); + + return response; + } + + CURL* curl_handle; + std::string sockPath; +}; + +} diff --git a/src/Container.cpp b/src/Container.cpp index c9bcad3..b288cf1 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -13,45 +13,9 @@ #include "robots/Robot.h" #include "Team.h" -#define POST "POST" -#define GET "GET" -#define DELETE "DELETE" -#define PUT "PUT" - -/* -Endpoints and parameters: -https://docs.docker.com/reference/api/engine/version/v1.39/ -*/ - -#define CREATE_OK_RESPONSE 201 -#define START_OK_RESPONSE 204 -#define STOP_OK_RESPONSE 204 -#define DELETE_OK_RESPONSE 204 - namespace spqr { -inline std::string create_container_endpoint(const std::string& name) { - return "/containers/create?name=" + name; -} - -inline std::string start_container_endpoint(const std::string& id) { - return "/containers/" + id + "/start"; -} - -inline std::string stop_container_endpoint(const std::string& id) { - return "/containers/" + id + "/stop?t=0"; // TODO: forcing a SIGKILL trigger to 0 seconds as workaround. If the application - // is well behaved, this shouldn't be necessary -} - -inline std::string remove_container_endpoint(const std::string& id) { - return "/containers/" + id; -} - -Container::Container(const std::string& name, const std::string& sockPath) : name(name), sockPath(sockPath), state(ContainerState::NONE) { - curl_handle = curl_easy_init(); - if (!curl_handle) - throw std::runtime_error("Failed to init curl handle"); -} +Container::Container(const std::string& name, const std::string& sockPath) : name(name), curlClient(sockPath), state(ContainerState::NONE) {} Container::~Container() { switch (state) { @@ -67,9 +31,6 @@ Container::~Container() { case ContainerState::NONE: break; } - - if (curl_handle) - curl_easy_cleanup(curl_handle); } void Container::create(const std::shared_ptr& robot, const std::string& image, const std::vector& binds) { @@ -96,7 +57,7 @@ void Container::create(const std::shared_ptr& robot, const std::string& i payload["OpenStdin"] = true; std::string endpoint = create_container_endpoint(name); - std::string resp_raw = request(POST, endpoint, CREATE_OK_RESPONSE, &payload); + std::string resp_raw = curlClient.request(POST, endpoint, CREATE_OK_RESPONSE, &payload); nlohmann::json resp = nlohmann::json::parse(resp_raw); if (!resp.contains("Id")) @@ -104,11 +65,6 @@ void Container::create(const std::shared_ptr& robot, const std::string& i id = resp["Id"]; state = ContainerState::IDLE; - - // YAML::Node settingsRoot = loadYamlFile("/home/francesco/SPQRBOOSTER2026/FRAMEWORK/spqrbooster2026/src/app/config/settings.yaml"); - // settingsRoot["teamNumber"] = 51; - // settingsRoot["playerNumber"] = 3; - // // TODO write a tar } void Container::start() { @@ -116,7 +72,7 @@ void Container::start() { throw std::runtime_error("Failed to start container " + name + " which is not IDLE."); const std::string endpoint = start_container_endpoint(id); - request(POST, endpoint, START_OK_RESPONSE); + curlClient.request(POST, endpoint, START_OK_RESPONSE); state = ContainerState::RUNNING; } @@ -125,7 +81,7 @@ void Container::stop() { throw std::runtime_error("Failed to stop container " + name + " which is not RUNNING."); const std::string endpoint = stop_container_endpoint(id); - request(POST, endpoint, 0); + curlClient.request(POST, endpoint, 0); state = ContainerState::IDLE; } @@ -134,51 +90,8 @@ void Container::remove() { throw std::runtime_error("Failed to remove container " + name + " which is not IDLE."); const std::string endpoint = remove_container_endpoint(id); - request(DELETE, endpoint, DELETE_OK_RESPONSE); + curlClient.request(DELETE, endpoint, DELETE_OK_RESPONSE); state = ContainerState::REMOVED; } -std::string Container::request(const std::string& method, const std::string& endpoint, const long expected_response, const nlohmann::json* body) { - curl_easy_reset(curl_handle); - std::string url = "http://localhost" + endpoint; - - curl_easy_setopt(curl_handle, CURLOPT_UNIX_SOCKET_PATH, sockPath.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_CUSTOMREQUEST, method.c_str()); - - struct curl_slist* headers = nullptr; - headers = curl_slist_append(headers, "Content-Type: application/json"); - curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, headers); - - const auto write_callback = +[](char* ptr, size_t size, size_t nmemb, void* userdata) -> size_t { - std::string* resp = static_cast(userdata); - resp->append(ptr, size * nmemb); - return size * nmemb; - }; - - curl_easy_setopt(curl_handle, CURLOPT_WRITEFUNCTION, write_callback); - std::string response; - curl_easy_setopt(curl_handle, CURLOPT_WRITEDATA, &response); - - std::string postData; - if (body != nullptr) { - postData = body->dump(); - curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, postData.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDSIZE, postData.size()); - } - - long response_code = 0; - CURLcode res = curl_easy_perform(curl_handle); - curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &response_code); - curl_slist_free_all(headers); - - if (res != CURLE_OK) - throw std::runtime_error(std::string("Curl error: ") + curl_easy_strerror(res)); - - if (expected_response && response_code != expected_response) - throw std::runtime_error("Docker API request to " + endpoint + " failed: HTTP " + std::to_string(response_code) + ". " + response); - - return response; -} - } // namespace spqr diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 3bd4c83..0c85639 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -86,7 +86,7 @@ void RobotManager::startContainers() { for (std::shared_ptr r : robots_) { r->container = std::make_unique("CIRCUS_" + r->name + "_container"); - r->container->create(r, image, binds); // this is for the next commit, don't mind it for now + r->container->create(r, image, binds); r->container->start(); } } From c94d987f1a7324b290bc1f4c560ebf2ff41d3ad0 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Tue, 17 Feb 2026 12:18:18 +0100 Subject: [PATCH 21/56] Turn Team into a class Still paving the way for the future (team subnets) --- include/RobotManager.h | 2 +- include/Team.h | 6 +++++- include/robots/BoosterK1.h | 2 +- include/robots/BoosterT1.h | 2 +- include/robots/Robot.h | 2 +- src/SceneParser.cpp | 4 +--- 6 files changed, 10 insertions(+), 8 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index e17532a..9807253 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -24,7 +24,7 @@ #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { -struct Team; // Forward declaration +class Team; // Forward declaration class RobotManager { public: diff --git a/include/Team.h b/include/Team.h index 9c66eac..fd1b1ef 100644 --- a/include/Team.h +++ b/include/Team.h @@ -8,7 +8,11 @@ namespace spqr { -struct Team { +class Team { + public: + Team(const std::string& name, int number) : name(name), number(number) {} + ~Team() {} + std::string name; int number; std::vector> robots; diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 715c456..403c4e5 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -25,7 +25,7 @@ #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { -struct Team; // Forward declaration +class Team; // Forward declaration class BoosterK1 : public Robot { public: diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 6c0fc41..8905381 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -25,7 +25,7 @@ #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { -struct Team; // Forward declaration +class Team; // Forward declaration class BoosterT1 : public Robot { public: diff --git a/include/robots/Robot.h b/include/robots/Robot.h index dfdcd6a..9cfcb52 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -21,7 +21,7 @@ #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { -struct Team; // Forward declaration +class Team; // Forward declaration class Robot { public: diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index c9afd14..f81eb9f 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -97,9 +97,7 @@ SceneParser::SceneParser(const string& yamlPath) { if (!robotsNode.IsSequence()) throw runtime_error("Each team must be a sequence of robots."); - shared_ptr teamSpec = std::make_shared(); - teamSpec->name = teamName; - teamSpec->number = teamNumber; + shared_ptr teamSpec = std::make_shared(teamName, teamNumber); uint8_t typeIndex = 0; for (const YAML::Node& robotNode : robotsNode) { From 9208aec6e574b970b4a91925cdfe37cf91c2d448 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Tue, 17 Feb 2026 12:26:15 +0100 Subject: [PATCH 22/56] Rename cleaning command and include networks --- clean-containers.sh => clean-docker.sh | 12 ++++++++++-- pixi.toml | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) rename clean-containers.sh => clean-docker.sh (63%) diff --git a/clean-containers.sh b/clean-docker.sh similarity index 63% rename from clean-containers.sh rename to clean-docker.sh index 023e5df..30231ae 100644 --- a/clean-containers.sh +++ b/clean-docker.sh @@ -6,7 +6,7 @@ if [ -n "$RUNNING_IDS" ]; then echo "Stopping circus containers" docker stop $RUNNING_IDS else - echo "Nothing to stop" + echo "No containers to stop" fi EXITED_IDS=$(docker ps --filter name="CIRCUS_*" -aq) @@ -14,5 +14,13 @@ if [ -n "$EXITED_IDS" ]; then echo "Removing circus containers" docker rm $EXITED_IDS else - echo "Nothing to remove" + echo "No containers to remove" +fi + +NETWORK_IDS=$(docker network list --filter name="CIRCUS_*" -q) +if [ -n "$NETWORK_IDS" ]; then + echo "Removing circus networks" + docker network rm $NETWORK_IDS +else + echo "No networks to remove" fi diff --git a/pixi.toml b/pixi.toml index 0625b16..67f6579 100644 --- a/pixi.toml +++ b/pixi.toml @@ -42,7 +42,7 @@ libegl-devel = "==1.7.0" [tasks] test = "python -c 'import circuspy; circuspy.test()'" -clean-containers = "bash clean-containers.sh" +clean-docker = "bash clean-docker.sh" [activation] # Add DYLD_LIBRARY_PATH on macOS so the MuJoCo dylib is found when running binaries directly From 4bff84a783d29913a2376ea0de6a9ea194cdb9e9 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Tue, 17 Feb 2026 12:28:27 +0100 Subject: [PATCH 23/56] Fix indent --- include/robots/Robot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 9cfcb52..5494e64 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -34,7 +34,7 @@ class Robot { color = {0, 103, 120}; } else { throw std::runtime_error("Team color currently unsupported: " + colorName); - } + } } virtual ~Robot() = default; virtual void bindMujoco(MujocoContext* mujContext) = 0; From df3dd410b791e7acc15949e722b3575a7605482e Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Tue, 17 Feb 2026 14:36:12 +0100 Subject: [PATCH 24/56] Add two /24 team subnets Most of this will be reverted in the enxt commit b/c I actually needed a single /16 net, but I'm not deleting all that I self-learned about docker nets just like that Still, this is completely stable btw --- include/Container.h | 9 ++++++++ include/DockerREST.h | 16 +++++++++++++++ include/Team.h | 37 +++++++++++++++++++++++++++++++-- src/AppWindow.cpp | 1 + src/Container.cpp | 45 ++++++++++++++++++++++++++++++++++++++++ src/Team.cpp | 49 ++++++++++++++++++++++++++++++++++++++++++++ 6 files changed, 155 insertions(+), 2 deletions(-) create mode 100644 src/Team.cpp diff --git a/include/Container.h b/include/Container.h index 9bd454e..016ef6d 100644 --- a/include/Container.h +++ b/include/Container.h @@ -9,6 +9,7 @@ namespace spqr { class Robot; // forwards declaration +class Team; // Forward declaration enum class ContainerState { NONE, IDLE, RUNNING, REMOVED }; class Container { @@ -23,6 +24,13 @@ class Container { void stop(); void remove(); + void connect(std::shared_ptr team, int robotNumber); + void disconnect(); + + inline bool isConnected() { + return connected_subnet_id.size() > 0; + } + std::string getId() const { return id; } @@ -31,6 +39,7 @@ class Container { std::string id; ContainerState state; std::string name; + std::string connected_subnet_id = ""; CURLClient curlClient; }; diff --git a/include/DockerREST.h b/include/DockerREST.h index 51cad00..f6827ef 100644 --- a/include/DockerREST.h +++ b/include/DockerREST.h @@ -20,6 +20,8 @@ Endpoints and parameters: #define START_OK_RESPONSE 204 #define STOP_OK_RESPONSE 204 #define DELETE_OK_RESPONSE 204 +#define CONNECT_OK_RESPONSE 200 +#define DISCONNECT_OK_RESPONSE 200 namespace spqr { @@ -40,6 +42,20 @@ inline std::string remove_container_endpoint(const std::string& id) { return "/containers/" + id; } +const std::string create_network_endpoint = "/networks/create"; + +inline std::string connect_network_endpoint(const std::string& id) { + return "/networks/" + id + "/connect"; +} + +inline std::string disconnect_network_endpoint(const std::string& id) { + return "/networks/" + id + "/disconnect"; +} + +inline std::string remove_network_endpoint(const std::string& id) { + return "/networks/" + id; +} + class CURLClient { public: CURLClient(const std::string& sockPath) : sockPath(sockPath) { diff --git a/include/Team.h b/include/Team.h index fd1b1ef..2bc09f5 100644 --- a/include/Team.h +++ b/include/Team.h @@ -5,17 +5,38 @@ #include "RobotManager.h" #include "robots/Robot.h" +#include "DockerREST.h" + +#define UAN_SEVEN_CIU "172.18." // TEMP, non so ancora se voglio pestare i piedi alla rete 172.17 o se separarmi sulla 172.18 namespace spqr { class Team { public: - Team(const std::string& name, int number) : name(name), number(number) {} - ~Team() {} + Team(const std::string& name, int number, const std::string& sockPath = "/var/run/docker.sock") : name(name), number(number), curlClient(sockPath) {} + + ~Team() { + if (has_subnet()) { + remove_subnet(); + } + } + + inline std::string subnet_name() { // TODO togliere se usato solo una volta + return "CIRCUS_" + name + "_network"; + } + inline bool has_subnet() { + return subnetId.size() > 0; + } + + void create_subnet(); + void remove_subnet(); + std::string name; int number; std::vector> robots; + CURLClient curlClient; + std::string subnetId = ""; }; class TeamManager { @@ -55,6 +76,18 @@ class TeamManager { teams_.clear(); } + void createSubnets() { + for (std::shared_ptr team : teams_) { + team->create_subnet(); + if (team->robots.size() == 0) { + std::cout << "Warning: the team is empty on 'connecting to subnets' time. It shouldn't at this step." << std::endl; + } + for (std::shared_ptr r : team->robots) { + r->container->connect(team, r->number); + } + } + } + private: TeamManager() = default; ~TeamManager() = default; diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index cf089f2..0b5b650 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -210,6 +210,7 @@ void AppWindow::loadScene(const QString& yaml_file) { }); RobotManager::instance().startContainers(); + TeamManager::instance().createSubnets(); RobotManager::instance().bindMujoco(mujContext.get()); // Set initial simulation state (playing when scene is loaded) diff --git a/src/Container.cpp b/src/Container.cpp index b288cf1..c950984 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -1,6 +1,9 @@ #include "Container.h" #include +#include +#include +#include #include #include "Constants.h" @@ -18,6 +21,9 @@ namespace spqr { Container::Container(const std::string& name, const std::string& sockPath) : name(name), curlClient(sockPath), state(ContainerState::NONE) {} Container::~Container() { + if (isConnected()) { + disconnect(); + } switch (state) { case ContainerState::RUNNING: stop(); @@ -94,4 +100,43 @@ void Container::remove() { state = ContainerState::REMOVED; } +void Container::connect(std::shared_ptr team, int robotNumber) { + if (!team->has_subnet()) { + throw std::runtime_error("Attempted to connect a robot to a team's subnet when it has none."); + } + if (isConnected()) { + throw std::runtime_error("Already connected."); + } + + nlohmann::json payload; + payload["Container"] = id; + payload["EndpointConfig"] = { + {"IPAddress", UAN_SEVEN_CIU + std::to_string(team->number) + "." + std::to_string(robotNumber + 10)}, + {"Gateway", UAN_SEVEN_CIU + std::to_string(team->number) + ".1"}, + {"IPPrefixLen", 16}, + {"IPAMConfig", { + {"IPv4Address", UAN_SEVEN_CIU + std::to_string(team->number) + "." + std::to_string(robotNumber + 10)} + }} + }; + + const std::string endpoint = connect_network_endpoint(team->subnetId); + curlClient.request(POST, endpoint, CONNECT_OK_RESPONSE, &payload); + + connected_subnet_id = team->subnetId; +} + +void Container::disconnect() { + if (!isConnected()) { + throw std::runtime_error("Already disconnected."); + } + + nlohmann::json payload; + payload["Container"] = id; + + const std::string endpoint = disconnect_network_endpoint(connected_subnet_id); + curlClient.request(POST, endpoint, DISCONNECT_OK_RESPONSE, &payload); + + connected_subnet_id = ""; +} + } // namespace spqr diff --git a/src/Team.cpp b/src/Team.cpp new file mode 100644 index 0000000..9222656 --- /dev/null +++ b/src/Team.cpp @@ -0,0 +1,49 @@ +#include "Team.h" +#include + +#include +#include +#include "DockerREST.h" + +namespace spqr { + +void Team::create_subnet() { + if (has_subnet()) { + throw std::runtime_error("Attempted to create a subnet when one already exists for this team."); + } + + nlohmann::json payload; + std::string subnet = UAN_SEVEN_CIU + std::to_string(number) + ".0/24"; + std::string gateway = UAN_SEVEN_CIU + std::to_string(number) + ".1"; + payload["Name"] = subnet_name(); + payload["IPAM"]["Config"] = {{ // https://docs.docker.com/reference/api/engine/version/v1.53/#tag/Network/operation/NetworkCreate + {"Subnet", subnet}, + {"IPRange", subnet}, + {"Gateway", gateway} + }}; + // payload["Options"] = { // una di queste opzioni rompe la rete, studiare con cautela in caso ci servissero! + // {"com.docker.network.bridge.default_bridge", "true"}, + // {"com.docker.network.bridge.enable_icc", "true"}, + // {"com.docker.network.bridge.enable_ip_masquerade", "true"}, + // {"com.docker.network.bridge.host_binding_ipv4", "0.0.0.0"}, + // // {"com.docker.network.bridge.name", "docker0"}, + // {"com.docker.network.driver.mtu", "1500"} + // }; + + std::string resp_raw = curlClient.request(POST, create_network_endpoint, CREATE_OK_RESPONSE, &payload); + + nlohmann::json resp = nlohmann::json::parse(resp_raw); + if (!resp.contains("Id")) + throw std::runtime_error("Docker subnet create failed"); + + subnetId = resp["Id"]; +} + +void Team::remove_subnet() { + if (!has_subnet()) { + throw std::runtime_error("Attempted to remove this team's subnet when it has none."); + } + curlClient.request(DELETE, remove_network_endpoint(subnetId), DELETE_OK_RESPONSE); +} + +} \ No newline at end of file From 3adcd16d4d272f90ef9fd4f44fd41cd71fbcd94d Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Tue, 17 Feb 2026 16:38:42 +0100 Subject: [PATCH 25/56] Switch to one /16 network Revert most of the previous commit and make a single network for all the containers to connect to with static IPs. GC is ok and teammates hear each other in 5v5. --- include/CircusNetwork.h | 77 +++++++++++++++++++++++++++++++++++++++++ include/Container.h | 9 ----- include/DockerREST.h | 1 - include/Team.h | 37 ++------------------ src/AppWindow.cpp | 3 +- src/Container.cpp | 72 +++++++++++--------------------------- src/Team.cpp | 49 -------------------------- 7 files changed, 102 insertions(+), 146 deletions(-) create mode 100644 include/CircusNetwork.h delete mode 100644 src/Team.cpp diff --git a/include/CircusNetwork.h b/include/CircusNetwork.h new file mode 100644 index 0000000..c0da201 --- /dev/null +++ b/include/CircusNetwork.h @@ -0,0 +1,77 @@ +#include +#include +#include "DockerREST.h" + +#define CIRCUS_NETWORK_NAME "CIRCUS_network" +#define UAN_SEVEN_CIU "172.18." // Per coerenza + +namespace spqr { + +class CircusNetwork { + public: + // Singleton class + static CircusNetwork& instance() { + static CircusNetwork mgr; + return mgr; + } + + inline bool isUp() { + return networkId.size() > 0; + } + + std::string getId() { + if (!isUp()) { + throw std::runtime_error("Attempted to get the network ID when it's not initialized."); + } + return networkId; + } + + void init() { + if (isUp()) { + throw std::runtime_error("Attempted to initialize the network when it is already up."); + } + + nlohmann::json payload; + std::string subnet = UAN_SEVEN_CIU "0.0/16"; + std::string gateway = UAN_SEVEN_CIU "0.1"; + payload["Name"] = CIRCUS_NETWORK_NAME; + payload["IPAM"]["Config"] = {{ // https://docs.docker.com/reference/api/engine/version/v1.53/#tag/Network/operation/NetworkCreate + {"Subnet", subnet}, + {"IPRange", subnet}, + {"Gateway", gateway} + }}; + payload["Options"] = { + {"com.docker.network.bridge.name", "docker-circus"}, + {"com.docker.network.bridge.enable_ip_masquerade", "false"}, + {"com.docker.network.driver.mtu", "1500"}, + {"com.docker.network.container_iface_prefix", "circus"} + }; + + std::string resp_raw = curlClient.request(POST, create_network_endpoint, CREATE_OK_RESPONSE, &payload); + + nlohmann::json resp = nlohmann::json::parse(resp_raw); + if (!resp.contains("Id")) + throw std::runtime_error("Docker subnet create failed"); + + networkId = resp["Id"]; + } + + private: + CircusNetwork(const std::string& sockPath = "/var/run/docker.sock") : curlClient(sockPath) {}; + + ~CircusNetwork() { + if (isUp()) { + curlClient.request(DELETE, remove_network_endpoint(networkId), DELETE_OK_RESPONSE); + } + }; + + std::string networkId = ""; + CURLClient curlClient; + + public: + CircusNetwork(CircusNetwork const&) = delete; + void operator=(CircusNetwork const&) = delete; + +}; + +} // namespace spqr diff --git a/include/Container.h b/include/Container.h index 016ef6d..9bd454e 100644 --- a/include/Container.h +++ b/include/Container.h @@ -9,7 +9,6 @@ namespace spqr { class Robot; // forwards declaration -class Team; // Forward declaration enum class ContainerState { NONE, IDLE, RUNNING, REMOVED }; class Container { @@ -24,13 +23,6 @@ class Container { void stop(); void remove(); - void connect(std::shared_ptr team, int robotNumber); - void disconnect(); - - inline bool isConnected() { - return connected_subnet_id.size() > 0; - } - std::string getId() const { return id; } @@ -39,7 +31,6 @@ class Container { std::string id; ContainerState state; std::string name; - std::string connected_subnet_id = ""; CURLClient curlClient; }; diff --git a/include/DockerREST.h b/include/DockerREST.h index f6827ef..039ddbe 100644 --- a/include/DockerREST.h +++ b/include/DockerREST.h @@ -66,7 +66,6 @@ class CURLClient { ~CURLClient() { if (curl_handle) curl_easy_cleanup(curl_handle); - std::cout << "A CURLClient was destroyed!" << std::endl; }; std::string request(const std::string& method, const std::string& endpoint, const long expected_response, const nlohmann::json* body = nullptr) { diff --git a/include/Team.h b/include/Team.h index 2bc09f5..ef4e36f 100644 --- a/include/Team.h +++ b/include/Team.h @@ -5,38 +5,17 @@ #include "RobotManager.h" #include "robots/Robot.h" -#include "DockerREST.h" - -#define UAN_SEVEN_CIU "172.18." // TEMP, non so ancora se voglio pestare i piedi alla rete 172.17 o se separarmi sulla 172.18 namespace spqr { class Team { public: - Team(const std::string& name, int number, const std::string& sockPath = "/var/run/docker.sock") : name(name), number(number), curlClient(sockPath) {} - - ~Team() { - if (has_subnet()) { - remove_subnet(); - } - } - - inline std::string subnet_name() { // TODO togliere se usato solo una volta - return "CIRCUS_" + name + "_network"; - } - - inline bool has_subnet() { - return subnetId.size() > 0; - } - - void create_subnet(); - void remove_subnet(); + Team(const std::string& name, int number) : name(name), number(number) {} + ~Team() {} std::string name; int number; std::vector> robots; - CURLClient curlClient; - std::string subnetId = ""; }; class TeamManager { @@ -76,18 +55,6 @@ class TeamManager { teams_.clear(); } - void createSubnets() { - for (std::shared_ptr team : teams_) { - team->create_subnet(); - if (team->robots.size() == 0) { - std::cout << "Warning: the team is empty on 'connecting to subnets' time. It shouldn't at this step." << std::endl; - } - for (std::shared_ptr r : team->robots) { - r->container->connect(team, r->number); - } - } - } - private: TeamManager() = default; ~TeamManager() = default; diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 0b5b650..6b1ce37 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -17,6 +17,7 @@ #include #endif +#include "CircusNetwork.h" #include "Constants.h" #include "GameController.h" #include "MujocoContext.h" @@ -209,8 +210,8 @@ void AppWindow::loadScene(const QString& yaml_file) { Qt::QueuedConnection); }); + CircusNetwork::instance().init(); RobotManager::instance().startContainers(); - TeamManager::instance().createSubnets(); RobotManager::instance().bindMujoco(mujContext.get()); // Set initial simulation state (playing when scene is loaded) diff --git a/src/Container.cpp b/src/Container.cpp index c950984..3f66824 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -1,9 +1,6 @@ #include "Container.h" #include -#include -#include -#include #include #include "Constants.h" @@ -12,6 +9,8 @@ #include #include +#include "CircusNetwork.h" + // for the forward declarations #include "robots/Robot.h" #include "Team.h" @@ -21,9 +20,6 @@ namespace spqr { Container::Container(const std::string& name, const std::string& sockPath) : name(name), curlClient(sockPath), state(ContainerState::NONE) {} Container::~Container() { - if (isConnected()) { - disconnect(); - } switch (state) { case ContainerState::RUNNING: stop(); @@ -43,12 +39,25 @@ void Container::create(const std::shared_ptr& robot, const std::string& i nlohmann::json payload; payload["Image"] = image; - payload["HostConfig"] = {{"Binds", binds}, - {"IpcMode", "host"}, - {"CapAdd", {"SYS_NICE", "IPC_LOCK"}}, - {"SecurityOpt", {"seccomp=unconfined"}}, - {"Ulimits", nlohmann::json::array({{{"Name", "memlock"}, {"Soft", -1}, {"Hard", -1}}})}, - {"Privileged", true}}; + payload["HostConfig"] = { + {"Binds", binds}, + {"IpcMode", "host"}, + {"CapAdd", {"SYS_NICE", "IPC_LOCK"}}, + {"SecurityOpt", {"seccomp=unconfined"}}, + {"Ulimits", nlohmann::json::array({{{"Name", "memlock"}, {"Soft", -1}, {"Hard", -1}}})}, + {"Privileged", true}, + {"NetworkMode", CIRCUS_NETWORK_NAME} + }; + + payload["NetworkingConfig"] = { + {"EndpointsConfig", { + {CIRCUS_NETWORK_NAME, { + {"IPAMConfig", { + {"IPv4Address", UAN_SEVEN_CIU + std::to_string(robot->team->number) + "." + std::to_string(robot->number + 10)} + }} + }} + }} + }; payload["Env"] = { "ROBOT_NAME=" + robot->name, @@ -100,43 +109,4 @@ void Container::remove() { state = ContainerState::REMOVED; } -void Container::connect(std::shared_ptr team, int robotNumber) { - if (!team->has_subnet()) { - throw std::runtime_error("Attempted to connect a robot to a team's subnet when it has none."); - } - if (isConnected()) { - throw std::runtime_error("Already connected."); - } - - nlohmann::json payload; - payload["Container"] = id; - payload["EndpointConfig"] = { - {"IPAddress", UAN_SEVEN_CIU + std::to_string(team->number) + "." + std::to_string(robotNumber + 10)}, - {"Gateway", UAN_SEVEN_CIU + std::to_string(team->number) + ".1"}, - {"IPPrefixLen", 16}, - {"IPAMConfig", { - {"IPv4Address", UAN_SEVEN_CIU + std::to_string(team->number) + "." + std::to_string(robotNumber + 10)} - }} - }; - - const std::string endpoint = connect_network_endpoint(team->subnetId); - curlClient.request(POST, endpoint, CONNECT_OK_RESPONSE, &payload); - - connected_subnet_id = team->subnetId; -} - -void Container::disconnect() { - if (!isConnected()) { - throw std::runtime_error("Already disconnected."); - } - - nlohmann::json payload; - payload["Container"] = id; - - const std::string endpoint = disconnect_network_endpoint(connected_subnet_id); - curlClient.request(POST, endpoint, DISCONNECT_OK_RESPONSE, &payload); - - connected_subnet_id = ""; -} - } // namespace spqr diff --git a/src/Team.cpp b/src/Team.cpp deleted file mode 100644 index 9222656..0000000 --- a/src/Team.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include "Team.h" -#include - -#include -#include -#include "DockerREST.h" - -namespace spqr { - -void Team::create_subnet() { - if (has_subnet()) { - throw std::runtime_error("Attempted to create a subnet when one already exists for this team."); - } - - nlohmann::json payload; - std::string subnet = UAN_SEVEN_CIU + std::to_string(number) + ".0/24"; - std::string gateway = UAN_SEVEN_CIU + std::to_string(number) + ".1"; - payload["Name"] = subnet_name(); - payload["IPAM"]["Config"] = {{ // https://docs.docker.com/reference/api/engine/version/v1.53/#tag/Network/operation/NetworkCreate - {"Subnet", subnet}, - {"IPRange", subnet}, - {"Gateway", gateway} - }}; - // payload["Options"] = { // una di queste opzioni rompe la rete, studiare con cautela in caso ci servissero! - // {"com.docker.network.bridge.default_bridge", "true"}, - // {"com.docker.network.bridge.enable_icc", "true"}, - // {"com.docker.network.bridge.enable_ip_masquerade", "true"}, - // {"com.docker.network.bridge.host_binding_ipv4", "0.0.0.0"}, - // // {"com.docker.network.bridge.name", "docker0"}, - // {"com.docker.network.driver.mtu", "1500"} - // }; - - std::string resp_raw = curlClient.request(POST, create_network_endpoint, CREATE_OK_RESPONSE, &payload); - - nlohmann::json resp = nlohmann::json::parse(resp_raw); - if (!resp.contains("Id")) - throw std::runtime_error("Docker subnet create failed"); - - subnetId = resp["Id"]; -} - -void Team::remove_subnet() { - if (!has_subnet()) { - throw std::runtime_error("Attempted to remove this team's subnet when it has none."); - } - curlClient.request(DELETE, remove_network_endpoint(subnetId), DELETE_OK_RESPONSE); -} - -} \ No newline at end of file From 47deb94e309c789058dc088f41fff5b3a14baf9f Mon Sep 17 00:00:00 2001 From: michelebri Date: Thu, 19 Feb 2026 19:02:56 +0000 Subject: [PATCH 26/56] camera images --- include/RobotManager.h | 92 +++++++++++++++++++++----- include/robots/BoosterK1.h | 4 ++ include/robots/BoosterT1.h | 4 ++ resources/config/framework_config.yaml | 5 +- simbridge | 1 + 5 files changed, 86 insertions(+), 20 deletions(-) create mode 160000 simbridge diff --git a/include/RobotManager.h b/include/RobotManager.h index 24de599..6b1facf 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -8,6 +9,9 @@ #include #include +#include +#include +#include #include #include #include @@ -150,6 +154,68 @@ class RobotManager { RobotManager(const RobotManager&) = delete; RobotManager& operator=(const RobotManager&) = delete; + static bool readExact_(const int fd, void* dst, const size_t size) { + size_t total_read = 0; + auto* out = static_cast(dst); + + while (total_read < size) { + const ssize_t n = recv(fd, out + total_read, size - total_read, 0); + if (n == 0) + return false; + if (n < 0) { + if (errno == EINTR) + continue; + return false; + } + total_read += static_cast(n); + } + return true; + } + + static bool sendFramedMsgpack_(const int fd, const msgpack::sbuffer& payload) { + const auto payload_size = static_cast(payload.size()); + const uint32_t net_size = htonl(payload_size); + + size_t sent_total = 0; + const auto* size_ptr = reinterpret_cast(&net_size); + while (sent_total < sizeof(net_size)) { + const ssize_t n = send(fd, size_ptr + sent_total, sizeof(net_size) - sent_total, 0); + if (n <= 0) { + if (errno == EINTR) + continue; + return false; + } + sent_total += static_cast(n); + } + + sent_total = 0; + const auto* payload_ptr = reinterpret_cast(payload.data()); + while (sent_total < payload.size()) { + const ssize_t n = send(fd, payload_ptr + sent_total, payload.size() - sent_total, 0); + if (n <= 0) { + if (errno == EINTR) + continue; + return false; + } + sent_total += static_cast(n); + } + + return true; + } + + static bool recvFramedMsgpack_(const int fd, std::vector& payload) { + uint32_t net_size = 0; + if (!readExact_(fd, &net_size, sizeof(net_size))) + return false; + + const uint32_t payload_size = ntohl(net_size); + if ((payload_size == 0) || (payload_size > MAX_MSG_SIZE)) + return false; + + payload.resize(payload_size); + return readExact_(fd, payload.data(), payload_size); + } + void _serverInternal(int port) { int server_fd = socket(AF_INET, SOCK_STREAM, 0); if (server_fd < 0) @@ -189,18 +255,15 @@ class RobotManager { if (client_fd >= 0) { fds.push_back({client_fd, POLLIN, 0}); - // Receive initial message with robot name - char buffer[MAX_MSG_SIZE]; - int n = read(client_fd, buffer, sizeof(buffer) - 1); - - if (n <= 0) { + std::vector payload; + if (!recvFramedMsgpack_(client_fd, payload)) { std::cerr << "Error reading the initial message.\n"; - // close(client_fd); + close(client_fd); + fds.pop_back(); continue; } - // unpack of the MsgPack message - msgpack::object_handle oh = msgpack::unpack(buffer, n); + msgpack::object_handle oh = msgpack::unpack(payload.data(), payload.size()); msgpack::object obj = oh.get(); // First message is the robot name as a string @@ -232,8 +295,7 @@ class RobotManager { if (sbuf.size() > 0) { std::cout << "Connected Robot: " << robotName << "\n"; std::cout << "Sending initial message to " << robotName << std::endl; - ssize_t bytes_sent = send(client_fd, sbuf.data(), sbuf.size(), 0); - if (bytes_sent <= 0) { + if (!sendFramedMsgpack_(client_fd, sbuf)) { perror("Error in sending initial message"); } } @@ -242,16 +304,15 @@ class RobotManager { } else { // The events for other fds indicate either an incoming message or a closed connection // the read call disambiguates the two cases - char buffer[MAX_MSG_SIZE]; - int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); - if (n <= 0) { + std::vector payload; + if (!recvFramedMsgpack_(fds[i].fd, payload)) { close(fds[i].fd); fds.erase(fds.begin() + i); --i; continue; } - msgpack::object_handle oh = msgpack::unpack(buffer, n); + msgpack::object_handle oh = msgpack::unpack(payload.data(), payload.size()); auto data_map = oh.get().as>(); auto it = data_map.find("robot_name"); if (it == data_map.end()) @@ -282,8 +343,7 @@ class RobotManager { if (answOk) { msgpack::pack(sbuf, answ); if (sbuf.size() > 0) { - ssize_t bytes_sent = send(fds[i].fd, sbuf.data(), sbuf.size(), 0); - if (bytes_sent <= 0) { + if (!sendFramedMsgpack_(fds[i].fd, sbuf)) { perror("Error in sending message"); } } diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 86fa51c..6acc034 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -87,6 +87,10 @@ class BoosterK1 : public Robot { msg["pose"] = pose->serialize(buffer_zone_); msg["imu"] = imu->serialize(buffer_zone_); msg["joints"] = joints->serialize(buffer_zone_); + msg["camera_left"] = cameras[0]->serialize(buffer_zone_); + msg["camera_right"] = cameras[1]->serialize(buffer_zone_); + msg["camera_width"] = msgpack::object(cameras[0]->getWidth(), buffer_zone_); + msg["camera_height"] = msgpack::object(cameras[0]->getHeight(), buffer_zone_); return msg; } diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index a641568..9c0a6a4 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -124,6 +124,10 @@ class BoosterT1 : public Robot { msg["pose"] = pose->serialize(buffer_zone_); msg["imu"] = imu->serialize(buffer_zone_); msg["joints"] = joints->serialize(buffer_zone_); + msg["camera_left"] = cameras[0]->serialize(buffer_zone_); + msg["camera_right"] = cameras[1]->serialize(buffer_zone_); + msg["camera_width"] = msgpack::object(cameras[0]->getWidth(), buffer_zone_); + msg["camera_height"] = msgpack::object(cameras[0]->getHeight(), buffer_zone_); return msg; } diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 61f2600..93daa16 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -3,7 +3,4 @@ image: spqr:booster volumes: - "/tools/booster_motion:/app/booster_motion" - - "/.pixi/envs/default:/app/bridge" - - "/.pixi/envs/default:/app/maximus" - - "/src/app/config:/app/maximus/config" - - "/src/app/behaviors/trees:/app/maximus/behaviors" + - "/.pixi/envs/default:/app/bridge" \ No newline at end of file diff --git a/simbridge b/simbridge new file mode 160000 index 0000000..37190e7 --- /dev/null +++ b/simbridge @@ -0,0 +1 @@ +Subproject commit 37190e79cc4986dc11f972ab8c734dacf820a91c From e4d90775263ef5d24361d11bfcdebf2819a535ff Mon Sep 17 00:00:00 2001 From: michelebri Date: Fri, 20 Feb 2026 11:18:47 +0000 Subject: [PATCH 27/56] Revert to stable raw msgpack exchange baseline --- include/RobotManager.h | 92 +++++++------------------------------- include/robots/BoosterK1.h | 4 -- include/robots/BoosterT1.h | 4 -- 3 files changed, 16 insertions(+), 84 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 6b1facf..24de599 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -9,9 +8,6 @@ #include #include -#include -#include -#include #include #include #include @@ -154,68 +150,6 @@ class RobotManager { RobotManager(const RobotManager&) = delete; RobotManager& operator=(const RobotManager&) = delete; - static bool readExact_(const int fd, void* dst, const size_t size) { - size_t total_read = 0; - auto* out = static_cast(dst); - - while (total_read < size) { - const ssize_t n = recv(fd, out + total_read, size - total_read, 0); - if (n == 0) - return false; - if (n < 0) { - if (errno == EINTR) - continue; - return false; - } - total_read += static_cast(n); - } - return true; - } - - static bool sendFramedMsgpack_(const int fd, const msgpack::sbuffer& payload) { - const auto payload_size = static_cast(payload.size()); - const uint32_t net_size = htonl(payload_size); - - size_t sent_total = 0; - const auto* size_ptr = reinterpret_cast(&net_size); - while (sent_total < sizeof(net_size)) { - const ssize_t n = send(fd, size_ptr + sent_total, sizeof(net_size) - sent_total, 0); - if (n <= 0) { - if (errno == EINTR) - continue; - return false; - } - sent_total += static_cast(n); - } - - sent_total = 0; - const auto* payload_ptr = reinterpret_cast(payload.data()); - while (sent_total < payload.size()) { - const ssize_t n = send(fd, payload_ptr + sent_total, payload.size() - sent_total, 0); - if (n <= 0) { - if (errno == EINTR) - continue; - return false; - } - sent_total += static_cast(n); - } - - return true; - } - - static bool recvFramedMsgpack_(const int fd, std::vector& payload) { - uint32_t net_size = 0; - if (!readExact_(fd, &net_size, sizeof(net_size))) - return false; - - const uint32_t payload_size = ntohl(net_size); - if ((payload_size == 0) || (payload_size > MAX_MSG_SIZE)) - return false; - - payload.resize(payload_size); - return readExact_(fd, payload.data(), payload_size); - } - void _serverInternal(int port) { int server_fd = socket(AF_INET, SOCK_STREAM, 0); if (server_fd < 0) @@ -255,15 +189,18 @@ class RobotManager { if (client_fd >= 0) { fds.push_back({client_fd, POLLIN, 0}); - std::vector payload; - if (!recvFramedMsgpack_(client_fd, payload)) { + // Receive initial message with robot name + char buffer[MAX_MSG_SIZE]; + int n = read(client_fd, buffer, sizeof(buffer) - 1); + + if (n <= 0) { std::cerr << "Error reading the initial message.\n"; - close(client_fd); - fds.pop_back(); + // close(client_fd); continue; } - msgpack::object_handle oh = msgpack::unpack(payload.data(), payload.size()); + // unpack of the MsgPack message + msgpack::object_handle oh = msgpack::unpack(buffer, n); msgpack::object obj = oh.get(); // First message is the robot name as a string @@ -295,7 +232,8 @@ class RobotManager { if (sbuf.size() > 0) { std::cout << "Connected Robot: " << robotName << "\n"; std::cout << "Sending initial message to " << robotName << std::endl; - if (!sendFramedMsgpack_(client_fd, sbuf)) { + ssize_t bytes_sent = send(client_fd, sbuf.data(), sbuf.size(), 0); + if (bytes_sent <= 0) { perror("Error in sending initial message"); } } @@ -304,15 +242,16 @@ class RobotManager { } else { // The events for other fds indicate either an incoming message or a closed connection // the read call disambiguates the two cases - std::vector payload; - if (!recvFramedMsgpack_(fds[i].fd, payload)) { + char buffer[MAX_MSG_SIZE]; + int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); + if (n <= 0) { close(fds[i].fd); fds.erase(fds.begin() + i); --i; continue; } - msgpack::object_handle oh = msgpack::unpack(payload.data(), payload.size()); + 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()) @@ -343,7 +282,8 @@ class RobotManager { if (answOk) { msgpack::pack(sbuf, answ); if (sbuf.size() > 0) { - if (!sendFramedMsgpack_(fds[i].fd, sbuf)) { + ssize_t bytes_sent = send(fds[i].fd, sbuf.data(), sbuf.size(), 0); + if (bytes_sent <= 0) { perror("Error in sending message"); } } diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 6acc034..86fa51c 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -87,10 +87,6 @@ class BoosterK1 : public Robot { msg["pose"] = pose->serialize(buffer_zone_); msg["imu"] = imu->serialize(buffer_zone_); msg["joints"] = joints->serialize(buffer_zone_); - msg["camera_left"] = cameras[0]->serialize(buffer_zone_); - msg["camera_right"] = cameras[1]->serialize(buffer_zone_); - msg["camera_width"] = msgpack::object(cameras[0]->getWidth(), buffer_zone_); - msg["camera_height"] = msgpack::object(cameras[0]->getHeight(), buffer_zone_); return msg; } diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 9c0a6a4..a641568 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -124,10 +124,6 @@ class BoosterT1 : public Robot { msg["pose"] = pose->serialize(buffer_zone_); msg["imu"] = imu->serialize(buffer_zone_); msg["joints"] = joints->serialize(buffer_zone_); - msg["camera_left"] = cameras[0]->serialize(buffer_zone_); - msg["camera_right"] = cameras[1]->serialize(buffer_zone_); - msg["camera_width"] = msgpack::object(cameras[0]->getWidth(), buffer_zone_); - msg["camera_height"] = msgpack::object(cameras[0]->getHeight(), buffer_zone_); return msg; } From ecc7b3d7f562200f1f3ac8c5d06771fbed533a05 Mon Sep 17 00:00:00 2001 From: michelebri Date: Fri, 20 Feb 2026 13:28:51 +0000 Subject: [PATCH 28/56] images via shared memory --- dockerfiles/Dockerfile | 25 +++- include/robots/BoosterT1.h | 26 ++++- include/sensors/ImageSharedMemoryWriter.h | 132 ++++++++++++++++++++++ resources/config/framework_config.yaml | 3 +- simbridge | 1 - 5 files changed, 183 insertions(+), 4 deletions(-) create mode 100644 include/sensors/ImageSharedMemoryWriter.h delete mode 160000 simbridge diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile index 7fa1851..54733fc 100644 --- a/dockerfiles/Dockerfile +++ b/dockerfiles/Dockerfile @@ -1,4 +1,4 @@ -FROM ros:humble-ros-base +FROM ros:humble ENV DEBIAN_FRONTEND=noninteractive # install dependencies @@ -68,4 +68,27 @@ RUN chmod +x /app/delayed_start.sh RUN apt install -y iputils-ping netcat net-tools +# Base tools +RUN apt-get update && apt-get install -y --no-install-recommends \ + git curl ca-certificates unzip \ + python3-vcstool python3-rosdep python3-colcon-common-extensions \ + ros-humble-gazebo-ros-pkgs \ + ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-slam-toolbox \ + ros-humble-xacro ros-humble-robot-state-publisher ros-humble-joint-state-publisher-gui \ + ros-humble-rmw-cyclonedds-cpp + +RUN rosdep update + +# ── TIAGo workspace ──────────────────────────────────────────────────────────── + +ENV WS=/root/tiago_public_ws +RUN mkdir -p ${WS}/src +WORKDIR ${WS} + +RUN vcs import --input https://raw.githubusercontent.com/pal-robotics/tiago_tutorials/humble-devel/tiago_public.repos src + +RUN rosdep install --from-paths src -y --ignore-src +RUN . /opt/ros/humble/setup.sh && colcon build --symlink-install + + CMD ["/app/entrypoint.sh"] diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index a641568..711d5ca 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -18,6 +19,7 @@ #include "MujocoContext.h" #include "robots/Robot.h" #include "sensors/Camera.h" +#include "sensors/ImageSharedMemoryWriter.h" #include "sensors/Imu.h" #include "sensors/Joint.h" #include "sensors/Pose.h" @@ -59,7 +61,10 @@ class BoosterT1 : public Robot { {JointValue::HIP_RIGHT_YAW, name + "_Right_Hip_Yaw"}, {JointValue::KNEE_RIGHT_PITCH, name + "_Right_Knee_Pitch"}, {JointValue::ANKLE_RIGHT_PITCH, name + "_Right_Ankle_Pitch"}, - {JointValue::ANKLE_RIGHT_ROLL, name + "_Right_Ankle_Roll"}} {} + {JointValue::ANKLE_RIGHT_ROLL, name + "_Right_Ankle_Roll"}} { + //Where to put the images + shm_dir_ = "/tmp/circus_ipc"; + } void bindMujoco(MujocoContext* mujCtx) override { pose = new Pose(mujCtx->model, mujCtx->data, (name + "_position").c_str(), (name + "_orientation").c_str()); @@ -92,6 +97,13 @@ class BoosterT1 : public Robot { cameras[0] = new Camera(mujCtx, (name + "_left_cam").c_str()); cameras[1] = new Camera(mujCtx, (name + "_right_cam").c_str()); + + //Configure the writer for the shared memory file + const int width = cameras[0]->getWidth(); + const int height = cameras[0]->getHeight(); + left_writer_.configure(shmFilePath_("left"), width, height, 3); + right_writer_.configure(shmFilePath_("right"), width, height, 3); + } void receiveMessage(const std::map& message) override { @@ -125,6 +137,11 @@ class BoosterT1 : public Robot { msg["imu"] = imu->serialize(buffer_zone_); msg["joints"] = joints->serialize(buffer_zone_); + // Write in the shared file the information + left_writer_.write(cameras[0]->getImage()); + right_writer_.write(cameras[1]->getImage()); + + return msg; } @@ -149,7 +166,14 @@ class BoosterT1 : public Robot { ~BoosterT1() = default; private: + std::string shmFilePath_(const std::string& camera) const { + return shm_dir_ + "/" + name + "_" + camera + ".shm"; + } + std::map joint_map; + std::string shm_dir_; + ImageSharedMemoryWriter left_writer_; + ImageSharedMemoryWriter right_writer_; }; } // namespace spqr diff --git a/include/sensors/ImageSharedMemoryWriter.h b/include/sensors/ImageSharedMemoryWriter.h new file mode 100644 index 0000000..9ed1719 --- /dev/null +++ b/include/sensors/ImageSharedMemoryWriter.h @@ -0,0 +1,132 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace spqr { + +class ImageSharedMemoryWriter { + public: + ImageSharedMemoryWriter() = default; + + ~ImageSharedMemoryWriter() { + close_(); + } + + void configure(const std::string& path, int width, int height, int channels, int ring_slots = 3) { + if (width <= 0 || height <= 0 || channels <= 0 || ring_slots <= 0) { + throw std::invalid_argument("Invalid shared-memory image configuration"); + } + // Set the sizes of bytes + const size_t slot_bytes = static_cast(width) * static_cast(height) * static_cast(channels); + const size_t total_bytes = sizeof(Header) + static_cast(ring_slots) * slot_bytes; + + close_(); + + const std::filesystem::path p(path); + if (p.has_parent_path()) { + std::filesystem::create_directories(p.parent_path()); + } + + fd_ = open(path.c_str(), O_RDWR | O_CREAT, 0666); + if (fd_ < 0) { + throw std::runtime_error("Failed to open shared image file: " + path); + } + + if (ftruncate(fd_, static_cast(total_bytes)) != 0) { + close_(); + throw std::runtime_error("Failed to resize shared image file: " + path); + } + + map_bytes_ = total_bytes; + // map the file in the process memory, map_ptr is the pointer of the memory area linked to that file + map_ptr_ = mmap(nullptr, map_bytes_, PROT_READ | PROT_WRITE, MAP_SHARED, fd_, 0); + if (map_ptr_ == MAP_FAILED) { + map_ptr_ = nullptr; + close_(); + throw std::runtime_error("Failed to mmap shared image file: " + path); + } + + header_ = static_cast(map_ptr_); + if (header_->magic != kMagic || header_->version != kVersion || header_->slot_bytes != slot_bytes + || header_->slot_count != static_cast(ring_slots)) { + std::memset(header_, 0, sizeof(Header)); + header_->magic = kMagic; + header_->version = kVersion; + header_->width = static_cast(width); + header_->height = static_cast(height); + header_->channels = static_cast(channels); + header_->slot_count = static_cast(ring_slots); + header_->slot_bytes = slot_bytes; + header_->seq = 0; + } + + path_ = path; + } + + bool isReady() const { + return header_ != nullptr; + } + + void write(const std::vector& frame) { + if (!header_) { + return; + } + if (frame.size() != header_->slot_bytes) { + return; + } + + const uint64_t next_seq = __atomic_load_n(&header_->seq, __ATOMIC_RELAXED) + 1; + const uint32_t slot_idx = static_cast(next_seq % header_->slot_count); + uint8_t* slots_base = static_cast(map_ptr_) + sizeof(Header); + uint8_t* dst = slots_base + static_cast(slot_idx) * header_->slot_bytes; + std::memcpy(dst, frame.data(), frame.size()); + __atomic_store_n(&header_->seq, next_seq, __ATOMIC_RELEASE); + } + + private: + struct Header { + uint32_t magic = 0; + uint32_t version = 0; + uint32_t width = 0; + uint32_t height = 0; + uint32_t channels = 0; + uint32_t slot_count = 0; + size_t slot_bytes = 0; + uint64_t seq = 0; + }; + + static constexpr uint32_t kMagic = 0x5348514d; // SHQM + static constexpr uint32_t kVersion = 1; + + void close_() { + if (map_ptr_) { + munmap(map_ptr_, map_bytes_); + } + map_ptr_ = nullptr; + header_ = nullptr; + map_bytes_ = 0; + if (fd_ >= 0) { + close(fd_); + } + fd_ = -1; + path_.clear(); + } + + int fd_ = -1; + void* map_ptr_ = nullptr; + size_t map_bytes_ = 0; + Header* header_ = nullptr; + std::string path_; +}; + +} // namespace spqr diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 93daa16..7c5bf7a 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -3,4 +3,5 @@ image: spqr:booster volumes: - "/tools/booster_motion:/app/booster_motion" - - "/.pixi/envs/default:/app/bridge" \ No newline at end of file + - "/.pixi/envs/default:/app/bridge" + - "/tmp/circus_ipc:/tmp/circus_ipc" diff --git a/simbridge b/simbridge deleted file mode 160000 index 37190e7..0000000 --- a/simbridge +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 37190e79cc4986dc11f972ab8c734dacf820a91c From 7948824c4c76686be08eb1d9e5a34050ed8ff5e5 Mon Sep 17 00:00:00 2001 From: michelebri Date: Fri, 20 Feb 2026 16:14:30 +0000 Subject: [PATCH 29/56] docker --- dockerfiles/Dockerfile | 3 ++- src/Container.cpp | 31 +++++++++++++++++++++++++++++-- 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile index 54733fc..feba90f 100644 --- a/dockerfiles/Dockerfile +++ b/dockerfiles/Dockerfile @@ -75,7 +75,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-humble-gazebo-ros-pkgs \ ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-slam-toolbox \ ros-humble-xacro ros-humble-robot-state-publisher ros-humble-joint-state-publisher-gui \ - ros-humble-rmw-cyclonedds-cpp + ros-humble-rmw-cyclonedds-cpp ros-humble-image-view RUN rosdep update @@ -91,4 +91,5 @@ RUN rosdep install --from-paths src -y --ignore-src RUN . /opt/ros/humble/setup.sh && colcon build --symlink-install +RUN apt install ros-humble-image-view -y CMD ["/app/entrypoint.sh"] diff --git a/src/Container.cpp b/src/Container.cpp index 2156239..05aef60 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -1,6 +1,7 @@ #include "Container.h" #include +#include #include "Constants.h" @@ -64,17 +65,43 @@ Container::~Container() { } void Container::create(const std::string& robot_name, const std::string& image, const std::vector& binds) { + auto envOrDefault = [](const char* key, const char* fallback) -> std::string { + const char* value = std::getenv(key); + return (value && *value) ? std::string(value) : std::string(fallback); + }; + nlohmann::json payload; payload["Image"] = image; - payload["HostConfig"] = {{"Binds", binds}, + std::vector binds_with_x11 = binds; + binds_with_x11.push_back("/tmp/.X11-unix:/tmp/.X11-unix:rw"); + + payload["HostConfig"] = {{"Binds", binds_with_x11}, {"IpcMode", "host"}, {"CapAdd", {"SYS_NICE", "IPC_LOCK"}}, {"SecurityOpt", {"seccomp=unconfined"}}, {"Ulimits", nlohmann::json::array({{{"Name", "memlock"}, {"Soft", -1}, {"Hard", -1}}})}, {"Privileged", true}}; - payload["Env"] = {"ROBOT_NAME=" + robot_name, "SERVER_IP=172.17.0.1", "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort)}; + payload["HostConfig"]["DeviceRequests"] = nlohmann::json::array(); + payload["HostConfig"]["DeviceRequests"].push_back({ + {"Driver", "nvidia"}, + {"Count", -1}, + {"Capabilities", nlohmann::json::array({nlohmann::json::array({"gpu"})})}, + }); + + payload["Env"] = {"ROBOT_NAME=" + robot_name, + "SERVER_IP=172.17.0.1", + "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort), + "DISPLAY=" + envOrDefault("DISPLAY", ":0"), + "QT_X11_NO_MITSHM=1", + "NVIDIA_VISIBLE_DEVICES=all", + "NVIDIA_DRIVER_CAPABILITIES=all", + "ROBOT_STACK=booster", + "CIRCUS_IMAGE_SHM_DIR=" + envOrDefault("CIRCUS_IMAGE_SHM_DIR", "/tmp/circus_ipc")}; + + payload["Entrypoint"] = {"/bin/bash", "-lc"}; + payload["Cmd"] = {"/app/entrypoint.sh"}; payload["Tty"] = true; payload["OpenStdin"] = true; From 6f2a8c9083ca21f9a5b0b73ae08e2241f0e43608 Mon Sep 17 00:00:00 2001 From: michelebri Date: Fri, 20 Feb 2026 16:50:15 +0000 Subject: [PATCH 30/56] removed unsued file for my project --- dockerfiles/Dockerfile | 95 ------------------- .../booster_msgs/CMakeLists.txt | 31 ------ .../booster_msgs/msg/RpcReqMsg.msg | 3 - .../LocoApiPackage/booster_msgs/package.xml | 24 ----- dockerfiles/booster.conf | 45 --------- dockerfiles/configs/bashrc | 23 ----- .../configs/system_settings_config.yaml | 4 - dockerfiles/delayed_start.sh | 9 -- dockerfiles/entrypoint.sh | 12 --- 9 files changed, 246 deletions(-) delete mode 100644 dockerfiles/Dockerfile delete mode 100644 dockerfiles/LocoApiPackage/booster_msgs/CMakeLists.txt delete mode 100644 dockerfiles/LocoApiPackage/booster_msgs/msg/RpcReqMsg.msg delete mode 100644 dockerfiles/LocoApiPackage/booster_msgs/package.xml delete mode 100644 dockerfiles/booster.conf delete mode 100644 dockerfiles/configs/bashrc delete mode 100644 dockerfiles/configs/system_settings_config.yaml delete mode 100644 dockerfiles/delayed_start.sh delete mode 100644 dockerfiles/entrypoint.sh diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile deleted file mode 100644 index feba90f..0000000 --- a/dockerfiles/Dockerfile +++ /dev/null @@ -1,95 +0,0 @@ -FROM ros:humble -ENV DEBIAN_FRONTEND=noninteractive - -# install dependencies -RUN apt-get update && apt-get install -y \ - curl \ - supervisor \ - gnupg2 \ - lsb-release \ - build-essential \ - libtinyxml2-dev \ - libtinyxml2-9 \ - libpulse0 \ - libpulse-mainloop-glib0 \ - libasound2 \ - libxcursor1 \ - libxinerama1 \ - libxi6 \ - libxfixes3 \ - libxrandr2 \ - libxss1 \ - libxxf86vm1 \ - libdrm2 \ - libgbm1 \ - libwayland-egl1 \ - libwayland-client0 \ - libwayland-cursor0 \ - libxkbcommon0 \ - libdecor-0-0 \ - ament-cmake \ - && rm -rf /var/lib/apt/lists/* - -# copy configs -RUN mkdir /opt/booster -RUN mkdir /opt/booster/configs -COPY configs/system_settings_config.yaml /opt/booster/configs -COPY booster.conf /etc/supervisor/conf.d/ - -# copy and install booster sdk -COPY booster_robotics_sdk /opt/booster/sdk -RUN /opt/booster/sdk/install.sh - -# compile examples too (loco client in particular) -RUN mkdir opt/booster/sdk/build -RUN cd opt/booster/sdk/build && cmake .. && make - -# copy, compile and install booster ros sdk -COPY booster_robotics_sdk_ros2 /opt/booster/sdk_ros2 - -# Build with minimal optimization and sequential execution -RUN /bin/bash -c "source /opt/ros/humble/setup.bash; cd /opt/booster/sdk_ros2 && colcon build" - -COPY LocoApiPackage /opt/booster/LocoApiPackage -# RUN /bin/bash -c "source /opt/ros/humble/setup.bash; cd /opt/booster/LocoApiPackage && colcon build --packages-select booster_msgs - -# add custom bashrc -COPY configs/bashrc /root/.bashrc_addendum -RUN echo ". ~/.bashrc_addendum" >> ~/.bashrc - -RUN mkdir /app -COPY entrypoint.sh /app -RUN chmod +x /app/entrypoint.sh - -COPY delayed_start.sh /app -RUN chmod +x /app/delayed_start.sh - -# used to test communication, can be removed at a later date if so desired -RUN apt install -y iputils-ping netcat net-tools - - -# Base tools -RUN apt-get update && apt-get install -y --no-install-recommends \ - git curl ca-certificates unzip \ - python3-vcstool python3-rosdep python3-colcon-common-extensions \ - ros-humble-gazebo-ros-pkgs \ - ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-slam-toolbox \ - ros-humble-xacro ros-humble-robot-state-publisher ros-humble-joint-state-publisher-gui \ - ros-humble-rmw-cyclonedds-cpp ros-humble-image-view - -RUN rosdep update - -# ── TIAGo workspace ──────────────────────────────────────────────────────────── - -ENV WS=/root/tiago_public_ws -RUN mkdir -p ${WS}/src -WORKDIR ${WS} - -RUN vcs import --input https://raw.githubusercontent.com/pal-robotics/tiago_tutorials/humble-devel/tiago_public.repos src - -RUN rosdep install --from-paths src -y --ignore-src -RUN . /opt/ros/humble/setup.sh && colcon build --symlink-install - - -RUN apt install ros-humble-image-view -y -CMD ["/app/entrypoint.sh"] diff --git a/dockerfiles/LocoApiPackage/booster_msgs/CMakeLists.txt b/dockerfiles/LocoApiPackage/booster_msgs/CMakeLists.txt deleted file mode 100644 index 81dc731..0000000 --- a/dockerfiles/LocoApiPackage/booster_msgs/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(booster_msgs) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -# Generate interfaces -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/RpcReqMsg.msg" - DEPENDENCIES std_msgs -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/dockerfiles/LocoApiPackage/booster_msgs/msg/RpcReqMsg.msg b/dockerfiles/LocoApiPackage/booster_msgs/msg/RpcReqMsg.msg deleted file mode 100644 index 9d9ff86..0000000 --- a/dockerfiles/LocoApiPackage/booster_msgs/msg/RpcReqMsg.msg +++ /dev/null @@ -1,3 +0,0 @@ -string m_uuid -string m_header -string m_body diff --git a/dockerfiles/LocoApiPackage/booster_msgs/package.xml b/dockerfiles/LocoApiPackage/booster_msgs/package.xml deleted file mode 100644 index 97f493d..0000000 --- a/dockerfiles/LocoApiPackage/booster_msgs/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - booster_msgs - 0.0.0 - TODO: Package description - flavio - TODO: License declaration - - ament_cmake - - rosidl_default_generators - rosidl_default_runtime - std_msgs - - rosidl_interface_packages - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf deleted file mode 100644 index bbb5588..0000000 --- a/dockerfiles/booster.conf +++ /dev/null @@ -1,45 +0,0 @@ -[unix_http_server] -file=/var/run/supervisor.sock - -[supervisorctl] -serverurl=unix:///var/run/supervisor.sock - -[rpcinterface:supervisor] -supervisor.rpcinterface_factory = supervisor.rpcinterface:make_main_rpcinterface - -[supervisord] -nodaemon=true - -[program:booster-motion] -directory=/app/booster_motion -command=/app/booster_motion/booster-motion -mode sim -config ./configs/config_isaac.lua -autostart=true -autorestart=true -priority=1 -stdout_logfile=/var/log/booster-motion.log -stderr_logfile=/var/log/booster-motion.err - -[program:delayed-starter] -command=/app/delayed_start.sh -autostart=true -autorestart=false -priority=2 -startsecs=0 -stdout_logfile=/var/log/delayed-starter.log -stderr_logfile=/var/log/delayed-starter.err - -[program:simbridge] -directory=/app/bridge -command=/app/bridge/bin/simbridge_node -autostart=false -autorestart=true -stdout_logfile=/var/log/simbridge.log -stderr_logfile=/var/log/simbridge.err - -[program:maximus] -directory=/app/maximus -command=/app/maximus/bin/maximus_main -autostart=false -autorestart=true -stdout_logfile=/var/log/maximus.log -stderr_logfile=/var/log/maximus.err \ No newline at end of file diff --git a/dockerfiles/configs/bashrc b/dockerfiles/configs/bashrc deleted file mode 100644 index 521ab31..0000000 --- a/dockerfiles/configs/bashrc +++ /dev/null @@ -1,23 +0,0 @@ -source /opt/ros/humble/setup.sh -source /opt/booster/sdk_ros2/install/setup.bash - -# Export dyanmic libraries and fastdds profile for booster_motion execution -export LD_LIBRARY_PATH=/app/booster_motion/lib:/app/booster_motion/lib-usr-local:/app/booster_motion/lib-x86_64-linux-gnu:$LD_LIBRARY_PATH -export FASTRTPS_DEFAULT_PROFILES_FILE=/app/booster_motion/fastdds_profile.xml - -alias loco="/opt/booster/sdk/build/b1_loco_example_client 127.0.0.1" -alias looc="loco" -alias lcoo="loco" -alias mw="loco" -alias mp="loco" - -alias echocmd="ros2 topic echo /booster/ros2_k2_joint_cmd" -alias echostate="ros2 topic echo /booster/ros2_k2_joint_states" -alias echolow="ros2 topic echo /low_state" - -alias bmlog="cat /var/log/booster-motion.log" -alias bmerr="cat /var/log/booster-motion.err" -alias maxlog="cat /var/log/maximus.log" -alias maxerr="cat /var/log/maximus.err" -alias brilog="cat /var/log/simbridge.log" -alias brierr="cat /var/log/simbridge.err" diff --git a/dockerfiles/configs/system_settings_config.yaml b/dockerfiles/configs/system_settings_config.yaml deleted file mode 100644 index 5ab38bf..0000000 --- a/dockerfiles/configs/system_settings_config.yaml +++ /dev/null @@ -1,4 +0,0 @@ -audio_settings: - enable_dance_music: true -debug_mode_settings: - enable_debug_mode: false diff --git a/dockerfiles/delayed_start.sh b/dockerfiles/delayed_start.sh deleted file mode 100644 index e57e1fe..0000000 --- a/dockerfiles/delayed_start.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/bin/bash -# Wait for 5 seconds -sleep 6 - -# Start simbridge and maximus in parallel -# This substitute the timer of 5 seconds inside the bridge. -# In this way the bridge (or maximus) does not need to have the -# check on the timer if the code is run on the robot or on simulator -supervisorctl -c /etc/supervisor/conf.d/booster.conf start simbridge maximus \ No newline at end of file diff --git a/dockerfiles/entrypoint.sh b/dockerfiles/entrypoint.sh deleted file mode 100644 index 9e63089..0000000 --- a/dockerfiles/entrypoint.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/bash - -# Export dyanmic libraries and fastdds profile for booster_motion execution -export LD_LIBRARY_PATH=/app/booster_motion/lib:/app/booster_motion/lib-usr-local:/app/booster_motion/lib-x86_64-linux-gnu:$LD_LIBRARY_PATH -export FASTRTPS_DEFAULT_PROFILES_FILE=/app/booster_motion/fastdds_profile.xml - -# Export environment variables for MAXIMUS framework -export SPQR_CONFIG_ROOT=/app/maximus/config -export SPQR_BEHAVIOR_TREE_PATH=/app/maximus/behaviors - -# Start supervisord to manage processes -/usr/bin/supervisord -n -c /etc/supervisor/conf.d/booster.conf From 4c0f11493e300dcdf44e6373ed8be25e679415e6 Mon Sep 17 00:00:00 2001 From: michelebri Date: Fri, 20 Feb 2026 18:21:15 +0000 Subject: [PATCH 31/56] Fix shm directory permissions and camera data race --- include/sensors/Camera.h | 10 +++++++--- src/AppWindow.cpp | 9 +++++++++ 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/include/sensors/Camera.h b/include/sensors/Camera.h index e70016a..5cf59e3 100644 --- a/include/sensors/Camera.h +++ b/include/sensors/Camera.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,7 @@ class Camera : public Sensor { QImage resized = flipped.scaled(w, h, Qt::IgnoreAspectRatio, Qt::SmoothTransformation); // Copy resized data back to image buffer + std::lock_guard lock(imageMutex_); memcpy(image.data(), resized.constBits(), w * h * 3); // Restore window buffer @@ -101,7 +103,8 @@ class Camera : public Sensor { return cam; } - const std::vector& getImage() const { + std::vector getImage() const { + std::lock_guard lock(imageMutex_); return image; } @@ -114,14 +117,15 @@ class Camera : public Sensor { } msgpack::object doSerialize(msgpack::zone& z) override { - std::vector img_copy(image.begin(), image.end()); - return msgpack::object(img_copy, z); + std::lock_guard lock(imageMutex_); + return msgpack::object(image, z); } private: int w, h; MujocoContext* mujContext; std::vector image; + mutable std::mutex imageMutex_; mjvCamera cam{}; std::string cameraName_; }; diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index cf089f2..06680b9 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -209,6 +210,14 @@ void AppWindow::loadScene(const QString& yaml_file) { Qt::QueuedConnection); }); + // Ensure the shared memory directory exists and is writable by the current user. + // Docker bind mounts create missing host dirs as root, so remove and recreate if needed. + const std::filesystem::path shmDir("/tmp/circus_ipc"); + if (std::filesystem::exists(shmDir)) { + std::filesystem::remove_all(shmDir); + } + std::filesystem::create_directories(shmDir); + RobotManager::instance().startContainers(); RobotManager::instance().bindMujoco(mujContext.get()); From 2f1969425351d80c4b89c34019fe4b41b2d9bb99 Mon Sep 17 00:00:00 2001 From: Flavio Date: Sun, 1 Mar 2026 02:38:17 +0100 Subject: [PATCH 32/56] other robots position in local RF + timer adjustment (works with also timer inside simbridge) + path change of simbridge --- dockerfiles/booster.conf | 2 +- dockerfiles/delayed_start.sh | 2 +- include/robots/BoosterT1.h | 4 +- include/sensors/Oracle.h | 111 +++++++++++++++++++++++-- resources/config/framework_config.yaml | 2 +- 5 files changed, 111 insertions(+), 10 deletions(-) diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index 0860ec2..bbb5588 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -30,7 +30,7 @@ stderr_logfile=/var/log/delayed-starter.err [program:simbridge] directory=/app/bridge -command=/app/bridge/simbridge +command=/app/bridge/bin/simbridge_node autostart=false autorestart=true stdout_logfile=/var/log/simbridge.log diff --git a/dockerfiles/delayed_start.sh b/dockerfiles/delayed_start.sh index e57e1fe..36dd7af 100644 --- a/dockerfiles/delayed_start.sh +++ b/dockerfiles/delayed_start.sh @@ -1,6 +1,6 @@ #!/bin/bash # Wait for 5 seconds -sleep 6 +sleep 3 # Start simbridge and maximus in parallel # This substitute the timer of 5 seconds inside the bridge. diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 3b1a765..8223515 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -23,7 +23,6 @@ #include "sensors/Pose.h" #include "sensors/Oracle.h" - #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { @@ -96,7 +95,8 @@ class BoosterT1 : public Robot { cameras[0] = new Camera(mujCtx, (name + "_left_cam").c_str()); cameras[1] = new Camera(mujCtx, (name + "_right_cam").c_str()); - oracle = new Oracle(mujCtx->model, mujCtx->data, pose); + // Create Oracle with the pose and all robots + oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); } void receiveMessage(const std::map& message) override { diff --git a/include/sensors/Oracle.h b/include/sensors/Oracle.h index 729f003..8b1f2c4 100644 --- a/include/sensors/Oracle.h +++ b/include/sensors/Oracle.h @@ -3,18 +3,23 @@ #include #include +#include +#include +#include #include "sensors/Sensor.h" - namespace spqr { +// Forward declarations +class Pose; // L'oracle is defined as a sensor that provides the ground truth about the world. // It should contain information about the position of the ball, robots, obstacles, ... // It can contain also the information their velocities, ... class Oracle : public Sensor { public: - Oracle(mjModel* mujModel, mjData* mujData, Pose* robotPose) : mujModel(mujModel), mujData(mujData), robotPose(robotPose) { + Oracle(mjModel* mujModel, mjData* mujData, std::string robotName, Pose* robotPose) + : mujModel(mujModel), mujData(mujData), robotName(robotName), robotPose(robotPose) { ballId = mj_name2id(mujModel, mjOBJ_BODY, "ball"); ballAdr = mujModel->body_jntadr[ballId]; @@ -24,18 +29,31 @@ class Oracle : public Sensor { // AGGIUNGERE TUTTI I CAMPI CHE CI INTERESSANO SAPERE COME ORACLE - + discoverOtherRobotSensors(); } void doUpdate() override { - Eigen::Vector3d ballPosGlobal = Eigen::Vector3d(Eigen::Map(mujData->qpos + ballPosAdr)); - ballPosition = globalToLocalPosition(ballPosGlobal, robotPose->getTransformationMatrix()); + // Update local ball position + updateBallPosition(); + + // Update all other robots' local positions + updateOtherRobotsLocalPositions(); } msgpack::object doSerialize(msgpack::zone& z) override { + // Serialize ball position std::vector ball_pos = {ballPosition(0), ballPosition(1), ballPosition(2)}; std::map oracle_data; oracle_data["ball_position"] = msgpack::object(ball_pos, z); + + // Serialize other robots' positions + std::map robots_data; + for (const auto& [robotName, robotLocalPos] : otherRobotsLocalPositions) { + std::vector robot_pos = {robotLocalPos(0), robotLocalPos(1), robotLocalPos(2)}; + robots_data[robotName] = msgpack::object(robot_pos, z); + } + oracle_data["robots_positions"] = msgpack::object(robots_data, z); + return msgpack::object(oracle_data, z); } @@ -43,6 +61,29 @@ class Oracle : public Sensor { return ballPosition; } + /** + * @brief Get the local position of another robot + * + * @param robotName The name of the robot + * @return The position of the robot in the local frame, or nullopt if not found + */ + std::optional getRobotLocalPosition(const std::string& robotName) const { + auto it = otherRobotsLocalPositions.find(robotName); + if (it != otherRobotsLocalPositions.end()) { + return it->second; + } + return std::nullopt; + } + + /** + * @brief Get all other robots' positions in local frame + * + * @return Map of robot names to their local positions + */ + const std::map& getAllRobotsLocalPositions() const { + return otherRobotsLocalPositions; + } + private: /** @@ -67,13 +108,73 @@ class Oracle : public Sensor { return Eigen::Vector3d(localPosHomogeneous(0), localPosHomogeneous(1), localPosHomogeneous(2)); } + void discoverOtherRobotSensors() { + // Scan all sensors in MuJoCo to find position sensors of other robots + for (int i = 0; i < mujModel->nsensor; i++) { + const char* sensorName = mj_id2name(mujModel, mjOBJ_SENSOR, i); + if (!sensorName) continue; + + std::string sName(sensorName); + + // Look for position sensors (assuming naming convention: {robotName}_position) + size_t pos = sName.rfind("_position"); + if (pos != std::string::npos) { + std::string extractedRobotName = sName.substr(0, pos); + + // Skip if it's the current robot + if (extractedRobotName == robotName) { + continue; + } + + // Store the sensor address for this robot + int positionAdr = mujModel->sensor_adr[i]; + otherRobotSensorAddrs[extractedRobotName] = positionAdr; + } + } + } + + void updateBallPosition() { + Eigen::Vector3d ballPosGlobal = Eigen::Vector3d(Eigen::Map(mujData->qpos + ballPosAdr)); + ballPosition = globalToLocalPosition(ballPosGlobal, robotPose->getTransformationMatrix()); + } + + /** + * @brief Update positions of all other robots in local frame + * + * Iterates through all robots in the world (except the current one) and converts + * their global positions to local coordinates relative to this robot's pose. + */ + void updateOtherRobotsLocalPositions() { + otherRobotsLocalPositions.clear(); + + // Get positions of other robots using their sensor data + for (const auto& [otherRobotName, sensorAdr] : otherRobotSensorAddrs) { + // Read position from sensor data (3D vector) + Eigen::Vector3d robotGlobalPos = Eigen::Vector3d( + Eigen::Map(mujData->sensordata + sensorAdr) + ); + + // Convert to local frame + Eigen::Vector3d robotLocalPos = globalToLocalPosition( + robotGlobalPos, + robotPose->getTransformationMatrix() + ); + + otherRobotsLocalPositions[otherRobotName] = robotLocalPos; + + } + } Eigen::Vector3d ballPosition; int ballId, ballAdr, ballPosAdr, ballVelAdr; mjModel* mujModel; mjData* mujData; + std::string robotName; Pose* robotPose; + std::map otherRobotsLocalPositions; + + std::map otherRobotSensorAddrs; // robotName -> sensorAdr }; diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 153b671..61f2600 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -3,7 +3,7 @@ image: spqr:booster volumes: - "/tools/booster_motion:/app/booster_motion" - - "/build:/app/bridge" + - "/.pixi/envs/default:/app/bridge" - "/.pixi/envs/default:/app/maximus" - "/src/app/config:/app/maximus/config" - "/src/app/behaviors/trees:/app/maximus/behaviors" From e7e5b9d0b52d97ceeb341854570bfccf32231ea2 Mon Sep 17 00:00:00 2001 From: Flavio Date: Mon, 2 Mar 2026 01:14:40 +0100 Subject: [PATCH 33/56] sending teammates/opponents local positions --- include/sensors/Oracle.h | 85 +++++++++++++++++++++++++++++++--------- 1 file changed, 66 insertions(+), 19 deletions(-) diff --git a/include/sensors/Oracle.h b/include/sensors/Oracle.h index 8b1f2c4..c30a87d 100644 --- a/include/sensors/Oracle.h +++ b/include/sensors/Oracle.h @@ -6,6 +6,7 @@ #include #include #include +#include #include "sensors/Sensor.h" namespace spqr { @@ -21,6 +22,9 @@ class Oracle : public Sensor { Oracle(mjModel* mujModel, mjData* mujData, std::string robotName, Pose* robotPose) : mujModel(mujModel), mujData(mujData), robotName(robotName), robotPose(robotPose) { + size_t pos = robotName.find('_'); + teamName = robotName.substr(0, pos); + ballId = mj_name2id(mujModel, mjOBJ_BODY, "ball"); ballAdr = mujModel->body_jntadr[ballId]; @@ -37,7 +41,8 @@ class Oracle : public Sensor { updateBallPosition(); // Update all other robots' local positions - updateOtherRobotsLocalPositions(); + updateTeammatesLocalPositions(); + updateOpponentsLocalPositions(); } msgpack::object doSerialize(msgpack::zone& z) override { @@ -46,13 +51,22 @@ class Oracle : public Sensor { std::map oracle_data; oracle_data["ball_position"] = msgpack::object(ball_pos, z); - // Serialize other robots' positions - std::map robots_data; - for (const auto& [robotName, robotLocalPos] : otherRobotsLocalPositions) { - std::vector robot_pos = {robotLocalPos(0), robotLocalPos(1), robotLocalPos(2)}; - robots_data[robotName] = msgpack::object(robot_pos, z); + // Serialize teammates' positions + std::map teammates_data; + for (const auto& [robotName, robotLocalPos] : teammatesLocalPositions) { + std::vector robot_pos = {robotLocalPos(0), robotLocalPos(1), robotLocalPos(2)}; + teammates_data[robotName] = msgpack::object(robot_pos, z); + } + + // Serialize opponents' positions + std::map opponents_data; + for (const auto& [robotName, robotLocalPos] : opponentsLocalPositions) { + std::vector robot_pos = {robotLocalPos(0), robotLocalPos(1), robotLocalPos(2)}; + opponents_data[robotName] = msgpack::object(robot_pos, z); } - oracle_data["robots_positions"] = msgpack::object(robots_data, z); + + oracle_data["teammates_positions"] = msgpack::object(teammates_data, z); + oracle_data["opponents_positions"] = msgpack::object(opponents_data, z); return msgpack::object(oracle_data, z); } @@ -67,9 +81,9 @@ class Oracle : public Sensor { * @param robotName The name of the robot * @return The position of the robot in the local frame, or nullopt if not found */ - std::optional getRobotLocalPosition(const std::string& robotName) const { - auto it = otherRobotsLocalPositions.find(robotName); - if (it != otherRobotsLocalPositions.end()) { + std::optional getTeammatePosition(const std::string& robotName) const { + auto it = teammatesLocalPositions.find(robotName); + if (it != teammatesLocalPositions.end()) { return it->second; } return std::nullopt; @@ -80,8 +94,8 @@ class Oracle : public Sensor { * * @return Map of robot names to their local positions */ - const std::map& getAllRobotsLocalPositions() const { - return otherRobotsLocalPositions; + const std::map& getAllTeammatesLocalPositions() const { + return teammatesLocalPositions; } @@ -128,7 +142,16 @@ class Oracle : public Sensor { // Store the sensor address for this robot int positionAdr = mujModel->sensor_adr[i]; - otherRobotSensorAddrs[extractedRobotName] = positionAdr; + + size_t pos = extractedRobotName.find('_'); + std::string extractedRobotTeamName = extractedRobotName.substr(0, pos); + + if(extractedRobotTeamName == teamName) { + teammatesSensorAddrs[extractedRobotName] = positionAdr; + } + else { + opponentsSensorAddrs[extractedRobotName] = positionAdr; + } } } } @@ -144,11 +167,32 @@ class Oracle : public Sensor { * Iterates through all robots in the world (except the current one) and converts * their global positions to local coordinates relative to this robot's pose. */ - void updateOtherRobotsLocalPositions() { - otherRobotsLocalPositions.clear(); + void updateTeammatesLocalPositions() { + teammatesLocalPositions.clear(); + + // Get positions of other robots using their sensor data + for (const auto& [teammateName, sensorAdr] : teammatesSensorAddrs) { + // Read position from sensor data (3D vector) + Eigen::Vector3d robotGlobalPos = Eigen::Vector3d( + Eigen::Map(mujData->sensordata + sensorAdr) + ); + + // Convert to local frame + Eigen::Vector3d robotLocalPos = globalToLocalPosition( + robotGlobalPos, + robotPose->getTransformationMatrix() + ); + + teammatesLocalPositions[teammateName] = robotLocalPos; + + } + } + + void updateOpponentsLocalPositions() { + opponentsLocalPositions.clear(); // Get positions of other robots using their sensor data - for (const auto& [otherRobotName, sensorAdr] : otherRobotSensorAddrs) { + for (const auto& [opponentName, sensorAdr] : opponentsSensorAddrs) { // Read position from sensor data (3D vector) Eigen::Vector3d robotGlobalPos = Eigen::Vector3d( Eigen::Map(mujData->sensordata + sensorAdr) @@ -160,7 +204,7 @@ class Oracle : public Sensor { robotPose->getTransformationMatrix() ); - otherRobotsLocalPositions[otherRobotName] = robotLocalPos; + opponentsLocalPositions[opponentName] = robotLocalPos; } } @@ -171,10 +215,13 @@ class Oracle : public Sensor { mjModel* mujModel; mjData* mujData; std::string robotName; + std::string teamName; Pose* robotPose; - std::map otherRobotsLocalPositions; + std::map teammatesLocalPositions; + std::map opponentsLocalPositions; - std::map otherRobotSensorAddrs; // robotName -> sensorAdr + std::map teammatesSensorAddrs; // robotName -> sensorAdr + std::map opponentsSensorAddrs; // robotName -> sensorAdr }; From 9574b64ce25044ddbc6b3d3ae416e16c918b7a0e Mon Sep 17 00:00:00 2001 From: michelebri Date: Wed, 4 Mar 2026 16:45:11 +0000 Subject: [PATCH 34/56] fix tensoRRT in docker circus --- dockerfiles/Dockerfile | 23 +++++++++++++---------- dockerfiles/booster.conf | 3 ++- dockerfiles/entrypoint.sh | 2 +- resources/config/framework_config.yaml | 1 + src/Container.cpp | 17 +++++++++++------ 5 files changed, 28 insertions(+), 18 deletions(-) diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile index 67b33f1..3acf1e0 100644 --- a/dockerfiles/Dockerfile +++ b/dockerfiles/Dockerfile @@ -28,13 +28,17 @@ RUN apt-get update && apt-get install -y \ libxkbcommon0 \ libdecor-0-0 \ ament-cmake \ - && rm -rf /var/lib/apt/lists/* + iputils-ping \ + netcat \ + ros-humble-image-view \ + net-tools + +RUN rm -rf /var/lib/apt/lists/* +# used to test communication, can be removed at a later date if so desired # copy configs RUN mkdir /opt/booster RUN mkdir /opt/booster/configs -COPY configs/system_settings_config.yaml /opt/booster/configs -COPY booster.conf /etc/supervisor/conf.d/ # copy and install booster sdk COPY booster_robotics_sdk /opt/booster/sdk @@ -53,10 +57,6 @@ RUN /bin/bash -c "source /opt/ros/humble/setup.bash; cd /opt/booster/sdk_ros2 && COPY LocoApiPackage /opt/booster/LocoApiPackage # RUN /bin/bash -c "source /opt/ros/humble/setup.bash; cd /opt/booster/LocoApiPackage && colcon build --packages-select booster_msgs -# add custom bashrc -COPY configs/bashrc /root/.bashrc_addendum -RUN echo ". ~/.bashrc_addendum" >> ~/.bashrc - RUN mkdir /app COPY entrypoint.sh /app RUN chmod +x /app/entrypoint.sh @@ -64,9 +64,12 @@ RUN chmod +x /app/entrypoint.sh COPY delayed_start.sh /app RUN chmod +x /app/delayed_start.sh -# used to test communication, can be removed at a later date if so desired -RUN apt install -y iputils-ping netcat net-tools +# add custom bashrc +COPY configs/bashrc /root/.bashrc_addendum +RUN echo ". ~/.bashrc_addendum" >> ~/.bashrc + +COPY configs/system_settings_config.yaml /opt/booster/configs +COPY booster.conf /etc/supervisor/conf.d/ -RUN apt install ros-humble-image-view -y CMD ["/app/entrypoint.sh"] diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index bbb5588..5b6a038 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -39,7 +39,8 @@ stderr_logfile=/var/log/simbridge.err [program:maximus] directory=/app/maximus command=/app/maximus/bin/maximus_main +environment=LD_LIBRARY_PATH="/app/maximus/tools/vision/tensorrt/TensorRT/targets/x86_64-linux-gnu/lib:/app/booster_motion/lib:/app/booster_motion/lib-usr-local:/app/booster_motion/lib-x86_64-linux-gnu",FASTRTPS_DEFAULT_PROFILES_FILE="/app/booster_motion/fastdds_profile.xml",SPQR_CONFIG_ROOT="/app/maximus/config",SPQR_BEHAVIOR_TREE_PATH="/app/maximus/behaviors" autostart=false autorestart=true stdout_logfile=/var/log/maximus.log -stderr_logfile=/var/log/maximus.err \ No newline at end of file +stderr_logfile=/var/log/maximus.err diff --git a/dockerfiles/entrypoint.sh b/dockerfiles/entrypoint.sh index 9e63089..00523c7 100644 --- a/dockerfiles/entrypoint.sh +++ b/dockerfiles/entrypoint.sh @@ -1,6 +1,6 @@ #!/bin/bash -# Export dyanmic libraries and fastdds profile for booster_motion execution +# Export dynamic libraries and fastdds profile for booster_motion execution export LD_LIBRARY_PATH=/app/booster_motion/lib:/app/booster_motion/lib-usr-local:/app/booster_motion/lib-x86_64-linux-gnu:$LD_LIBRARY_PATH export FASTRTPS_DEFAULT_PROFILES_FILE=/app/booster_motion/fastdds_profile.xml diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 8faba12..53a819c 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -5,6 +5,7 @@ volumes: - "/tools/booster_motion:/app/booster_motion" - "/.pixi/envs/default:/app/bridge" - "/.pixi/envs/default:/app/maximus" + - "/tools/vision:/app/maximus/tools/vision" - "/src/app/config:/app/maximus/config" - "/src/app/behaviors/trees:/app/maximus/behaviors" - "/dev/shm/circus_ipc:/dev/shm/circus_ipc" diff --git a/src/Container.cpp b/src/Container.cpp index e1abd17..9b1253b 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -76,9 +76,6 @@ void Container::create(const std::string& robot_name, const std::string& image, std::vector binds_with_x11 = binds; binds_with_x11.push_back("/tmp/.X11-unix:/tmp/.X11-unix:rw"); - std::string xauth_path = envOrDefault("XAUTHORITY", "/root/.Xauthority"); - binds_with_x11.push_back(xauth_path + ":/root/.Xauthority:rw"); - payload["HostConfig"] = {{"Binds", binds_with_x11}, {"IpcMode", "host"}, {"CapAdd", {"SYS_NICE", "IPC_LOCK"}}, @@ -86,15 +83,23 @@ void Container::create(const std::string& robot_name, const std::string& image, {"Ulimits", nlohmann::json::array({{{"Name", "memlock"}, {"Soft", -1}, {"Hard", -1}}})}, {"Privileged", true}}; + payload["HostConfig"]["DeviceRequests"] = nlohmann::json::array(); + payload["HostConfig"]["DeviceRequests"].push_back({ + {"Driver", "nvidia"}, + {"Count", -1}, + {"Capabilities", nlohmann::json::array({nlohmann::json::array({"gpu"})})}, + }); + payload["Env"] = {"ROBOT_NAME=" + robot_name, "SERVER_IP=172.17.0.1", "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort), "DISPLAY=" + envOrDefault("DISPLAY", ":0"), - "XAUTHORITY=/root/.Xauthority", - "XDG_RUNTIME_DIR=/run/user/0", "QT_X11_NO_MITSHM=1", + "NVIDIA_VISIBLE_DEVICES=all", + "NVIDIA_DRIVER_CAPABILITIES=all", "ROBOT_STACK=booster", - "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc"}; + "CIRCUS_IMAGE_SHM_DIR=" + envOrDefault("CIRCUS_IMAGE_SHM_DIR", "/tmp/circus_ipc"), + }; payload["Entrypoint"] = {"/bin/bash", "-lc"}; payload["Cmd"] = {"/app/entrypoint.sh"}; From fe3512ee123f107fb1919f4c3e34b321f0f2f600 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Wed, 4 Mar 2026 18:16:00 +0100 Subject: [PATCH 35/56] Add symlink to tensorrt --- dockerfiles/entrypoint.sh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dockerfiles/entrypoint.sh b/dockerfiles/entrypoint.sh index 00523c7..7e734ee 100644 --- a/dockerfiles/entrypoint.sh +++ b/dockerfiles/entrypoint.sh @@ -8,5 +8,9 @@ export FASTRTPS_DEFAULT_PROFILES_FILE=/app/booster_motion/fastdds_profile.xml export SPQR_CONFIG_ROOT=/app/maximus/config export SPQR_BEHAVIOR_TREE_PATH=/app/maximus/behaviors +source /app/maximus/tools/vision/scripts/detect-tensorrt.sh +TENSORRT_LINKNAME=$(echo $TENSORRT_DIR | sed 's:[^/]*$:TensorRT:') +ln -s $TENSORRT_DIR $TENSORRT_LINKNAME + # Start supervisord to manage processes /usr/bin/supervisord -n -c /etc/supervisor/conf.d/booster.conf From c17531999f9ef9ec19a7c4a91961269d9b11bfcd Mon Sep 17 00:00:00 2001 From: Flavio Date: Sat, 7 Mar 2026 19:00:45 +0100 Subject: [PATCH 36/56] goal posts positions in oracle --- include/sensors/Oracle.h | 52 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 51 insertions(+), 1 deletion(-) diff --git a/include/sensors/Oracle.h b/include/sensors/Oracle.h index c30a87d..0ef99bb 100644 --- a/include/sensors/Oracle.h +++ b/include/sensors/Oracle.h @@ -43,6 +43,8 @@ class Oracle : public Sensor { // Update all other robots' local positions updateTeammatesLocalPositions(); updateOpponentsLocalPositions(); + updateGoalPostsLocalPositions(); + } msgpack::object doSerialize(msgpack::zone& z) override { @@ -65,8 +67,17 @@ class Oracle : public Sensor { opponents_data[robotName] = msgpack::object(robot_pos, z); } + // Serialize goal posts' positions + std::map goal_posts_data; + for (const auto& [postName, postLocalPos] : goalPostsLocalPositions) { + std::vector post_pos = {postLocalPos(0), postLocalPos(1), postLocalPos(2)}; + goal_posts_data[postName] = msgpack::object(post_pos, z); + } + oracle_data["teammates_positions"] = msgpack::object(teammates_data, z); oracle_data["opponents_positions"] = msgpack::object(opponents_data, z); + oracle_data["goal_posts_positions"] = msgpack::object(goal_posts_data, z); + return msgpack::object(oracle_data, z); } @@ -98,6 +109,11 @@ class Oracle : public Sensor { return teammatesLocalPositions; } + const std::map& getGoalPostsLocalPositions() const { + return goalPostsLocalPositions; + } + + private: /** @@ -209,6 +225,33 @@ class Oracle : public Sensor { } } + void updateGoalPostsLocalPositions() { + goalPostsLocalPositions.clear(); + + for (const auto& postName : goalPostNames) { + int geomId = mj_name2id(mujModel, mjOBJ_GEOM, postName.c_str()); + if (geomId < 0) { + continue; + } + + // Get global position from xpos (geom positions in world frame) + // For geoms, xpos is indexed by geom ID + Eigen::Vector3d goalPostGlobalPos( + mujData->geom_xpos[geomId * 3 + 0], + mujData->geom_xpos[geomId * 3 + 1], + mujData->geom_xpos[geomId * 3 + 2] + ); + + // Convert to local frame + Eigen::Vector3d goalPostLocalPos = globalToLocalPosition( + goalPostGlobalPos, + robotPose->getTransformationMatrix() + ); + + goalPostsLocalPositions[postName] = goalPostLocalPos; + } + } + Eigen::Vector3d ballPosition; int ballId, ballAdr, ballPosAdr, ballVelAdr; @@ -219,10 +262,17 @@ class Oracle : public Sensor { Pose* robotPose; std::map teammatesLocalPositions; std::map opponentsLocalPositions; + std::map goalPostsLocalPositions; + std::map teammatesSensorAddrs; // robotName -> sensorAdr std::map opponentsSensorAddrs; // robotName -> sensorAdr - + std::vector goalPostNames = { + "left_goal_left_post", + "left_goal_right_post", + "right_goal_left_post", + "right_goal_right_post", + }; }; } // namespace spqr From faac6b2322149ef3d7059ae862e0c762dbf10c1e Mon Sep 17 00:00:00 2001 From: Flavio Date: Sat, 7 Mar 2026 19:34:48 +0100 Subject: [PATCH 37/56] fix for using image_view inside docker + correct path for shm in ram --- src/Container.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/Container.cpp b/src/Container.cpp index 9b1253b..0438a93 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -76,6 +76,9 @@ void Container::create(const std::string& robot_name, const std::string& image, std::vector binds_with_x11 = binds; binds_with_x11.push_back("/tmp/.X11-unix:/tmp/.X11-unix:rw"); + std::string xauth_path = envOrDefault("XAUTHORITY", "/root/.Xauthority"); + binds_with_x11.push_back(xauth_path + ":/root/.Xauthority:rw"); + payload["HostConfig"] = {{"Binds", binds_with_x11}, {"IpcMode", "host"}, {"CapAdd", {"SYS_NICE", "IPC_LOCK"}}, @@ -89,7 +92,7 @@ void Container::create(const std::string& robot_name, const std::string& image, {"Count", -1}, {"Capabilities", nlohmann::json::array({nlohmann::json::array({"gpu"})})}, }); - + payload["Env"] = {"ROBOT_NAME=" + robot_name, "SERVER_IP=172.17.0.1", "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort), @@ -97,9 +100,10 @@ void Container::create(const std::string& robot_name, const std::string& image, "QT_X11_NO_MITSHM=1", "NVIDIA_VISIBLE_DEVICES=all", "NVIDIA_DRIVER_CAPABILITIES=all", + "XAUTHORITY=/root/.Xauthority", + "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", - "CIRCUS_IMAGE_SHM_DIR=" + envOrDefault("CIRCUS_IMAGE_SHM_DIR", "/tmp/circus_ipc"), - }; + "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc"}; payload["Entrypoint"] = {"/bin/bash", "-lc"}; payload["Cmd"] = {"/app/entrypoint.sh"}; From a918cf8bb0659a567da92f729054396be693a42c Mon Sep 17 00:00:00 2001 From: Flavio Date: Sun, 8 Mar 2026 02:16:50 +0100 Subject: [PATCH 38/56] german opens instructions to run a simulation using the framework --- GERMAN-OPEN 2026 Instructions.md | 42 ++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 GERMAN-OPEN 2026 Instructions.md diff --git a/GERMAN-OPEN 2026 Instructions.md b/GERMAN-OPEN 2026 Instructions.md new file mode 100644 index 0000000..b9f831c --- /dev/null +++ b/GERMAN-OPEN 2026 Instructions.md @@ -0,0 +1,42 @@ +# GERMAN-OPEN 2026 Instructions + +Questa è la guida per far funzionare la simulazione con il framework spqrbooster2026. + +I seguenti passaggi vanno eseguiti in ordine per setuppare tutto + +## Simbridge +0) Assicurati di essere nel branch `dev` +1) Lancia il comando `pixi install`. Non serve fare altro + +## Maximus +0) Assicurati che in `config/framework.yaml` ci sia la configurazione giusta di topic e moduli +1) Assciurati che in `src/app/pixi.toml` ci sia la versione di `booster_robotics_sdk` corretta +2) Lancia il comando `pixi install` per installare l'ambiente +3) Lancia il comando `pixi run main` per buildare e lanciare il programma + +## Circus +1) Assicurati di essere nel branch `oracle` +2) Clona dentro la cartella `dockerfiles` le repo di booster: +``` +cd dockerfiles +git clone https://github.com/BoosterRobotics/booster_robotics_sdk.git +git clone https://github.com/BoosterRobotics/booster_robotics_sdk_ros2.git +``` +PS: questo step non è realmente necessario per il funzionamento di tutto, serve per poter interagire con i messaggi ros dentro al docker. Se non volete clonare le repo, dovete modificare il dockerFile per non eseguire le istruzioni a loro collegate. Secondo me per il momento conviene farlo. + +3) Builda il docker: +``` +cd dockerfiles +docker build -t spqr:booster . +``` + +4) Dentro la cartella `resources` crea il file `path_constants.yaml` inserendo i path assoluti del tuo pc dei seguenti componenti: +``` +maximus: /home/flavio/Scrivania/RoboCup/spqrbooster2026 +circus: /home/flavio/Scrivania/RoboCup/circus +booster_robotics_sdk: /home/flavio/Scrivania/RoboCup/circus/dockerfiles/booster_robotics_sdk +simbridge: /home/flavio/Scrivania/RoboCup/simbridge +``` + +5) Istalla l'ambiente con il comando `pixi install` +6) Lancia la simulazione con il comando `pixi run circus` From 2cf13d14f15247df3c09db961e70fc067ff33f0a Mon Sep 17 00:00:00 2001 From: Flavio Date: Sun, 8 Mar 2026 02:47:37 +0100 Subject: [PATCH 39/56] other information for go26 --- GERMAN-OPEN 2026 Instructions.md | 49 ++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/GERMAN-OPEN 2026 Instructions.md b/GERMAN-OPEN 2026 Instructions.md index b9f831c..bc41467 100644 --- a/GERMAN-OPEN 2026 Instructions.md +++ b/GERMAN-OPEN 2026 Instructions.md @@ -40,3 +40,52 @@ simbridge: /home/flavio/Scrivania/RoboCup/simbridge 5) Istalla l'ambiente con il comando `pixi install` 6) Lancia la simulazione con il comando `pixi run circus` + + +## Dopo l'istallazione + +Una volta che avete istallato tutto correttamente, vi basta rilanciare `pixi run circus` per avviare la simulazione +NB: se modificate il framework (maximus) ricordate di compilarlo nuovamente con `pixi run main` +NB: se modificate il simbridge per qualche motivo, ricordate di compilarlo nuovamente con `pixi install` +NB: se modificate il Dockerfile o un qualsiasi file dentro la cartella `dockerfiles`, ricordate di compilare nuovamente il docker con `docker build -t spqr:booster dockerfiles` (se vi trovare nella root), oppure `docker build -t spqr:booster .` se vi trovate già in `dockerfiles`) + +--- +## Utils +- Prima che la simulazione parta, i robot si connettono al simulatore e dichiarano di essere pronti. Dopo qualche secondo, una volta che tutti i robot sono pronti, la simulazione parte. Un esempio delle stampe che dovrebbero apparire è questo: +``` +Connected Robot: red_Booster-T1_0 +Sending initial message to red_Booster-T1_0 +Connected Robot: blue_Booster-T1_0 +Sending initial message to blue_Booster-T1_0 +Robot ready: red_Booster-T1_0 +Robot ready: blue_Booster-T1_0 +All robots are ready! +Starting simulation! +``` +- Se non tutti i robot sono `ready`, la simulazione non parte. Potete capire chi è il robot problematico da quello che non dichiara di essere `ready`. +- Se un robot non parte, potete entrare nel suo docker con + ``` + docker exec -it CIRCUS_red_Booster-T1_0_container bash + ``` + Una volta entrati, con il comando `cat supervisord.log` potete vedere gli stati dei processi, se qualcuno è crashato, se qualcuno era crashato e ha tentato di ripartire, ... + Se tutto è andato a buon fine, il vostro output dovrebbe essere come questo: + ``` + 2026-03-08 01:37:31,915 CRIT Supervisor is running as root. Privileges were not dropped because no user is specified in the config file. If you intend to run as root, you can set user=root in the config file to avoid this message. + 2026-03-08 01:37:31,929 INFO RPC interface 'supervisor' initialized + 2026-03-08 01:37:31,929 CRIT Server 'unix_http_server' running without any HTTP authentication checking + 2026-03-08 01:37:31,929 INFO supervisord started with pid 8 + 2026-03-08 01:37:33,775 INFO spawned: 'booster-motion' with pid 9 + 2026-03-08 01:37:33,811 INFO spawned: 'delayed-starter' with pid 10 + 2026-03-08 01:37:34,813 INFO success: booster-motion entered RUNNING state, process has stayed up for > than 1 seconds (startsecs) + 2026-03-08 01:37:34,814 INFO success: delayed-starter entered RUNNING state, process has stayed up for > than 0 seconds (startsecs) + 2026-03-08 01:37:38,456 INFO spawned: 'simbridge' with pid 32 + 2026-03-08 01:37:39,458 INFO success: simbridge entered RUNNING state, process has stayed up for > than 1 seconds (startsecs) + 2026-03-08 01:37:40,526 INFO spawned: 'maximus' with pid 42 + 2026-03-08 01:37:41,614 INFO success: maximus entered RUNNING state, process has stayed up for > than 1 seconds (startsecs) + 2026-03-08 01:37:41,663 INFO exited: delayed-starter (exit status 0; expected) + ``` + Il fatto che il processo `delayed-starter` termini è corretto, sono gli altri processi che non devono terminare. +- Per vedere i log di maximus, lancia il comando `cat /var/log/maximus.log` (oppure `maximus.err` per i log di errore) +- Per vedere i log di simbridge, lancia il comando `cat /var/log/simbridge.log` (oppure `simbridge.err` per i log di errore) +- Per vedere i log di booster-motion, lancia il comando `cat /var/log/booster-motion.log` (oppure `booster-motion.err` per i log di errore). Nel file `booster-motion.err`, non vi preoccupate se c'è `msgget: No such file or directory` + From 96f178b49075d34d9933bf77d07251cd696e3037 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 9 Mar 2026 13:52:48 +0100 Subject: [PATCH 40/56] Change team numbers in scene --- resources/scenes/1v1.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index 8e91e04..3bdd712 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -1,14 +1,14 @@ simulation_config: default teams: red: - number: 8 + number: 19 players: - type: Booster-T1 number: 3 position: [-1.0, 0.0, 0.68] orientation: [0.0, 0.0, 0.0] blue: - number: 51 + number: 5 players: - type: Booster-T1 number: 3 From 58b8cbcff9661e39eb5e320adbd82f38bffdc4d7 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 9 Mar 2026 13:54:08 +0100 Subject: [PATCH 41/56] Move send_all to cpp --- include/RobotManager.h | 28 +--------------------------- src/RobotManager.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 6be78d3..2c09ed0 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -59,33 +59,7 @@ class RobotManager { RobotManager(const RobotManager&) = delete; RobotManager& operator=(const RobotManager&) = delete; - // Source - https://stackoverflow.com/a - // Posted by Arun, modified by community. See post 'Timeline' for change history - // Retrieved 2026-01-12, License - CC BY-SA 3.0 - // TCP Communication, it sends before the size of the message and then the message itself - ssize_t send_all(int fd, char *buf, size_t len) - { - // First, send the size of the message - uint32_t msg_size = htonl(len); - ssize_t bytes_sent = send(fd, &msg_size, sizeof(msg_size), 0); - if (bytes_sent != sizeof(msg_size)) { - return -1; - } - - ssize_t total = 0; // how many bytes we've sent - size_t bytesleft = len; // how many we have left to send - ssize_t n = 0; - while(total < len) { - n = send(fd, buf+total, bytesleft, 0); - if (n == -1) { - /* print/log error details */ - return -1; - } - total += n; - bytesleft -= n; - } - return total; - } + ssize_t send_all(int fd, char *buf, size_t len); void _serverInternal(int port); diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 9246f12..d562c96 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -268,6 +268,33 @@ void RobotManager::_serverInternal(int port) { close(fd.fd); } +// Source - https://stackoverflow.com/a +// Posted by Arun, modified by community. See post 'Timeline' for change history +// Retrieved 2026-01-12, License - CC BY-SA 3.0 +// TCP Communication, it sends before the size of the message and then the message itself +ssize_t RobotManager::send_all(int fd, char *buf, size_t len) { + // First, send the size of the message + uint32_t msg_size = htonl(len); + ssize_t bytes_sent = send(fd, &msg_size, sizeof(msg_size), 0); + if (bytes_sent != sizeof(msg_size)) { + return -1; + } + + ssize_t total = 0; // how many bytes we've sent + size_t bytesleft = len; // how many we have left to send + ssize_t n = 0; + while(total < len) { + n = send(fd, buf+total, bytesleft, 0); + if (n == -1) { + /* print/log error details */ + return -1; + } + total += n; + bytesleft -= n; + } + return total; +} + void RobotManager::setAreAllRobotsReadyCallback(std::function cb) { From 8ab16db6dc5e665788ccd6656988be36104d1c85 Mon Sep 17 00:00:00 2001 From: Francesco Petri Date: Mon, 9 Mar 2026 14:00:15 +0100 Subject: [PATCH 42/56] Format --- GERMAN-OPEN 2026 Instructions.md | 1 - dockerfiles/Dockerfile | 4 +- dockerfiles/delayed_start.sh | 4 +- include/CircusNetwork.h | 25 +++-- include/DockerREST.h | 100 ++++++++++--------- include/RobotManager.h | 58 +++++------ include/Team.h | 2 +- include/robots/BoosterT1.h | 13 ++- include/robots/Robot.h | 14 +-- include/sensors/ImageSharedMemoryWriter.h | 16 +-- include/sensors/Oracle.h | 115 ++++++++-------------- resources/scenes/1v1.yaml | 16 +-- resources/scenes/5v5.yaml | 80 +++++++-------- src/AppWindow.cpp | 2 +- src/Container.cpp | 74 ++++++-------- src/RobotManager.cpp | 34 +++---- src/Utils.cpp | 1 + 17 files changed, 253 insertions(+), 306 deletions(-) diff --git a/GERMAN-OPEN 2026 Instructions.md b/GERMAN-OPEN 2026 Instructions.md index bc41467..7bfc50c 100644 --- a/GERMAN-OPEN 2026 Instructions.md +++ b/GERMAN-OPEN 2026 Instructions.md @@ -88,4 +88,3 @@ Starting simulation! - Per vedere i log di maximus, lancia il comando `cat /var/log/maximus.log` (oppure `maximus.err` per i log di errore) - Per vedere i log di simbridge, lancia il comando `cat /var/log/simbridge.log` (oppure `simbridge.err` per i log di errore) - Per vedere i log di booster-motion, lancia il comando `cat /var/log/booster-motion.log` (oppure `booster-motion.err` per i log di errore). Nel file `booster-motion.err`, non vi preoccupate se c'è `msgget: No such file or directory` - diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile index 3acf1e0..6bc8469 100644 --- a/dockerfiles/Dockerfile +++ b/dockerfiles/Dockerfile @@ -28,10 +28,10 @@ RUN apt-get update && apt-get install -y \ libxkbcommon0 \ libdecor-0-0 \ ament-cmake \ - iputils-ping \ + iputils-ping \ netcat \ ros-humble-image-view \ - net-tools + net-tools RUN rm -rf /var/lib/apt/lists/* # used to test communication, can be removed at a later date if so desired diff --git a/dockerfiles/delayed_start.sh b/dockerfiles/delayed_start.sh index 36dd7af..b6293bc 100644 --- a/dockerfiles/delayed_start.sh +++ b/dockerfiles/delayed_start.sh @@ -5,5 +5,5 @@ sleep 3 # Start simbridge and maximus in parallel # This substitute the timer of 5 seconds inside the bridge. # In this way the bridge (or maximus) does not need to have the -# check on the timer if the code is run on the robot or on simulator -supervisorctl -c /etc/supervisor/conf.d/booster.conf start simbridge maximus \ No newline at end of file +# check on the timer if the code is run on the robot or on simulator +supervisorctl -c /etc/supervisor/conf.d/booster.conf start simbridge maximus diff --git a/include/CircusNetwork.h b/include/CircusNetwork.h index c0da201..262f379 100644 --- a/include/CircusNetwork.h +++ b/include/CircusNetwork.h @@ -1,5 +1,6 @@ #include #include + #include "DockerREST.h" #define CIRCUS_NETWORK_NAME "CIRCUS_network" @@ -35,17 +36,14 @@ class CircusNetwork { std::string subnet = UAN_SEVEN_CIU "0.0/16"; std::string gateway = UAN_SEVEN_CIU "0.1"; payload["Name"] = CIRCUS_NETWORK_NAME; - payload["IPAM"]["Config"] = {{ // https://docs.docker.com/reference/api/engine/version/v1.53/#tag/Network/operation/NetworkCreate - {"Subnet", subnet}, - {"IPRange", subnet}, - {"Gateway", gateway} - }}; - payload["Options"] = { - {"com.docker.network.bridge.name", "docker-circus"}, - {"com.docker.network.bridge.enable_ip_masquerade", "false"}, - {"com.docker.network.driver.mtu", "1500"}, - {"com.docker.network.container_iface_prefix", "circus"} - }; + payload["IPAM"]["Config"] = {{// https://docs.docker.com/reference/api/engine/version/v1.53/#tag/Network/operation/NetworkCreate + {"Subnet", subnet}, + {"IPRange", subnet}, + {"Gateway", gateway}}}; + payload["Options"] = {{"com.docker.network.bridge.name", "docker-circus"}, + {"com.docker.network.bridge.enable_ip_masquerade", "false"}, + {"com.docker.network.driver.mtu", "1500"}, + {"com.docker.network.container_iface_prefix", "circus"}}; std::string resp_raw = curlClient.request(POST, create_network_endpoint, CREATE_OK_RESPONSE, &payload); @@ -57,7 +55,7 @@ class CircusNetwork { } private: - CircusNetwork(const std::string& sockPath = "/var/run/docker.sock") : curlClient(sockPath) {}; + CircusNetwork(const std::string& sockPath = "/var/run/docker.sock") : curlClient(sockPath){}; ~CircusNetwork() { if (isUp()) { @@ -67,11 +65,10 @@ class CircusNetwork { std::string networkId = ""; CURLClient curlClient; - + public: CircusNetwork(CircusNetwork const&) = delete; void operator=(CircusNetwork const&) = delete; - }; } // namespace spqr diff --git a/include/DockerREST.h b/include/DockerREST.h index 039ddbe..f0b9c86 100644 --- a/include/DockerREST.h +++ b/include/DockerREST.h @@ -1,9 +1,10 @@ #pragma once #include +#include #include #include -#include + #include "curl/curl.h" #define POST "POST" @@ -58,61 +59,62 @@ inline std::string remove_network_endpoint(const std::string& id) { class CURLClient { public: - CURLClient(const std::string& sockPath) : sockPath(sockPath) { - curl_handle = curl_easy_init(); - if (!curl_handle) - throw std::runtime_error("Failed to init curl handle"); - } - ~CURLClient() { - if (curl_handle) - curl_easy_cleanup(curl_handle); - }; - - std::string request(const std::string& method, const std::string& endpoint, const long expected_response, const nlohmann::json* body = nullptr) { - curl_easy_reset(curl_handle); - std::string url = "http://localhost" + endpoint; - - curl_easy_setopt(curl_handle, CURLOPT_UNIX_SOCKET_PATH, sockPath.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_CUSTOMREQUEST, method.c_str()); - - struct curl_slist* headers = nullptr; - headers = curl_slist_append(headers, "Content-Type: application/json"); - curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, headers); - - const auto write_callback = +[](char* ptr, size_t size, size_t nmemb, void* userdata) -> size_t { - std::string* resp = static_cast(userdata); - resp->append(ptr, size * nmemb); - return size * nmemb; + CURLClient(const std::string& sockPath) : sockPath(sockPath) { + curl_handle = curl_easy_init(); + if (!curl_handle) + throw std::runtime_error("Failed to init curl handle"); + } + ~CURLClient() { + if (curl_handle) + curl_easy_cleanup(curl_handle); }; - curl_easy_setopt(curl_handle, CURLOPT_WRITEFUNCTION, write_callback); - std::string response; - curl_easy_setopt(curl_handle, CURLOPT_WRITEDATA, &response); + std::string request(const std::string& method, const std::string& endpoint, const long expected_response, + const nlohmann::json* body = nullptr) { + curl_easy_reset(curl_handle); + std::string url = "http://localhost" + endpoint; - std::string postData; - if (body != nullptr) { - postData = body->dump(); - curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, postData.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDSIZE, postData.size()); - } + curl_easy_setopt(curl_handle, CURLOPT_UNIX_SOCKET_PATH, sockPath.c_str()); + curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str()); + curl_easy_setopt(curl_handle, CURLOPT_CUSTOMREQUEST, method.c_str()); + + struct curl_slist* headers = nullptr; + headers = curl_slist_append(headers, "Content-Type: application/json"); + curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, headers); - long response_code = 0; - CURLcode res = curl_easy_perform(curl_handle); - curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &response_code); - curl_slist_free_all(headers); + const auto write_callback = +[](char* ptr, size_t size, size_t nmemb, void* userdata) -> size_t { + std::string* resp = static_cast(userdata); + resp->append(ptr, size * nmemb); + return size * nmemb; + }; - if (res != CURLE_OK) - throw std::runtime_error(std::string("Curl error: ") + curl_easy_strerror(res)); + curl_easy_setopt(curl_handle, CURLOPT_WRITEFUNCTION, write_callback); + std::string response; + curl_easy_setopt(curl_handle, CURLOPT_WRITEDATA, &response); - if (expected_response && response_code != expected_response) - throw std::runtime_error("Docker API request to " + endpoint + " failed: HTTP " + std::to_string(response_code) + ". " + response); + std::string postData; + if (body != nullptr) { + postData = body->dump(); + curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, postData.c_str()); + curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDSIZE, postData.size()); + } - return response; - } + long response_code = 0; + CURLcode res = curl_easy_perform(curl_handle); + curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &response_code); + curl_slist_free_all(headers); - CURL* curl_handle; - std::string sockPath; + if (res != CURLE_OK) + throw std::runtime_error(std::string("Curl error: ") + curl_easy_strerror(res)); + + if (expected_response && response_code != expected_response) + throw std::runtime_error("Docker API request to " + endpoint + " failed: HTTP " + std::to_string(response_code) + ". " + response); + + return response; + } + + CURL* curl_handle; + std::string sockPath; }; -} +} // namespace spqr diff --git a/include/RobotManager.h b/include/RobotManager.h index 2c09ed0..fd1fe5d 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -27,12 +27,12 @@ namespace spqr { class Team; // Forward declaration class RobotManager { - public: - // Singleton class - static RobotManager& instance() { - static RobotManager mgr; - return mgr; - } + public: + // Singleton class + static RobotManager& instance() { + static RobotManager mgr; + return mgr; + } void registerRobot(std::shared_ptr robot); std::vector> getRobots() const; @@ -52,40 +52,36 @@ class RobotManager { void setAreAllRobotsReadyCallback(std::function cb); - private: - RobotManager() = default; - ~RobotManager() = default; + private: + RobotManager() = default; + ~RobotManager() = default; - RobotManager(const RobotManager&) = delete; - RobotManager& operator=(const RobotManager&) = delete; + RobotManager(const RobotManager&) = delete; + RobotManager& operator=(const RobotManager&) = delete; - ssize_t send_all(int fd, char *buf, size_t len); + ssize_t send_all(int fd, char* buf, size_t len); - void _serverInternal(int port); + void _serverInternal(int port); - void areAllRobotsReadyWrapper(); - bool areAllRobotsReady() const; + void areAllRobotsReadyWrapper(); + bool areAllRobotsReady() const; - std::atomic serverRunning_ = false; - std::thread serverThread_; + std::atomic serverRunning_ = false; + std::thread serverThread_; - mutable std::mutex mutex_; - std::vector> robots_; - std::function areAllRobotsReadyCallback_; + mutable std::mutex mutex_; + std::vector> robots_; + std::function areAllRobotsReadyCallback_; - using RobotCreator - = std::function(const std::string&, const std::string&, uint8_t, const Eigen::Vector3d&, const Eigen::Vector3d&, - const std::string&, const std::shared_ptr&)>; + using RobotCreator = std::function(const std::string&, const std::string&, uint8_t, const Eigen::Vector3d&, + const Eigen::Vector3d&, const std::string&, const std::shared_ptr&)>; std::unordered_map robotFactory - = { - {"Booster-K1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& colorName, auto&& team) { - return std::make_shared(name, type, number, pos, ori, colorName, team); - }}, - {"Booster-T1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& colorName, auto&& team) { - return std::make_shared(name, type, number, pos, ori, colorName, team); - }} - }; + = {{"Booster-K1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& colorName, + auto&& team) { return std::make_shared(name, type, number, pos, ori, colorName, team); }}, + {"Booster-T1", [](auto&& name, auto&& type, uint8_t number, auto&& pos, auto&& ori, auto&& colorName, auto&& team) { + return std::make_shared(name, type, number, pos, ori, colorName, team); + }}}; }; } // namespace spqr diff --git a/include/Team.h b/include/Team.h index ef4e36f..fd1b1ef 100644 --- a/include/Team.h +++ b/include/Team.h @@ -12,7 +12,7 @@ class Team { public: Team(const std::string& name, int number) : name(name), number(number) {} ~Team() {} - + std::string name; int number; std::vector> robots; diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 4b73a7b..819dd44 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -18,13 +18,13 @@ #include "MujocoContext.h" #include "robots/Robot.h" -#include "sensors/ImageSharedMemoryWriter.h" #include "sensors/CameraDepth.h" #include "sensors/CameraRGB.h" +#include "sensors/ImageSharedMemoryWriter.h" #include "sensors/Imu.h" #include "sensors/Joint.h" -#include "sensors/Pose.h" #include "sensors/Oracle.h" +#include "sensors/Pose.h" #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { @@ -66,7 +66,7 @@ class BoosterT1 : public Robot { {JointValue::KNEE_RIGHT_PITCH, name + "_Right_Knee_Pitch"}, {JointValue::ANKLE_RIGHT_PITCH, name + "_Right_Ankle_Pitch"}, {JointValue::ANKLE_RIGHT_ROLL, name + "_Right_Ankle_Roll"}} { - //Where to put the images + // Where to put the images shm_dir_ = "/dev/shm/circus_ipc"; } @@ -98,16 +98,15 @@ class BoosterT1 : public Robot { {JointValue::KNEE_RIGHT_PITCH, 0}, {JointValue::ANKLE_RIGHT_PITCH, 0}, {JointValue::ANKLE_RIGHT_ROLL, 0}}); - + rgbCamera = new CameraRGB(mujCtx, (name + "_rgb_cam").c_str()); depthCamera = new CameraDepth(mujCtx, (name + "_depth_cam").c_str()); - //Configure the writer for the shared memory file + // Configure the writer for the shared memory file const int width = rgbCamera->getWidth(); const int height = rgbCamera->getHeight(); rgb_writer_.configure(shmFilePath_("rgb"), width, height, 3); depth_writer_.configure(shmFilePath_("depth"), width, height, 1); - // Create Oracle with the pose and all robots oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); @@ -145,7 +144,7 @@ class BoosterT1 : public Robot { msg["joints"] = joints->serialize(buffer_zone_); msg["oracle"] = oracle->serialize(buffer_zone_); - // Write in the shared file the information + // Write in the shared file the information rgb_writer_.write(rgbCamera->getImage()); depth_writer_.write(depthCamera->getDepth8bit()); diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 18289b5..403fd9e 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -27,14 +27,14 @@ class Robot { Robot(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team) : name(name), type(type), number(number), initPosition(initPosition), initOrientation(initOrientation), colorName(colorName), team(team) { - if (colorName == "red") { - color = {130, 36, 51}; - } else if (colorName == "blue") { - color = {0, 103, 120}; - } else { - throw std::runtime_error("Team color currently unsupported: " + colorName); - } + if (colorName == "red") { + color = {130, 36, 51}; + } else if (colorName == "blue") { + color = {0, 103, 120}; + } else { + throw std::runtime_error("Team color currently unsupported: " + colorName); } + } virtual ~Robot() = default; virtual void bindMujoco(MujocoContext* mujContext) = 0; virtual void update() = 0; diff --git a/include/sensors/ImageSharedMemoryWriter.h b/include/sensors/ImageSharedMemoryWriter.h index 9ed1719..7abf3b6 100644 --- a/include/sensors/ImageSharedMemoryWriter.h +++ b/include/sensors/ImageSharedMemoryWriter.h @@ -95,14 +95,14 @@ class ImageSharedMemoryWriter { private: struct Header { - uint32_t magic = 0; - uint32_t version = 0; - uint32_t width = 0; - uint32_t height = 0; - uint32_t channels = 0; - uint32_t slot_count = 0; - size_t slot_bytes = 0; - uint64_t seq = 0; + uint32_t magic = 0; + uint32_t version = 0; + uint32_t width = 0; + uint32_t height = 0; + uint32_t channels = 0; + uint32_t slot_count = 0; + size_t slot_bytes = 0; + uint64_t seq = 0; }; static constexpr uint32_t kMagic = 0x5348514d; // SHQM diff --git a/include/sensors/Oracle.h b/include/sensors/Oracle.h index 0ef99bb..db2ba7f 100644 --- a/include/sensors/Oracle.h +++ b/include/sensors/Oracle.h @@ -5,8 +5,8 @@ #include #include #include -#include #include +#include #include "sensors/Sensor.h" namespace spqr { @@ -19,9 +19,8 @@ class Pose; // It can contain also the information their velocities, ... class Oracle : public Sensor { public: - Oracle(mjModel* mujModel, mjData* mujData, std::string robotName, Pose* robotPose) + Oracle(mjModel* mujModel, mjData* mujData, std::string robotName, Pose* robotPose) : mujModel(mujModel), mujData(mujData), robotName(robotName), robotPose(robotPose) { - size_t pos = robotName.find('_'); teamName = robotName.substr(0, pos); @@ -30,7 +29,6 @@ class Oracle : public Sensor { ballPosAdr = mujModel->jnt_qposadr[ballAdr]; ballVelAdr = mujModel->jnt_dofadr[ballAdr]; - // AGGIUNGERE TUTTI I CAMPI CHE CI INTERESSANO SAPERE COME ORACLE discoverOtherRobotSensors(); @@ -44,7 +42,6 @@ class Oracle : public Sensor { updateTeammatesLocalPositions(); updateOpponentsLocalPositions(); updateGoalPostsLocalPositions(); - } msgpack::object doSerialize(msgpack::zone& z) override { @@ -52,18 +49,18 @@ class Oracle : public Sensor { std::vector ball_pos = {ballPosition(0), ballPosition(1), ballPosition(2)}; std::map oracle_data; oracle_data["ball_position"] = msgpack::object(ball_pos, z); - + // Serialize teammates' positions std::map teammates_data; for (const auto& [robotName, robotLocalPos] : teammatesLocalPositions) { - std::vector robot_pos = {robotLocalPos(0), robotLocalPos(1), robotLocalPos(2)}; + std::vector robot_pos = {robotLocalPos(0), robotLocalPos(1), robotLocalPos(2)}; teammates_data[robotName] = msgpack::object(robot_pos, z); } // Serialize opponents' positions std::map opponents_data; for (const auto& [robotName, robotLocalPos] : opponentsLocalPositions) { - std::vector robot_pos = {robotLocalPos(0), robotLocalPos(1), robotLocalPos(2)}; + std::vector robot_pos = {robotLocalPos(0), robotLocalPos(1), robotLocalPos(2)}; opponents_data[robotName] = msgpack::object(robot_pos, z); } @@ -73,12 +70,11 @@ class Oracle : public Sensor { std::vector post_pos = {postLocalPos(0), postLocalPos(1), postLocalPos(2)}; goal_posts_data[postName] = msgpack::object(post_pos, z); } - + oracle_data["teammates_positions"] = msgpack::object(teammates_data, z); oracle_data["opponents_positions"] = msgpack::object(opponents_data, z); oracle_data["goal_posts_positions"] = msgpack::object(goal_posts_data, z); - - + return msgpack::object(oracle_data, z); } @@ -88,7 +84,7 @@ class Oracle : public Sensor { /** * @brief Get the local position of another robot - * + * * @param robotName The name of the robot * @return The position of the robot in the local frame, or nullopt if not found */ @@ -102,7 +98,7 @@ class Oracle : public Sensor { /** * @brief Get all other robots' positions in local frame - * + * * @return Map of robot names to their local positions */ const std::map& getAllTeammatesLocalPositions() const { @@ -113,28 +109,23 @@ class Oracle : public Sensor { return goalPostsLocalPositions; } - - private: /** * @brief Transform a global position to local position relative to a robot's pose - * + * * This function can be used to convert any global world coordinates to local * robot-centric coordinates using the transformation matrix. - * + * * @param globalPosition The position in global world frame (x, y, z) * @param robotTransformationMatrix The robot's 4x4 transformation matrix (world-to-robot) * @return The position relative to the robot's local frame */ - static Eigen::Vector3d globalToLocalPosition( - const Eigen::Vector3d& globalPosition, - const Eigen::Matrix4d& robotTransformationMatrix) { - + static Eigen::Vector3d globalToLocalPosition(const Eigen::Vector3d& globalPosition, const Eigen::Matrix4d& robotTransformationMatrix) { // The transformation matrix T converts from local to global: p_global = T * p_local // So to get p_local: p_local = T^(-1) * p_global Eigen::Vector4d globalPosHomogeneous(globalPosition(0), globalPosition(1), globalPosition(2), 1.0); Eigen::Vector4d localPosHomogeneous = robotTransformationMatrix.inverse() * globalPosHomogeneous; - + return Eigen::Vector3d(localPosHomogeneous(0), localPosHomogeneous(1), localPosHomogeneous(2)); } @@ -142,32 +133,32 @@ class Oracle : public Sensor { // Scan all sensors in MuJoCo to find position sensors of other robots for (int i = 0; i < mujModel->nsensor; i++) { const char* sensorName = mj_id2name(mujModel, mjOBJ_SENSOR, i); - if (!sensorName) continue; - + if (!sensorName) + continue; + std::string sName(sensorName); - + // Look for position sensors (assuming naming convention: {robotName}_position) size_t pos = sName.rfind("_position"); if (pos != std::string::npos) { std::string extractedRobotName = sName.substr(0, pos); - + // Skip if it's the current robot if (extractedRobotName == robotName) { continue; } - + // Store the sensor address for this robot int positionAdr = mujModel->sensor_adr[i]; - + size_t pos = extractedRobotName.find('_'); std::string extractedRobotTeamName = extractedRobotName.substr(0, pos); - - if(extractedRobotTeamName == teamName) { + + if (extractedRobotTeamName == teamName) { teammatesSensorAddrs[extractedRobotName] = positionAdr; - } - else { + } else { opponentsSensorAddrs[extractedRobotName] = positionAdr; - } + } } } } @@ -179,49 +170,37 @@ class Oracle : public Sensor { /** * @brief Update positions of all other robots in local frame - * + * * Iterates through all robots in the world (except the current one) and converts * their global positions to local coordinates relative to this robot's pose. */ void updateTeammatesLocalPositions() { teammatesLocalPositions.clear(); - + // Get positions of other robots using their sensor data for (const auto& [teammateName, sensorAdr] : teammatesSensorAddrs) { // Read position from sensor data (3D vector) - Eigen::Vector3d robotGlobalPos = Eigen::Vector3d( - Eigen::Map(mujData->sensordata + sensorAdr) - ); - + Eigen::Vector3d robotGlobalPos = Eigen::Vector3d(Eigen::Map(mujData->sensordata + sensorAdr)); + // Convert to local frame - Eigen::Vector3d robotLocalPos = globalToLocalPosition( - robotGlobalPos, - robotPose->getTransformationMatrix() - ); - + Eigen::Vector3d robotLocalPos = globalToLocalPosition(robotGlobalPos, robotPose->getTransformationMatrix()); + teammatesLocalPositions[teammateName] = robotLocalPos; - } } void updateOpponentsLocalPositions() { opponentsLocalPositions.clear(); - + // Get positions of other robots using their sensor data for (const auto& [opponentName, sensorAdr] : opponentsSensorAddrs) { // Read position from sensor data (3D vector) - Eigen::Vector3d robotGlobalPos = Eigen::Vector3d( - Eigen::Map(mujData->sensordata + sensorAdr) - ); - + Eigen::Vector3d robotGlobalPos = Eigen::Vector3d(Eigen::Map(mujData->sensordata + sensorAdr)); + // Convert to local frame - Eigen::Vector3d robotLocalPos = globalToLocalPosition( - robotGlobalPos, - robotPose->getTransformationMatrix() - ); - + Eigen::Vector3d robotLocalPos = globalToLocalPosition(robotGlobalPos, robotPose->getTransformationMatrix()); + opponentsLocalPositions[opponentName] = robotLocalPos; - } } @@ -233,28 +212,22 @@ class Oracle : public Sensor { if (geomId < 0) { continue; } - + // Get global position from xpos (geom positions in world frame) // For geoms, xpos is indexed by geom ID - Eigen::Vector3d goalPostGlobalPos( - mujData->geom_xpos[geomId * 3 + 0], - mujData->geom_xpos[geomId * 3 + 1], - mujData->geom_xpos[geomId * 3 + 2] - ); - + Eigen::Vector3d goalPostGlobalPos(mujData->geom_xpos[geomId * 3 + 0], mujData->geom_xpos[geomId * 3 + 1], + mujData->geom_xpos[geomId * 3 + 2]); + // Convert to local frame - Eigen::Vector3d goalPostLocalPos = globalToLocalPosition( - goalPostGlobalPos, - robotPose->getTransformationMatrix() - ); - + Eigen::Vector3d goalPostLocalPos = globalToLocalPosition(goalPostGlobalPos, robotPose->getTransformationMatrix()); + goalPostsLocalPositions[postName] = goalPostLocalPos; } } Eigen::Vector3d ballPosition; int ballId, ballAdr, ballPosAdr, ballVelAdr; - + mjModel* mujModel; mjData* mujData; std::string robotName; @@ -264,7 +237,6 @@ class Oracle : public Sensor { std::map opponentsLocalPositions; std::map goalPostsLocalPositions; - std::map teammatesSensorAddrs; // robotName -> sensorAdr std::map opponentsSensorAddrs; // robotName -> sensorAdr std::vector goalPostNames = { @@ -273,6 +245,5 @@ class Oracle : public Sensor { "right_goal_left_post", "right_goal_right_post", }; - }; -} // namespace spqr +} // namespace spqr diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index 3bdd712..cf5ec78 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -3,14 +3,14 @@ teams: red: number: 19 players: - - type: Booster-T1 - number: 3 - position: [-1.0, 0.0, 0.68] - orientation: [0.0, 0.0, 0.0] + - type: Booster-T1 + number: 3 + position: [-1.0, 0.0, 0.68] + orientation: [0.0, 0.0, 0.0] blue: number: 5 players: - - type: Booster-T1 - number: 3 - position: [1.0, 1.0, 0.68] - orientation: [0.0, 0.0, 3.14] + - type: Booster-T1 + number: 3 + position: [1.0, 1.0, 0.68] + orientation: [0.0, 0.0, 3.14] diff --git a/resources/scenes/5v5.yaml b/resources/scenes/5v5.yaml index 78672a8..6c6cc66 100644 --- a/resources/scenes/5v5.yaml +++ b/resources/scenes/5v5.yaml @@ -3,47 +3,47 @@ teams: red: number: 8 players: - - type: Booster-T1 - number: 1 - position: [-7.0, 0.0, 0.68] - orientation: [0.0, 0.0, 0.0] - - type: Booster-T1 - number: 2 - position: [-5.0, 1.5, 0.68] - orientation: [0.0, 0.0, 0.0] - - type: Booster-T1 - number: 3 - position: [-5.0, -1.5, 0.68] - orientation: [0.0, 0.0, 0.0] - - type: Booster-T1 - number: 4 - position: [-3.0, 0.0, 0.68] - orientation: [0.0, 0.0, 0.0] - - type: Booster-T1 - number: 5 - position: [-1.0, 0.0, 0.68] - orientation: [0.0, 0.0, 0.0] + - type: Booster-T1 + number: 1 + position: [-7.0, 0.0, 0.68] + orientation: [0.0, 0.0, 0.0] + - type: Booster-T1 + number: 2 + position: [-5.0, 1.5, 0.68] + orientation: [0.0, 0.0, 0.0] + - type: Booster-T1 + number: 3 + position: [-5.0, -1.5, 0.68] + orientation: [0.0, 0.0, 0.0] + - type: Booster-T1 + number: 4 + position: [-3.0, 0.0, 0.68] + orientation: [0.0, 0.0, 0.0] + - type: Booster-T1 + number: 5 + position: [-1.0, 0.0, 0.68] + orientation: [0.0, 0.0, 0.0] blue: number: 51 players: - - type: Booster-T1 - number: 1 - position: [7.0, 0.0, 0.68] - orientation: [0.0, 0.0, 3.14] - - type: Booster-T1 - number: 2 - position: [5.0, 1.5, 0.68] - orientation: [0.0, 0.0, 3.14] - - type: Booster-T1 - number: 3 - position: [5.0, -1.5, 0.68] - orientation: [0.0, 0.0, 3.14] - - type: Booster-T1 - number: 4 - position: [3.0, 0.0, 0.68] - orientation: [0.0, 0.0, 3.14] - - type: Booster-T1 - number: 5 - position: [1.0, 0.0, 0.68] - orientation: [0.0, 0.0, 3.14] + - type: Booster-T1 + number: 1 + position: [7.0, 0.0, 0.68] + orientation: [0.0, 0.0, 3.14] + - type: Booster-T1 + number: 2 + position: [5.0, 1.5, 0.68] + orientation: [0.0, 0.0, 3.14] + - type: Booster-T1 + number: 3 + position: [5.0, -1.5, 0.68] + orientation: [0.0, 0.0, 3.14] + - type: Booster-T1 + number: 4 + position: [3.0, 0.0, 0.68] + orientation: [0.0, 0.0, 3.14] + - type: Booster-T1 + number: 5 + position: [1.0, 0.0, 0.68] + orientation: [0.0, 0.0, 3.14] diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index ceee8f0..3abf93f 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -218,7 +218,7 @@ void AppWindow::loadScene(const QString& yaml_file) { std::filesystem::remove_all(shmDir); } std::filesystem::create_directories(shmDir); - + CircusNetwork::instance().init(); RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server RobotManager::instance().startContainers(); diff --git a/src/Container.cpp b/src/Container.cpp index 019ac59..d15bcf1 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -1,20 +1,19 @@ #include "Container.h" +#include +#include + #include -#include #include +#include +#include "CircusNetwork.h" #include "Constants.h" - #include "robots/Robot.h" -#include -#include - -#include "CircusNetwork.h" // for the forward declarations -#include "robots/Robot.h" #include "Team.h" +#include "robots/Robot.h" namespace spqr { @@ -51,15 +50,13 @@ void Container::create(const std::shared_ptr& robot, const std::string& i std::string xauth_path = envOrDefault("XAUTHORITY", "/root/.Xauthority"); binds_with_x11.push_back(xauth_path + ":/root/.Xauthority:rw"); - payload["HostConfig"] = { - {"Binds", binds_with_x11}, - {"IpcMode", "host"}, - {"CapAdd", {"SYS_NICE", "IPC_LOCK"}}, - {"SecurityOpt", {"seccomp=unconfined"}}, - {"Ulimits", nlohmann::json::array({{{"Name", "memlock"}, {"Soft", -1}, {"Hard", -1}}})}, - {"Privileged", true}, - {"NetworkMode", CIRCUS_NETWORK_NAME} - }; + payload["HostConfig"] = {{"Binds", binds_with_x11}, + {"IpcMode", "host"}, + {"CapAdd", {"SYS_NICE", "IPC_LOCK"}}, + {"SecurityOpt", {"seccomp=unconfined"}}, + {"Ulimits", nlohmann::json::array({{{"Name", "memlock"}, {"Soft", -1}, {"Hard", -1}}})}, + {"Privileged", true}, + {"NetworkMode", CIRCUS_NETWORK_NAME}}; payload["HostConfig"]["DeviceRequests"] = nlohmann::json::array(); payload["HostConfig"]["DeviceRequests"].push_back({ @@ -67,33 +64,26 @@ void Container::create(const std::shared_ptr& robot, const std::string& i {"Count", -1}, {"Capabilities", nlohmann::json::array({nlohmann::json::array({"gpu"})})}, }); - - payload["NetworkingConfig"] = { - {"EndpointsConfig", { - {CIRCUS_NETWORK_NAME, { - {"IPAMConfig", { - {"IPv4Address", UAN_SEVEN_CIU + std::to_string(robot->team->number) + "." + std::to_string(robot->number + 10)} - }} - }} - }} - }; - payload["Env"] = { - "ROBOT_NAME=" + robot->name, - "SERVER_IP=172.17.0.1", - "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort), - "TEAM_NUMBER=" + std::to_string(robot->team->number), - "PLAYER_NUMBER=" + std::to_string(robot->number), - "TEAM_COLOR=" + robot->colorName, - "DISPLAY=" + envOrDefault("DISPLAY", ":0"), - "QT_X11_NO_MITSHM=1", - "NVIDIA_VISIBLE_DEVICES=all", - "NVIDIA_DRIVER_CAPABILITIES=all", - "XAUTHORITY=/root/.Xauthority", - "XDG_RUNTIME_DIR=/run/user/0", - "ROBOT_STACK=booster", - "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc" - }; + payload["NetworkingConfig"] + = {{"EndpointsConfig", + {{CIRCUS_NETWORK_NAME, + {{"IPAMConfig", {{"IPv4Address", UAN_SEVEN_CIU + std::to_string(robot->team->number) + "." + std::to_string(robot->number + 10)}}}}}}}}; + + payload["Env"] = {"ROBOT_NAME=" + robot->name, + "SERVER_IP=172.17.0.1", + "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort), + "TEAM_NUMBER=" + std::to_string(robot->team->number), + "PLAYER_NUMBER=" + std::to_string(robot->number), + "TEAM_COLOR=" + robot->colorName, + "DISPLAY=" + envOrDefault("DISPLAY", ":0"), + "QT_X11_NO_MITSHM=1", + "NVIDIA_VISIBLE_DEVICES=all", + "NVIDIA_DRIVER_CAPABILITIES=all", + "XAUTHORITY=/root/.Xauthority", + "XDG_RUNTIME_DIR=/run/user/0", + "ROBOT_STACK=booster", + "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc"}; payload["Entrypoint"] = {"/bin/bash", "-lc"}; payload["Cmd"] = {"/app/entrypoint.sh"}; diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index d562c96..47e1b86 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -1,10 +1,9 @@ #include "RobotManager.h" -#include "Team.h" // needed for the forward declaration in the .h #include "Constants.h" +#include "Team.h" // needed for the forward declaration in the .h #include "Utils.h" - namespace spqr { void RobotManager::registerRobot(std::shared_ptr robot) { @@ -108,9 +107,6 @@ void RobotManager::stopCommunicationServer() { serverThread_.join(); } - - - void RobotManager::_serverInternal(int port) { int server_fd = socket(AF_INET, SOCK_STREAM, 0); if (server_fd < 0) @@ -119,16 +115,14 @@ void RobotManager::_serverInternal(int port) { int opt = 1; setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); int send_buf_size = 1 * 1024 * 1024; - if (setsockopt(server_fd, SOL_SOCKET, SO_SNDBUF, - &send_buf_size, sizeof(send_buf_size)) < 0) { + if (setsockopt(server_fd, SOL_SOCKET, SO_SNDBUF, &send_buf_size, sizeof(send_buf_size)) < 0) { perror("setsockopt(SO_SNDBUF)"); } int recv_buf_size = 1 * 1024 * 1024; - if (setsockopt(server_fd, SOL_SOCKET, SO_RCVBUF, - &recv_buf_size, sizeof(recv_buf_size)) < 0) { + if (setsockopt(server_fd, SOL_SOCKET, SO_RCVBUF, &recv_buf_size, sizeof(recv_buf_size)) < 0) { perror("setsockopt(SO_RCVBUF)"); } - + sockaddr_in address{}; address.sin_family = AF_INET; address.sin_addr.s_addr = INADDR_ANY; @@ -149,7 +143,7 @@ void RobotManager::_serverInternal(int port) { int ret = poll(fds.data(), fds.size(), 100); if (ret <= 0) continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is - // still true) + // still true) for (size_t i = 0; i < fds.size(); ++i) { // An event occured for the i-th fd @@ -272,7 +266,7 @@ void RobotManager::_serverInternal(int port) { // Posted by Arun, modified by community. See post 'Timeline' for change history // Retrieved 2026-01-12, License - CC BY-SA 3.0 // TCP Communication, it sends before the size of the message and then the message itself -ssize_t RobotManager::send_all(int fd, char *buf, size_t len) { +ssize_t RobotManager::send_all(int fd, char* buf, size_t len) { // First, send the size of the message uint32_t msg_size = htonl(len); ssize_t bytes_sent = send(fd, &msg_size, sizeof(msg_size), 0); @@ -280,23 +274,21 @@ ssize_t RobotManager::send_all(int fd, char *buf, size_t len) { return -1; } - ssize_t total = 0; // how many bytes we've sent - size_t bytesleft = len; // how many we have left to send + ssize_t total = 0; // how many bytes we've sent + size_t bytesleft = len; // how many we have left to send ssize_t n = 0; - while(total < len) { - n = send(fd, buf+total, bytesleft, 0); - if (n == -1) { + while (total < len) { + n = send(fd, buf + total, bytesleft, 0); + if (n == -1) { /* print/log error details */ return -1; } total += n; bytesleft -= n; } - return total; + return total; } - - void RobotManager::setAreAllRobotsReadyCallback(std::function cb) { std::lock_guard lock(mutex_); areAllRobotsReadyCallback_ = std::move(cb); @@ -314,4 +306,4 @@ bool RobotManager::areAllRobotsReady() const { return true; } -} \ No newline at end of file +} // namespace spqr diff --git a/src/Utils.cpp b/src/Utils.cpp index 3619d4b..b1a86d9 100644 --- a/src/Utils.cpp +++ b/src/Utils.cpp @@ -1,4 +1,5 @@ #include "Utils.h" + #include namespace spqr { From 4f468bb65243ea800f57185f284653159da38078 Mon Sep 17 00:00:00 2001 From: neverorfrog <97flavio.maiorana@gmail.com> Date: Tue, 10 Mar 2026 00:00:34 +0100 Subject: [PATCH 43/56] added joystick stuff and some new aliases --- dockerfiles/configs/bashrc | 6 ++++++ include/CircusNetwork.h | 2 +- pixi.lock | 24 +++++++++++------------- pixi.toml | 2 +- src/Container.cpp | 3 ++- 5 files changed, 21 insertions(+), 16 deletions(-) diff --git a/dockerfiles/configs/bashrc b/dockerfiles/configs/bashrc index 521ab31..eec04d6 100644 --- a/dockerfiles/configs/bashrc +++ b/dockerfiles/configs/bashrc @@ -21,3 +21,9 @@ alias maxlog="cat /var/log/maximus.log" alias maxerr="cat /var/log/maximus.err" alias brilog="cat /var/log/simbridge.log" alias brierr="cat /var/log/simbridge.err" +alias maxwatch="tail -f /var/log/maximus.log" +alias briwatch="tail -f /var/log/simbridge.log" +alias bmwatch="tail -f /var/log/booster-motion.log" +alias bmerrwatch="tail -f /var/log/booster-motion.err" +alias maxerrwatch="tail -f /var/log/maximus.err" +alias brierrwatch="tail -f /var/log/simbridge.err" diff --git a/include/CircusNetwork.h b/include/CircusNetwork.h index 262f379..eb4fc34 100644 --- a/include/CircusNetwork.h +++ b/include/CircusNetwork.h @@ -4,7 +4,7 @@ #include "DockerREST.h" #define CIRCUS_NETWORK_NAME "CIRCUS_network" -#define UAN_SEVEN_CIU "172.18." // Per coerenza +#define UAN_SEVEN_CIU "172.21." // Per coerenza namespace spqr { diff --git a/pixi.lock b/pixi.lock index 4521825..f212ee9 100644 --- a/pixi.lock +++ b/pixi.lock @@ -4,6 +4,8 @@ environments: channels: - url: https://prefix.dev/pixi-build-backends/ - url: https://prefix.dev/conda-forge/ + options: + pypi-prerelease-mode: if-necessary-or-explicit packages: linux-64: - conda: https://prefix.dev/conda-forge/linux-64/_libgcc_mutex-0.1-conda_forge.tar.bz2 @@ -149,7 +151,7 @@ environments: - conda: https://prefix.dev/conda-forge/osx-arm64/libclang-cpp19.1-19.1.7-default_h73dfc95_5.conda - conda: https://prefix.dev/conda-forge/osx-arm64/libclang13-21.1.6-default_h6e8f826_0.conda - conda: https://prefix.dev/conda-forge/osx-arm64/libcurl-8.17.0-hdece5d2_0.conda - - conda: https://prefix.dev/conda-forge/osx-arm64/libcxx-21.1.6-hf598326_0.conda + - conda: https://prefix.dev/conda-forge/osx-arm64/libcxx-22.1.0-h55c6f16_1.conda - conda: https://prefix.dev/conda-forge/osx-arm64/libdeflate-1.25-hc11a715_0.conda - conda: https://prefix.dev/conda-forge/osx-arm64/libedit-3.1.20250104-pl5321hafb1f1b_0.conda - conda: https://prefix.dev/conda-forge/osx-arm64/libev-4.33-h93a5062_2.conda @@ -321,8 +323,10 @@ packages: version: 0.1.0 build: h60d57d3_0 subdir: osx-arm64 + variants: + target_platform: osx-arm64 depends: - - libcxx >=21 + - libcxx >=22 - qt6-charts >=6.9.2,<6.10.0a0 - libcurl >=8.14.1,<9.0a0 - pugixml >=1.15,<1.16.0a0 @@ -332,9 +336,6 @@ packages: - libmujoco >=3.3.3,<3.3.4.0a0 - msgpack-cxx >=7.0.0,<8.0a0 - libboost >=1.88.0,<1.89.0a0 - input: - hash: 705b89ab5dc60f96852095e04858eedbe3eed8d48f167c14f2abfad75c38daff - globs: [] - conda: . name: circus version: 0.1.0 @@ -354,9 +355,6 @@ packages: - libboost >=1.88.0,<1.89.0a0 - libgl >=1.7.0,<2.0a0 - libegl >=1.7.0,<2.0a0 - input: - hash: 705b89ab5dc60f96852095e04858eedbe3eed8d48f167c14f2abfad75c38daff - globs: [] - conda: https://prefix.dev/conda-forge/linux-64/cyrus-sasl-2.1.28-hd9c7081_0.conda sha256: ee09ad7610c12c7008262d713416d0b58bf365bc38584dce48950025850bdf3f md5: cae723309a49399d2949362f4ab5c9e4 @@ -807,15 +805,15 @@ packages: license_family: MIT size: 394183 timestamp: 1762334288445 -- conda: https://prefix.dev/conda-forge/osx-arm64/libcxx-21.1.6-hf598326_0.conda - sha256: 6c8d5c50f398035c39f118a6decf91b11d2461c88aef99f81e5c5de200d2a7fa - md5: 3ea79e55a64bff6c3cbd4588c89a527a +- conda: https://prefix.dev/conda-forge/osx-arm64/libcxx-22.1.0-h55c6f16_1.conda + sha256: ce1049fa6fda9cf08ff1c50fb39573b5b0ea6958375d8ea7ccd8456ab81a0bcb + md5: e9c56daea841013e7774b5cd46f41564 depends: - __osx >=11.0 license: Apache-2.0 WITH LLVM-exception license_family: Apache - size: 569823 - timestamp: 1763470498512 + size: 568910 + timestamp: 1772001095642 - conda: https://prefix.dev/conda-forge/linux-64/libdeflate-1.25-h17f619e_0.conda sha256: aa8e8c4be9a2e81610ddf574e05b64ee131fab5e0e3693210c9d6d2fba32c680 md5: 6c77a605a7a689d17d4819c0f8ac9a00 diff --git a/pixi.toml b/pixi.toml index 67f6579..a24ebba 100644 --- a/pixi.toml +++ b/pixi.toml @@ -15,7 +15,7 @@ name = "circus" version = "0.1.0" [package.build] -backend = { name = "pixi-build-cmake", version = "==0.3.5" } +backend = { name = "pixi-build-cmake", version = "*" } [package.build-dependencies] cmake = ">=3.27,<4" diff --git a/src/Container.cpp b/src/Container.cpp index d15bcf1..735ba0c 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -83,7 +83,8 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "XAUTHORITY=/root/.Xauthority", "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", - "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc"}; + "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc", + "JOYSTICK_DEVICE=" + envOrDefault("JOYSTICK_DEVICE", "/dev/input/js0")}; payload["Entrypoint"] = {"/bin/bash", "-lc"}; payload["Cmd"] = {"/app/entrypoint.sh"}; From b2b77870a207a50b49788345df046587ed6ae666 Mon Sep 17 00:00:00 2001 From: michelebri Date: Tue, 10 Mar 2026 15:11:39 +0100 Subject: [PATCH 44/56] fix depth reprojection --- include/robots/BoosterT1.h | 4 +++- include/sensors/CameraDepth.h | 27 ++++++++++++++++++------ resources/robots/Booster-T1/instance.xml | 2 +- resources/scenes/1v1.yaml | 10 ++++----- 4 files changed, 28 insertions(+), 15 deletions(-) diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index e0914e4..e8f24bf 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -100,7 +100,9 @@ class BoosterT1 : public Robot { {JointValue::ANKLE_RIGHT_ROLL, 0}}); rgbCamera = new CameraRGB(mujCtx, (name + "_rgb_cam").c_str()); - depthCamera = new CameraDepth(mujCtx, (name + "_depth_cam").c_str()); + // Use RGB viewpoint for simulated depth to provide aligned depth-to-color. + // This avoids parallax between rgb_cam and depth_cam when unprojecting RGB detections. + depthCamera = new CameraDepth(mujCtx, (name + "_rgb_cam").c_str()); //Configure the writer for the shared memory file const int width = rgbCamera->getWidth(); diff --git a/include/sensors/CameraDepth.h b/include/sensors/CameraDepth.h index 02460f6..d482563 100644 --- a/include/sensors/CameraDepth.h +++ b/include/sensors/CameraDepth.h @@ -6,6 +6,8 @@ #include #include #include +#include +#include #include #include @@ -78,21 +80,32 @@ class CameraDepth : public Sensor { std::vector tempDepth(viewWidth * viewHeight); mjr_readPixels(nullptr, tempDepth.data(), viewport, &mujContext->ctx); - float znear = mujContext->model->vis.map.znear; // 0.0001 - float zfar = mujContext->model->vis.map.zfar; // 50.0 + // MuJoCo map.znear/zfar are expressed relative to model extent. + //DEPTH FIX - MICHELE : Verify. + // MuJoCo stores vis.map.znear/zfar as fractions of model->stat.extent, not absolute meters. + // Multiply by stat.extent to convert them to metric near/far distances used by depth linearization. + + const float extent = static_cast(mujContext->model->stat.extent); + const float znear = static_cast(mujContext->model->vis.map.znear) * extent; + const float zfar = static_cast(mujContext->model->vis.map.zfar) * extent; + constexpr float kDepthMaxMeters = 10.0f; // Keep in sync with SimBridge mono8 decoding. float max_u16 = static_cast(std::numeric_limits::max()); - // Process depth to match camera resolution + // Resample offscreen depth to camera resolution and convert to metric depth. for (int y = 0; y < h; y++) { - int srcRow = (h - 1 - y) * w; // flip y-axes + int srcY = static_cast((static_cast(y) / static_cast(h)) * static_cast(viewHeight)); + srcY = std::clamp(srcY, 0, viewHeight - 1); + int srcRow = (viewHeight - 1 - srcY) * viewWidth; // flip y-axis int dstRow = y * w; for (int x = 0; x < w; x++) { - float z_raw = tempDepth[srcRow + x]; + int srcX = static_cast((static_cast(x) / static_cast(w)) * static_cast(viewWidth)); + srcX = std::clamp(srcX, 0, viewWidth - 1); + float z_raw = tempDepth[srcRow + srcX]; float z_converted = (znear * zfar) / (zfar - z_raw * (zfar - znear)); depthNormalized[dstRow + x] = z_converted; - float clampedDepth = std::min(z_converted / 1.0f, 1.0f); - depth[dstRow + x] = static_cast(clampedDepth * max_u16); + float normalizedDepth = std::clamp(z_converted / kDepthMaxMeters, 0.0f, 1.0f); + depth[dstRow + x] = static_cast(normalizedDepth * max_u16); } } diff --git a/resources/robots/Booster-T1/instance.xml b/resources/robots/Booster-T1/instance.xml index cb1b08e..9f37728 100644 --- a/resources/robots/Booster-T1/instance.xml +++ b/resources/robots/Booster-T1/instance.xml @@ -35,7 +35,7 @@ - + diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index 1150980..fbdb376 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -3,10 +3,8 @@ teams: red: - type: Booster-T1 number: 3 - position: [-1.0, 0.0, 0.68] + position: [-6.8, 0.0, 0.68] orientation: [0.0, 0.0, 0.0] - blue: - - type: Booster-T1 - number: 3 - position: [1.0, 1.0, 0.68] - orientation: [0.0, 0.0, 3.14] + + + From b67561aaf683673c61060f722ade1858f318ce78 Mon Sep 17 00:00:00 2001 From: michelebri Date: Tue, 10 Mar 2026 15:37:23 +0100 Subject: [PATCH 45/56] scene --- resources/scenes/1v1.yaml | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index cf5ec78..b6b46a3 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -3,14 +3,7 @@ teams: red: number: 19 players: - - type: Booster-T1 - number: 3 - position: [-1.0, 0.0, 0.68] - orientation: [0.0, 0.0, 0.0] - blue: - number: 5 - players: - - type: Booster-T1 - number: 3 - position: [1.0, 1.0, 0.68] - orientation: [0.0, 0.0, 3.14] + - type: Booster-T1 + number: 3 + position: [-1.0, 0.0, 0.68] + orientation: [0.0, 0.0, 0.0] \ No newline at end of file From 1624df1253f98f523b1e5b23d155ad96b999f735 Mon Sep 17 00:00:00 2001 From: Flavio Date: Sat, 28 Mar 2026 19:05:25 +0100 Subject: [PATCH 46/56] changes into the workflow of torque application --- include/RobotManager.h | 1 + include/robots/BoosterK1.h | 4 +++- include/robots/BoosterT1.h | 18 +++++++++++++----- include/robots/Robot.h | 6 +++++- src/RobotManager.cpp | 6 ++++++ src/SimulationThread.cpp | 6 +++++- 6 files changed, 33 insertions(+), 8 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index fd1fe5d..ef777a7 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -51,6 +51,7 @@ class RobotManager { void stopCommunicationServer(); void setAreAllRobotsReadyCallback(std::function cb); + void applyCommands(); private: RobotManager() = default; diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 90267d6..821d080 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -102,7 +102,9 @@ class BoosterK1 : public Robot { sensors["depth_camera"] = depthCamera; return sensors; } - + void applyCommands() override { + return; + } void update() override { pose->update(); imu->update(); diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index dcb0610..a87b850 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -128,13 +128,13 @@ class BoosterT1 : public Robot { + std::to_string(joint_map.size()) + ")"); } - std::unordered_map torque_map; size_t i = 0; - for (const auto& [joint_value, joint_name] : joint_map) { - torque_map[joint_value] = joint_torques[i++]; + { + std::lock_guard lock(mutex_); + for (const auto& [joint_value, joint_name] : joint_map) { + latestTorques[joint_value] = joint_torques[i++]; + } } - - joints->set_torque(torque_map); } std::map sendMessage() override { @@ -163,6 +163,11 @@ class BoosterT1 : public Robot { return sensors; } + void applyCommands() override { + std::lock_guard lock(mutex_); + joints->set_torque(latestTorques); + } + void update() override { pose->update(); imu->update(); @@ -180,9 +185,12 @@ class BoosterT1 : public Robot { } std::map joint_map; + std::unordered_map latestTorques; + std::string shm_dir_; ImageSharedMemoryWriter rgb_writer_; ImageSharedMemoryWriter depth_writer_; + }; } // namespace spqr diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 403fd9e..e7728a8 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -11,7 +11,9 @@ #include #include #include +#include #include + #include "Container.h" #include "MujocoContext.h" @@ -41,7 +43,8 @@ class Robot { virtual void receiveMessage(const std::map& message) = 0; virtual std::map sendMessage() = 0; virtual std::map getSensors() = 0; - + virtual void applyCommands() = 0; + std::string name; std::string type; uint8_t number; @@ -53,6 +56,7 @@ class Robot { std::shared_ptr team; msgpack::zone buffer_zone_; + mutable std::mutex mutex_; bool isConnected = false; bool isReady = false; diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 47e1b86..74a7824 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -6,6 +6,12 @@ namespace spqr { +void RobotManager::applyCommands() { + for (std::shared_ptr r : robots_) { + r->applyCommands(); + } +} + void RobotManager::registerRobot(std::shared_ptr robot) { std::lock_guard lock(mutex_); diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 06e691c..a578627 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -19,9 +19,13 @@ void SimulationThread::run() { auto next_step_time = clock::now(); while (running_) { if (!paused_) { - mj_step(model_, data_); + mj_step1(model_, data_); + RobotManager::instance().applyCommands(); + mj_step2(model_, data_); RobotManager::instance().update(); GameController::instance().update(); + + std::memset(data_->xfrc_applied, 0, model_->nbody * 6 * sizeof(mjtNum)); if (maxSimulationTime_ > 0 && data_->time >= maxSimulationTime_) { running_ = false; From b59af0d30269a500487bf050e97d3dd4694541d9 Mon Sep 17 00:00:00 2001 From: michelebri Date: Sat, 28 Mar 2026 19:28:20 +0100 Subject: [PATCH 47/56] report Claude --- ISSUES.md | 333 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 333 insertions(+) create mode 100644 ISSUES.md diff --git a/ISSUES.md b/ISSUES.md new file mode 100644 index 0000000..6f6327d --- /dev/null +++ b/ISSUES.md @@ -0,0 +1,333 @@ +# Circus — Issue Report + +--- + +## Issue #1: Disaccoppiamento temporale tra simulation thread e TCP thread + +### Contesto + +Circus usa due thread che operano in modo completamente asincrono e indipendente: + +- **SimulationThread** (`src/SimulationThread.cpp`) — esegue il loop fisico MuJoCo +- **TCP Server Thread** (`src/RobotManager.cpp::_serverInternal`) — gestisce la comunicazione con i container Docker + +### Loop di simulazione (SimulationThread.cpp:12-47) + +```cpp +while (running_) { + if (!paused_) { + mj_step1(model_, data_); // fase 1: forward dynamics (NO ctrl ancora) + RobotManager::instance().applyCommands(); // applica ctrl[] ai robot + mj_step2(model_, data_); // fase 2: integrazione + RobotManager::instance().update(); // legge stato post-step → sensor cache + GameController::instance().update(); + // ... + std::this_thread::sleep_until(next_step_time); + } +} +``` + +Nota: il loop è stato aggiornato rispetto alla prima versione — usa `mj_step1`/`mj_step2` separati con `applyCommands()` in mezzo, il che è corretto dal punto di vista fisico (le torque vengono applicate dopo che le forze di contatto sono già state calcolate in `mj_step1`). + +### Loop TCP (RobotManager.cpp:116-269) + +```cpp +while (serverRunning_) { + int ret = poll(fds.data(), fds.size(), 100); // 100ms timeout + // ... + { + std::unique_lock lock(mutex_); + r->receiveMessage(data_map); // scrive latestTorques nel robot (riga 245) + answ = r->sendMessage(); // legge sensor cache (riga 246) + } + // send answ back to container +} +``` + +### Problema 1a: Disaccoppiamento causale + +Il flusso ideale deterministico sarebbe: + +``` +Container → torques_t → Simulator → mj_step → stato_{t+1} → Container +``` + +Il flusso reale è: + +``` +TCP Thread: Sim Thread: +recv torques_t ... +receiveMessage() → latestTorques ... +sendMessage() → legge sensor cache ... ← quale step? +send stato_? applyCommands() → ctrl[] + mj_step1/2 + update() → sensor cache +``` + +Non esiste garanzia che lo stato inviato al container corrisponda allo step che ha usato le torque appena ricevute. Il TCP thread legge la sensor cache immediatamente dopo `receiveMessage()`, che è l'ultimo stato calcolato dal sim thread — che potrebbe essere di 0, 1, o più step fa rispetto all'applicazione delle nuove torque. + +### Problema 1b: Assenza di lock su `applyCommands` vs `mj_step` + +`applyCommands()` è chiamato dal **SimulationThread** (riga 23 di SimulationThread.cpp): + +```cpp +// RobotManager.cpp:9-12 +void RobotManager::applyCommands() { + for (std::shared_ptr r : robots_) { + r->applyCommands(); // ← NESSUN mutex_ qui + } +} +``` + +`BoosterT1::applyCommands()` ([robots/BoosterT1.h:166-169](include/robots/BoosterT1.h)): + +```cpp +void applyCommands() override { + std::lock_guard lock(mutex_); // ← mutex_ del robot + joints->set_torque(latestTorques); +} +``` + +`BoosterT1::receiveMessage()` ([robots/BoosterT1.h:117-138](include/robots/BoosterT1.h)) è chiamato dal **TCP Thread** e scrive in `latestTorques` sotto lo stesso `mutex_` del robot: + +```cpp +void receiveMessage(...) override { + // ... + std::lock_guard lock(mutex_); // ← stesso mutex_ del robot + for (const auto& [joint_value, joint_name] : joint_map) { + latestTorques[joint_value] = joint_torques[i++]; + } +} +``` + +**Il mutex_ del robot protegge correttamente `latestTorques`** da accessi concorrenti tra `receiveMessage()` (TCP thread) e `applyCommands()` (sim thread). Questo è l'unico punto dove la sincronizzazione è corretta. + +### Problema 1c: `set_torque` scrive direttamente in `mujData->ctrl[]` senza lock globale + +`Joints::set_torque()` ([sensors/Joint.h:98-109](include/sensors/Joint.h)): + +```cpp +void set_torque(const std::unordered_map& values) { + for (const auto& [joint, val] : values) { + // ... + mujData->ctrl[act_id] = std::clamp(val, ...); // scrittura diretta su mjData + } +} +``` + +Questa scrittura avviene all'interno di `applyCommands()` che è chiamato dal sim thread **tra** `mj_step1` e `mj_step2`. Questo è corretto per il flusso fisico, ma `mujData` è un puntatore condiviso con tutti gli altri sensori. Non c'è nessun lock globale su `mujData` tra il sim thread e potenziali accessi concorrenti. + +### Problema 1d: `RobotManager::applyCommands()` non acquisisce `mutex_` del RobotManager + +```cpp +// RobotManager.cpp:9-12 +void RobotManager::applyCommands() { + for (std::shared_ptr r : robots_) { // itera su robots_ SENZA mutex_ + r->applyCommands(); + } +} +``` + +Tutte le altre operazioni su `robots_` (`update()`, `registerRobot()`, ecc.) acquisiscono `mutex_`. `applyCommands()` no. Se un robot venisse registrato o rimosso durante l'iterazione, ci sarebbe una race condition su `robots_`. In pratica non accade perché i robot vengono registrati prima di avviare i thread, ma è un'inconsistenza. + +### Problema 1e: `Sensor::update()` vs `Sensor::serialize()` — shared_mutex parzialmente efficace + +`Sensor` ([sensors/Sensor.h](include/sensors/Sensor.h)) usa `shared_mutex`: + +```cpp +virtual void update() final { + std::unique_lock lock(mtx_); // esclusivo: solo il sim thread + doUpdate(); +} + +virtual msgpack::object serialize(msgpack::zone& z) final { + std::shared_lock lock(mtx_); // condiviso: lettura concorrente ok + return doSerialize(z); +} +``` + +Questo protegge i campi interni del sensore (`position`, `velocity`, ecc.) da race tra `update()` (sim thread) e `serialize()` (TCP thread). **Questo funziona correttamente.** + +Tuttavia, `doUpdate()` in `Joints` legge da `mujData` (es. `mujData->qpos`, `mujData->qvel`) e `set_torque()` scrive su `mujData->ctrl[]`. Entrambi accedono a `mujData` ma il `shared_mutex` del sensore **non copre gli accessi a `mujData`** — copre solo i campi cached del sensore. + +### Sequenza temporale del problema causale — diagramma + +``` +Tempo → + +SimThread: [mj_step1]─[applyCommands]─[mj_step2]─[update sensors] sleep [mj_step1]─... + ↑ ↓ + ctrl[] ← latestTorques sensor cache ← mjData + +TCPThread: ...[recv torques_t]─[receiveMessage]─[sendMessage]─[send]... + ↑ ↓ + latestTorques sensor cache (da quale step?) + (sarà usato al (è l'ultimo completato, + prossimo ma non necessariamente quello + applyCommands) che userà torques_t) +``` + +Lo stato inviato al container dopo `receiveMessage(torques_t)` è lo stato prodotto dallo step che ha usato `torques_{t-k}` per un `k` indeterminato. Il container riceve una risposta che non corrisponde causalmente ai comandi appena inviati. + +### Contesto quantitativo + +- Sim a `model_->opt.timestep` (tipicamente 1ms → 1000Hz) +- TCP a ~100Hz (poll con timeout 100ms, ma in pratica limitato dalla rete) +- Rapporto: ~10 step di simulazione per ogni ciclo TCP + +Con questo rapporto il jitter è al massimo ~10ms, accettabile per molti usi. Diventa rilevante per RL policy che assumono causalità stretta o per debugging deterministico. + +### Soluzioni analizzate + +#### A — Minimal fix: mutex globale su ctrl[] + +Aggiungere un lock su `mujData->ctrl[]` tra sim thread e TCP thread. + +- **Pro**: 5 righe di codice +- **Con**: Non risolve la causalità, solo la race condition su ctrl[] (che in realtà non esiste già grazie al mutex_ del robot + applyCommands nel sim thread) +- **Valutazione**: Inutile dato che la race è già gestita + +#### B — Double-buffer atomico per robot (raccomandato) + +Ogni robot ha due buffer separati scambiati atomicamente: uno per le torque (TCP scrive, sim legge) e uno per lo stato (sim scrive, TCP legge). + +``` +TCP Thread: Sim Thread: +recv torques swap torque_pending → ctrl[] +write torque_pending (atomic swap) mj_step1/2 +read state_latest update() → write state_latest (atomic swap) +send state +``` + +```cpp +struct RobotMailbox { + std::unordered_map torques[2]; + std::atomic torque_write_idx{0}; // TCP scrive in [write_idx], sim legge da [1-write_idx] + + SensorSnapshot state[2]; + std::atomic state_write_idx{0}; // sim scrive in [write_idx], TCP legge da [1-write_idx] +}; +``` + +- **Pro**: Zero blocking, robot completamente indipendenti, jitter ~1ms, sim thread non aspetta mai +- **Con**: No causalità stretta (jitter di 1 step), scambio di buffer richiede breve sincronizzazione sullo swap +- **Valutazione**: Ottimale per questo use case (10 step/ciclo TCP rende il jitter irrilevante) + +#### C — Sim-driven non-blocking I/O + +Il sim thread gestisce direttamente la rete con `poll(..., timeout=0)` non bloccante ogni step (o ogni N step per matchare 100Hz): + +```cpp +// Nel loop di SimulationThread +poll(robot_fds, N, 0); // non-blocking +for each robot with data: recv torques (MSG_DONTWAIT) +mj_step1(); +applyCommands(); +mj_step2(); +update(); +for each robot: send state (MSG_DONTWAIT) +``` + +- **Pro**: Causalità perfetta, zero sincronizzazione, architettura identica a SimRobot/BHuman +- **Con**: Refactor significativo, dipendenza da velocità rete nel loop fisico, send bloccante se buffer pieno +- **Valutazione**: Architetturalmente più pulito ma richiede gestione errori I/O nel thread critico + +#### D — Per-robot condition variable + +Il TCP thread aspetta una notifica dopo ogni step prima di rispondere: + +```cpp +// TCP Thread per robot i: +receiveMessage(); +robot.step_cv.wait(lock, [&]{ return robot.state_ready; }); +robot.state_ready = false; +sendMessage(); + +// Sim Thread dopo update(): +for each robot: + robot.state_ready = true; + robot.step_cv.notify_one(); +``` + +- **Pro**: Causalità garantita, robot indipendenti +- **Con**: Il TCP thread blocca fino al prossimo step (~1ms max). Se il sim è lento, anche il TCP è lento. Introduce dipendenza temporale tra physics rate e network rate +- **Valutazione**: Valido se la causalità stretta è necessaria (es. training RL) + +### Soluzione raccomandata + +**Approccio B** per uso generale (sviluppo, test gameplay). +**Approccio D** se si vuole usare Circus per training RL deterministico. + +--- + +## Issue #2: Incompatibilità CMake 4.x con ROS Humble (simbridge) + +### Descrizione + +Il build dei pacchetti messaggi ROS 2 di simbridge (`booster_interface`, `booster_msgs`, `spqr_msgs`) fallisce quando pixi risolve CMake >= 4.0. + +### Causa radice + +CMake 4.0 ha rimosso i moduli legacy `FindPythonInterp` e `FindPythonLibs` (policy CMP0148 promossa da WARN a ERROR). + +Il file `rosidl_generator_py_generate_interfaces.cmake` di ROS Humble (pacchetto `python_cmake_module`) fa questa sequenza: + +```cmake +# rosidl_generator_py_generate_interfaces.cmake:20-35 +find_package(python_cmake_module REQUIRED) +find_package(PythonExtra REQUIRED) # chiama FindPythonExtra → FindPythonInterp (legacy) +find_package(Python REQUIRED # chiama FindPython moderno + COMPONENTS Interpreter Development NumPy) +``` + +Con CMake 4.3 nell'ambiente conda cross-compilation: +- `FindPythonInterp` → funziona (trovato `$PREFIX/bin/python3`) ma genera WARNING CMP0148 +- `FindPythonLibs` → funziona ma genera WARNING CMP0148 +- `FindPython` → **FALLISCE** perché non trova `Python_NumPy_INCLUDE_DIRS`, `Development.Module`, `Development.Embed` + +L'errore esatto: + +``` +CMake Error: Could NOT find Python (missing: Python_EXECUTABLE Python_INCLUDE_DIRS +Python_LIBRARIES Python_NumPy_INCLUDE_DIRS Interpreter Development NumPy +Development.Module Development.Embed) +at: $BUILD_PREFIX/share/cmake-4.3/Modules/FindPackageHandleStandardArgs.cmake:290 +``` + +Il modulo moderno `FindPython` non riesce a trovare i componenti `Development.Embed` e `Development.Module` nell'ambiente conda cross-compilation perché il `BUILD_PREFIX` e il `PREFIX` sono separati e le librerie Python sono in `PREFIX/lib` ma CMake cerca in percorsi di sistema. + +### Tentativi falliti + +1. `-DROSIDL_GENERATOR_PY=OFF` — non supportato da `rosidl_cmake`, il generatore Python è sempre caricato +2. `-DCMAKE_POLICY_DEFAULT_CMP0148=OLD` — risolve i warning ma non il `find_package(Python ...)` moderno alla riga 34 + +### Fix applicata + +Pinnato `cmake >=3.20,<4.0` nei recipe dei tre pacchetti: + +```yaml +# src/msgs/booster_interface/recipe.yaml +# src/msgs/booster_msgs/recipe.yaml +# src/msgs/spqr_msgs/recipe.yaml +requirements: + build: + - cmake >=3.20,<4.0 + - python + - numpy + - ros-humble-rosidl-default-generators + host: + - ros-humble-rosidl-generator-py + - python + - numpy + - ros-humble-rosidl-default-runtime +``` + +Con CMake 3.x `FindPython` funziona correttamente nell'ambiente conda perché il modulo legacy conosce i percorsi conda. + +### Nota di monitoraggio + +ROS Humble ha supporto ufficiale fino a maggio 2027 ma non supporterà mai CMake 4.x. Il problema si ripresenterà se: +- Si migra a ROS Iron/Jazzy (che hanno patch per CMake 4.x) +- Si rimuove il pin `<4.0` dai recipe + +Al momento del passaggio a una distro ROS più recente sarà necessario aggiornare i recipe rimuovendo il pin e verificando che `rosidl_generator_py` sia stato patchato per CMake 4.x. From 9ab7fa64f029abdf148dcb4d2039cb7949187958 Mon Sep 17 00:00:00 2001 From: Flavio Date: Wed, 1 Apr 2026 21:19:14 +0200 Subject: [PATCH 48/56] simulation with one thread --- include/RobotManager.h | 2 + include/SimulationThread.h | 28 ++++ src/AppWindow.cpp | 45 ++++-- src/RobotManager.cpp | 7 +- src/SceneParser.cpp | 2 +- src/SimulationThread.cpp | 287 ++++++++++++++++++++++++++++++++++++- 6 files changed, 354 insertions(+), 17 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index ef777a7..87dee0d 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -52,6 +52,8 @@ class RobotManager { void setAreAllRobotsReadyCallback(std::function cb); void applyCommands(); + std::vector> getRobots(); + private: RobotManager() = default; diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 46d56ea..91e8cc9 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -3,6 +3,12 @@ #include #include +#include +#include +#include + +#include "robots/Robot.h" + namespace spqr { @@ -10,16 +16,21 @@ class SimulationThread : public QThread { Q_OBJECT public: SimulationThread(const mjModel* model, mjData* data); + void run() override; void stop(); void pause(); void play(); bool isPaused(); void setMaxSimulationTime(int maxTime); + void initializeSocket(int port); + void waitInitialMessages(); + signals: void stepCompleted(); void maxSimulationTimeReached(); + void allRobotsReady(); private: const mjModel* model_; @@ -27,6 +38,23 @@ class SimulationThread : public QThread { std::atomic running_; std::atomic paused_; int maxSimulationTime_ = -1; // -1 means no limit + std::map entity_fd_map; + + std::function areAllRobotsReadyCallback_; + + // Socket for communication stuff + int server_fd; + std::vector> robots_; + std::vector fds; + mutable std::mutex mutex_; + ssize_t send_all(int fd, char* buf, size_t len); + + void areAllRobotsReadyWrapper(); + bool areAllRobotsReady() const; + + void receiveCommandMessages(); + + }; } // namespace spqr diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 3abf93f..f2f7489 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -197,19 +197,28 @@ void AppWindow::loadScene(const QString& yaml_file) { sim->setMaxSimulationTime(parser.getSceneInfo().simulationConfig.simulation.max_simulation_time); connect(sim.get(), &SimulationThread::maxSimulationTimeReached, this, &AppWindow::close); + connect(sim.get(), &SimulationThread::allRobotsReady, + this, + [this]() { + if (sim) { + std::cout << "Starting simulation!" << std::endl; + sim->start(); + } + }, + Qt::QueuedConnection); // Callback to start the simulation // Simulation starts when the all the robots are ready - RobotManager::instance().setAreAllRobotsReadyCallback([this]() { - QMetaObject::invokeMethod( - this, - [this]() { - if (sim) { - std::cout << "Starting simulation!" << std::endl; - sim->start(); - } - }, - Qt::QueuedConnection); - }); + // RobotManager::instance().setAreAllRobotsReadyCallback([this]() { + // QMetaObject::invokeMethod( + // this, + // [this]() { + // if (sim) { + // std::cout << "Starting simulation!" << std::endl; + // sim->start(); + // } + // }, + // Qt::QueuedConnection); + // }); // Ensure the shared memory directory exists and is writable by the current user. // Docker bind mounts create missing host dirs as root, so remove and recreate if needed. @@ -221,7 +230,21 @@ void AppWindow::loadScene(const QString& yaml_file) { CircusNetwork::instance().init(); RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server + sim->initializeSocket(frameworkCommunicationPort); + + std::cout << "STEP 0" << std::endl; RobotManager::instance().startContainers(); + std::cout << "FINALMENTE 1" << std::endl; + sim->waitInitialMessages(); + std::cout << "FINALMENTE 2" << std::endl; + + /* + // Pseudocode + // Può essere aggiunta una logica di tentare la connessione appena il server diventa disponibile + start_server(); // bind + listen + spawn_containers(); + accept_connections(); + */ // Set initial simulation state (playing when scene is loaded) toolsPanel->setSimulationPlaying(true); diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 74a7824..3e9d1fe 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -59,7 +59,7 @@ std::shared_ptr RobotManager::create(const std::string& name, const std:: } void RobotManager::startContainers() { - startCommunicationServer(frameworkCommunicationPort); + // startCommunicationServer(frameworkCommunicationPort); YAML::Node pathsRoot = loadYamlFile(pathsConfigPath); YAML::Node configRoot = loadYamlFile(frameworkConfigPath); @@ -88,7 +88,6 @@ void RobotManager::startContainers() { } binds.push_back(v2); } - for (std::shared_ptr r : robots_) { r->container = std::make_unique("CIRCUS_" + r->name + "_container"); r->container->create(r, image, binds); @@ -113,6 +112,10 @@ void RobotManager::stopCommunicationServer() { serverThread_.join(); } +std::vector> RobotManager::getRobots() { + return robots_; +} + void RobotManager::_serverInternal(int port) { int server_fd = socket(AF_INET, SOCK_STREAM, 0); if (server_fd < 0) diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index f81eb9f..9de6e43 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -140,7 +140,7 @@ string SceneParser::buildMuJoCoXml() { // TODO: The simulation options can be parametrized. I don't know if we may want to change the parameters. xml_node option = mujoco.append_child("option"); - option.append_attribute("timestep") = "0.0001"; + option.append_attribute("timestep") = "0.002"; option.append_attribute("iterations") = "50"; option.append_attribute("tolerance") = "1e-10"; option.append_attribute("solver") = "Newton"; diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index a578627..180d0c5 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -9,10 +9,262 @@ namespace spqr { SimulationThread::SimulationThread(const mjModel* model, mjData* data) : model_(model), data_(data), running_(true), paused_(false) {} + +// Source - https://stackoverflow.com/a +// Posted by Arun, modified by community. See post 'Timeline' for change history +// Retrieved 2026-01-12, License - CC BY-SA 3.0 +// TCP Communication, it sends before the size of the message and then the message itself +ssize_t SimulationThread::send_all(int fd, char* buf, size_t len) { + // First, send the size of the message + uint32_t msg_size = htonl(len); + ssize_t bytes_sent = send(fd, &msg_size, sizeof(msg_size), 0); + if (bytes_sent != sizeof(msg_size)) { + return -1; + } + + ssize_t total = 0; // how many bytes we've sent + size_t bytesleft = len; // how many we have left to send + ssize_t n = 0; + while (total < len) { + n = send(fd, buf + total, bytesleft, 0); + if (n == -1) { + /* print/log error details */ + return -1; + } + total += n; + bytesleft -= n; + } + return total; +} + +void SimulationThread::initializeSocket(int port) { + server_fd = socket(AF_INET, SOCK_STREAM, 0); + if (server_fd < 0) + throw std::runtime_error("Failed to create socket"); + + int opt = 1; + setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + int send_buf_size = 1 * 1024 * 1024; + if (setsockopt(server_fd, SOL_SOCKET, SO_SNDBUF, &send_buf_size, sizeof(send_buf_size)) < 0) { + perror("setsockopt(SO_SNDBUF)"); + } + int recv_buf_size = 1 * 1024 * 1024; + if (setsockopt(server_fd, SOL_SOCKET, SO_RCVBUF, &recv_buf_size, sizeof(recv_buf_size)) < 0) { + perror("setsockopt(SO_RCVBUF)"); + } + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(port); + + robots_ = RobotManager::instance().getRobots(); + if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0){ + perror("bind"); + throw std::runtime_error("Socket bind failed"); + } + if (listen(server_fd, robots_.size()) < 0) + throw std::runtime_error("Listen failed"); + + fds.push_back({server_fd, POLLIN, 0}); +} + +void SimulationThread::receiveCommandMessages() { + int robot_size = robots_.size(); + bool ready = false; + int done = 0; + while (done < robot_size) { + int ret = poll(fds.data(), fds.size(), 100); + if (ret <= 0) + continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is + + for (size_t i = 0; i < fds.size(); ++i) { + // An event occured for the i-th fd + if (fds[i].revents & POLLIN) { + char buffer[MAX_MSG_SIZE]; + int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); + if (n <= 0) { + close(fds[i].fd); + fds.erase(fds.begin() + i); + --i; + continue; + } + + 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()) + continue; + + std::string messageRecipient = it->second.as(); + + msgpack::sbuffer sbuf; + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (r->name == messageRecipient) { + r->receiveMessage(data_map); + ++done; + break; + } + } + } + } + } + } +} + + +void SimulationThread::waitInitialMessages() { + bool ready = false; + while (!ready) { + int ret = poll(fds.data(), fds.size(), 100); + if (ret <= 0) + continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is + + for (size_t i = 0; i < fds.size(); ++i) { + // An event occured for the i-th fd + if (fds[i].revents & POLLIN) { + if (fds[i].fd == server_fd) { + if(!entity_fd_map["server"]){ + entity_fd_map["server"] = server_fd; + } + // The only event for the server is someone knocking + int client_fd = accept(server_fd, nullptr, nullptr); + if (client_fd >= 0) { + fds.push_back({client_fd, POLLIN, 0}); + + // Receive initial message with robot name + char buffer[MAX_MSG_SIZE]; + int n = read(client_fd, buffer, sizeof(buffer) - 1); + + if (n <= 0) { + std::cerr << "Error reading the initial message.\n"; + // close(client_fd); + continue; + } + + // unpack of the MsgPack message + msgpack::object_handle oh = msgpack::unpack(buffer, n); + msgpack::object obj = oh.get(); + + // First message is the robot name as a string + if (obj.type != msgpack::type::STR) { + std::cerr << "First message must be a string. Ignore it...\n"; + continue; + } + + std::string robotName = obj.as(); + + // Send message with initial state + msgpack::sbuffer sbuf; + std::map answ; + bool answOk = false; + // Pack initial message + { + std::lock_guard lock(mutex_); + for (auto& r : robots_) { + if (r->name == robotName) { + r->isConnected = true; + answ = r->sendMessage(); + answOk = true; + break; + } + } + } + if (answOk) { + msgpack::pack(sbuf, answ); + if (sbuf.size() > 0) { + std::cout << "Connected Robot: " << robotName << "\n"; + std::cout << "Sending initial message to " << robotName << std::endl; + ssize_t bytes_sent = send_all(client_fd, sbuf.data(), sbuf.size()); + if (bytes_sent <= 0) { + perror("Error in sending initial message"); + } + } + } + } + } + else { + // The events for other fds indicate either an incoming message or a closed connection + // the read call disambiguates the two cases + char buffer[MAX_MSG_SIZE]; + int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); + if (n <= 0) { + close(fds[i].fd); + fds.erase(fds.begin() + i); + --i; + continue; + } + + 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()) + continue; + + std::string messageRecipient = it->second.as(); + + msgpack::sbuffer sbuf; + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (r->name == messageRecipient) { + // TODO: metti dentro una map nome del robot con suo fd + if (!r->isReady) { + r->isReady = true; + // Save robot_name <---> fd + // LA POSSO SPOSTARE SU + entity_fd_map[messageRecipient] = fds[i].fd; + std::cout << "Robot ready: " << r->name << std::endl; + r->receiveMessage(data_map); + } + if(areAllRobotsReady()){ + ready = true; + emit allRobotsReady(); + } + // TODO: vedere se va bene cosi o se va cambiato + } + } + } + } + } + } + } +} + +void SimulationThread::areAllRobotsReadyWrapper() { + if (areAllRobotsReady()) { + emit allRobotsReady(); + } +} +bool SimulationThread::areAllRobotsReady() const { + for (const auto& r : robots_) + if (!r->isReady) + return false; + std::cout << "All robots are ready!" << std::endl; + return true; +} + + + +/* +// SimulationThread IDEA + mj_step1(); + applyCommands(); + mj_step2(); + update(); + // ogni N step (per matchare ~100Hz): + for each robot: + send(state) // non-blocking + recv(torques) // bloccante o con timeout +*/ + void SimulationThread::run() { if (!model_) throw std::runtime_error("Cannot start simulation without mujoco model"); + std::cout << "RUNNNNNN" << std::endl; double sim_dt = model_->opt.timestep; using clock = std::chrono::steady_clock; @@ -33,11 +285,40 @@ void SimulationThread::run() { break; } + // for each robot: + // send(state) // non-blocking + // recv(torques) // bloccante o con timeout + + for(auto& r : robots_){ + msgpack::sbuffer sbuf; + std::map answ; + bool answOk = false; + { + std::unique_lock lock(mutex_); + answ = r->sendMessage(); + answOk = true; + } + + if (answOk) { + msgpack::pack(sbuf, answ); + if (sbuf.size() > 0) { + int fd = entity_fd_map[r->name]; + if(!fd) + perror("file descriptor not existing"); + ssize_t bytes_sent = send_all(fd, sbuf.data(), sbuf.size()); + if (bytes_sent <= 0) { + perror("Sending message"); + } + } + } + } + receiveCommandMessages(); + next_step_time += std::chrono::duration_cast(std::chrono::duration(sim_dt)); - std::this_thread::sleep_until(next_step_time); + // std::this_thread::sleep_until(next_step_time); - if (clock::now() > next_step_time) - next_step_time = clock::now(); + // if (clock::now() > next_step_time) + // next_step_time = clock::now(); } else { // When paused, sleep briefly to avoid busy-waiting std::this_thread::sleep_for(std::chrono::milliseconds(10)); From 2629424147d648e9579285978489fc55289e89fb Mon Sep 17 00:00:00 2001 From: Flavio Date: Thu, 2 Apr 2026 01:25:44 +0200 Subject: [PATCH 49/56] code refactoring of robot connections (changed a little bit) Now the connection and the ready are decoupled. Still some functions must be moved out from SimulationThread --- include/SimulationThread.h | 8 ++-- src/AppWindow.cpp | 36 +++++------------ src/RobotManager.cpp | 2 + src/SimulationThread.cpp | 83 ++++++++++++++------------------------ 4 files changed, 46 insertions(+), 83 deletions(-) diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 91e8cc9..4572a05 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -24,13 +24,14 @@ class SimulationThread : public QThread { bool isPaused(); void setMaxSimulationTime(int maxTime); void initializeSocket(int port); - void waitInitialMessages(); + void waitRobotConnections(); + void receiveCommandMessages(); signals: void stepCompleted(); void maxSimulationTimeReached(); - void allRobotsReady(); + void allRobotsReadySignal(); private: const mjModel* model_; @@ -47,12 +48,13 @@ class SimulationThread : public QThread { std::vector> robots_; std::vector fds; mutable std::mutex mutex_; + ssize_t send_all(int fd, char* buf, size_t len); void areAllRobotsReadyWrapper(); bool areAllRobotsReady() const; + bool areAllRobotsConnected() const; - void receiveCommandMessages(); }; diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index f2f7489..5c6e992 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -197,7 +197,7 @@ void AppWindow::loadScene(const QString& yaml_file) { sim->setMaxSimulationTime(parser.getSceneInfo().simulationConfig.simulation.max_simulation_time); connect(sim.get(), &SimulationThread::maxSimulationTimeReached, this, &AppWindow::close); - connect(sim.get(), &SimulationThread::allRobotsReady, + connect(sim.get(), &SimulationThread::allRobotsReadySignal, this, [this]() { if (sim) { @@ -206,20 +206,7 @@ void AppWindow::loadScene(const QString& yaml_file) { } }, Qt::QueuedConnection); - // Callback to start the simulation - // Simulation starts when the all the robots are ready - // RobotManager::instance().setAreAllRobotsReadyCallback([this]() { - // QMetaObject::invokeMethod( - // this, - // [this]() { - // if (sim) { - // std::cout << "Starting simulation!" << std::endl; - // sim->start(); - // } - // }, - // Qt::QueuedConnection); - // }); - + // Ensure the shared memory directory exists and is writable by the current user. // Docker bind mounts create missing host dirs as root, so remove and recreate if needed. const std::filesystem::path shmDir("/dev/shm/circus_ipc"); @@ -232,19 +219,14 @@ void AppWindow::loadScene(const QString& yaml_file) { RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server sim->initializeSocket(frameworkCommunicationPort); - std::cout << "STEP 0" << std::endl; + std::cout << "Starting containers..." << std::endl; RobotManager::instance().startContainers(); - std::cout << "FINALMENTE 1" << std::endl; - sim->waitInitialMessages(); - std::cout << "FINALMENTE 2" << std::endl; - - /* - // Pseudocode - // Può essere aggiunta una logica di tentare la connessione appena il server diventa disponibile - start_server(); // bind + listen - spawn_containers(); - accept_connections(); - */ + + std::cout << "Connecting Robots..." << std::endl; + sim->waitRobotConnections(); + + std::cout << "Waiting Robots are Ready..." << std::endl; + sim->receiveCommandMessages(); // Set initial simulation state (playing when scene is loaded) toolsPanel->setSimulationPlaying(true); diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 3e9d1fe..9b6e364 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -59,6 +59,7 @@ std::shared_ptr RobotManager::create(const std::string& name, const std:: } void RobotManager::startContainers() { + // TODO: remove // startCommunicationServer(frameworkCommunicationPort); YAML::Node pathsRoot = loadYamlFile(pathsConfigPath); @@ -93,6 +94,7 @@ void RobotManager::startContainers() { r->container->create(r, image, binds); r->container->start(); } + std::cout << "Containers started successfully!" << std::endl; } void RobotManager::startCommunicationServer(int port) { diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 180d0c5..6396a27 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -71,13 +71,12 @@ void SimulationThread::initializeSocket(int port) { void SimulationThread::receiveCommandMessages() { int robot_size = robots_.size(); - bool ready = false; int done = 0; while (done < robot_size) { int ret = poll(fds.data(), fds.size(), 100); if (ret <= 0) continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is - + for (size_t i = 0; i < fds.size(); ++i) { // An event occured for the i-th fd if (fds[i].revents & POLLIN) { @@ -89,7 +88,7 @@ void SimulationThread::receiveCommandMessages() { --i; continue; } - + msgpack::object_handle oh = msgpack::unpack(buffer, n); auto data_map = oh.get().as>(); auto it = data_map.find("robot_name"); @@ -97,12 +96,20 @@ void SimulationThread::receiveCommandMessages() { continue; std::string messageRecipient = it->second.as(); - + msgpack::sbuffer sbuf; { std::unique_lock lock(mutex_); for (auto& r : robots_) { if (r->name == messageRecipient) { + if (!r->isReady) { + r->isReady = true; + std::cout << "Robot ready: " << r->name << std::endl; + if(areAllRobotsReady()){ + emit allRobotsReadySignal(); + } + } + r->receiveMessage(data_map); ++done; break; @@ -114,10 +121,9 @@ void SimulationThread::receiveCommandMessages() { } } - -void SimulationThread::waitInitialMessages() { - bool ready = false; - while (!ready) { +void SimulationThread::waitRobotConnections() { + bool areAllConnected = false; + while (!areAllConnected) { int ret = poll(fds.data(), fds.size(), 100); if (ret <= 0) continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is @@ -155,6 +161,7 @@ void SimulationThread::waitInitialMessages() { } std::string robotName = obj.as(); + entity_fd_map[robotName] = client_fd; // Send message with initial state msgpack::sbuffer sbuf; @@ -183,59 +190,30 @@ void SimulationThread::waitInitialMessages() { } } } - } - } - else { - // The events for other fds indicate either an incoming message or a closed connection - // the read call disambiguates the two cases - char buffer[MAX_MSG_SIZE]; - int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); - if (n <= 0) { - close(fds[i].fd); - fds.erase(fds.begin() + i); - --i; - continue; - } - - 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()) - continue; - - std::string messageRecipient = it->second.as(); - - msgpack::sbuffer sbuf; - { - std::unique_lock lock(mutex_); - for (auto& r : robots_) { - if (r->name == messageRecipient) { - // TODO: metti dentro una map nome del robot con suo fd - if (!r->isReady) { - r->isReady = true; - // Save robot_name <---> fd - // LA POSSO SPOSTARE SU - entity_fd_map[messageRecipient] = fds[i].fd; - std::cout << "Robot ready: " << r->name << std::endl; - r->receiveMessage(data_map); - } - if(areAllRobotsReady()){ - ready = true; - emit allRobotsReady(); - } - // TODO: vedere se va bene cosi o se va cambiato - } + if(areAllRobotsConnected()){ + areAllConnected = true; + std::cout << "All Robots are connected!" << std::endl; + break; } } - } + } } } } +} + +bool SimulationThread::areAllRobotsConnected() const{ + for (auto& r : robots_) { + if(!r->isConnected){ + return false; + } + } + return true; } void SimulationThread::areAllRobotsReadyWrapper() { if (areAllRobotsReady()) { - emit allRobotsReady(); + emit allRobotsReadySignal(); } } bool SimulationThread::areAllRobotsReady() const { @@ -264,7 +242,6 @@ void SimulationThread::run() { if (!model_) throw std::runtime_error("Cannot start simulation without mujoco model"); - std::cout << "RUNNNNNN" << std::endl; double sim_dt = model_->opt.timestep; using clock = std::chrono::steady_clock; From c6318fe3089408df3005be7072a18bd5469f580d Mon Sep 17 00:00:00 2001 From: michelebri Date: Thu, 2 Apr 2026 21:10:29 +0200 Subject: [PATCH 50/56] resend message --- resources/scenes/1v1.yaml | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index b6b46a3..a73143b 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -1,9 +1,20 @@ simulation_config: default teams: red: - number: 19 + number: 8 players: - - type: Booster-T1 - number: 3 - position: [-1.0, 0.0, 0.68] - orientation: [0.0, 0.0, 0.0] \ No newline at end of file + - type: Booster-T1 + number: 1 + position: [-7.0, 0.0, 0.68] + orientation: [0.0, 0.0, 0.0] + blue: + number: 51 + players: + - type: Booster-T1 + number: 1 + position: [7.0, 0.0, 0.68] + orientation: [0.0, 0.0, 3.14] + - type: Booster-T1 + number: 2 + position: [5.0, 1.5, 0.68] + orientation: [0.0, 0.0, 3.14] \ No newline at end of file From f8d64785de0d1862ee7d7747fd481a60ca1e31b3 Mon Sep 17 00:00:00 2001 From: michelebri Date: Thu, 2 Apr 2026 21:10:40 +0200 Subject: [PATCH 51/56] resend message --- resources/scenes/1v1.yaml | 8 ++------ src/SimulationThread.cpp | 43 ++++++++++++++++++++++++++++++++------- 2 files changed, 38 insertions(+), 13 deletions(-) diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index a73143b..75bb88b 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -5,16 +5,12 @@ teams: players: - type: Booster-T1 number: 1 - position: [-7.0, 0.0, 0.68] + position: [-4.0, 0.0, 0.68] orientation: [0.0, 0.0, 0.0] blue: number: 51 players: - type: Booster-T1 number: 1 - position: [7.0, 0.0, 0.68] - orientation: [0.0, 0.0, 3.14] - - type: Booster-T1 - number: 2 - position: [5.0, 1.5, 0.68] + position: [4.0, 0.0, 0.68] orientation: [0.0, 0.0, 3.14] \ No newline at end of file diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 6396a27..bb7aa14 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -1,5 +1,6 @@ #include "SimulationThread.h" +#include #include #include "GameController.h" @@ -72,11 +73,39 @@ void SimulationThread::initializeSocket(int port) { void SimulationThread::receiveCommandMessages() { int robot_size = robots_.size(); int done = 0; + // Track which robots have not yet replied this step + std::set pendingRobots; + for (auto& r : robots_) + pendingRobots.insert(r->name); + + int timeoutCount = 0; while (done < robot_size) { - int ret = poll(fds.data(), fds.size(), 100); - if (ret <= 0) - continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is - + int ret = poll(fds.data(), fds.size(), 500); + if (ret <= 0) { + ++timeoutCount; + // Resend state only every 5 timeouts (2.5s) to avoid flooding the TCP buffer + if (timeoutCount % 5 != 0) + continue; + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (pendingRobots.count(r->name)) { + std::cout << "[receiveCommandMessages] Timeout, resending state to: " << r->name << std::endl; + msgpack::sbuffer sbuf; + auto answ = r->sendMessage(); + msgpack::pack(sbuf, answ); + if (sbuf.size() > 0) { + int fd = entity_fd_map[r->name]; + if (fd) + send_all(fd, sbuf.data(), sbuf.size()); + else + std::cerr << "[receiveCommandMessages] No fd for: " << r->name << std::endl; + } + } + } + continue; + } + timeoutCount = 0; + for (size_t i = 0; i < fds.size(); ++i) { // An event occured for the i-th fd if (fds[i].revents & POLLIN) { @@ -88,7 +117,7 @@ void SimulationThread::receiveCommandMessages() { --i; continue; } - + msgpack::object_handle oh = msgpack::unpack(buffer, n); auto data_map = oh.get().as>(); auto it = data_map.find("robot_name"); @@ -96,8 +125,7 @@ void SimulationThread::receiveCommandMessages() { continue; std::string messageRecipient = it->second.as(); - - msgpack::sbuffer sbuf; + { std::unique_lock lock(mutex_); for (auto& r : robots_) { @@ -111,6 +139,7 @@ void SimulationThread::receiveCommandMessages() { } r->receiveMessage(data_map); + pendingRobots.erase(messageRecipient); ++done; break; } From d5d648b29aba3ab15aa3dfd20bffedd2e958a7df Mon Sep 17 00:00:00 2001 From: Flavio Date: Fri, 3 Apr 2026 19:33:23 +0200 Subject: [PATCH 52/56] da vedere se funziona --- include/RobotManager.h | 11 ++++-- include/SimulationThread.h | 8 +--- src/RobotManager.cpp | 15 ++++--- src/SimulationThread.cpp | 80 +++++++++++++++----------------------- 4 files changed, 49 insertions(+), 65 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 87dee0d..25b109b 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -39,6 +39,10 @@ class RobotManager { size_t count() const; void update(); void clear(); + + void areAllRobotsReadyWrapper(); + bool areAllRobotsReady() const; + bool areAllRobotsConnected() const; void bindMujoco(MujocoContext* mujContext); @@ -52,8 +56,10 @@ class RobotManager { void setAreAllRobotsReadyCallback(std::function cb); void applyCommands(); - std::vector> getRobots(); + + signals: + void allRobotsReadySignal(); private: RobotManager() = default; @@ -66,9 +72,6 @@ class RobotManager { void _serverInternal(int port); - void areAllRobotsReadyWrapper(); - bool areAllRobotsReady() const; - std::atomic serverRunning_ = false; std::thread serverThread_; diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 4572a05..01b1ab3 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -31,7 +31,6 @@ class SimulationThread : public QThread { signals: void stepCompleted(); void maxSimulationTimeReached(); - void allRobotsReadySignal(); private: const mjModel* model_; @@ -51,12 +50,7 @@ class SimulationThread : public QThread { ssize_t send_all(int fd, char* buf, size_t len); - void areAllRobotsReadyWrapper(); - bool areAllRobotsReady() const; - bool areAllRobotsConnected() const; - - - + void sendStateMessages(); }; } // namespace spqr diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 9b6e364..860c161 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -300,13 +300,18 @@ ssize_t RobotManager::send_all(int fd, char* buf, size_t len) { return total; } -void RobotManager::setAreAllRobotsReadyCallback(std::function cb) { - std::lock_guard lock(mutex_); - areAllRobotsReadyCallback_ = std::move(cb); +bool RobotManager::areAllRobotsConnected() const{ + for (auto& r : robots_) { + if(!r->isConnected){ + return false; + } + } + return true; } + void RobotManager::areAllRobotsReadyWrapper() { - if (areAllRobotsReady() && areAllRobotsReadyCallback_) { - areAllRobotsReadyCallback_(); + if (areAllRobotsReady()) { + emit allRobotsReadySignal(); } } bool RobotManager::areAllRobotsReady() const { diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 6396a27..a1588d7 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -105,8 +105,8 @@ void SimulationThread::receiveCommandMessages() { if (!r->isReady) { r->isReady = true; std::cout << "Robot ready: " << r->name << std::endl; - if(areAllRobotsReady()){ - emit allRobotsReadySignal(); + if(RobotManager::instance().areAllRobotsReady()){ + emit RobotManager::instance().allRobotsReadySignal(); } } @@ -190,7 +190,7 @@ void SimulationThread::waitRobotConnections() { } } } - if(areAllRobotsConnected()){ + if(RobotManager::instance().areAllRobotsConnected()){ areAllConnected = true; std::cout << "All Robots are connected!" << std::endl; break; @@ -202,29 +202,6 @@ void SimulationThread::waitRobotConnections() { } } -bool SimulationThread::areAllRobotsConnected() const{ - for (auto& r : robots_) { - if(!r->isConnected){ - return false; - } - } - return true; -} - -void SimulationThread::areAllRobotsReadyWrapper() { - if (areAllRobotsReady()) { - emit allRobotsReadySignal(); - } -} -bool SimulationThread::areAllRobotsReady() const { - for (const auto& r : robots_) - if (!r->isReady) - return false; - std::cout << "All robots are ready!" << std::endl; - return true; -} - - /* // SimulationThread IDEA @@ -266,29 +243,7 @@ void SimulationThread::run() { // send(state) // non-blocking // recv(torques) // bloccante o con timeout - for(auto& r : robots_){ - msgpack::sbuffer sbuf; - std::map answ; - bool answOk = false; - { - std::unique_lock lock(mutex_); - answ = r->sendMessage(); - answOk = true; - } - - if (answOk) { - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - int fd = entity_fd_map[r->name]; - if(!fd) - perror("file descriptor not existing"); - ssize_t bytes_sent = send_all(fd, sbuf.data(), sbuf.size()); - if (bytes_sent <= 0) { - perror("Sending message"); - } - } - } - } + sendStateMessages(); receiveCommandMessages(); next_step_time += std::chrono::duration_cast(std::chrono::duration(sim_dt)); @@ -305,6 +260,33 @@ void SimulationThread::run() { } } +void SimulationThread::sendStateMessages() { + for(auto& r : robots_){ + msgpack::sbuffer sbuf; + std::map answ; + bool answOk = false; + { + std::unique_lock lock(mutex_); + answ = r->sendMessage(); + answOk = true; + } + + if (answOk) { + msgpack::pack(sbuf, answ); + if (sbuf.size() > 0) { + int fd = entity_fd_map[r->name]; + if(!fd) + perror("file descriptor not existing"); + ssize_t bytes_sent = send_all(fd, sbuf.data(), sbuf.size()); + if (bytes_sent <= 0) { + perror("Sending message"); + } + } + } + } + +} + void SimulationThread::stop() { running_ = false; wait(); From d14dae9c342debdfe4a48cd2b09b50bb68ed7321 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 10 Apr 2026 01:04:53 +0200 Subject: [PATCH 53/56] refactoring --- include/RobotManager.h | 10 -- include/SimulationThread.h | 1 + src/AppWindow.cpp | 3 - src/Container.cpp | 9 -- src/RobotManager.cpp | 210 ------------------------------------- src/SimulationThread.cpp | 2 +- 6 files changed, 2 insertions(+), 233 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 25b109b..71608f5 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -40,7 +40,6 @@ class RobotManager { void update(); void clear(); - void areAllRobotsReadyWrapper(); bool areAllRobotsReady() const; bool areAllRobotsConnected() const; @@ -51,16 +50,9 @@ class RobotManager { void startContainers(); - void startCommunicationServer(int port); - void stopCommunicationServer(); - void setAreAllRobotsReadyCallback(std::function cb); void applyCommands(); - - signals: - void allRobotsReadySignal(); - private: RobotManager() = default; ~RobotManager() = default; @@ -70,8 +62,6 @@ class RobotManager { ssize_t send_all(int fd, char* buf, size_t len); - void _serverInternal(int port); - std::atomic serverRunning_ = false; std::thread serverThread_; diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 01b1ab3..f0e77e7 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -31,6 +31,7 @@ class SimulationThread : public QThread { signals: void stepCompleted(); void maxSimulationTimeReached(); + void allRobotsReadySignal(); private: const mjModel* model_; diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 5c6e992..862dfbc 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -112,7 +112,6 @@ void AppWindow::openScene() { void AppWindow::loadScene(const QString& yaml_file) { try { TeamManager::instance().clear(); - RobotManager::instance().stopCommunicationServer(); if (sim) { sim->stop(); @@ -336,7 +335,6 @@ void AppWindow::signalHandler(int signal) { std::cerr.flush(); TeamManager::instance().clear(); - RobotManager::instance().stopCommunicationServer(); std::cerr << "Cleanup complete. Re-raising signal." << std::endl; std::cerr.flush(); @@ -349,7 +347,6 @@ AppWindow::~AppWindow() { if (sim != nullptr && sim->isRunning()) sim->stop(); TeamManager::instance().clear(); - RobotManager::instance().stopCommunicationServer(); } } // namespace spqr diff --git a/src/Container.cpp b/src/Container.cpp index 735ba0c..e3815bd 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -58,13 +58,6 @@ void Container::create(const std::shared_ptr& robot, const std::string& i {"Privileged", true}, {"NetworkMode", CIRCUS_NETWORK_NAME}}; - payload["HostConfig"]["DeviceRequests"] = nlohmann::json::array(); - payload["HostConfig"]["DeviceRequests"].push_back({ - {"Driver", "nvidia"}, - {"Count", -1}, - {"Capabilities", nlohmann::json::array({nlohmann::json::array({"gpu"})})}, - }); - payload["NetworkingConfig"] = {{"EndpointsConfig", {{CIRCUS_NETWORK_NAME, @@ -78,8 +71,6 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "TEAM_COLOR=" + robot->colorName, "DISPLAY=" + envOrDefault("DISPLAY", ":0"), "QT_X11_NO_MITSHM=1", - "NVIDIA_VISIBLE_DEVICES=all", - "NVIDIA_DRIVER_CAPABILITIES=all", "XAUTHORITY=/root/.Xauthority", "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 860c161..37f671c 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -59,8 +59,6 @@ std::shared_ptr RobotManager::create(const std::string& name, const std:: } void RobotManager::startContainers() { - // TODO: remove - // startCommunicationServer(frameworkCommunicationPort); YAML::Node pathsRoot = loadYamlFile(pathsConfigPath); YAML::Node configRoot = loadYamlFile(frameworkConfigPath); @@ -97,209 +95,6 @@ void RobotManager::startContainers() { std::cout << "Containers started successfully!" << std::endl; } -void RobotManager::startCommunicationServer(int port) { - if (serverRunning_) - throw std::runtime_error("Server already running"); - serverRunning_ = true; - serverThread_ = std::thread(&RobotManager::_serverInternal, this, port); -} - -void RobotManager::stopCommunicationServer() { - if (!serverRunning_) - return; - - serverRunning_ = false; - - if (serverThread_.joinable()) - serverThread_.join(); -} - -std::vector> RobotManager::getRobots() { - return robots_; -} - -void RobotManager::_serverInternal(int port) { - int server_fd = socket(AF_INET, SOCK_STREAM, 0); - if (server_fd < 0) - throw std::runtime_error("Failed to create socket"); - - int opt = 1; - setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); - int send_buf_size = 1 * 1024 * 1024; - if (setsockopt(server_fd, SOL_SOCKET, SO_SNDBUF, &send_buf_size, sizeof(send_buf_size)) < 0) { - perror("setsockopt(SO_SNDBUF)"); - } - int recv_buf_size = 1 * 1024 * 1024; - if (setsockopt(server_fd, SOL_SOCKET, SO_RCVBUF, &recv_buf_size, sizeof(recv_buf_size)) < 0) { - perror("setsockopt(SO_RCVBUF)"); - } - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = INADDR_ANY; - address.sin_port = htons(port); - - if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) - throw std::runtime_error("Socket bind failed"); - if (listen(server_fd, robots_.size()) < 0) - throw std::runtime_error("Listen failed"); - - std::vector fds; - fds.push_back({server_fd, POLLIN, 0}); - - // Using a polling server. It isn't immediately intuitive, but it is efficient for this use case. - while (serverRunning_) { - // the poll blocks until a new connection arrives on server_fd or data arrives in one of the - // monitored fd or a socket closes or the timeout expires. - int ret = poll(fds.data(), fds.size(), 100); - if (ret <= 0) - continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is - // still true) - - for (size_t i = 0; i < fds.size(); ++i) { - // An event occured for the i-th fd - if (fds[i].revents & POLLIN) { - if (fds[i].fd == server_fd) { - // The only event for the server is someone knocking - int client_fd = accept(server_fd, nullptr, nullptr); - if (client_fd >= 0) { - fds.push_back({client_fd, POLLIN, 0}); - - // Receive initial message with robot name - char buffer[MAX_MSG_SIZE]; - int n = read(client_fd, buffer, sizeof(buffer) - 1); - - if (n <= 0) { - std::cerr << "Error reading the initial message.\n"; - // close(client_fd); - continue; - } - - // unpack of the MsgPack message - msgpack::object_handle oh = msgpack::unpack(buffer, n); - msgpack::object obj = oh.get(); - - // First message is the robot name as a string - if (obj.type != msgpack::type::STR) { - std::cerr << "First message must be a string. Ignore it...\n"; - continue; - } - - std::string robotName = obj.as(); - - // Send message with initial state - msgpack::sbuffer sbuf; - std::map answ; - bool answOk = false; - // Pack initial message - { - std::lock_guard lock(mutex_); - for (auto& r : robots_) { - if (r->name == robotName) { - r->isConnected = true; - answ = r->sendMessage(); - answOk = true; - break; - } - } - } - if (answOk) { - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - std::cout << "Connected Robot: " << robotName << "\n"; - std::cout << "Sending initial message to " << robotName << std::endl; - ssize_t bytes_sent = send_all(client_fd, sbuf.data(), sbuf.size()); - if (bytes_sent <= 0) { - perror("Error in sending initial message"); - } - } - } - } - } else { - // The events for other fds indicate either an incoming message or a closed connection - // the read call disambiguates the two cases - char buffer[MAX_MSG_SIZE]; - int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); - if (n <= 0) { - close(fds[i].fd); - fds.erase(fds.begin() + i); - --i; - continue; - } - - 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()) - continue; - - std::string messageRecipient = it->second.as(); - - msgpack::sbuffer sbuf; - std::map answ; - bool answOk = false; - { - std::unique_lock lock(mutex_); - for (auto& r : robots_) { - if (r->name == messageRecipient) { - if (!r->isReady) { - r->isReady = true; - std::cout << "Robot ready: " << r->name << std::endl; - areAllRobotsReadyWrapper(); - } - r->receiveMessage(data_map); - answ = r->sendMessage(); - answOk = true; - break; - } - } - } - - if (answOk) { - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - ssize_t bytes_sent = send_all(fds[i].fd, sbuf.data(), sbuf.size()); - if (bytes_sent <= 0) { - perror("Sending message"); - } - } - } - } - } - } - } - - for (auto& fd : fds) - close(fd.fd); -} - -// Source - https://stackoverflow.com/a -// Posted by Arun, modified by community. See post 'Timeline' for change history -// Retrieved 2026-01-12, License - CC BY-SA 3.0 -// TCP Communication, it sends before the size of the message and then the message itself -ssize_t RobotManager::send_all(int fd, char* buf, size_t len) { - // First, send the size of the message - uint32_t msg_size = htonl(len); - ssize_t bytes_sent = send(fd, &msg_size, sizeof(msg_size), 0); - if (bytes_sent != sizeof(msg_size)) { - return -1; - } - - ssize_t total = 0; // how many bytes we've sent - size_t bytesleft = len; // how many we have left to send - ssize_t n = 0; - while (total < len) { - n = send(fd, buf + total, bytesleft, 0); - if (n == -1) { - /* print/log error details */ - return -1; - } - total += n; - bytesleft -= n; - } - return total; -} - bool RobotManager::areAllRobotsConnected() const{ for (auto& r : robots_) { if(!r->isConnected){ @@ -309,11 +104,6 @@ bool RobotManager::areAllRobotsConnected() const{ return true; } -void RobotManager::areAllRobotsReadyWrapper() { - if (areAllRobotsReady()) { - emit allRobotsReadySignal(); - } -} bool RobotManager::areAllRobotsReady() const { for (const auto& r : robots_) if (!r->isReady) diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index a1588d7..26dd603 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -106,7 +106,7 @@ void SimulationThread::receiveCommandMessages() { r->isReady = true; std::cout << "Robot ready: " << r->name << std::endl; if(RobotManager::instance().areAllRobotsReady()){ - emit RobotManager::instance().allRobotsReadySignal(); + emit allRobotsReadySignal(); } } From c6bbbfa26e05c44ce84b83ab77333482ff36fc3b Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 10 Apr 2026 15:03:15 +0200 Subject: [PATCH 54/56] nvidia stuff readded --- src/Container.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/Container.cpp b/src/Container.cpp index e3815bd..735ba0c 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -58,6 +58,13 @@ void Container::create(const std::shared_ptr& robot, const std::string& i {"Privileged", true}, {"NetworkMode", CIRCUS_NETWORK_NAME}}; + payload["HostConfig"]["DeviceRequests"] = nlohmann::json::array(); + payload["HostConfig"]["DeviceRequests"].push_back({ + {"Driver", "nvidia"}, + {"Count", -1}, + {"Capabilities", nlohmann::json::array({nlohmann::json::array({"gpu"})})}, + }); + payload["NetworkingConfig"] = {{"EndpointsConfig", {{CIRCUS_NETWORK_NAME, @@ -71,6 +78,8 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "TEAM_COLOR=" + robot->colorName, "DISPLAY=" + envOrDefault("DISPLAY", ":0"), "QT_X11_NO_MITSHM=1", + "NVIDIA_VISIBLE_DEVICES=all", + "NVIDIA_DRIVER_CAPABILITIES=all", "XAUTHORITY=/root/.Xauthority", "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", From 271a89227dc456a7ec55ab662e02e7ffac27bee6 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 10 Apr 2026 15:20:40 +0200 Subject: [PATCH 55/56] pre commit --- include/RobotManager.h | 2 +- include/SimulationThread.h | 6 ++---- include/robots/BoosterT1.h | 2 +- include/robots/Robot.h | 3 +-- include/sensors/CameraDepth.h | 4 ++-- resources/scenes/1v1.yaml | 2 +- src/AppWindow.cpp | 2 +- src/SimulationThread.cpp | 25 +++++++++++-------------- 8 files changed, 20 insertions(+), 26 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 71608f5..cceace3 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -39,7 +39,7 @@ class RobotManager { size_t count() const; void update(); void clear(); - + bool areAllRobotsReady() const; bool areAllRobotsConnected() const; diff --git a/include/SimulationThread.h b/include/SimulationThread.h index f0e77e7..ae5d9b4 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -3,13 +3,12 @@ #include #include -#include -#include #include +#include +#include #include "robots/Robot.h" - namespace spqr { class SimulationThread : public QThread { @@ -27,7 +26,6 @@ class SimulationThread : public QThread { void waitRobotConnections(); void receiveCommandMessages(); - signals: void stepCompleted(); void maxSimulationTimeReached(); diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index a87b850..4600615 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -186,7 +186,7 @@ class BoosterT1 : public Robot { std::map joint_map; std::unordered_map latestTorques; - + std::string shm_dir_; ImageSharedMemoryWriter rgb_writer_; ImageSharedMemoryWriter depth_writer_; diff --git a/include/robots/Robot.h b/include/robots/Robot.h index e7728a8..5fe0e91 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -13,7 +13,6 @@ #include #include #include - #include "Container.h" #include "MujocoContext.h" @@ -44,7 +43,7 @@ class Robot { virtual std::map sendMessage() = 0; virtual std::map getSensors() = 0; virtual void applyCommands() = 0; - + std::string name; std::string type; uint8_t number; diff --git a/include/sensors/CameraDepth.h b/include/sensors/CameraDepth.h index d482563..2f802c1 100644 --- a/include/sensors/CameraDepth.h +++ b/include/sensors/CameraDepth.h @@ -5,9 +5,9 @@ #include #include -#include #include #include +#include #include #include @@ -81,7 +81,7 @@ class CameraDepth : public Sensor { mjr_readPixels(nullptr, tempDepth.data(), viewport, &mujContext->ctx); // MuJoCo map.znear/zfar are expressed relative to model extent. - //DEPTH FIX - MICHELE : Verify. + // DEPTH FIX - MICHELE : Verify. // MuJoCo stores vis.map.znear/zfar as fractions of model->stat.extent, not absolute meters. // Multiply by stat.extent to convert them to metric near/far distances used by depth linearization. diff --git a/resources/scenes/1v1.yaml b/resources/scenes/1v1.yaml index 75bb88b..49eb948 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -13,4 +13,4 @@ teams: - type: Booster-T1 number: 1 position: [4.0, 0.0, 0.68] - orientation: [0.0, 0.0, 3.14] \ No newline at end of file + orientation: [0.0, 0.0, 3.14] diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 862dfbc..3aa2f86 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -205,7 +205,7 @@ void AppWindow::loadScene(const QString& yaml_file) { } }, Qt::QueuedConnection); - + // Ensure the shared memory directory exists and is writable by the current user. // Docker bind mounts create missing host dirs as root, so remove and recreate if needed. const std::filesystem::path shmDir("/dev/shm/circus_ipc"); diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 21f77ff..4453aa7 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -10,7 +10,6 @@ namespace spqr { SimulationThread::SimulationThread(const mjModel* model, mjData* data) : model_(model), data_(data), running_(true), paused_(false) {} - // Source - https://stackoverflow.com/a // Posted by Arun, modified by community. See post 'Timeline' for change history // Retrieved 2026-01-12, License - CC BY-SA 3.0 @@ -60,13 +59,13 @@ void SimulationThread::initializeSocket(int port) { address.sin_port = htons(port); robots_ = RobotManager::instance().getRobots(); - if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0){ + if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) { perror("bind"); throw std::runtime_error("Socket bind failed"); } if (listen(server_fd, robots_.size()) < 0) throw std::runtime_error("Listen failed"); - + fds.push_back({server_fd, POLLIN, 0}); } @@ -133,7 +132,7 @@ void SimulationThread::receiveCommandMessages() { if (!r->isReady) { r->isReady = true; std::cout << "Robot ready: " << r->name << std::endl; - if(RobotManager::instance().areAllRobotsReady()){ + if (RobotManager::instance().areAllRobotsReady()) { emit allRobotsReadySignal(); } } @@ -156,12 +155,12 @@ void SimulationThread::waitRobotConnections() { int ret = poll(fds.data(), fds.size(), 100); if (ret <= 0) continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is - + for (size_t i = 0; i < fds.size(); ++i) { // An event occured for the i-th fd if (fds[i].revents & POLLIN) { if (fds[i].fd == server_fd) { - if(!entity_fd_map["server"]){ + if (!entity_fd_map["server"]) { entity_fd_map["server"] = server_fd; } // The only event for the server is someone knocking @@ -219,18 +218,17 @@ void SimulationThread::waitRobotConnections() { } } } - if(RobotManager::instance().areAllRobotsConnected()){ + if (RobotManager::instance().areAllRobotsConnected()) { areAllConnected = true; std::cout << "All Robots are connected!" << std::endl; break; } } - } + } } } } -} - +} /* // SimulationThread IDEA @@ -259,7 +257,7 @@ void SimulationThread::run() { mj_step2(model_, data_); RobotManager::instance().update(); GameController::instance().update(); - + std::memset(data_->xfrc_applied, 0, model_->nbody * 6 * sizeof(mjtNum)); if (maxSimulationTime_ > 0 && data_->time >= maxSimulationTime_) { @@ -290,7 +288,7 @@ void SimulationThread::run() { } void SimulationThread::sendStateMessages() { - for(auto& r : robots_){ + for (auto& r : robots_) { msgpack::sbuffer sbuf; std::map answ; bool answOk = false; @@ -304,7 +302,7 @@ void SimulationThread::sendStateMessages() { msgpack::pack(sbuf, answ); if (sbuf.size() > 0) { int fd = entity_fd_map[r->name]; - if(!fd) + if (!fd) perror("file descriptor not existing"); ssize_t bytes_sent = send_all(fd, sbuf.data(), sbuf.size()); if (bytes_sent <= 0) { @@ -313,7 +311,6 @@ void SimulationThread::sendStateMessages() { } } } - } void SimulationThread::stop() { From 8843bf1f81a97c3ecad4c2592ff18ab4b96702e1 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 10 Apr 2026 19:47:31 +0200 Subject: [PATCH 56/56] precommit 2 --- include/robots/BoosterT1.h | 1 - src/AppWindow.cpp | 6 +++--- src/RobotManager.cpp | 5 ++--- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 4600615..49ac809 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -190,7 +190,6 @@ class BoosterT1 : public Robot { std::string shm_dir_; ImageSharedMemoryWriter rgb_writer_; ImageSharedMemoryWriter depth_writer_; - }; } // namespace spqr diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 3aa2f86..86bcdb1 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -196,15 +196,15 @@ void AppWindow::loadScene(const QString& yaml_file) { sim->setMaxSimulationTime(parser.getSceneInfo().simulationConfig.simulation.max_simulation_time); connect(sim.get(), &SimulationThread::maxSimulationTimeReached, this, &AppWindow::close); - connect(sim.get(), &SimulationThread::allRobotsReadySignal, - this, + connect( + sim.get(), &SimulationThread::allRobotsReadySignal, this, [this]() { if (sim) { std::cout << "Starting simulation!" << std::endl; sim->start(); } }, - Qt::QueuedConnection); + Qt::QueuedConnection); // Ensure the shared memory directory exists and is writable by the current user. // Docker bind mounts create missing host dirs as root, so remove and recreate if needed. diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 37f671c..90fb4f0 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -59,7 +59,6 @@ std::shared_ptr RobotManager::create(const std::string& name, const std:: } void RobotManager::startContainers() { - YAML::Node pathsRoot = loadYamlFile(pathsConfigPath); YAML::Node configRoot = loadYamlFile(frameworkConfigPath); @@ -95,9 +94,9 @@ void RobotManager::startContainers() { std::cout << "Containers started successfully!" << std::endl; } -bool RobotManager::areAllRobotsConnected() const{ +bool RobotManager::areAllRobotsConnected() const { for (auto& r : robots_) { - if(!r->isConnected){ + if (!r->isConnected) { return false; } }