diff --git a/GERMAN-OPEN 2026 Instructions.md b/GERMAN-OPEN 2026 Instructions.md new file mode 100644 index 0000000..7bfc50c --- /dev/null +++ b/GERMAN-OPEN 2026 Instructions.md @@ -0,0 +1,90 @@ +# 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` + + +## 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` 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. diff --git a/clean-docker.sh b/clean-docker.sh new file mode 100644 index 0000000..30231ae --- /dev/null +++ b/clean-docker.sh @@ -0,0 +1,26 @@ +# 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 "No containers to stop" +fi + +EXITED_IDS=$(docker ps --filter name="CIRCUS_*" -aq) +if [ -n "$EXITED_IDS" ]; then + echo "Removing circus containers" + docker rm $EXITED_IDS +else + 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/dockerfiles/Dockerfile b/dockerfiles/Dockerfile index b57a800..6bc8469 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 @@ -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,12 +57,19 @@ 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 +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 + # 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 configs/system_settings_config.yaml /opt/booster/configs +COPY booster.conf /etc/supervisor/conf.d/ + CMD ["/app/entrypoint.sh"] diff --git a/dockerfiles/booster.conf b/dockerfiles/booster.conf index dfea72d..5b6a038 100644 --- a/dockerfiles/booster.conf +++ b/dockerfiles/booster.conf @@ -1,18 +1,46 @@ +[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 +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 +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 diff --git a/dockerfiles/configs/bashrc b/dockerfiles/configs/bashrc index b401216..eec04d6 100644 --- a/dockerfiles/configs/bashrc +++ b/dockerfiles/configs/bashrc @@ -14,3 +14,16 @@ 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" +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/dockerfiles/delayed_start.sh b/dockerfiles/delayed_start.sh new file mode 100644 index 0000000..b6293bc --- /dev/null +++ b/dockerfiles/delayed_start.sh @@ -0,0 +1,9 @@ +#!/bin/bash +# Wait for 5 seconds +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 diff --git a/dockerfiles/entrypoint.sh b/dockerfiles/entrypoint.sh index a9cfb26..7e734ee 100644 --- a/dockerfiles/entrypoint.sh +++ b/dockerfiles/entrypoint.sh @@ -1,9 +1,16 @@ #!/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 +# Export environment variables for MAXIMUS framework +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 diff --git a/include/CircusNetwork.h b/include/CircusNetwork.h new file mode 100644 index 0000000..eb4fc34 --- /dev/null +++ b/include/CircusNetwork.h @@ -0,0 +1,74 @@ +#include +#include + +#include "DockerREST.h" + +#define CIRCUS_NETWORK_NAME "CIRCUS_network" +#define UAN_SEVEN_CIU "172.21." // 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 1a0860e..9bd454e 100644 --- a/include/Container.h +++ b/include/Container.h @@ -1,12 +1,15 @@ #pragma once +#include #include #include #include -#include "curl/curl.h" +#include "DockerREST.h" 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 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(); @@ -25,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..f0b9c86 --- /dev/null +++ b/include/DockerREST.h @@ -0,0 +1,120 @@ +#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 +#define CONNECT_OK_RESPONSE 200 +#define DISCONNECT_OK_RESPONSE 200 + +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; +} + +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) { + 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; + }; + + 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; +}; + +} // namespace spqr diff --git a/include/RobotManager.h b/include/RobotManager.h index 24de599..cceace3 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -12,14 +12,11 @@ #include #include #include -#include #include #include #include -#include "Constants.h" #include "MujocoContext.h" -#include "Utils.h" #include "robots/BoosterK1.h" #include "robots/BoosterT1.h" #include "robots/Robot.h" @@ -27,7 +24,7 @@ #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { -struct Team; // Forward declaration +class Team; // Forward declaration class RobotManager { public: @@ -37,111 +34,24 @@ 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(); - } + bool areAllRobotsReady() const; + bool areAllRobotsConnected() const; - 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, std::tuple color, 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 nullptr; - } - - void 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, 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; + const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team); - serverRunning_ = false; + void startContainers(); - if (serverThread_.joinable()) - serverThread_.join(); - } - - void setAreAllRobotsReadyCallback(std::function cb) { - std::lock_guard lock(mutex_); - areAllRobotsReadyCallback_ = std::move(cb); - } + void setAreAllRobotsReadyCallback(std::function cb); + void applyCommands(); private: RobotManager() = default; @@ -150,165 +60,7 @@ 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(); - - // 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; - } + ssize_t send_all(int fd, char* buf, size_t len); std::atomic serverRunning_ = false; std::thread serverThread_; @@ -317,15 +69,14 @@ class RobotManager { 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&)>; + 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&& 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); }}}; }; diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 46d56ea..ae5d9b4 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -3,6 +3,11 @@ #include #include +#include +#include +#include + +#include "robots/Robot.h" namespace spqr { @@ -10,16 +15,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 waitRobotConnections(); + void receiveCommandMessages(); signals: void stepCompleted(); void maxSimulationTimeReached(); + void allRobotsReadySignal(); private: const mjModel* model_; @@ -27,6 +37,19 @@ 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 sendStateMessages(); }; } // namespace spqr diff --git a/include/Team.h b/include/Team.h index 6f49838..fd1b1ef 100644 --- a/include/Team.h +++ b/include/Team.h @@ -8,8 +8,13 @@ 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/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/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index daeea28..821d080 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -26,7 +26,7 @@ #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { -struct Team; // Forward declaration +class Team; // Forward declaration class BoosterK1 : public Robot { public: @@ -37,8 +37,8 @@ class BoosterK1 : public Robot { CameraDepth* depthCamera; 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"}, @@ -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 4357f26..49ac809 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -19,26 +20,29 @@ #include "robots/Robot.h" #include "sensors/CameraDepth.h" #include "sensors/CameraRGB.h" +#include "sensors/ImageSharedMemoryWriter.h" #include "sensors/Imu.h" #include "sensors/Joint.h" +#include "sensors/Oracle.h" #include "sensors/Pose.h" #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { -struct Team; // Forward declaration +class Team; // Forward declaration class BoosterT1 : public Robot { public: Pose* pose = nullptr; Imu* imu = nullptr; Joints* joints = nullptr; + Oracle* oracle = nullptr; CameraRGB* rgbCamera; CameraDepth* depthCamera; 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"}, @@ -61,7 +65,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_ = "/dev/shm/circus_ipc"; + } void bindMujoco(MujocoContext* mujCtx) override { pose = new Pose(mujCtx->model, mujCtx->data, (name + "_position").c_str(), (name + "_orientation").c_str()); @@ -93,7 +100,18 @@ 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(); + 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); } void receiveMessage(const std::map& message) override { @@ -110,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 { @@ -126,6 +144,11 @@ 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_); + + // Write in the shared file the information + rgb_writer_.write(rgbCamera->getImage()); + depth_writer_.write(depthCamera->getDepth8bit()); return msg; } @@ -140,10 +163,16 @@ 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(); joints->update(); + oracle->update(); rgbCamera->update(); depthCamera->update(); } @@ -151,7 +180,16 @@ 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::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 f31de1e..5fe0e91 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include "Container.h" @@ -20,30 +21,41 @@ #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { -struct Team; // Forward declaration +class 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; 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; Eigen::Vector3d initPosition; Eigen::Vector3d initOrientation; // Euler angles + std::string colorName; std::tuple color; std::unique_ptr container; std::shared_ptr team; msgpack::zone buffer_zone_; + mutable std::mutex mutex_; bool isConnected = false; bool isReady = false; diff --git a/include/sensors/CameraDepth.h b/include/sensors/CameraDepth.h index fe5cb2d..2f802c1 100644 --- a/include/sensors/CameraDepth.h +++ b/include/sensors/CameraDepth.h @@ -5,6 +5,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); } } @@ -117,9 +130,19 @@ class CameraDepth : public Sensor { } const std::vector& getDepth() const { + std::lock_guard lock(depthMutex_); return depth; } + std::vector getDepth8bit() const { + std::lock_guard lock(depthMutex_); + std::vector depth8(depth.size()); + for (size_t i = 0; i < depth.size(); ++i) { + depth8[i] = static_cast(depth[i] >> 8); // Convert uint16 to uint8 + } + return depth8; + } + int getWidth() const { return w; } @@ -133,6 +156,7 @@ class CameraDepth : public Sensor { } msgpack::object doSerialize(msgpack::zone& z) override { + std::lock_guard lock(depthMutex_); std::vector img_copy(depth.begin(), depth.end()); return msgpack::object(img_copy, z); } @@ -141,6 +165,7 @@ class CameraDepth : public Sensor { int w, h; double fovy_deg; MujocoContext* mujContext; + mutable std::mutex depthMutex_; std::vector depthNormalized; std::vector depth; mjvCamera cam{}; diff --git a/include/sensors/CameraRGB.h b/include/sensors/CameraRGB.h index e186659..bbe7eb0 100644 --- a/include/sensors/CameraRGB.h +++ b/include/sensors/CameraRGB.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,7 @@ class CameraRGB : 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 CameraRGB : 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 CameraRGB : 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/include/sensors/ImageSharedMemoryWriter.h b/include/sensors/ImageSharedMemoryWriter.h new file mode 100644 index 0000000..7abf3b6 --- /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/include/sensors/Oracle.h b/include/sensors/Oracle.h new file mode 100644 index 0000000..db2ba7f --- /dev/null +++ b/include/sensors/Oracle.h @@ -0,0 +1,249 @@ +#pragma once + +#include + +#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, 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]; + + ballPosAdr = mujModel->jnt_qposadr[ballAdr]; + ballVelAdr = mujModel->jnt_dofadr[ballAdr]; + + // AGGIUNGERE TUTTI I CAMPI CHE CI INTERESSANO SAPERE COME ORACLE + discoverOtherRobotSensors(); + } + + void doUpdate() override { + // Update local ball position + updateBallPosition(); + + // Update all other robots' local positions + updateTeammatesLocalPositions(); + updateOpponentsLocalPositions(); + updateGoalPostsLocalPositions(); + } + + 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 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); + } + + // 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); + } + + Eigen::Vector3d getBallPosition() const { + 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 getTeammatePosition(const std::string& robotName) const { + auto it = teammatesLocalPositions.find(robotName); + if (it != teammatesLocalPositions.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& getAllTeammatesLocalPositions() const { + return teammatesLocalPositions; + } + + const std::map& getGoalPostsLocalPositions() const { + 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) { + // 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)); + } + + 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]; + + size_t pos = extractedRobotName.find('_'); + std::string extractedRobotTeamName = extractedRobotName.substr(0, pos); + + if (extractedRobotTeamName == teamName) { + teammatesSensorAddrs[extractedRobotName] = positionAdr; + } else { + opponentsSensorAddrs[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 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& [opponentName, sensorAdr] : opponentsSensorAddrs) { + // 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()); + + opponentsLocalPositions[opponentName] = robotLocalPos; + } + } + + 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; + + mjModel* mujModel; + mjData* mujData; + std::string robotName; + std::string teamName; + 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 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 0625b16..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" @@ -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 diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 8a52b32..53a819c 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -3,4 +3,9 @@ image: spqr:booster volumes: - "/tools/booster_motion:/app/booster_motion" - - "/build:/app/bridge" + - "/.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/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 5db32d6..49eb948 100644 --- a/resources/scenes/1v1.yaml +++ b/resources/scenes/1v1.yaml @@ -1,12 +1,16 @@ simulation_config: default teams: red: - - type: Booster-T1 - number: 3 - position: [-1.0, 0.0, 0.68] - orientation: [0.0, 0.0, 0.0] + number: 8 + players: + - type: Booster-T1 + number: 1 + position: [-4.0, 0.0, 0.68] + orientation: [0.0, 0.0, 0.0] blue: - - type: Booster-T1 - number: 3 - position: [1.0, 0.0, 0.68] - orientation: [0.0, 0.0, 3.14] + number: 51 + players: + - type: Booster-T1 + number: 1 + position: [4.0, 0.0, 0.68] + orientation: [0.0, 0.0, 3.14] diff --git a/resources/scenes/5v5.yaml b/resources/scenes/5v5.yaml index 31a2ff7..6c6cc66 100644 --- a/resources/scenes/5v5.yaml +++ b/resources/scenes/5v5.yaml @@ -1,45 +1,49 @@ simulation_config: default teams: red: - - 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] + 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] blue: - - 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] + 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] diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 6657df9..86bcdb1 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -17,6 +18,7 @@ #include #endif +#include "CircusNetwork.h" #include "Constants.h" #include "GameController.h" #include "MujocoContext.h" @@ -110,7 +112,6 @@ void AppWindow::openScene() { void AppWindow::loadScene(const QString& yaml_file) { try { TeamManager::instance().clear(); - RobotManager::instance().stopCommunicationServer(); if (sim) { sim->stop(); @@ -195,23 +196,37 @@ void AppWindow::loadScene(const QString& yaml_file) { sim->setMaxSimulationTime(parser.getSceneInfo().simulationConfig.simulation.max_simulation_time); connect(sim.get(), &SimulationThread::maxSimulationTimeReached, this, &AppWindow::close); - // 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); - }); + connect( + sim.get(), &SimulationThread::allRobotsReadySignal, 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"); + if (std::filesystem::exists(shmDir)) { + 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 + sim->initializeSocket(frameworkCommunicationPort); + + std::cout << "Starting containers..." << std::endl; RobotManager::instance().startContainers(); + 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); @@ -320,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(); @@ -333,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 2156239..735ba0c 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -1,48 +1,23 @@ #include "Container.h" +#include +#include + #include +#include +#include +#include "CircusNetwork.h" #include "Constants.h" +#include "robots/Robot.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 +// for the forward declarations +#include "Team.h" +#include "robots/Robot.h" 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) { @@ -58,29 +33,67 @@ Container::~Container() { case ContainerState::NONE: break; } - - if (curl_handle) - 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::shared_ptr& robot, 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"); + + 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}}; - - payload["Env"] = {"ROBOT_NAME=" + robot_name, "SERVER_IP=172.17.0.1", "CIRCUS_PORT=" + std::to_string(frameworkCommunicationPort)}; + {"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, + {{"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", + "JOYSTICK_DEVICE=" + envOrDefault("JOYSTICK_DEVICE", "/dev/input/js0")}; + + payload["Entrypoint"] = {"/bin/bash", "-lc"}; + payload["Cmd"] = {"/app/entrypoint.sh"}; payload["Tty"] = true; 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")) @@ -95,7 +108,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; } @@ -104,7 +117,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; } @@ -113,51 +126,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 new file mode 100644 index 0000000..90fb4f0 --- /dev/null +++ b/src/RobotManager.cpp @@ -0,0 +1,114 @@ +#include "RobotManager.h" + +#include "Constants.h" +#include "Team.h" // needed for the forward declaration in the .h +#include "Utils.h" + +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_); + + 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() { + 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, image, binds); + r->container->start(); + } + std::cout << "Containers started successfully!" << std::endl; +} + +bool RobotManager::areAllRobotsConnected() const { + for (auto& r : robots_) { + if (!r->isConnected) { + return false; + } + } + return true; +} + +bool RobotManager::areAllRobotsReady() const { + for (const auto& r : robots_) + if (!r->isReady) + return false; + std::cout << "All robots are ready!" << std::endl; + return true; +} + +} // namespace spqr diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index 6fea1a1..9de6e43 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -89,13 +89,15 @@ 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; + shared_ptr teamSpec = std::make_shared(teamName, teamNumber); uint8_t typeIndex = 0; for (const YAML::Node& robotNode : robotsNode) { @@ -120,14 +122,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)); @@ -145,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 06e691c..4453aa7 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -1,5 +1,6 @@ #include "SimulationThread.h" +#include #include #include "GameController.h" @@ -9,6 +10,238 @@ 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(); + 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(), 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) { + 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(); + + { + 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 (RobotManager::instance().areAllRobotsReady()) { + emit allRobotsReadySignal(); + } + } + + r->receiveMessage(data_map); + pendingRobots.erase(messageRecipient); + ++done; + break; + } + } + } + } + } + } +} + +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 + + 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(); + entity_fd_map[robotName] = client_fd; + + // 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"); + } + } + } + if (RobotManager::instance().areAllRobotsConnected()) { + areAllConnected = true; + std::cout << "All Robots are connected!" << std::endl; + break; + } + } + } + } + } + } +} + +/* +// 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"); @@ -19,21 +252,32 @@ 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; emit maxSimulationTimeReached(); break; } + // for each robot: + // send(state) // non-blocking + // recv(torques) // bloccante o con timeout + + sendStateMessages(); + 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)); @@ -43,6 +287,32 @@ 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(); diff --git a/src/Utils.cpp b/src/Utils.cpp index 41803a5..b1a86d9 100644 --- a/src/Utils.cpp +++ b/src/Utils.cpp @@ -1,5 +1,7 @@ #include "Utils.h" +#include + namespace spqr { YAML::Node loadYamlFile(const char* path) { try { @@ -10,6 +12,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) {