From 4fcf3c385c4bcd4ad6f051c5eb60c35b4c1569e3 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Sun, 26 Jul 2026 12:35:12 +0200 Subject: [PATCH 01/25] simulation step according to framework frequency --- include/SimulationThread.h | 7 +++++ src/SimulationThread.cpp | 56 +++++++++++++++++++++++++++++++------- 2 files changed, 53 insertions(+), 10 deletions(-) diff --git a/include/SimulationThread.h b/include/SimulationThread.h index ae5d9b4..ca5b300 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -4,7 +4,9 @@ #include #include +#include #include +#include #include #include "robots/Robot.h" @@ -47,6 +49,11 @@ class SimulationThread : public QThread { std::vector fds; mutable std::mutex mutex_; + // Per-fd streaming parser state: needed because incoming command + // messages have no length-prefix framing, so a single read() can + // return multiple concatenated messages or a partial one. + std::unordered_map unpackers_; + ssize_t send_all(int fd, char* buf, size_t len); void sendStateMessages(); diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 4453aa7..8ee1b34 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "GameController.h" #include "RobotManager.h" @@ -108,17 +109,35 @@ void SimulationThread::receiveCommandMessages() { 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); + int fd = fds[i].fd; + msgpack::unpacker& unp = unpackers_[fd]; + unp.reserve_buffer(MAX_MSG_SIZE); + int n = read(fd, unp.buffer(), unp.buffer_capacity()); if (n <= 0) { - close(fds[i].fd); + close(fd); fds.erase(fds.begin() + i); --i; + unpackers_.erase(fd); continue; } + unp.buffer_consumed(static_cast(n)); + + // Drain every complete message currently buffered (a single + // read() can return several concatenated messages if we were + // slow to read), keeping only the latest one -- older ones + // queued up behind it are superseded and discarded on + // purpose, never applied. + msgpack::object_handle oh; + msgpack::object_handle latest; + bool gotOne = false; + while (unp.next(oh)) { + latest = std::move(oh); + gotOne = true; + } + if (!gotOne) + continue; // only a partial message so far; wait for more bytes - msgpack::object_handle oh = msgpack::unpack(buffer, n); - auto data_map = oh.get().as>(); + auto data_map = latest.get().as>(); auto it = data_map.find("robot_name"); if (it == data_map.end()) continue; @@ -248,6 +267,16 @@ void SimulationThread::run() { double sim_dt = model_->opt.timestep; + // Physics integrates at the model's native sim_dt (kept small for + // numerical accuracy), but robot control frameworks only produce new + // commands at their own, much lower, rate (~50Hz). Only exchange + // state/commands every kControlDecimation physics steps instead of + // every single one; in between, RobotManager::applyCommands() keeps + // re-applying the last received torque (zero-order hold) every step, + // unchanged. + constexpr int kControlDecimation = 1; + int stepsSinceLastControl = 0; + using clock = std::chrono::steady_clock; auto next_step_time = clock::now(); while (running_) { @@ -273,11 +302,18 @@ void SimulationThread::run() { sendStateMessages(); receiveCommandMessages(); - next_step_time += std::chrono::duration_cast(std::chrono::duration(sim_dt)); - // std::this_thread::sleep_until(next_step_time); - - // if (clock::now() > next_step_time) - // next_step_time = clock::now(); + // TEST HOOK: pad each iteration (integration + comms) to ~20ms + // total real time instead of pacing to sim_dt, to test whether + // feeding the policy's gait-phase/velocity-ramp accumulators a + // matching per-tick dt (see Locomotion.h POLICY_DT, also set to + // sim_dt for this test) keeps the simulation physically correct + // while running ~10x slower than real time. + constexpr double kTestControlPeriod = 0.02; + next_step_time += std::chrono::duration_cast(std::chrono::duration(kTestControlPeriod)); + std::this_thread::sleep_until(next_step_time); + + if (clock::now() >= next_step_time) + next_step_time = clock::now(); } else { // When paused, sleep briefly to avoid busy-waiting std::this_thread::sleep_for(std::chrono::milliseconds(10)); From bb67ba90cee7a4def431725a054c6d8a28a867f4 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Mon, 27 Jul 2026 01:07:53 +0200 Subject: [PATCH 02/25] simulation loop rewrite I cambiamenti del funzionamento sono scritti nel commento lasciato nel file --- src/SimulationThread.cpp | 106 +++++++++++++++++++++++++++------------ 1 file changed, 75 insertions(+), 31 deletions(-) diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 8ee1b34..806658d 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -265,51 +265,95 @@ void SimulationThread::run() { if (!model_) throw std::runtime_error("Cannot start simulation without mujoco model"); + // Qui viene spiegato il funzionamento del ciclo di simulazione, poiché si deve interfacciare + // con il framework e con la sua frequenza di update (del nodo Brain) + // Parametri: + // kControlDecimation = numero di step da eseguire per ogni comando desiderato che arriva dal framework (posizione dei giunti desiderata) + // kTimestepPolicy = tempo (in secondi) corrispondente alla frequenza del nodo Brain del framework + // (al momento 0.2 perché ho settato la frequenza di update del nodo Brain a 5Hz, quindi 1/5 = 0.2) + // sim_dt = dt di quanto viene incrementata la simulazione ogni volta che viene chiamato mj_step (viene preso da SceneParser) + // + // Parametri del framework: + // POLICY_DT = quanto viene aumentata la fase del gait ogni volta che viene pubblicato un comando (vale per le policy custom, + // e sarebbe buona pratica essere uguale "kTimestepPolicy", ma possono anche differire. La consequenza sarebbe una + // simulazione più lenta della realtà). Si trova al momento in Locomotion.h + // + // L'idea del funzionamento è la seguente: + // per ogni comando desiderato (che rappresenta la posizione desiderata dei giunti), l'idea è che quel comando verrà applicato più di una volta. + // In particolare, lo applicheremo "kControlDecimation" volte, e deve valere la relazione per cui: + // POLICY_DT = kControlDecimation * sim_dt + // In questo modo, ogni volta che applichiamo un comando ad alto livello, teniamo conto che l'avanzamento totale della simulazione sarà di "POLICY_DT". + // + // Invece, ogni iterazione del ciclo interno "kControlDecimation" trasforma i comandi ad alto livello in torques a basso livello da dover + // applicare alla simulazione per farla avanzare di un solo "sim_dt". Quando questi comandi sono stati applicati e la simulazione è avanzata, + // il nuovo stato della simulazione deve essere inviato per poter calcolare i nuovi comandi a basso livello (i.e. "sendStateMessages"), e questi + // devono poter essere ricevuti (i.e. "receiveCommandMessages"). + // In pratica, il loop "for" funziona come se ci fosse un PID ad alta frequenza che calcola le torques a partire dalle joints desiderate e dai + // joint states attuali. + // + // Una volta terminati tutti i "kControlDecimation" steps, addormenta il thread finché il nodo di Brain non è pronto a mandare un + // nuovo comando desiderato. + // + // N.B.1: in questa pipeline, è molto importante che il tempo di computazione dei "kControlDecimation" steps sia inferiore a "kTimestepPolicy", + // perché altrimenti il nodo di Brain manda un nuovo comando desiderato mentre si sta ancora aggiornando la simulazione con il comando + // desiderato precedente. + // + // N.B.2: POLICY_DT e kTimestepPolicy possono potenzialmente differire, il che significa che ogni volta che il nodo di Brain invia un comando + // desiderato (cioè ogni "kTimestepPolicy" secondi), il comando è quello giusto per far avanzare la simulazione di "POLICY_DT" secondi + // + // N.B.3: il bottleneck al momento sembra essere sendStateMessages+receiveCommandMessages; aumentando il numero di kControlDecimation steps, + // ovviamente il sistema rallenta. Quello che non deve succedere è quello spiegato in N.B.1 + // + // TODO: il problema da risolvere è trovare la combinazione giusta di parametri. Probabilmente una configurazione che vada sempre bene non c'è, + // anche perché dipendente dal numero di robot simulati e dal pc utilizzato. + // Idee per risolvere questo problema: + // - abbassare la frequenza del nodo di Brain (se la policy non dà problemi), + // - alzare il sim_dt (fino a un certo punto, altrimenti lo step di integrazione diventa una cattiva approssimazione) + // per abbassare kControlDecimation + // - entrambe, cioè trovare una formulazione adattiva che possa conciliare tutti questi aspetti + // double sim_dt = model_->opt.timestep; - // Physics integrates at the model's native sim_dt (kept small for - // numerical accuracy), but robot control frameworks only produce new - // commands at their own, much lower, rate (~50Hz). Only exchange - // state/commands every kControlDecimation physics steps instead of - // every single one; in between, RobotManager::applyCommands() keeps - // re-applying the last received torque (zero-order hold) every step, - // unchanged. - constexpr int kControlDecimation = 1; + constexpr int kControlDecimation = 10; + constexpr double kTimestepPolicy = 0.2; int stepsSinceLastControl = 0; using clock = std::chrono::steady_clock; auto next_step_time = clock::now(); while (running_) { if (!paused_) { - 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; + auto stepsStart = clock::now(); + for(int i=0; ixfrc_applied, 0, model_->nbody * 6 * sizeof(mjtNum)); + + if (maxSimulationTime_ > 0 && data_->time >= maxSimulationTime_) { + running_ = false; + emit maxSimulationTimeReached(); + break; + } + + sendStateMessages(); + receiveCommandMessages(); } + // DEBUG: real time spent computing kControlDecimation physics + // steps, vs. the sim-time budget they represent. + double stepsElapsedMs = std::chrono::duration(clock::now() - stepsStart).count(); + double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; + std::cout << "[SimulationThread] " << kControlDecimation << " physics steps took " + << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; // for each robot: // send(state) // non-blocking // recv(torques) // bloccante o con timeout - sendStateMessages(); - receiveCommandMessages(); - - // TEST HOOK: pad each iteration (integration + comms) to ~20ms - // total real time instead of pacing to sim_dt, to test whether - // feeding the policy's gait-phase/velocity-ramp accumulators a - // matching per-tick dt (see Locomotion.h POLICY_DT, also set to - // sim_dt for this test) keeps the simulation physically correct - // while running ~10x slower than real time. - constexpr double kTestControlPeriod = 0.02; - next_step_time += std::chrono::duration_cast(std::chrono::duration(kTestControlPeriod)); + + next_step_time += std::chrono::duration_cast(std::chrono::duration(kTimestepPolicy)); std::this_thread::sleep_until(next_step_time); if (clock::now() >= next_step_time) From c02d5f7465f5103574f51ff6e24f50ca67f8c4fa Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Sat, 1 Aug 2026 19:42:08 +0200 Subject: [PATCH 03/25] shared memory initial implementation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A questo punto dell'implementazione c'è una bozza di struttura per la shm in direzione simulatore->simbridge. Le struttture di astrazione forse vanno aggiustate un po'. Per ora pusho per non perdere traccia --- include/ipc/SharedMemoryWriter.h | 152 ++++++++++++++++++++++ include/robots/BoosterK1.h | 4 + include/robots/BoosterT1.h | 21 +++ include/robots/Robot.h | 5 + include/sensors/ImageSharedMemoryWriter.h | 121 +++-------------- include/sensors/Imu.h | 16 +++ include/sensors/Joint.h | 24 ++++ include/sensors/Oracle.h | 60 +++++++++ include/sensors/Pose.h | 34 +++++ src/SimulationThread.cpp | 7 + 10 files changed, 338 insertions(+), 106 deletions(-) create mode 100644 include/ipc/SharedMemoryWriter.h diff --git a/include/ipc/SharedMemoryWriter.h b/include/ipc/SharedMemoryWriter.h new file mode 100644 index 0000000..bd3d643 --- /dev/null +++ b/include/ipc/SharedMemoryWriter.h @@ -0,0 +1,152 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace spqr { + +// No metadata (default): use when the reader already knows how to interpret +// the payload (e.g. a fixed compile-time state struct written with element_count == 1). +struct NoSharedMemoryMeta {}; + +// Lock-free single-writer/multi-reader shared-memory ring buffer for any +// trivially-copyable T, with an optional trivially-copyable Meta blob written +// once at configure() time (e.g. image width/height/channels, needed by a +// reader that doesn't otherwise know the payload's shape). +// +// Shared memory is just shared pages, not a wire protocol: a trivially-copyable +// type's bytes are already valid in any process built from the same struct +// definition, so plain memcpy is sufficient (and fastest) -- no serialization +// step is needed or performed. +template +class SharedMemoryWriter { + public: + static_assert(std::is_trivially_copyable_v, "SharedMemoryWriter requires a trivially-copyable T"); + static_assert(std::is_trivially_copyable_v, "SharedMemoryWriter requires a trivially-copyable Meta"); + + SharedMemoryWriter() = default; + + ~SharedMemoryWriter() { + close_(); + } + + // element_count: how many T's fit in a single slot (e.g. width*height*channels + // for a raw uint8_t image; 1 for a single POD message struct). + void configure(const std::string& path, size_t element_count, Meta meta = {}, int ring_slots = 3) { + if (element_count == 0 || ring_slots <= 0) { + throw std::invalid_argument("Invalid shared-memory configuration"); + } + const size_t slot_bytes = element_count * sizeof(T); + 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 memory file: " + path); + } + + if (ftruncate(fd_, static_cast(total_bytes)) != 0) { + close_(); + throw std::runtime_error("Failed to resize shared memory file: " + path); + } + + map_bytes_ = total_bytes; + 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 memory 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_->meta = meta; + header_->slot_count = static_cast(ring_slots); + header_->slot_bytes = slot_bytes; + header_->seq = 0; + } else { + // Segment already initialized with a compatible layout; keep it current. + header_->meta = meta; + } + + path_ = path; + } + + bool isReady() const { + return header_ != nullptr; + } + + void write(const T* data, size_t count) { + if (!header_) { + return; + } + if (count * sizeof(T) != 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, data, header_->slot_bytes); + __atomic_store_n(&header_->seq, next_seq, __ATOMIC_RELEASE); + } + + void write(const T& value) { + write(&value, 1); + } + + private: + struct Header { + uint32_t magic = 0; + uint32_t version = 0; + Meta meta{}; + 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/robots/BoosterK1.h b/include/robots/BoosterK1.h index 821d080..aff7663 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -93,6 +93,10 @@ class BoosterK1 : public Robot { return msg; } + void publishSharedState() override { + // Socket-only for now: BoosterK1 doesn't yet have a shared-memory state channel. + } + std::map getSensors() override { std::map sensors; sensors["pose"] = pose; diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 49ac809..b571ec2 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -17,6 +17,7 @@ #include #include "MujocoContext.h" +#include "ipc/SharedMemoryWriter.h" #include "robots/Robot.h" #include "sensors/CameraDepth.h" #include "sensors/CameraRGB.h" @@ -31,6 +32,19 @@ namespace spqr { class Team; // Forward declaration +constexpr size_t kBoosterT1JointCount = 23; + +// Per-tick shared-memory payload: everything BoosterT1's sendMessage() sends +// over the socket (minus robot_name, which the shared-memory file path already +// encodes), bundled into one trivially-copyable struct so a reader sees all +// fields from the same tick atomically (one SharedMemoryWriter -> one seq). +struct BoosterT1SharedState { + PoseData pose; + ImuData imu; + JointState joints; + OracleData oracle; +}; + class BoosterT1 : public Robot { public: Pose* pose = nullptr; @@ -109,6 +123,7 @@ class BoosterT1 : public Robot { const int height = rgbCamera->getHeight(); rgb_writer_.configure(shmFilePath_("rgb"), width, height, 3); depth_writer_.configure(shmFilePath_("depth"), width, height, 1); + state_writer_.configure(shmFilePath_("state"), /*element_count=*/1); // Create Oracle with the pose and all robots oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); @@ -153,6 +168,11 @@ class BoosterT1 : public Robot { return msg; } + void publishSharedState() override { + state_writer_.write(BoosterT1SharedState{pose->toSharedState(), imu->toSharedState(), + joints->toSharedState(), oracle->toSharedState()}); + } + std::map getSensors() override { std::map sensors; sensors["pose"] = pose; @@ -190,6 +210,7 @@ class BoosterT1 : public Robot { std::string shm_dir_; ImageSharedMemoryWriter rgb_writer_; ImageSharedMemoryWriter depth_writer_; + SharedMemoryWriter state_writer_; }; } // namespace spqr diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 6451dce..65690a2 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -42,6 +42,11 @@ class Robot { virtual void update() = 0; virtual void receiveMessage(const std::map& message) = 0; virtual std::map sendMessage() = 0; + // Publishes this robot's per-tick state to shared memory, for the case where + // circus and simbridge run on the same machine. Mirrors sendMessage()'s role + // for the socket path; a robot that only needs the socket path implements + // this with an empty body. + virtual void publishSharedState() = 0; virtual std::map getSensors() = 0; virtual void applyCommands() = 0; diff --git a/include/sensors/ImageSharedMemoryWriter.h b/include/sensors/ImageSharedMemoryWriter.h index 7abf3b6..2e549ee 100644 --- a/include/sensors/ImageSharedMemoryWriter.h +++ b/include/sensors/ImageSharedMemoryWriter.h @@ -1,132 +1,41 @@ #pragma once -#include -#include -#include -#include - #include -#include -#include #include #include #include +#include "ipc/SharedMemoryWriter.h" + namespace spqr { +struct ImageMeta { + uint32_t width = 0; + uint32_t height = 0; + uint32_t channels = 0; +}; + 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) { + if (width <= 0 || height <= 0 || channels <= 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; + const size_t element_count = static_cast(width) * static_cast(height) * static_cast(channels); + ImageMeta meta{static_cast(width), static_cast(height), static_cast(channels)}; + writer_.configure(path, element_count, meta, ring_slots); } bool isReady() const { - return header_ != nullptr; + return writer_.isReady(); } 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); + writer_.write(frame.data(), frame.size()); } 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_; + SharedMemoryWriter writer_; }; } // namespace spqr diff --git a/include/sensors/Imu.h b/include/sensors/Imu.h index 695ac6c..f504004 100644 --- a/include/sensors/Imu.h +++ b/include/sensors/Imu.h @@ -8,6 +8,13 @@ namespace spqr { +// Trivially-copyable snapshot of Imu's values, for publishing over shared memory +// (same numbers doSerialize() sends over the socket, without msgpack/std::vector). +struct ImuData { + double linearAcceleration[3] = {}; + double angularVelocity[3] = {}; +}; + class Imu : public Sensor { public: Imu(mjModel* mujModel, mjData* mujData, const char* linearAccelerationName, const char* angularVelocityName) @@ -36,6 +43,15 @@ class Imu : public Sensor { return msgpack::object(imu_data, z); } + ImuData toSharedState() const { + ImuData data; + for (int i = 0; i < 3; ++i) { + data.linearAcceleration[i] = linearAcceleration(i); + data.angularVelocity[i] = angularVelocity(i); + } + return data; + } + Eigen::Vector3d getLinearAcceleration() const { return linearAcceleration; } diff --git a/include/sensors/Joint.h b/include/sensors/Joint.h index 1e1620f..fe143f3 100644 --- a/include/sensors/Joint.h +++ b/include/sensors/Joint.h @@ -39,6 +39,17 @@ enum class JointValue { ANKLE_RIGHT_ROLL, }; +// Trivially-copyable snapshot of Joints' values, for publishing over shared memory +// (same numbers doSerialize() sends over the socket, without msgpack/std::vector). +// N must match the joint count of the robot type using it (i.e. its joint_map size). +template +struct JointState { + double position[N] = {}; + double velocity[N] = {}; + double acceleration[N] = {}; + double torque[N] = {}; +}; + class Joints : public Sensor { public: Joints(mjModel* mujModel, mjData* mujData, std::map map) : mujModel(mujModel), mujData(mujData) { @@ -123,6 +134,19 @@ class Joints : public Sensor { return msgpack::object(data, z); } + template + JointState toSharedState() const { + JointState data; + const size_t n = std::min(N, size); + for (size_t i = 0; i < n; ++i) { + data.position[i] = position[i]; + data.velocity[i] = velocity[i]; + data.acceleration[i] = acceleration[i]; + data.torque[i] = torque[i]; + } + return data; + } + Eigen::VectorXd getPosition() const { return Eigen::Map(position.data(), size); } diff --git a/include/sensors/Oracle.h b/include/sensors/Oracle.h index db2ba7f..8cae963 100644 --- a/include/sensors/Oracle.h +++ b/include/sensors/Oracle.h @@ -3,6 +3,8 @@ #include #include +#include +#include #include #include #include @@ -14,6 +16,30 @@ namespace spqr { // Forward declarations class Pose; +// Trivially-copyable snapshot of Oracle's values, for publishing over shared +// memory. teammatesLocalPositions/opponentsLocalPositions are runtime-varying, +// string-keyed maps (however many robots are currently on the field) -- not +// trivially copyable as-is, so this bounds them to a fixed max squad size +// instead of serializing: still plain memcpy, at the cost of a fixed capacity. +struct OracleData { + static constexpr size_t kMaxTeammates = 6; + static constexpr size_t kMaxOpponents = 6; + static constexpr size_t kNumGoalPosts = 4; + + struct Entry { + char name[32] = {}; + double position[3] = {}; + }; + + double ballPosition[3] = {}; + Entry teammates[kMaxTeammates] = {}; + uint32_t teammateCount = 0; + Entry opponents[kMaxOpponents] = {}; + uint32_t opponentCount = 0; + Entry goalPosts[kNumGoalPosts] = {}; + uint32_t goalPostCount = 0; +}; + // 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, ... @@ -78,6 +104,40 @@ class Oracle : public Sensor { return msgpack::object(oracle_data, z); } + static void fillEntry_(OracleData::Entry& entry, const std::string& name, const Eigen::Vector3d& pos) { + std::strncpy(entry.name, name.c_str(), sizeof(entry.name) - 1); + for (int i = 0; i < 3; ++i) { + entry.position[i] = pos(i); + } + } + + OracleData toSharedState() const { + OracleData data; + for (int i = 0; i < 3; ++i) { + data.ballPosition[i] = ballPosition(i); + } + + for (const auto& [teammateName, localPos] : teammatesLocalPositions) { + if (data.teammateCount >= OracleData::kMaxTeammates) + break; + fillEntry_(data.teammates[data.teammateCount++], teammateName, localPos); + } + + for (const auto& [opponentName, localPos] : opponentsLocalPositions) { + if (data.opponentCount >= OracleData::kMaxOpponents) + break; + fillEntry_(data.opponents[data.opponentCount++], opponentName, localPos); + } + + for (const auto& [postName, localPos] : goalPostsLocalPositions) { + if (data.goalPostCount >= OracleData::kNumGoalPosts) + break; + fillEntry_(data.goalPosts[data.goalPostCount++], postName, localPos); + } + + return data; + } + Eigen::Vector3d getBallPosition() const { return ballPosition; } diff --git a/include/sensors/Pose.h b/include/sensors/Pose.h index 57ef301..666d3b7 100644 --- a/include/sensors/Pose.h +++ b/include/sensors/Pose.h @@ -8,6 +8,16 @@ namespace spqr { +// Trivially-copyable snapshot of Pose's values, for publishing over shared memory +// (same numbers doSerialize() sends over the socket, without msgpack/std::vector). +struct PoseData { + double position[3] = {}; + double quatOrientation[4] = {}; + double eulerOrientation[3] = {}; + double rotationMatrix[9] = {}; + double transformationMatrix[16] = {}; +}; + class Pose : public Sensor { public: Pose(mjModel* mujModel, mjData* mujData, const char* positionName, const char* orientationName) : mujModel(mujModel), mujData(mujData) { @@ -55,6 +65,30 @@ class Pose : public Sensor { return msgpack::object(pose_data, z); } + // Row-major matrix flattening, matching doSerialize()'s element order + // (Eigen's own Map/storage is column-major, so this is spelled out explicitly). + PoseData toSharedState() const { + PoseData data; + for (int i = 0; i < 3; ++i) { + data.position[i] = position(i); + data.eulerOrientation[i] = eulerOrientation(i); + } + for (int i = 0; i < 4; ++i) { + data.quatOrientation[i] = quatOrientation(i); + } + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + data.rotationMatrix[r * 3 + c] = rotationMatrix(r, c); + } + } + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + data.transformationMatrix[r * 4 + c] = transformationMatrix(r, c); + } + } + return data; + } + Eigen::Vector3d getPosition() const { return position; } diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 806658d..2bb51cd 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -92,6 +92,7 @@ void SimulationThread::receiveCommandMessages() { std::cout << "[receiveCommandMessages] Timeout, resending state to: " << r->name << std::endl; msgpack::sbuffer sbuf; auto answ = r->sendMessage(); + r->publishSharedState(); msgpack::pack(sbuf, answ); if (sbuf.size() > 0) { int fd = entity_fd_map[r->name]; @@ -221,6 +222,7 @@ void SimulationThread::waitRobotConnections() { if (r->name == robotName) { r->isConnected = true; answ = r->sendMessage(); + r->publishSharedState(); answOk = true; break; } @@ -339,7 +341,11 @@ void SimulationThread::run() { } sendStateMessages(); + + auto receiveStart = clock::now(); receiveCommandMessages(); + double receiveElapsedMs = std::chrono::duration(clock::now() - receiveStart).count(); + std::cout << "[SimulationThread] receiveCommandMessages took " << receiveElapsedMs << " ms" << std::endl; } // DEBUG: real time spent computing kControlDecimation physics // steps, vs. the sim-time budget they represent. @@ -375,6 +381,7 @@ void SimulationThread::sendStateMessages() { { std::unique_lock lock(mutex_); answ = r->sendMessage(); + r->publishSharedState(); answOk = true; } From b02cb53f1fa4f6622cd46db2c24ad397580031ed Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Tue, 4 Aug 2026 01:33:13 +0200 Subject: [PATCH 04/25] shm correctly working MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implementazione di base completata in entrambe direzioni circus <-> simbridge per lo scambio di messaggi frequenti. Al momento la parte di connessione dei robot al simulatore avviene ancora tramite socket, è da smantellare TODO 1: togliere connessione iniziale dei robot tramite socket TODO 2A: modificare writer/reader delle immagini per usare la classe template TODO 2B: integrare le immagini direttamente dentro lo stato del robot TODO 3: refactoring (rivedi anche astrazioni delle classi) TODO 4: mettere doppia modalità (shm/socket) tramite file di config, cosi da usare quella che si vuole (default: shm) --- include/SimulationThread.h | 1 + include/ipc/SharedMemoryReader.h | 148 +++++++++++++++++++++++++++++++ include/robots/BoosterK1.h | 5 ++ include/robots/BoosterT1.h | 17 ++++ include/robots/Robot.h | 4 + include/sensors/Joint.h | 9 ++ src/AppWindow.cpp | 2 +- src/SimulationThread.cpp | 106 +++++++++++++--------- 8 files changed, 247 insertions(+), 45 deletions(-) create mode 100644 include/ipc/SharedMemoryReader.h diff --git a/include/SimulationThread.h b/include/SimulationThread.h index ca5b300..52a984b 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -27,6 +27,7 @@ class SimulationThread : public QThread { void initializeSocket(int port); void waitRobotConnections(); void receiveCommandMessages(); + void receiveCommandMessagesSHM(); signals: void stepCompleted(); diff --git a/include/ipc/SharedMemoryReader.h b/include/ipc/SharedMemoryReader.h new file mode 100644 index 0000000..944ac55 --- /dev/null +++ b/include/ipc/SharedMemoryReader.h @@ -0,0 +1,148 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "ipc/SharedMemoryWriter.h" // for NoSharedMemoryMeta + +namespace spqr { + +// Reader counterpart to SharedMemoryWriter (external/circus/include/ipc/SharedMemoryWriter.h). +// Used when circus is the *reader* (e.g. commands written by simbridge). Must +// stay in sync with whatever writes the segment: same Header layout, same +// magic/version, same ring-buffer/atomic-seq protocol. Mirrors +// external/simbridge/src/SharedMemoryReader.hpp -- kept as a separate copy on +// purpose, same duplication discipline used for the image Header struct. +template +class SharedMemoryReader { + public: + static_assert(std::is_trivially_copyable_v, "SharedMemoryReader requires a trivially-copyable T"); + static_assert(std::is_trivially_copyable_v, "SharedMemoryReader requires a trivially-copyable Meta"); + + SharedMemoryReader() = default; + + ~SharedMemoryReader() { + close_(); + } + + void configure(const std::string& path) { + path_ = path; + } + + // Reads the Meta blob written at configure() time on the writer side. + // Safe to call before any frame has been written (meta is set independently of seq). + bool getMeta(Meta& out) { + if (!ensureMapped_() || !header_ || header_->magic != kMagic || header_->version != kVersion) { + return false; + } + out = header_->meta; + return true; + } + + // out must point to at least `count` T's (count == element_count the writer was configured with). + bool readLatest(T* out, size_t count) { + if (!ensureMapped_()) { + return false; + } + if (!header_) { + return false; + } + if (header_->magic != kMagic || header_->version != kVersion || header_->slot_count == 0 || header_->slot_bytes == 0) { + return false; + } + if (count * sizeof(T) != header_->slot_bytes) { + return false; + } + + const uint64_t seq = __atomic_load_n(&header_->seq, __ATOMIC_ACQUIRE); + if (seq == 0 || seq == last_seq_) { + return false; + } + + const uint32_t slot_idx = static_cast(seq % header_->slot_count); + const uint8_t* slots_base = static_cast(map_ptr_) + sizeof(Header); + const uint8_t* src = slots_base + static_cast(slot_idx) * header_->slot_bytes; + + std::memcpy(out, src, header_->slot_bytes); + + last_seq_ = seq; + return true; + } + + // Convenience for element_count == 1 (single POD struct per slot). + bool readLatest(T& out) { + return readLatest(&out, 1); + } + + private: + struct Header { + uint32_t magic = 0; + uint32_t version = 0; + Meta meta{}; + 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; + + bool ensureMapped_() { + if (map_ptr_) { + return true; + } + if (path_.empty()) { + return false; + } + + fd_ = open(path_.c_str(), O_RDONLY); + if (fd_ < 0) { + return false; + } + + struct stat st {}; + if (fstat(fd_, &st) != 0 || st.st_size < static_cast(sizeof(Header))) { + close_(); + return false; + } + + map_bytes_ = static_cast(st.st_size); + map_ptr_ = mmap(nullptr, map_bytes_, PROT_READ, MAP_SHARED, fd_, 0); + if (map_ptr_ == MAP_FAILED) { + map_ptr_ = nullptr; + close_(); + return false; + } + header_ = static_cast(map_ptr_); + return true; + } + + 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; + } + + std::string path_; + int fd_ = -1; + void* map_ptr_ = nullptr; + size_t map_bytes_ = 0; + const Header* header_ = nullptr; + uint64_t last_seq_ = 0; +}; + +} // namespace spqr diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index aff7663..f902009 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -97,6 +97,11 @@ class BoosterK1 : public Robot { // Socket-only for now: BoosterK1 doesn't yet have a shared-memory state channel. } + bool receiveSharedCommand() override { + // Socket-only for now: BoosterK1 doesn't yet have a shared-memory command channel. + return false; + } + std::map getSensors() override { std::map sensors; sensors["pose"] = pose; diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index b571ec2..277f938 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -17,6 +17,7 @@ #include #include "MujocoContext.h" +#include "ipc/SharedMemoryReader.h" #include "ipc/SharedMemoryWriter.h" #include "robots/Robot.h" #include "sensors/CameraDepth.h" @@ -124,6 +125,7 @@ class BoosterT1 : public Robot { rgb_writer_.configure(shmFilePath_("rgb"), width, height, 3); depth_writer_.configure(shmFilePath_("depth"), width, height, 1); state_writer_.configure(shmFilePath_("state"), /*element_count=*/1); + command_reader_.configure(shmFilePath_("commands")); // Create Oracle with the pose and all robots oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); @@ -152,6 +154,20 @@ class BoosterT1 : public Robot { } } + bool receiveSharedCommand() override { + JointTorques torques; + if (!command_reader_.readLatest(torques)) { + return false; + } + + std::lock_guard lock(mutex_); + size_t i = 0; + for (const auto& [joint_value, joint_name] : joint_map) { + latestTorques[joint_value] = torques.torque[i++]; + } + return true; + } + std::map sendMessage() override { buffer_zone_.clear(); std::map msg; @@ -211,6 +227,7 @@ class BoosterT1 : public Robot { ImageSharedMemoryWriter rgb_writer_; ImageSharedMemoryWriter depth_writer_; SharedMemoryWriter state_writer_; + SharedMemoryReader> command_reader_; }; } // namespace spqr diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 65690a2..d8dfadf 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -47,6 +47,10 @@ class Robot { // for the socket path; a robot that only needs the socket path implements // this with an empty body. virtual void publishSharedState() = 0; + // Reads and applies the latest command from shared memory, if a new one has + // arrived since the last call. Returns true iff a new command was applied + // this call. Mirrors receiveMessage()'s role for the socket path. + virtual bool receiveSharedCommand() = 0; virtual std::map getSensors() = 0; virtual void applyCommands() = 0; diff --git a/include/sensors/Joint.h b/include/sensors/Joint.h index fe143f3..fd26d6a 100644 --- a/include/sensors/Joint.h +++ b/include/sensors/Joint.h @@ -50,6 +50,15 @@ struct JointState { double torque[N] = {}; }; +// Trivially-copyable joint torque command, for receiving over shared memory +// (same numbers previously carried as "joint_torques" in the msgpack command +// message). Order matches joint_map's ascending JointValue iteration, i.e. the +// same order used by BoosterT1::receiveMessage() today. +template +struct JointTorques { + double torque[N] = {}; +}; + class Joints : public Sensor { public: Joints(mjModel* mujModel, mjData* mujData, std::map map) : mujModel(mujModel), mujData(mujData) { diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 846c254..201d5dd 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -231,7 +231,7 @@ void AppWindow::loadScene(const QString& yaml_file) { sim->waitRobotConnections(); std::cout << "Waiting Robots are Ready..." << std::endl; - sim->receiveCommandMessages(); + sim->receiveCommandMessagesSHM(); // Set initial simulation state (playing when scene is loaded) toolsPanel->setSimulationPlaying(true); diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 2bb51cd..e11fdde 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -169,6 +169,58 @@ void SimulationThread::receiveCommandMessages() { } } +// Shared-memory counterpart to receiveCommandMessages(): same purpose (block +// until every robot has a fresh command applied, retrying with a periodic +// notice if one is stalled) but polling r->receiveSharedCommand() per robot +// instead of poll()/read()/msgpack-unpack on the socket. +void SimulationThread::receiveCommandMessagesSHM() { + int robot_size = robots_.size(); + int done = 0; + std::set pendingRobots; + for (auto& r : robots_) + pendingRobots.insert(r->name); + + auto windowStart = std::chrono::steady_clock::now(); + while (done < robot_size) { + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (!pendingRobots.count(r->name)) + continue; + if (r->receiveSharedCommand()) { + if (!r->isReady) { + r->isReady = true; + std::cout << "Robot ready: " << r->name << std::endl; + if (RobotManager::instance().areAllRobotsReady()) { + emit allRobotsReadySignal(); + } + } + pendingRobots.erase(r->name); + ++done; + } + } + } + if (done < robot_size) { + if (std::chrono::steady_clock::now() - windowStart > std::chrono::milliseconds(2500)) { + // Re-send state to robots that haven't replied yet. Load-bearing for + // bootstrap: waitRobotConnections() sends state only once and run() + // (with its recurring sendStateMessages()) hasn't started yet, so + // without this retry a framework that subscribes late never receives + // state, never publishes a command, and startup deadlocks. + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (!pendingRobots.count(r->name)) + continue; + std::cout << "[receiveCommandMessagesSHM] Timeout, resending state to: " << r->name << std::endl; + r->publishSharedState(); + } + windowStart = std::chrono::steady_clock::now(); + } + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } + } +} + void SimulationThread::waitRobotConnections() { bool areAllConnected = false; while (!areAllConnected) { @@ -211,34 +263,20 @@ void SimulationThread::waitRobotConnections() { 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 + // Answer the handshake with the initial state over shared memory. + // The socket is only used to receive the robot name above. { std::lock_guard lock(mutex_); for (auto& r : robots_) { if (r->name == robotName) { r->isConnected = true; - answ = r->sendMessage(); r->publishSharedState(); - answOk = true; + std::cout << "Connected Robot: " << robotName << "\n"; + std::cout << "Published initial state to " << robotName << " via shared memory" << std::endl; 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; @@ -272,7 +310,7 @@ void SimulationThread::run() { // Parametri: // kControlDecimation = numero di step da eseguire per ogni comando desiderato che arriva dal framework (posizione dei giunti desiderata) // kTimestepPolicy = tempo (in secondi) corrispondente alla frequenza del nodo Brain del framework - // (al momento 0.2 perché ho settato la frequenza di update del nodo Brain a 5Hz, quindi 1/5 = 0.2) + // (al momento 0.02 perché ho settato la frequenza di update del nodo Brain a 50Hz, quindi 1/50 = 0.02) // sim_dt = dt di quanto viene incrementata la simulazione ogni volta che viene chiamato mj_step (viene preso da SceneParser) // // Parametri del framework: @@ -317,7 +355,7 @@ void SimulationThread::run() { double sim_dt = model_->opt.timestep; constexpr int kControlDecimation = 10; - constexpr double kTimestepPolicy = 0.2; + constexpr double kTimestepPolicy = 0.02; int stepsSinceLastControl = 0; using clock = std::chrono::steady_clock; @@ -343,9 +381,9 @@ void SimulationThread::run() { sendStateMessages(); auto receiveStart = clock::now(); - receiveCommandMessages(); + receiveCommandMessagesSHM(); double receiveElapsedMs = std::chrono::duration(clock::now() - receiveStart).count(); - std::cout << "[SimulationThread] receiveCommandMessages took " << receiveElapsedMs << " ms" << std::endl; + std::cout << "[SimulationThread] receiveCommandMessagesSHM took " << receiveElapsedMs << " ms" << std::endl; } // DEBUG: real time spent computing kControlDecimation physics // steps, vs. the sim-time budget they represent. @@ -375,28 +413,8 @@ 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(); - r->publishSharedState(); - 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"); - } - } - } + std::unique_lock lock(mutex_); + r->publishSharedState(); } } From 8457d6f107674ef93a51d7429c43d36b1c993605 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Tue, 11 Aug 2026 01:04:15 +0200 Subject: [PATCH 05/25] unique shared memory writer/reader Unificato classe ImageSharedWriter/Reader nella classe SharedMemoryWriter/Reader. Sembra funzionare (da refactorare) --- include/ipc/SharedMemoryReader.h | 61 +++++++++++++---- include/ipc/SharedMemoryWriter.h | 81 ++++++++++++++++++++++- include/robots/BoosterT1.h | 54 ++++++++++----- include/sensors/ImageSharedMemoryWriter.h | 41 ------------ 4 files changed, 166 insertions(+), 71 deletions(-) delete mode 100644 include/sensors/ImageSharedMemoryWriter.h diff --git a/include/ipc/SharedMemoryReader.h b/include/ipc/SharedMemoryReader.h index 944ac55..60675cd 100644 --- a/include/ipc/SharedMemoryReader.h +++ b/include/ipc/SharedMemoryReader.h @@ -48,6 +48,25 @@ class SharedMemoryReader { // out must point to at least `count` T's (count == element_count the writer was configured with). bool readLatest(T* out, size_t count) { + return readSlot_(out, count * sizeof(T), nullptr, 0, /*exact_fit=*/true); + } + + // Convenience for element_count == 1 (single POD struct per slot). + bool readLatest(T& out) { + return readLatest(&out, 1); + } + + // Composite counterpart to SharedMemoryWriter::write(head, trailing): copies + // the typed head and the runtime-sized trailing region out of the same slot, + // so both come from the same tick. The head is copied into a properly typed + // object rather than mapped in place, which keeps this free of aliasing and + // object-lifetime concerns. + bool readLatest(T& head_out, uint8_t* trailing_out, size_t trailing_bytes) { + return readSlot_(&head_out, sizeof(T), trailing_out, trailing_bytes, /*exact_fit=*/false); + } + + private: + bool readSlot_(void* head_dst, size_t head_bytes, void* trailing_dst, size_t trailing_bytes, bool exact_fit) { if (!ensureMapped_()) { return false; } @@ -57,31 +76,45 @@ class SharedMemoryReader { if (header_->magic != kMagic || header_->version != kVersion || header_->slot_count == 0 || header_->slot_bytes == 0) { return false; } - if (count * sizeof(T) != header_->slot_bytes) { + const size_t requested = head_bytes + trailing_bytes; + // Composite slots are padded up to an 8-byte boundary, so the reader may + // ask for less than the full slot; a plain typed read must match exactly, + // which is also what catches a segment written with a different T. + if (exact_fit ? (requested != header_->slot_bytes) : (requested > header_->slot_bytes)) { return false; } - const uint64_t seq = __atomic_load_n(&header_->seq, __ATOMIC_ACQUIRE); - if (seq == 0 || seq == last_seq_) { + const uint64_t seq_before = __atomic_load_n(&header_->seq, __ATOMIC_ACQUIRE); + if (seq_before == 0 || seq_before == last_seq_) { return false; } - const uint32_t slot_idx = static_cast(seq % header_->slot_count); + const uint32_t slot_idx = static_cast(seq_before % header_->slot_count); const uint8_t* slots_base = static_cast(map_ptr_) + sizeof(Header); const uint8_t* src = slots_base + static_cast(slot_idx) * header_->slot_bytes; - std::memcpy(out, src, header_->slot_bytes); + if (head_bytes > 0) { + std::memcpy(head_dst, src, head_bytes); + } + if (trailing_bytes > 0) { + std::memcpy(trailing_dst, src + head_bytes, trailing_bytes); + } - last_seq_ = seq; - return true; - } + // Torn-read guard. Nothing stops the writer from lapping us mid-copy, and + // the copy is only getting longer now that images share the slot. The + // writer starts filling the slot for seq X before publishing X, so by the + // time seq has advanced by slot_count - 1 it may already be overwriting + // the slot we just read; discard rather than hand back spliced data. + // (With slot_count == 1 every read is unprotected and so always rejected.) + const uint64_t seq_after = __atomic_load_n(&header_->seq, __ATOMIC_ACQUIRE); + if (seq_after - seq_before >= header_->slot_count - 1) { + return false; + } - // Convenience for element_count == 1 (single POD struct per slot). - bool readLatest(T& out) { - return readLatest(&out, 1); + last_seq_ = seq_before; + return true; } - private: struct Header { uint32_t magic = 0; uint32_t version = 0; @@ -92,7 +125,9 @@ class SharedMemoryReader { }; static constexpr uint32_t kMagic = 0x5348514d; // SHQM - static constexpr uint32_t kVersion = 1; + // Must match SharedMemoryWriter::kVersion; see the note there on why the + // composite layout required a bump rather than a silent format change. + static constexpr uint32_t kVersion = 2; bool ensureMapped_() { if (map_ptr_) { diff --git a/include/ipc/SharedMemoryWriter.h b/include/ipc/SharedMemoryWriter.h index bd3d643..ba6d184 100644 --- a/include/ipc/SharedMemoryWriter.h +++ b/include/ipc/SharedMemoryWriter.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -18,6 +19,22 @@ namespace spqr { // the payload (e.g. a fixed compile-time state struct written with element_count == 1). struct NoSharedMemoryMeta {}; +// Metadata for raw image segments: lets a reader discover the frame's shape at +// runtime, since images are written as SharedMemoryWriter +// with element_count = width * height * channels (resolution comes from the +// MuJoCo model, so it is not known at compile time). +struct ImageMeta { + uint32_t width = 0; + uint32_t height = 0; + uint32_t channels = 0; +}; + +// One source region of a slot's trailing payload, for the composite write below. +struct SharedMemoryChunk { + const void* data = nullptr; + size_t bytes = 0; +}; + // Lock-free single-writer/multi-reader shared-memory ring buffer for any // trivially-copyable T, with an optional trivially-copyable Meta blob written // once at configure() time (e.g. image width/height/channels, needed by a @@ -27,6 +44,16 @@ struct NoSharedMemoryMeta {}; // type's bytes are already valid in any process built from the same struct // definition, so plain memcpy is sufficient (and fastest) -- no serialization // step is needed or performed. +// +// A slot is a fixed-size typed head (element_count T's) optionally followed by a +// runtime-sized trailing region of opaque bytes. That covers payloads whose size +// is only known at startup -- camera frames, whose resolution comes from the +// MuJoCo model -- without giving up the compile-time type of the head: T stays +// POD, and the shape of the trailing region travels in Meta. Head and trailing +// are published under a single seq, so a reader never mixes a head from one tick +// with trailing bytes from another. The two degenerate cases are the whole +// previous API: head only (a POD state struct) and trailing only via +// write(const T*, count) with T = uint8_t (a raw image segment). template class SharedMemoryWriter { public: @@ -42,10 +69,22 @@ class SharedMemoryWriter { // element_count: how many T's fit in a single slot (e.g. width*height*channels // for a raw uint8_t image; 1 for a single POD message struct). void configure(const std::string& path, size_t element_count, Meta meta = {}, int ring_slots = 3) { + configure(path, element_count, /*trailing_bytes=*/0, meta, ring_slots); + } + + // Composite slot: element_count T's followed by trailing_bytes of opaque + // bytes, written together by write(head, trailing). trailing_bytes is a + // runtime value (e.g. summed image sizes), fixed for the segment's lifetime. + void configure(const std::string& path, size_t element_count, size_t trailing_bytes, Meta meta, int ring_slots = 3) { if (element_count == 0 || ring_slots <= 0) { throw std::invalid_argument("Invalid shared-memory configuration"); } - const size_t slot_bytes = element_count * sizeof(T); + head_bytes_ = element_count * sizeof(T); + trailing_bytes_ = trailing_bytes; + // Pad so every slot start stays 8-aligned, keeping the head's doubles + // naturally aligned for a reader that maps the slot in place. + const size_t padded_trailing = (trailing_bytes + 7u) & ~static_cast(7u); + const size_t slot_bytes = head_bytes_ + padded_trailing; const size_t total_bytes = sizeof(Header) + static_cast(ring_slots) * slot_bytes; close_(); @@ -115,6 +154,39 @@ class SharedMemoryWriter { write(&value, 1); } + // Composite write: the typed head followed by the trailing chunks, all into + // one slot and published with a single seq bump, so head and trailing are + // always from the same tick. The chunk sizes must add up to the + // trailing_bytes passed to configure(); a mismatch is dropped like the + // size guard in write(const T*, count). + void write(const T& head, std::initializer_list trailing) { + if (!header_ || head_bytes_ != sizeof(T)) { + return; + } + size_t supplied = 0; + for (const SharedMemoryChunk& chunk : trailing) { + supplied += chunk.bytes; + } + if (supplied != trailing_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, &head, sizeof(T)); + dst += sizeof(T); + for (const SharedMemoryChunk& chunk : trailing) { + if (chunk.bytes > 0) { + std::memcpy(dst, chunk.data, chunk.bytes); + dst += chunk.bytes; + } + } + __atomic_store_n(&header_->seq, next_seq, __ATOMIC_RELEASE); + } + private: struct Header { uint32_t magic = 0; @@ -126,7 +198,10 @@ class SharedMemoryWriter { }; static constexpr uint32_t kMagic = 0x5348514d; // SHQM - static constexpr uint32_t kVersion = 1; + // v2 added the composite head+trailing slot layout. The bump matters because + // Meta sits inside Header: a v1 peer would still match magic/version at + // offsets 0 and 4 but read slot_count/slot_bytes from the wrong offsets. + static constexpr uint32_t kVersion = 2; void close_() { if (map_ptr_) { @@ -147,6 +222,8 @@ class SharedMemoryWriter { size_t map_bytes_ = 0; Header* header_ = nullptr; std::string path_; + size_t head_bytes_ = 0; // element_count * sizeof(T) + size_t trailing_bytes_ = 0; // unpadded; slot_bytes rounds this up to 8 }; } // namespace spqr diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 277f938..e9975c4 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -22,7 +22,6 @@ #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" @@ -46,6 +45,25 @@ struct BoosterT1SharedState { OracleData oracle; }; +// Identifies this exact state layout. Shared memory carries no schema, so a +// reader that opened another robot type's segment would otherwise just +// reinterpret the bytes; checking this first turns that into a clean failure. +constexpr uint32_t kBoosterT1SchemaId = 0xB0057E71; + +// Describes the camera frames that ride along in the trailing region of the +// state slot, in the order publishSharedState() writes them. Per-robot by +// design: another robot type declares its own SharedState / +// StateMeta pair with however many streams it actually has (the +// mechanism is generic; only the payload is robot-specific). If a robot ever +// needs a stream count that varies at runtime, bound it the way OracleData +// bounds teammates: a fixed-capacity array plus a count. +struct BoosterT1StateMeta { + uint32_t schema_id = kBoosterT1SchemaId; + uint32_t state_bytes = static_cast(sizeof(BoosterT1SharedState)); + ImageMeta rgb; + ImageMeta depth; +}; + class BoosterT1 : public Robot { public: Pose* pose = nullptr; @@ -119,12 +137,17 @@ class BoosterT1 : public Robot { // 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); - state_writer_.configure(shmFilePath_("state"), /*element_count=*/1); + // State and both camera frames share a single segment. The frames are + // runtime-sized (resolution comes from the MuJoCo model) but fixed once + // known, so they fit the writer's trailing region, with their shape + // carried in the meta blob for the reader. + const uint32_t width = static_cast(rgbCamera->getWidth()); + const uint32_t height = static_cast(rgbCamera->getHeight()); + const size_t rgbBytes = static_cast(width) * height * 3; + const size_t depthBytes = static_cast(width) * height * 1; + state_writer_.configure(shmFilePath_("state"), /*element_count=*/1, rgbBytes + depthBytes, + BoosterT1StateMeta{kBoosterT1SchemaId, static_cast(sizeof(BoosterT1SharedState)), + ImageMeta{width, height, 3}, ImageMeta{width, height, 1}}); command_reader_.configure(shmFilePath_("commands")); // Create Oracle with the pose and all robots @@ -177,16 +200,19 @@ class BoosterT1 : public Robot { 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; } void publishSharedState() override { + // One slot, one seq: pose/imu/joints/oracle and both frames are published + // together, so a reader can never pair a frame with a pose from a + // different tick -- which matters as soon as anything unprojects an image + // detection using the pose at capture time. + const auto& rgbFrame = rgbCamera->getImage(); + const auto& depthFrame = depthCamera->getDepth8bit(); state_writer_.write(BoosterT1SharedState{pose->toSharedState(), imu->toSharedState(), - joints->toSharedState(), oracle->toSharedState()}); + joints->toSharedState(), oracle->toSharedState()}, + {{rgbFrame.data(), rgbFrame.size()}, {depthFrame.data(), depthFrame.size()}}); } std::map getSensors() override { @@ -224,9 +250,7 @@ class BoosterT1 : public Robot { std::unordered_map latestTorques; std::string shm_dir_; - ImageSharedMemoryWriter rgb_writer_; - ImageSharedMemoryWriter depth_writer_; - SharedMemoryWriter state_writer_; + SharedMemoryWriter state_writer_; SharedMemoryReader> command_reader_; }; diff --git a/include/sensors/ImageSharedMemoryWriter.h b/include/sensors/ImageSharedMemoryWriter.h deleted file mode 100644 index 2e549ee..0000000 --- a/include/sensors/ImageSharedMemoryWriter.h +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "ipc/SharedMemoryWriter.h" - -namespace spqr { - -struct ImageMeta { - uint32_t width = 0; - uint32_t height = 0; - uint32_t channels = 0; -}; - -class ImageSharedMemoryWriter { - public: - void configure(const std::string& path, int width, int height, int channels, int ring_slots = 3) { - if (width <= 0 || height <= 0 || channels <= 0) { - throw std::invalid_argument("Invalid shared-memory image configuration"); - } - const size_t element_count = static_cast(width) * static_cast(height) * static_cast(channels); - ImageMeta meta{static_cast(width), static_cast(height), static_cast(channels)}; - writer_.configure(path, element_count, meta, ring_slots); - } - - bool isReady() const { - return writer_.isReady(); - } - - void write(const std::vector& frame) { - writer_.write(frame.data(), frame.size()); - } - - private: - SharedMemoryWriter writer_; -}; - -} // namespace spqr From a038e2380094d3cf803cf9f36d73af6f09f54adb Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Wed, 12 Aug 2026 01:35:55 +0200 Subject: [PATCH 06/25] removed element_count MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Argomento element_count era diventato fisso a 1 dopo l'aggiunta di trailing_bytes, quindi è stato rimosso come argomento delle funzioni --- include/ipc/SharedMemoryReader.h | 46 +++++++------------ include/ipc/SharedMemoryWriter.h | 77 ++++++++++---------------------- include/robots/BoosterT1.h | 2 +- 3 files changed, 40 insertions(+), 85 deletions(-) diff --git a/include/ipc/SharedMemoryReader.h b/include/ipc/SharedMemoryReader.h index 60675cd..57cda0a 100644 --- a/include/ipc/SharedMemoryReader.h +++ b/include/ipc/SharedMemoryReader.h @@ -46,27 +46,13 @@ class SharedMemoryReader { return true; } - // out must point to at least `count` T's (count == element_count the writer was configured with). - bool readLatest(T* out, size_t count) { - return readSlot_(out, count * sizeof(T), nullptr, 0, /*exact_fit=*/true); - } - - // Convenience for element_count == 1 (single POD struct per slot). - bool readLatest(T& out) { - return readLatest(&out, 1); - } - - // Composite counterpart to SharedMemoryWriter::write(head, trailing): copies - // the typed head and the runtime-sized trailing region out of the same slot, - // so both come from the same tick. The head is copied into a properly typed - // object rather than mapped in place, which keeps this free of aliasing and - // object-lifetime concerns. - bool readLatest(T& head_out, uint8_t* trailing_out, size_t trailing_bytes) { - return readSlot_(&head_out, sizeof(T), trailing_out, trailing_bytes, /*exact_fit=*/false); - } - - private: - bool readSlot_(void* head_dst, size_t head_bytes, void* trailing_dst, size_t trailing_bytes, bool exact_fit) { + // Reads the head, and the trailing_bytes of opaque data after it if any, out + // of the same slot -- the composite counterpart to SharedMemoryWriter::write. + // The head is copied into a properly typed object rather than mapped in + // place, which keeps this free of aliasing and object-lifetime concerns. + // trailing_out/trailing_bytes may be omitted for a plain POD struct with no + // trailing data. + bool readLatest(T& head_out, uint8_t* trailing_out = nullptr, size_t trailing_bytes = 0) { if (!ensureMapped_()) { return false; } @@ -76,11 +62,12 @@ class SharedMemoryReader { if (header_->magic != kMagic || header_->version != kVersion || header_->slot_count == 0 || header_->slot_bytes == 0) { return false; } - const size_t requested = head_bytes + trailing_bytes; - // Composite slots are padded up to an 8-byte boundary, so the reader may - // ask for less than the full slot; a plain typed read must match exactly, - // which is also what catches a segment written with a different T. - if (exact_fit ? (requested != header_->slot_bytes) : (requested > header_->slot_bytes)) { + const size_t requested = sizeof(T) + trailing_bytes; + // Composite slots are padded up to an 8-byte boundary, so the requested + // size may fall short of the slot by at most that much; anything short by + // more, or asking for more than the slot holds, means a size/type mismatch + // (e.g. a segment written with a different T) rather than padding slack. + if (requested > header_->slot_bytes || header_->slot_bytes - requested >= 8) { return false; } @@ -93,11 +80,9 @@ class SharedMemoryReader { const uint8_t* slots_base = static_cast(map_ptr_) + sizeof(Header); const uint8_t* src = slots_base + static_cast(slot_idx) * header_->slot_bytes; - if (head_bytes > 0) { - std::memcpy(head_dst, src, head_bytes); - } + std::memcpy(&head_out, src, sizeof(T)); if (trailing_bytes > 0) { - std::memcpy(trailing_dst, src + head_bytes, trailing_bytes); + std::memcpy(trailing_out, src + sizeof(T), trailing_bytes); } // Torn-read guard. Nothing stops the writer from lapping us mid-copy, and @@ -115,6 +100,7 @@ class SharedMemoryReader { return true; } + private: struct Header { uint32_t magic = 0; uint32_t version = 0; diff --git a/include/ipc/SharedMemoryWriter.h b/include/ipc/SharedMemoryWriter.h index ba6d184..6bb9e78 100644 --- a/include/ipc/SharedMemoryWriter.h +++ b/include/ipc/SharedMemoryWriter.h @@ -16,13 +16,12 @@ namespace spqr { // No metadata (default): use when the reader already knows how to interpret -// the payload (e.g. a fixed compile-time state struct written with element_count == 1). +// the payload (e.g. a fixed compile-time state struct with no trailing data). struct NoSharedMemoryMeta {}; -// Metadata for raw image segments: lets a reader discover the frame's shape at -// runtime, since images are written as SharedMemoryWriter -// with element_count = width * height * channels (resolution comes from the -// MuJoCo model, so it is not known at compile time). +// Metadata for a segment whose trailing region holds one or more camera frames: +// lets a reader discover each frame's shape at runtime, since the resolution +// comes from the MuJoCo model and so is not known at compile time. struct ImageMeta { uint32_t width = 0; uint32_t height = 0; @@ -45,15 +44,14 @@ struct SharedMemoryChunk { // definition, so plain memcpy is sufficient (and fastest) -- no serialization // step is needed or performed. // -// A slot is a fixed-size typed head (element_count T's) optionally followed by a -// runtime-sized trailing region of opaque bytes. That covers payloads whose size -// is only known at startup -- camera frames, whose resolution comes from the -// MuJoCo model -- without giving up the compile-time type of the head: T stays -// POD, and the shape of the trailing region travels in Meta. Head and trailing -// are published under a single seq, so a reader never mixes a head from one tick -// with trailing bytes from another. The two degenerate cases are the whole -// previous API: head only (a POD state struct) and trailing only via -// write(const T*, count) with T = uint8_t (a raw image segment). +// A slot is one T (the head) optionally followed by a runtime-sized trailing +// region of opaque bytes. That covers payloads whose size is only known at +// startup -- camera frames, whose resolution comes from the MuJoCo model -- +// without giving up the compile-time type of the head: T stays POD, and the +// shape of the trailing region travels in Meta. Head and trailing are +// published under a single seq, so a reader never mixes a head from one tick +// with trailing bytes from another. trailing_bytes = 0 is the degenerate case +// of a plain POD message struct with no trailing data. template class SharedMemoryWriter { public: @@ -66,25 +64,18 @@ class SharedMemoryWriter { close_(); } - // element_count: how many T's fit in a single slot (e.g. width*height*channels - // for a raw uint8_t image; 1 for a single POD message struct). - void configure(const std::string& path, size_t element_count, Meta meta = {}, int ring_slots = 3) { - configure(path, element_count, /*trailing_bytes=*/0, meta, ring_slots); - } - - // Composite slot: element_count T's followed by trailing_bytes of opaque - // bytes, written together by write(head, trailing). trailing_bytes is a - // runtime value (e.g. summed image sizes), fixed for the segment's lifetime. - void configure(const std::string& path, size_t element_count, size_t trailing_bytes, Meta meta, int ring_slots = 3) { - if (element_count == 0 || ring_slots <= 0) { + // trailing_bytes: how many bytes of opaque data follow the T head in each + // slot (e.g. summed image sizes), fixed for the segment's lifetime; 0 for a + // plain POD message struct with no trailing data. + void configure(const std::string& path, size_t trailing_bytes = 0, Meta meta = {}, int ring_slots = 3) { + if (ring_slots <= 0) { throw std::invalid_argument("Invalid shared-memory configuration"); } - head_bytes_ = element_count * sizeof(T); trailing_bytes_ = trailing_bytes; // Pad so every slot start stays 8-aligned, keeping the head's doubles // naturally aligned for a reader that maps the slot in place. const size_t padded_trailing = (trailing_bytes + 7u) & ~static_cast(7u); - const size_t slot_bytes = head_bytes_ + padded_trailing; + const size_t slot_bytes = sizeof(T) + padded_trailing; const size_t total_bytes = sizeof(Header) + static_cast(ring_slots) * slot_bytes; close_(); @@ -134,35 +125,14 @@ class SharedMemoryWriter { return header_ != nullptr; } - void write(const T* data, size_t count) { + // Writes the head, followed by the trailing chunks (if any), into one slot + // published with a single seq bump -- so head and trailing are always from + // the same tick. The chunk sizes must add up to the trailing_bytes passed to + // configure(); a mismatch is dropped rather than writing a partial slot. + void write(const T& head, std::initializer_list trailing = {}) { if (!header_) { return; } - if (count * sizeof(T) != 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, data, header_->slot_bytes); - __atomic_store_n(&header_->seq, next_seq, __ATOMIC_RELEASE); - } - - void write(const T& value) { - write(&value, 1); - } - - // Composite write: the typed head followed by the trailing chunks, all into - // one slot and published with a single seq bump, so head and trailing are - // always from the same tick. The chunk sizes must add up to the - // trailing_bytes passed to configure(); a mismatch is dropped like the - // size guard in write(const T*, count). - void write(const T& head, std::initializer_list trailing) { - if (!header_ || head_bytes_ != sizeof(T)) { - return; - } size_t supplied = 0; for (const SharedMemoryChunk& chunk : trailing) { supplied += chunk.bytes; @@ -222,7 +192,6 @@ class SharedMemoryWriter { size_t map_bytes_ = 0; Header* header_ = nullptr; std::string path_; - size_t head_bytes_ = 0; // element_count * sizeof(T) size_t trailing_bytes_ = 0; // unpadded; slot_bytes rounds this up to 8 }; diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index e9975c4..e0c7e1d 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -145,7 +145,7 @@ class BoosterT1 : public Robot { const uint32_t height = static_cast(rgbCamera->getHeight()); const size_t rgbBytes = static_cast(width) * height * 3; const size_t depthBytes = static_cast(width) * height * 1; - state_writer_.configure(shmFilePath_("state"), /*element_count=*/1, rgbBytes + depthBytes, + state_writer_.configure(shmFilePath_("state"), rgbBytes + depthBytes, BoosterT1StateMeta{kBoosterT1SchemaId, static_cast(sizeof(BoosterT1SharedState)), ImageMeta{width, height, 3}, ImageMeta{width, height, 1}}); command_reader_.configure(shmFilePath_("commands")); From 6e9f197a6b5daeadb888fe2a8a12296bfe381373 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Wed, 12 Aug 2026 23:55:14 +0200 Subject: [PATCH 07/25] [Refactoring] send_all moved + sendMessage renamed in packMessage + sendMessageSocket MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Spostamento di send_all temporaneo in un utils, in modo che non ci sia l'implementazione all'interno del simulation thread. Funzione sendMessage rinominata in packMessage perché invio tramite socket non avviene lì dentro. Funzione sendMessageSocket per invio tramite socket direttamente dentro Robot.h --- include/RobotManager.h | 2 -- include/SimulationThread.h | 2 -- include/ipc/utils.h | 38 ++++++++++++++++++++++++++++++++++++++ include/robots/BoosterK1.h | 2 +- include/robots/BoosterT1.h | 4 ++-- include/robots/Robot.h | 19 +++++++++++++++++-- src/SimulationThread.cpp | 38 +------------------------------------- 7 files changed, 59 insertions(+), 46 deletions(-) create mode 100644 include/ipc/utils.h diff --git a/include/RobotManager.h b/include/RobotManager.h index 2eea743..d5b1bea 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -62,8 +62,6 @@ class RobotManager { RobotManager(const RobotManager&) = delete; RobotManager& operator=(const RobotManager&) = delete; - ssize_t send_all(int fd, char* buf, size_t len); - std::atomic serverRunning_ = false; std::thread serverThread_; diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 52a984b..5cf8e41 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -55,8 +55,6 @@ class SimulationThread : public QThread { // return multiple concatenated messages or a partial one. std::unordered_map unpackers_; - ssize_t send_all(int fd, char* buf, size_t len); - void sendStateMessages(); }; diff --git a/include/ipc/utils.h b/include/ipc/utils.h new file mode 100644 index 0000000..5483e1b --- /dev/null +++ b/include/ipc/utils.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include + +namespace spqr { + +// TCP Communication: sends the size of the message first, then the message itself. +// Returns the number of bytes sent (excluding the size header), or -1 on error. + +// 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 +inline ssize_t send_all(int fd, const 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; +} + +} \ No newline at end of file diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index f902009..8f384e2 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -82,7 +82,7 @@ class BoosterK1 : public Robot { std::cout << "}" << std::endl; } - std::map sendMessage() override { + std::map packMessage() override { buffer_zone_.clear(); std::map msg; msg["robot_name"] = msgpack::object(name, buffer_zone_); diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index e0c7e1d..dd913d2 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -34,7 +34,7 @@ class Team; // Forward declaration constexpr size_t kBoosterT1JointCount = 23; -// Per-tick shared-memory payload: everything BoosterT1's sendMessage() sends +// Per-tick shared-memory payload: everything BoosterT1's packMessage() sends // over the socket (minus robot_name, which the shared-memory file path already // encodes), bundled into one trivially-copyable struct so a reader sees all // fields from the same tick atomically (one SharedMemoryWriter -> one seq). @@ -191,7 +191,7 @@ class BoosterT1 : public Robot { return true; } - std::map sendMessage() override { + std::map packMessage() override { buffer_zone_.clear(); std::map msg; msg["robot_name"] = msgpack::object(name, buffer_zone_); diff --git a/include/robots/Robot.h b/include/robots/Robot.h index d8dfadf..1db44e2 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -17,6 +17,7 @@ #include "Container.h" #include "MujocoContext.h" #include "sensors/Sensor.h" +#include "ipc/utils.h" #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { @@ -40,10 +41,19 @@ class Robot { virtual ~Robot() = default; virtual void bindMujoco(MujocoContext* mujContext) = 0; virtual void update() = 0; + + virtual void sendMessageSocket(int fd) final { + auto message = packMessage(); + msgpack::sbuffer sbuf; + msgpack::pack(sbuf, message); + if (sbuf.size() > 0) { + send_all(fd, sbuf.data(), sbuf.size()); + } + } + virtual void receiveMessage(const std::map& message) = 0; - virtual std::map sendMessage() = 0; // Publishes this robot's per-tick state to shared memory, for the case where - // circus and simbridge run on the same machine. Mirrors sendMessage()'s role + // circus and simbridge run on the same machine. Mirrors packMessage()'s role // for the socket path; a robot that only needs the socket path implements // this with an empty body. virtual void publishSharedState() = 0; @@ -54,6 +64,11 @@ class Robot { virtual std::map getSensors() = 0; virtual void applyCommands() = 0; + private: + virtual std::map packMessage() = 0; + + public: + std::string name; std::string type; uint8_t number; diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index e11fdde..9a0f0f5 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -11,33 +11,6 @@ namespace spqr { SimulationThread::SimulationThread(const mjModel* model, mjData* data) : model_(model), data_(data), running_(true), paused_(false) {} -// Source - https://stackoverflow.com/a -// Posted by Arun, modified by community. See post 'Timeline' for change history -// Retrieved 2026-01-12, License - CC BY-SA 3.0 -// 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) @@ -90,17 +63,8 @@ void SimulationThread::receiveCommandMessages() { 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(); + r->sendMessageSocket(entity_fd_map[r->name]); r->publishSharedState(); - 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; From b6f6d01a815918ac34abbe143869a7096788e3ad Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Thu, 13 Aug 2026 00:16:42 +0200 Subject: [PATCH 08/25] [refactoring] receiveMessage renamed in receiveMessageSocket --- docs/circus/architecture.md | 2 +- include/robots/BoosterK1.h | 2 +- include/robots/BoosterT1.h | 2 +- include/robots/Robot.h | 4 ++-- include/sensors/Joint.h | 2 +- src/SimulationThread.cpp | 5 ++++- 6 files changed, 10 insertions(+), 7 deletions(-) diff --git a/docs/circus/architecture.md b/docs/circus/architecture.md index b999808..e697c0b 100644 --- a/docs/circus/architecture.md +++ b/docs/circus/architecture.md @@ -150,7 +150,7 @@ When data arrives on a socket: 1. Read and unpack MessagePack message 2. Extract `robot_name` to identify sender 3. Find corresponding `Robot` object in registry -4. Call `robot->receiveMessage()` to process commands +4. Call `robot->receiveMessageSocket()` to process commands 5. Call `robot->sendMessage()` to get current state 6. Pack and send response back to container diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 8f384e2..347b641 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -70,7 +70,7 @@ class BoosterK1 : public Robot { depthCamera = new CameraDepth(mujCtx, (name + "_depth_cam").c_str()); } - void receiveMessage(const std::map& message) override { + void receiveMessageSocket(const std::map& message) override { std::cout << "Hi I'm " << name << " message received: {"; bool first = true; for (const auto& [key, val] : message) { diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index dd913d2..efa6cff 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -154,7 +154,7 @@ class BoosterT1 : public Robot { oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); } - void receiveMessage(const std::map& message) override { + void receiveMessageSocket(const std::map& message) override { auto it = message.find("joint_torques"); if (it == message.end()) { throw std::runtime_error("Error: 'joint_torques' key not found in message"); diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 1db44e2..05f068f 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -51,7 +51,7 @@ class Robot { } } - virtual void receiveMessage(const std::map& message) = 0; + virtual void receiveMessageSocket(const std::map& message) = 0; // Publishes this robot's per-tick state to shared memory, for the case where // circus and simbridge run on the same machine. Mirrors packMessage()'s role // for the socket path; a robot that only needs the socket path implements @@ -59,7 +59,7 @@ class Robot { virtual void publishSharedState() = 0; // Reads and applies the latest command from shared memory, if a new one has // arrived since the last call. Returns true iff a new command was applied - // this call. Mirrors receiveMessage()'s role for the socket path. + // this call. Mirrors receiveMessageSocket()'s role for the socket path. virtual bool receiveSharedCommand() = 0; virtual std::map getSensors() = 0; virtual void applyCommands() = 0; diff --git a/include/sensors/Joint.h b/include/sensors/Joint.h index fd26d6a..f27b83d 100644 --- a/include/sensors/Joint.h +++ b/include/sensors/Joint.h @@ -53,7 +53,7 @@ struct JointState { // Trivially-copyable joint torque command, for receiving over shared memory // (same numbers previously carried as "joint_torques" in the msgpack command // message). Order matches joint_map's ascending JointValue iteration, i.e. the -// same order used by BoosterT1::receiveMessage() today. +// same order used by BoosterT1::receiveMessageSocket() today. template struct JointTorques { double torque[N] = {}; diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 9a0f0f5..e6c01a2 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -121,7 +121,7 @@ void SimulationThread::receiveCommandMessages() { } } - r->receiveMessage(data_map); + r->receiveMessageSocket(data_map); pendingRobots.erase(messageRecipient); ++done; break; @@ -342,10 +342,13 @@ void SimulationThread::run() { break; } + // TODO: sposta questo in RobotManager se possibile sendStateMessages(); auto receiveStart = clock::now(); + // TODO: forse pure questo si può spostare in RobotManager se possibile receiveCommandMessagesSHM(); + double receiveElapsedMs = std::chrono::duration(clock::now() - receiveStart).count(); std::cout << "[SimulationThread] receiveCommandMessagesSHM took " << receiveElapsedMs << " ms" << std::endl; } From b2d1bdba35f930216cce3314a1ab28b754087c2a Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Thu, 13 Aug 2026 00:43:37 +0200 Subject: [PATCH 09/25] [refactoring] renamed shm functions --- include/robots/BoosterK1.h | 4 ++-- include/robots/BoosterT1.h | 6 +++--- include/robots/Robot.h | 6 ++++-- src/SimulationThread.cpp | 12 ++++++------ 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 347b641..7c44122 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -93,11 +93,11 @@ class BoosterK1 : public Robot { return msg; } - void publishSharedState() override { + void sendMessageSHM() override { // Socket-only for now: BoosterK1 doesn't yet have a shared-memory state channel. } - bool receiveSharedCommand() override { + bool receiveMessageSHM() override { // Socket-only for now: BoosterK1 doesn't yet have a shared-memory command channel. return false; } diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index efa6cff..e7f20d2 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -51,7 +51,7 @@ struct BoosterT1SharedState { constexpr uint32_t kBoosterT1SchemaId = 0xB0057E71; // Describes the camera frames that ride along in the trailing region of the -// state slot, in the order publishSharedState() writes them. Per-robot by +// state slot, in the order sendMessageSHM() writes them. Per-robot by // design: another robot type declares its own SharedState / // StateMeta pair with however many streams it actually has (the // mechanism is generic; only the payload is robot-specific). If a robot ever @@ -177,7 +177,7 @@ class BoosterT1 : public Robot { } } - bool receiveSharedCommand() override { + bool receiveMessageSHM() override { JointTorques torques; if (!command_reader_.readLatest(torques)) { return false; @@ -203,7 +203,7 @@ class BoosterT1 : public Robot { return msg; } - void publishSharedState() override { + void sendMessageSHM() override { // One slot, one seq: pose/imu/joints/oracle and both frames are published // together, so a reader can never pair a frame with a pose from a // different tick -- which matters as soon as anything unprojects an image diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 05f068f..3690273 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -52,15 +52,17 @@ class Robot { } virtual void receiveMessageSocket(const std::map& message) = 0; + // Publishes this robot's per-tick state to shared memory, for the case where // circus and simbridge run on the same machine. Mirrors packMessage()'s role // for the socket path; a robot that only needs the socket path implements // this with an empty body. - virtual void publishSharedState() = 0; + virtual void sendMessageSHM() = 0; // Reads and applies the latest command from shared memory, if a new one has // arrived since the last call. Returns true iff a new command was applied // this call. Mirrors receiveMessageSocket()'s role for the socket path. - virtual bool receiveSharedCommand() = 0; + virtual bool receiveMessageSHM() = 0; + virtual std::map getSensors() = 0; virtual void applyCommands() = 0; diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index e6c01a2..bd91326 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -64,7 +64,7 @@ void SimulationThread::receiveCommandMessages() { if (pendingRobots.count(r->name)) { std::cout << "[receiveCommandMessages] Timeout, resending state to: " << r->name << std::endl; r->sendMessageSocket(entity_fd_map[r->name]); - r->publishSharedState(); + r->sendMessageSHM(); } } continue; @@ -135,7 +135,7 @@ void SimulationThread::receiveCommandMessages() { // Shared-memory counterpart to receiveCommandMessages(): same purpose (block // until every robot has a fresh command applied, retrying with a periodic -// notice if one is stalled) but polling r->receiveSharedCommand() per robot +// notice if one is stalled) but polling r->receiveMessageSHM() per robot // instead of poll()/read()/msgpack-unpack on the socket. void SimulationThread::receiveCommandMessagesSHM() { int robot_size = robots_.size(); @@ -151,7 +151,7 @@ void SimulationThread::receiveCommandMessagesSHM() { for (auto& r : robots_) { if (!pendingRobots.count(r->name)) continue; - if (r->receiveSharedCommand()) { + if (r->receiveMessageSHM()) { if (!r->isReady) { r->isReady = true; std::cout << "Robot ready: " << r->name << std::endl; @@ -176,7 +176,7 @@ void SimulationThread::receiveCommandMessagesSHM() { if (!pendingRobots.count(r->name)) continue; std::cout << "[receiveCommandMessagesSHM] Timeout, resending state to: " << r->name << std::endl; - r->publishSharedState(); + r->sendMessageSHM(); } windowStart = std::chrono::steady_clock::now(); } @@ -234,7 +234,7 @@ void SimulationThread::waitRobotConnections() { for (auto& r : robots_) { if (r->name == robotName) { r->isConnected = true; - r->publishSharedState(); + r->sendMessageSHM(); std::cout << "Connected Robot: " << robotName << "\n"; std::cout << "Published initial state to " << robotName << " via shared memory" << std::endl; break; @@ -381,7 +381,7 @@ void SimulationThread::run() { void SimulationThread::sendStateMessages() { for (auto& r : robots_) { std::unique_lock lock(mutex_); - r->publishSharedState(); + r->sendMessageSHM(); } } From c324d86ded4b5933fad725f97801b1b0d44e36f5 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Thu, 13 Aug 2026 02:13:33 +0200 Subject: [PATCH 10/25] shm initial connection MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit La connessione iniziale dei robot al simulatore non funziona tramite scambio di messaggi, ma il simulatore controlla se il file in shm creato dai container dei robot esistono. Se c'è, considera quel robot connesso. Aggiunta connection mode che può essere "shm" o "socket" (da refactorare in maniera più chiara, al momento credo ci sia qualcosa di hardcodato) --- include/AppWindow.h | 1 + include/Container.h | 3 ++- include/RobotManager.h | 3 ++- include/SimulationThread.h | 1 + include/robots/Robot.h | 20 +++++++++++++-- src/AppWindow.cpp | 12 +++++++-- src/Container.cpp | 4 ++- src/RobotManager.cpp | 4 +-- src/SimulationThread.cpp | 51 +++++++++++++++++++++++++++++++++++--- 9 files changed, 87 insertions(+), 12 deletions(-) diff --git a/include/AppWindow.h b/include/AppWindow.h index 8ceef82..312920a 100644 --- a/include/AppWindow.h +++ b/include/AppWindow.h @@ -55,6 +55,7 @@ class AppWindow : public QMainWindow { std::string frameworkConfigPath_ = spqr::frameworkConfigPath; std::string pathsConfigPath_ = spqr::pathsConfigPath; + std::string connectMode_ = "shm"; }; } // namespace spqr diff --git a/include/Container.h b/include/Container.h index 9bd454e..184eb77 100644 --- a/include/Container.h +++ b/include/Container.h @@ -17,7 +17,8 @@ class Container { Container(const std::string& name, const std::string& sockPath = "/var/run/docker.sock"); ~Container(); - void create(const std::shared_ptr& robot, const std::string& image, const std::vector& binds); + void create(const std::shared_ptr& robot, const std::string& image, const std::vector& binds, + const std::string& connectMode = "shm"); void start(); void stop(); diff --git a/include/RobotManager.h b/include/RobotManager.h index d5b1bea..b5c4e93 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -50,7 +50,8 @@ class RobotManager { const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team, const std::string& role = "Striker"); - void startContainers(const std::string& fwkCfgPath = spqr::frameworkConfigPath, const std::string& pathsCfgPath = spqr::pathsConfigPath); + void startContainers(const std::string& fwkCfgPath = spqr::frameworkConfigPath, const std::string& pathsCfgPath = spqr::pathsConfigPath, + const std::string& connectMode = "shm"); void setAreAllRobotsReadyCallback(std::function cb); void applyCommands(); diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 5cf8e41..256084c 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -26,6 +26,7 @@ class SimulationThread : public QThread { void setMaxSimulationTime(int maxTime); void initializeSocket(int port); void waitRobotConnections(); + void waitRobotConnectionsSHM(); void receiveCommandMessages(); void receiveCommandMessagesSHM(); diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 3690273..353ff86 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -37,6 +38,12 @@ class Robot { } else { throw std::runtime_error("Team color currently unsupported: " + colorName); } + // Reuses the per-robot commands SHM segment as the connect signal: simbridge's + // BridgeNode constructor synchronously creates this file (via + // SharedMemoryWriter::configure()) before it dials the socket -- or, in "shm" + // connect mode, instead of dialing it at all -- so its presence on tmpfs alone + // proves the robot process is up, with no dedicated connect channel needed. + connect_shm_path_ = "/dev/shm/circus_ipc/" + name + "_commands.shm"; } virtual ~Robot() = default; virtual void bindMujoco(MujocoContext* mujContext) = 0; @@ -62,13 +69,22 @@ class Robot { // arrived since the last call. Returns true iff a new command was applied // this call. Mirrors receiveMessageSocket()'s role for the socket path. virtual bool receiveMessageSHM() = 0; - + + // True once this robot's simbridge process has announced itself via shared + // memory (SimulationThread::waitRobotConnectionsSHM(), the "shm" connect-mode + // counterpart to the socket handshake in waitRobotConnections()). + bool hasConnectSignal() const { + return std::filesystem::exists(connect_shm_path_); + } + virtual std::map getSensors() = 0; virtual void applyCommands() = 0; private: virtual std::map packMessage() = 0; - + + std::string connect_shm_path_; + public: std::string name; diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 201d5dd..00c968a 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -45,6 +45,8 @@ AppWindow::AppWindow(int& argc, char** argv) : QMainWindow() { frameworkConfigPath_ = argv[++i]; else if (arg == "--paths" && i + 1 < argc) pathsConfigPath_ = argv[++i]; + else if (arg == "--connect-mode" && i + 1 < argc) + connectMode_ = argv[++i]; } resize(spqr::initialWindowWidth, spqr::initialWindowHeight); @@ -225,10 +227,16 @@ void AppWindow::loadScene(const QString& yaml_file) { sim->initializeSocket(frameworkCommunicationPort); std::cout << "Starting containers..." << std::endl; - RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_); + RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); std::cout << "Connecting Robots..." << std::endl; - sim->waitRobotConnections(); + if (connectMode_ == "shm") { + std::cout << "Simulation starting with SHM approach..." << std::endl; + sim->waitRobotConnectionsSHM(); + } else { + std::cout << "Simulation starting with SOCKET approach..." << std::endl; + sim->waitRobotConnections(); + } std::cout << "Waiting Robots are Ready..." << std::endl; sim->receiveCommandMessagesSHM(); diff --git a/src/Container.cpp b/src/Container.cpp index f5e0a4e..3e47127 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -35,7 +35,8 @@ Container::~Container() { } } -void Container::create(const std::shared_ptr& robot, const std::string& image, const std::vector& binds) { +void Container::create(const std::shared_ptr& robot, const std::string& image, const std::vector& binds, + const std::string& connectMode) { 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); @@ -86,6 +87,7 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc", + "CONNECT_MODE=" + connectMode, "JOYSTICK_DEVICE=" + envOrDefault("JOYSTICK_DEVICE", "/dev/input/js0"), "INFERENCE_BACKEND=" + envOrDefault("INFERENCE_BACKEND", "trt"), "TASK_NAME=" + envOrDefault("TASK_NAME", "t1-velocity"), diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 8363e24..fcf05b3 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -64,7 +64,7 @@ std::shared_ptr RobotManager::create(const std::string& name, const std:: return nullptr; } -void RobotManager::startContainers(const std::string& fwkCfgPath, const std::string& pathsCfgPath) { +void RobotManager::startContainers(const std::string& fwkCfgPath, const std::string& pathsCfgPath, const std::string& connectMode) { YAML::Node configRoot = loadYamlFile(fwkCfgPath.c_str()); if (!configRoot["image"]) @@ -112,7 +112,7 @@ void RobotManager::startContainers(const std::string& fwkCfgPath, const std::str } for (std::shared_ptr r : robots_) { r->container = std::make_unique("CIRCUS_" + r->name + "_container"); - r->container->create(r, image, binds); + r->container->create(r, image, binds, connectMode); r->container->start(); } std::cout << "Containers started successfully!" << std::endl; diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index bd91326..936223a 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -253,6 +253,51 @@ void SimulationThread::waitRobotConnections() { } } +// "shm" connect-mode counterpart to waitRobotConnections(): instead of accepting +// a socket connection and reading the robot's name off it, this polls each +// still-pending robot's Robot::hasConnectSignal() (the presence of its +// _commands.shm segment, created by simbridge's BridgeNode constructor +// before it would otherwise dial the socket -- see Robot::connect_shm_path_). +// Selected exclusively via the --connect-mode flag (AppWindow.cpp); never run +// together with waitRobotConnections() in the same startup. +void SimulationThread::waitRobotConnectionsSHM() { + std::set pendingRobots; + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) + if (!r->isConnected) + pendingRobots.insert(r->name); + } + + auto lastStatusLog = std::chrono::steady_clock::now(); + while (!pendingRobots.empty()) { + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (pendingRobots.count(r->name) && r->hasConnectSignal()) { + r->isConnected = true; + r->sendMessageSHM(); // publish initial state, same as the socket path + pendingRobots.erase(r->name); + std::cout << "Connected Robot: " << r->name << " (shm)\n"; + } + } + } + if (pendingRobots.empty()) + break; + + if (std::chrono::steady_clock::now() - lastStatusLog > std::chrono::milliseconds(2500)) { + std::unique_lock lock(mutex_); + std::cout << "[waitRobotConnectionsSHM] still waiting for:"; + for (auto& n : pendingRobots) + std::cout << " " << n; + std::cout << std::endl; + lastStatusLog = std::chrono::steady_clock::now(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + std::cout << "All Robots are connected!" << std::endl; +} + /* // SimulationThread IDEA mj_step1(); @@ -350,14 +395,14 @@ void SimulationThread::run() { receiveCommandMessagesSHM(); double receiveElapsedMs = std::chrono::duration(clock::now() - receiveStart).count(); - std::cout << "[SimulationThread] receiveCommandMessagesSHM took " << receiveElapsedMs << " ms" << std::endl; + // std::cout << "[SimulationThread] receiveCommandMessagesSHM took " << receiveElapsedMs << " ms" << std::endl; } // DEBUG: real time spent computing kControlDecimation physics // steps, vs. the sim-time budget they represent. double stepsElapsedMs = std::chrono::duration(clock::now() - stepsStart).count(); double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; - std::cout << "[SimulationThread] " << kControlDecimation << " physics steps took " - << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; + // std::cout << "[SimulationThread] " << kControlDecimation << " physics steps took " + // << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; // for each robot: // send(state) // non-blocking From 839c35277a49b904eacf80814883ecbcc7085e5c Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Mon, 17 Aug 2026 02:22:01 +0200 Subject: [PATCH 11/25] [refactoring] change names + enabling choice both modes --- include/SimulationThread.h | 4 +-- src/AppWindow.cpp | 54 ++++++++++++++++++++++++-------------- src/SimulationThread.cpp | 23 +++++++++------- 3 files changed, 50 insertions(+), 31 deletions(-) diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 256084c..1903205 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -25,9 +25,9 @@ class SimulationThread : public QThread { bool isPaused(); void setMaxSimulationTime(int maxTime); void initializeSocket(int port); - void waitRobotConnections(); + void waitRobotConnectionsSocket(); void waitRobotConnectionsSHM(); - void receiveCommandMessages(); + void receiveCommandMessagesSocket(); void receiveCommandMessagesSHM(); signals: diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 00c968a..a477ebc 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -216,30 +216,46 @@ void AppWindow::loadScene(const QString& yaml_file) { // 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); + + if(connectMode_ == "shm") { + // TODO: il path sarebbe da mettere in config + 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); + CircusNetwork::instance().init(); + RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server - std::cout << "Starting containers..." << std::endl; - RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); + std::cout << "Starting containers..." << std::endl; + RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); - std::cout << "Connecting Robots..." << std::endl; - if (connectMode_ == "shm") { - std::cout << "Simulation starting with SHM approach..." << std::endl; + std::cout << "Simulation starting with SHM connection mode..." << std::endl; + std::cout << "Connecting Robots..." << std::endl; sim->waitRobotConnectionsSHM(); - } else { - std::cout << "Simulation starting with SOCKET approach..." << std::endl; - sim->waitRobotConnections(); - } - std::cout << "Waiting Robots are Ready..." << std::endl; - sim->receiveCommandMessagesSHM(); + std::cout << "Waiting Robots are Ready..." << std::endl; + sim->receiveCommandMessagesSHM(); + } + else { // socket mode + + 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(frameworkConfigPath_, pathsConfigPath_, connectMode_); + + + std::cout << "Simulation starting with SOCKET connection mode..." << std::endl; + std::cout << "Connecting Robots..." << std::endl; + sim->waitRobotConnectionsSocket(); + std::cout << "Waiting Robots are Ready..." << std::endl; + sim->receiveCommandMessagesSocket(); + + } + // Set initial simulation state (playing when scene is loaded) toolsPanel->setSimulationPlaying(true); diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 936223a..186e497 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -9,7 +9,9 @@ namespace spqr { -SimulationThread::SimulationThread(const mjModel* model, mjData* data) : model_(model), data_(data), running_(true), paused_(false) {} +SimulationThread::SimulationThread(const mjModel* model, mjData* data) : model_(model), data_(data), running_(true), paused_(false) { + robots_ = RobotManager::instance().getRobots(); +} void SimulationThread::initializeSocket(int port) { server_fd = socket(AF_INET, SOCK_STREAM, 0); @@ -32,7 +34,7 @@ void SimulationThread::initializeSocket(int port) { 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"); @@ -43,7 +45,7 @@ void SimulationThread::initializeSocket(int port) { fds.push_back({server_fd, POLLIN, 0}); } -void SimulationThread::receiveCommandMessages() { +void SimulationThread::receiveCommandMessagesSocket() { int robot_size = robots_.size(); int done = 0; // Track which robots have not yet replied this step @@ -62,7 +64,7 @@ void SimulationThread::receiveCommandMessages() { 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; + std::cout << "[receiveCommandMessagesSocket] Timeout, resending state to: " << r->name << std::endl; r->sendMessageSocket(entity_fd_map[r->name]); r->sendMessageSHM(); } @@ -185,7 +187,7 @@ void SimulationThread::receiveCommandMessagesSHM() { } } -void SimulationThread::waitRobotConnections() { +void SimulationThread::waitRobotConnectionsSocket() { bool areAllConnected = false; while (!areAllConnected) { int ret = poll(fds.data(), fds.size(), 100); @@ -234,8 +236,8 @@ void SimulationThread::waitRobotConnections() { for (auto& r : robots_) { if (r->name == robotName) { r->isConnected = true; - r->sendMessageSHM(); - std::cout << "Connected Robot: " << robotName << "\n"; + r->sendMessageSocket(client_fd); + std::cout << "Connected Robot (socket): " << robotName << "\n"; std::cout << "Published initial state to " << robotName << " via shared memory" << std::endl; break; } @@ -243,7 +245,6 @@ void SimulationThread::waitRobotConnections() { } if (RobotManager::instance().areAllRobotsConnected()) { areAllConnected = true; - std::cout << "All Robots are connected!" << std::endl; break; } } @@ -251,6 +252,8 @@ void SimulationThread::waitRobotConnections() { } } } + + std::cout << "All Robots are connected!" << std::endl; } // "shm" connect-mode counterpart to waitRobotConnections(): instead of accepting @@ -276,9 +279,9 @@ void SimulationThread::waitRobotConnectionsSHM() { for (auto& r : robots_) { if (pendingRobots.count(r->name) && r->hasConnectSignal()) { r->isConnected = true; - r->sendMessageSHM(); // publish initial state, same as the socket path + r->sendMessageSHM(); pendingRobots.erase(r->name); - std::cout << "Connected Robot: " << r->name << " (shm)\n"; + std::cout << "Connected Robot (shm): " << r->name << "\n"; } } } From cbe478ae5a85c92d106c934202d6ab4cb4abe8dd Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Mon, 24 Aug 2026 01:30:00 +0200 Subject: [PATCH 12/25] [refactoring] communication logic moved from SimulationThread to RobotManager + connection choice added MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Tutta la logica di comunicazione è stata spostata in RobotManager, per mantenere il thread di Simulazione più snello (vediamo la gestione della comunicazione dei robot come qualcosa riguardante il loro Manager). Aggiunta la possibilità di scegliere la modalità di connessione tramite argomento al lancio (mancano ancora alcune cose per completare entrambe). --- include/AppWindow.h | 1 + include/Constants.h | 1 + include/RobotManager.h | 20 +++ include/SimulationThread.h | 28 ---- include/robots/BoosterT1.h | 4 +- include/robots/Robot.h | 10 +- src/AppWindow.cpp | 17 +- src/Container.cpp | 4 +- src/RobotManager.cpp | 323 +++++++++++++++++++++++++++++++++++++ src/SimulationThread.cpp | 305 +--------------------------------- 10 files changed, 367 insertions(+), 346 deletions(-) diff --git a/include/AppWindow.h b/include/AppWindow.h index 312920a..7f2755c 100644 --- a/include/AppWindow.h +++ b/include/AppWindow.h @@ -55,6 +55,7 @@ class AppWindow : public QMainWindow { std::string frameworkConfigPath_ = spqr::frameworkConfigPath; std::string pathsConfigPath_ = spqr::pathsConfigPath; + std::string sharedMemoryPath_ = spqr::sharedMemoryPath; std::string connectMode_ = "shm"; }; diff --git a/include/Constants.h b/include/Constants.h index 7eef67e..e5a4cbf 100644 --- a/include/Constants.h +++ b/include/Constants.h @@ -6,5 +6,6 @@ constexpr unsigned initialWindowWidth = 1200; constexpr unsigned initialWindowHeight = 900; constexpr const char* frameworkConfigPath = "resources/config/framework_config.yaml"; constexpr const char* pathsConfigPath = "resources/config/path_constants.yaml"; +constexpr const char* sharedMemoryPath = "/dev/shm/circus_ipc/"; constexpr int frameworkCommunicationPort = 5555; } // namespace spqr diff --git a/include/RobotManager.h b/include/RobotManager.h index b5c4e93..1ed1574 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -8,12 +8,16 @@ #include #include +#include +#include #include #include #include #include +#include #include #include +#include #include #include "Constants.h" @@ -41,6 +45,18 @@ class RobotManager { void update(); void clear(); + void setRobotFd(const std::string& name, int fd); + int getRobotFd(const std::string& name) const; // -1 se sconosciuto + void removeRobotFd(int fd); + + void sendStateMessages(); + void receiveCommandMessagesSHM(); + void receiveCommandMessagesSocket(); + + void initializeSocket(int port); + void waitRobotConnectionsSocket(); + void waitRobotConnectionsSHM(); + bool areAllRobotsReady() const; bool areAllRobotsConnected() const; @@ -68,6 +84,10 @@ class RobotManager { mutable std::mutex mutex_; std::vector> robots_; + std::map robotFdMap_; + int serverFd_ = -1; + std::vector pollFds_; + std::unordered_map unpackers_; std::function areAllRobotsReadyCallback_; using RobotCreator = std::function(const std::string&, const std::string&, uint8_t, const Eigen::Vector3d&, diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 1903205..32130e4 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -3,13 +3,6 @@ #include #include -#include -#include -#include -#include -#include - -#include "robots/Robot.h" namespace spqr { @@ -24,11 +17,6 @@ class SimulationThread : public QThread { void play(); bool isPaused(); void setMaxSimulationTime(int maxTime); - void initializeSocket(int port); - void waitRobotConnectionsSocket(); - void waitRobotConnectionsSHM(); - void receiveCommandMessagesSocket(); - void receiveCommandMessagesSHM(); signals: void stepCompleted(); @@ -41,22 +29,6 @@ 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_; - - // Per-fd streaming parser state: needed because incoming command - // messages have no length-prefix framing, so a single read() can - // return multiple concatenated messages or a partial one. - std::unordered_map unpackers_; - - void sendStateMessages(); }; } // namespace spqr diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index e7f20d2..2c79d75 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -145,10 +145,10 @@ class BoosterT1 : public Robot { const uint32_t height = static_cast(rgbCamera->getHeight()); const size_t rgbBytes = static_cast(width) * height * 3; const size_t depthBytes = static_cast(width) * height * 1; - state_writer_.configure(shmFilePath_("state"), rgbBytes + depthBytes, + state_writer_.configure(send_shm_path, rgbBytes + depthBytes, BoosterT1StateMeta{kBoosterT1SchemaId, static_cast(sizeof(BoosterT1SharedState)), ImageMeta{width, height, 3}, ImageMeta{width, height, 1}}); - command_reader_.configure(shmFilePath_("commands")); + command_reader_.configure(receive_shm_path); // Create Oracle with the pose and all robots oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 353ff86..ae08485 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -16,6 +16,7 @@ #include #include "Container.h" +#include "Constants.h" #include "MujocoContext.h" #include "sensors/Sensor.h" #include "ipc/utils.h" @@ -43,7 +44,8 @@ class Robot { // SharedMemoryWriter::configure()) before it dials the socket -- or, in "shm" // connect mode, instead of dialing it at all -- so its presence on tmpfs alone // proves the robot process is up, with no dedicated connect channel needed. - connect_shm_path_ = "/dev/shm/circus_ipc/" + name + "_commands.shm"; + receive_shm_path = spqr::sharedMemoryPath + name + "_commands.shm"; + send_shm_path = spqr::sharedMemoryPath + name + "_state.shm"; } virtual ~Robot() = default; virtual void bindMujoco(MujocoContext* mujContext) = 0; @@ -74,7 +76,7 @@ class Robot { // memory (SimulationThread::waitRobotConnectionsSHM(), the "shm" connect-mode // counterpart to the socket handshake in waitRobotConnections()). bool hasConnectSignal() const { - return std::filesystem::exists(connect_shm_path_); + return std::filesystem::exists(receive_shm_path); } virtual std::map getSensors() = 0; @@ -83,7 +85,9 @@ class Robot { private: virtual std::map packMessage() = 0; - std::string connect_shm_path_; + protected: + std::string receive_shm_path; + std::string send_shm_path; public: diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index a477ebc..fc88502 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -46,7 +46,7 @@ AppWindow::AppWindow(int& argc, char** argv) : QMainWindow() { else if (arg == "--paths" && i + 1 < argc) pathsConfigPath_ = argv[++i]; else if (arg == "--connect-mode" && i + 1 < argc) - connectMode_ = argv[++i]; + connectMode_ = argv[++i]; // TODO: forse si può aggiungere come variabile d'ambiente?? } resize(spqr::initialWindowWidth, spqr::initialWindowHeight); @@ -218,8 +218,7 @@ void AppWindow::loadScene(const QString& yaml_file) { // Docker bind mounts create missing host dirs as root, so remove and recreate if needed. if(connectMode_ == "shm") { - // TODO: il path sarebbe da mettere in config - const std::filesystem::path shmDir("/dev/shm/circus_ipc"); + const std::filesystem::path shmDir(sharedMemoryPath_); if (std::filesystem::exists(shmDir)) { std::filesystem::remove_all(shmDir); } @@ -233,26 +232,26 @@ void AppWindow::loadScene(const QString& yaml_file) { std::cout << "Simulation starting with SHM connection mode..." << std::endl; std::cout << "Connecting Robots..." << std::endl; - sim->waitRobotConnectionsSHM(); + RobotManager::instance().waitRobotConnectionsSHM(); std::cout << "Waiting Robots are Ready..." << std::endl; - sim->receiveCommandMessagesSHM(); + RobotManager::instance().receiveCommandMessagesSHM(); } else { // socket mode CircusNetwork::instance().init(); RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server - sim->initializeSocket(frameworkCommunicationPort); + RobotManager::instance().initializeSocket(frameworkCommunicationPort); std::cout << "Starting containers..." << std::endl; RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); - + std::cout << "Simulation starting with SOCKET connection mode..." << std::endl; std::cout << "Connecting Robots..." << std::endl; - sim->waitRobotConnectionsSocket(); + RobotManager::instance().waitRobotConnectionsSocket(); std::cout << "Waiting Robots are Ready..." << std::endl; - sim->receiveCommandMessagesSocket(); + RobotManager::instance().receiveCommandMessagesSocket(); } diff --git a/src/Container.cpp b/src/Container.cpp index 3e47127..5cee481 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -86,8 +86,8 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "XAUTHORITY=/root/.Xauthority", "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", - "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc", - "CONNECT_MODE=" + connectMode, + "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc", // TODO: vedere se questo si può rimpiazzare con la costante + "CONNECT_MODE=" + connectMode, // SHM: questo serve per simbridge, per sapere che modalità attivare "JOYSTICK_DEVICE=" + envOrDefault("JOYSTICK_DEVICE", "/dev/input/js0"), "INFERENCE_BACKEND=" + envOrDefault("INFERENCE_BACKEND", "trt"), "TASK_NAME=" + envOrDefault("TASK_NAME", "t1-velocity"), diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index fcf05b3..96453fc 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -1,6 +1,7 @@ #include "RobotManager.h" #include +#include #include "Constants.h" #include "Team.h" // needed for the forward declaration in the .h @@ -30,6 +31,27 @@ size_t RobotManager::count() const { return robots_.size(); } +void RobotManager::setRobotFd(const std::string& name, int fd) { + std::lock_guard lock(mutex_); + robotFdMap_[name] = fd; +} + +int RobotManager::getRobotFd(const std::string& name) const { + std::lock_guard lock(mutex_); + auto it = robotFdMap_.find(name); + return it != robotFdMap_.end() ? it->second : -1; +} + +void RobotManager::removeRobotFd(int fd) { + std::lock_guard lock(mutex_); + for (auto it = robotFdMap_.begin(); it != robotFdMap_.end(); ++it) { + if (it->second == fd) { + robotFdMap_.erase(it); + break; + } + } +} + void RobotManager::update() { std::lock_guard lock(mutex_); for (std::shared_ptr r : robots_) { @@ -47,6 +69,302 @@ void RobotManager::clear() { robots_.clear(); } +void RobotManager::sendStateMessages() { + for (auto& r : robots_) { + std::unique_lock lock(mutex_); + r->sendMessageSHM(); + } +} + +// Shared-memory counterpart to receiveCommandMessagesSocket(): same purpose (block +// until every robot has a fresh command applied, retrying with a periodic +// notice if one is stalled) but polling r->receiveMessageSHM() per robot +// instead of poll()/read()/msgpack-unpack on the socket. +void RobotManager::receiveCommandMessagesSHM() { + int robot_size = robots_.size(); + int done = 0; + std::set pendingRobots; + for (auto& r : robots_) + pendingRobots.insert(r->name); + + auto windowStart = std::chrono::steady_clock::now(); + while (done < robot_size) { + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (!pendingRobots.count(r->name)) + continue; + if (r->receiveMessageSHM()) { + if (!r->isReady) { + r->isReady = true; + std::cout << "Robot ready: " << r->name << std::endl; + if (areAllRobotsReady() && areAllRobotsReadyCallback_) { + areAllRobotsReadyCallback_(); + } + } + pendingRobots.erase(r->name); + ++done; + } + } + } + if (done < robot_size) { + if (std::chrono::steady_clock::now() - windowStart > std::chrono::milliseconds(2500)) { + // Re-send state to robots that haven't replied yet. Load-bearing for + // bootstrap: waitRobotConnections() sends state only once and run() + // (with its recurring sendStateMessages()) hasn't started yet, so + // without this retry a framework that subscribes late never receives + // state, never publishes a command, and startup deadlocks. + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (!pendingRobots.count(r->name)) + continue; + std::cout << "[receiveCommandMessagesSHM] Timeout, resending state to: " << r->name << std::endl; + r->sendMessageSHM(); + } + windowStart = std::chrono::steady_clock::now(); + } + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } + } +} + +void RobotManager::receiveCommandMessagesSocket() { + 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(pollFds_.data(), pollFds_.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 << "[receiveCommandMessagesSocket] Timeout, resending state to: " << r->name << std::endl; + // mutex_ is already held here, so look up the fd directly + // instead of going through getRobotFd() (which would try to + // re-lock the same non-recursive mutex_ and deadlock). + auto fdIt = robotFdMap_.find(r->name); + r->sendMessageSocket(fdIt != robotFdMap_.end() ? fdIt->second : -1); + r->sendMessageSHM(); + } + } + continue; + } + timeoutCount = 0; + + for (size_t i = 0; i < pollFds_.size(); ++i) { + // An event occured for the i-th fd + if (pollFds_[i].revents & POLLIN) { + int fd = pollFds_[i].fd; + msgpack::unpacker& unp = unpackers_[fd]; + unp.reserve_buffer(MAX_MSG_SIZE); + int n = read(fd, unp.buffer(), unp.buffer_capacity()); + if (n <= 0) { + close(fd); + pollFds_.erase(pollFds_.begin() + i); + --i; + unpackers_.erase(fd); + removeRobotFd(fd); + continue; + } + unp.buffer_consumed(static_cast(n)); + + // Drain every complete message currently buffered (a single + // read() can return several concatenated messages if we were + // slow to read), keeping only the latest one -- older ones + // queued up behind it are superseded and discarded on + // purpose, never applied. + msgpack::object_handle oh; + msgpack::object_handle latest; + bool gotOne = false; + while (unp.next(oh)) { + latest = std::move(oh); + gotOne = true; + } + if (!gotOne) + continue; // only a partial message so far; wait for more bytes + + auto data_map = latest.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 (areAllRobotsReady() && areAllRobotsReadyCallback_) { + areAllRobotsReadyCallback_(); + } + } + + r->receiveMessageSocket(data_map); + pendingRobots.erase(messageRecipient); + ++done; + break; + } + } + } + } + } + } +} + +void RobotManager::initializeSocket(int port) { + serverFd_ = socket(AF_INET, SOCK_STREAM, 0); + if (serverFd_ < 0) + throw std::runtime_error("Failed to create socket"); + + int opt = 1; + setsockopt(serverFd_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + int send_buf_size = 1 * 1024 * 1024; + if (setsockopt(serverFd_, 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(serverFd_, SOL_SOCKET, SO_RCVBUF, &recv_buf_size, sizeof(recv_buf_size)) < 0) { + perror("setsockopt(SO_RCVBUF)"); + } + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(port); + + if (bind(serverFd_, (struct sockaddr*)&address, sizeof(address)) < 0) { + perror("bind"); + throw std::runtime_error("Socket bind failed"); + } + if (listen(serverFd_, count()) < 0) + throw std::runtime_error("Listen failed"); + + pollFds_.push_back({serverFd_, POLLIN, 0}); +} + +void RobotManager::waitRobotConnectionsSocket() { + bool areAllConnected = false; + while (!areAllConnected) { + int ret = poll(pollFds_.data(), pollFds_.size(), 100); + if (ret <= 0) + continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is + + for (size_t i = 0; i < pollFds_.size(); ++i) { + // An event occured for the i-th fd + if (pollFds_[i].revents & POLLIN) { + if (pollFds_[i].fd == serverFd_) { + // The only event for the server is someone knocking + int client_fd = accept(serverFd_, nullptr, nullptr); + if (client_fd >= 0) { + pollFds_.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(); + setRobotFd(robotName, client_fd); + + // Answer the handshake with the initial state over shared memory. + // The socket is only used to receive the robot name above. + { + std::lock_guard lock(mutex_); + for (auto& r : robots_) { + if (r->name == robotName) { + r->isConnected = true; + r->sendMessageSocket(client_fd); + std::cout << "Connected Robot (socket): " << robotName << "\n"; + std::cout << "Published initial state to " << robotName << " via shared memory" << std::endl; + break; + } + } + } + if (areAllRobotsConnected()) { + areAllConnected = true; + break; + } + } + } + } + } + } + + std::cout << "All Robots are connected!" << std::endl; +} + +// "shm" connect-mode counterpart to waitRobotConnectionsSocket(): instead of accepting +// a socket connection and reading the robot's name off it, this polls each +// still-pending robot's Robot::hasConnectSignal() (the presence of its +// _commands.shm segment, created by simbridge's BridgeNode constructor +// before it would otherwise dial the socket -- see Robot::connect_shm_path_). +// Selected exclusively via the --connect-mode flag (AppWindow.cpp); never run +// together with waitRobotConnectionsSocket() in the same startup. +void RobotManager::waitRobotConnectionsSHM() { + std::set pendingRobots; + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) + if (!r->isConnected) + pendingRobots.insert(r->name); + } + + auto lastStatusLog = std::chrono::steady_clock::now(); + while (!pendingRobots.empty()) { + { + std::unique_lock lock(mutex_); + for (auto& r : robots_) { + if (pendingRobots.count(r->name) && r->hasConnectSignal()) { + r->isConnected = true; + r->sendMessageSHM(); + pendingRobots.erase(r->name); + std::cout << "Connected Robot (shm): " << r->name << "\n"; + } + } + } + if (pendingRobots.empty()) + break; + + if (std::chrono::steady_clock::now() - lastStatusLog > std::chrono::milliseconds(2500)) { + std::unique_lock lock(mutex_); + std::cout << "[waitRobotConnectionsSHM] still waiting for:"; + for (auto& n : pendingRobots) + std::cout << " " << n; + std::cout << std::endl; + lastStatusLog = std::chrono::steady_clock::now(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + std::cout << "All Robots are connected!" << std::endl; +} + void RobotManager::bindMujoco(MujocoContext* mujContext) { for (std::shared_ptr r : robots_) r->bindMujoco(mujContext); @@ -135,4 +453,9 @@ bool RobotManager::areAllRobotsReady() const { return true; } +void RobotManager::setAreAllRobotsReadyCallback(std::function cb) { + std::lock_guard lock(mutex_); + areAllRobotsReadyCallback_ = std::move(cb); +} + } // namespace spqr diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 186e497..bce67db 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -1,8 +1,6 @@ #include "SimulationThread.h" -#include #include -#include #include "GameController.h" #include "RobotManager.h" @@ -10,295 +8,7 @@ namespace spqr { SimulationThread::SimulationThread(const mjModel* model, mjData* data) : model_(model), data_(data), running_(true), paused_(false) { - robots_ = RobotManager::instance().getRobots(); -} - -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); - - - 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::receiveCommandMessagesSocket() { - 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 << "[receiveCommandMessagesSocket] Timeout, resending state to: " << r->name << std::endl; - r->sendMessageSocket(entity_fd_map[r->name]); - r->sendMessageSHM(); - } - } - 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) { - int fd = fds[i].fd; - msgpack::unpacker& unp = unpackers_[fd]; - unp.reserve_buffer(MAX_MSG_SIZE); - int n = read(fd, unp.buffer(), unp.buffer_capacity()); - if (n <= 0) { - close(fd); - fds.erase(fds.begin() + i); - --i; - unpackers_.erase(fd); - continue; - } - unp.buffer_consumed(static_cast(n)); - - // Drain every complete message currently buffered (a single - // read() can return several concatenated messages if we were - // slow to read), keeping only the latest one -- older ones - // queued up behind it are superseded and discarded on - // purpose, never applied. - msgpack::object_handle oh; - msgpack::object_handle latest; - bool gotOne = false; - while (unp.next(oh)) { - latest = std::move(oh); - gotOne = true; - } - if (!gotOne) - continue; // only a partial message so far; wait for more bytes - - auto data_map = latest.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->receiveMessageSocket(data_map); - pendingRobots.erase(messageRecipient); - ++done; - break; - } - } - } - } - } - } -} - -// Shared-memory counterpart to receiveCommandMessages(): same purpose (block -// until every robot has a fresh command applied, retrying with a periodic -// notice if one is stalled) but polling r->receiveMessageSHM() per robot -// instead of poll()/read()/msgpack-unpack on the socket. -void SimulationThread::receiveCommandMessagesSHM() { - int robot_size = robots_.size(); - int done = 0; - std::set pendingRobots; - for (auto& r : robots_) - pendingRobots.insert(r->name); - - auto windowStart = std::chrono::steady_clock::now(); - while (done < robot_size) { - { - std::unique_lock lock(mutex_); - for (auto& r : robots_) { - if (!pendingRobots.count(r->name)) - continue; - if (r->receiveMessageSHM()) { - if (!r->isReady) { - r->isReady = true; - std::cout << "Robot ready: " << r->name << std::endl; - if (RobotManager::instance().areAllRobotsReady()) { - emit allRobotsReadySignal(); - } - } - pendingRobots.erase(r->name); - ++done; - } - } - } - if (done < robot_size) { - if (std::chrono::steady_clock::now() - windowStart > std::chrono::milliseconds(2500)) { - // Re-send state to robots that haven't replied yet. Load-bearing for - // bootstrap: waitRobotConnections() sends state only once and run() - // (with its recurring sendStateMessages()) hasn't started yet, so - // without this retry a framework that subscribes late never receives - // state, never publishes a command, and startup deadlocks. - std::unique_lock lock(mutex_); - for (auto& r : robots_) { - if (!pendingRobots.count(r->name)) - continue; - std::cout << "[receiveCommandMessagesSHM] Timeout, resending state to: " << r->name << std::endl; - r->sendMessageSHM(); - } - windowStart = std::chrono::steady_clock::now(); - } - std::this_thread::sleep_for(std::chrono::microseconds(200)); - } - } -} - -void SimulationThread::waitRobotConnectionsSocket() { - 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; - - // Answer the handshake with the initial state over shared memory. - // The socket is only used to receive the robot name above. - { - std::lock_guard lock(mutex_); - for (auto& r : robots_) { - if (r->name == robotName) { - r->isConnected = true; - r->sendMessageSocket(client_fd); - std::cout << "Connected Robot (socket): " << robotName << "\n"; - std::cout << "Published initial state to " << robotName << " via shared memory" << std::endl; - break; - } - } - } - if (RobotManager::instance().areAllRobotsConnected()) { - areAllConnected = true; - break; - } - } - } - } - } - } - - std::cout << "All Robots are connected!" << std::endl; -} - -// "shm" connect-mode counterpart to waitRobotConnections(): instead of accepting -// a socket connection and reading the robot's name off it, this polls each -// still-pending robot's Robot::hasConnectSignal() (the presence of its -// _commands.shm segment, created by simbridge's BridgeNode constructor -// before it would otherwise dial the socket -- see Robot::connect_shm_path_). -// Selected exclusively via the --connect-mode flag (AppWindow.cpp); never run -// together with waitRobotConnections() in the same startup. -void SimulationThread::waitRobotConnectionsSHM() { - std::set pendingRobots; - { - std::unique_lock lock(mutex_); - for (auto& r : robots_) - if (!r->isConnected) - pendingRobots.insert(r->name); - } - - auto lastStatusLog = std::chrono::steady_clock::now(); - while (!pendingRobots.empty()) { - { - std::unique_lock lock(mutex_); - for (auto& r : robots_) { - if (pendingRobots.count(r->name) && r->hasConnectSignal()) { - r->isConnected = true; - r->sendMessageSHM(); - pendingRobots.erase(r->name); - std::cout << "Connected Robot (shm): " << r->name << "\n"; - } - } - } - if (pendingRobots.empty()) - break; - - if (std::chrono::steady_clock::now() - lastStatusLog > std::chrono::milliseconds(2500)) { - std::unique_lock lock(mutex_); - std::cout << "[waitRobotConnectionsSHM] still waiting for:"; - for (auto& n : pendingRobots) - std::cout << " " << n; - std::cout << std::endl; - lastStatusLog = std::chrono::steady_clock::now(); - } - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - std::cout << "All Robots are connected!" << std::endl; + RobotManager::instance().setAreAllRobotsReadyCallback([this]() { emit allRobotsReadySignal(); }); } /* @@ -390,12 +100,10 @@ void SimulationThread::run() { break; } - // TODO: sposta questo in RobotManager se possibile - sendStateMessages(); + RobotManager::instance().sendStateMessages(); auto receiveStart = clock::now(); - // TODO: forse pure questo si può spostare in RobotManager se possibile - receiveCommandMessagesSHM(); + RobotManager::instance().receiveCommandMessagesSHM(); double receiveElapsedMs = std::chrono::duration(clock::now() - receiveStart).count(); // std::cout << "[SimulationThread] receiveCommandMessagesSHM took " << receiveElapsedMs << " ms" << std::endl; @@ -426,13 +134,6 @@ void SimulationThread::run() { } } -void SimulationThread::sendStateMessages() { - for (auto& r : robots_) { - std::unique_lock lock(mutex_); - r->sendMessageSHM(); - } -} - void SimulationThread::stop() { running_ = false; wait(); From 0c895c681303c01fad51646334cb3f13f30e6dc0 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Mon, 24 Aug 2026 01:34:49 +0200 Subject: [PATCH 13/25] [refactoring] change name for SHM --- include/RobotManager.h | 2 +- src/RobotManager.cpp | 4 ++-- src/SimulationThread.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 1ed1574..9367281 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -49,7 +49,7 @@ class RobotManager { int getRobotFd(const std::string& name) const; // -1 se sconosciuto void removeRobotFd(int fd); - void sendStateMessages(); + void sendStateMessagesSHM(); void receiveCommandMessagesSHM(); void receiveCommandMessagesSocket(); diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 96453fc..48e97f8 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -69,7 +69,7 @@ void RobotManager::clear() { robots_.clear(); } -void RobotManager::sendStateMessages() { +void RobotManager::sendStateMessagesSHM() { for (auto& r : robots_) { std::unique_lock lock(mutex_); r->sendMessageSHM(); @@ -111,7 +111,7 @@ void RobotManager::receiveCommandMessagesSHM() { if (std::chrono::steady_clock::now() - windowStart > std::chrono::milliseconds(2500)) { // Re-send state to robots that haven't replied yet. Load-bearing for // bootstrap: waitRobotConnections() sends state only once and run() - // (with its recurring sendStateMessages()) hasn't started yet, so + // (with its recurring sendStateMessagesSHM()) hasn't started yet, so // without this retry a framework that subscribes late never receives // state, never publishes a command, and startup deadlocks. std::unique_lock lock(mutex_); diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index bce67db..33ffa48 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -100,7 +100,7 @@ void SimulationThread::run() { break; } - RobotManager::instance().sendStateMessages(); + RobotManager::instance().sendStateMessagesSHM(); auto receiveStart = clock::now(); RobotManager::instance().receiveCommandMessagesSHM(); From 6ec49e1db8c49f15b23fe5dfc05d74a773a7c620 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Mon, 24 Aug 2026 01:47:41 +0200 Subject: [PATCH 14/25] sendStateMessagesSocket added --- include/RobotManager.h | 1 + src/RobotManager.cpp | 13 ++++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 9367281..9cf4a8c 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -50,6 +50,7 @@ class RobotManager { void removeRobotFd(int fd); void sendStateMessagesSHM(); + void sendStateMessagesSocket(); void receiveCommandMessagesSHM(); void receiveCommandMessagesSocket(); diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 48e97f8..a61f13d 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -76,6 +76,17 @@ void RobotManager::sendStateMessagesSHM() { } } +void RobotManager::sendStateMessagesSocket() { + for (auto& r : robots_) { + std::unique_lock lock(mutex_); + // mutex_ is already held here, so look up the fd directly instead of + // going through getRobotFd() (which would try to re-lock the same + // non-recursive mutex_ and deadlock). + auto fdIt = robotFdMap_.find(r->name); + r->sendMessageSocket(fdIt != robotFdMap_.end() ? fdIt->second : -1); + } +} + // Shared-memory counterpart to receiveCommandMessagesSocket(): same purpose (block // until every robot has a fresh command applied, retrying with a periodic // notice if one is stalled) but polling r->receiveMessageSHM() per robot @@ -153,7 +164,7 @@ void RobotManager::receiveCommandMessagesSocket() { // re-lock the same non-recursive mutex_ and deadlock). auto fdIt = robotFdMap_.find(r->name); r->sendMessageSocket(fdIt != robotFdMap_.end() ? fdIt->second : -1); - r->sendMessageSHM(); + // r->sendMessageSHM(); } } continue; From 26c98e8ddc366c781115b097d5396c9ba22fd157 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Mon, 31 Aug 2026 01:15:08 +0200 Subject: [PATCH 15/25] [refactoring] private functions + generalization MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adesso le due modalità shm e socket sono entrambe funzionanti. Reso alcune funzioni private per rendere codice ad alto livello più pulito --- include/RobotManager.h | 23 ++++++++------ include/robots/Robot.h | 9 +----- src/AppWindow.cpp | 29 ++++++------------ src/RobotManager.cpp | 65 ++++++++++++++++++++++++---------------- src/SimulationThread.cpp | 38 +++++++++-------------- 5 files changed, 77 insertions(+), 87 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 9cf4a8c..0476326 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -49,14 +49,11 @@ class RobotManager { int getRobotFd(const std::string& name) const; // -1 se sconosciuto void removeRobotFd(int fd); - void sendStateMessagesSHM(); - void sendStateMessagesSocket(); - void receiveCommandMessagesSHM(); - void receiveCommandMessagesSocket(); + void sendStateMessages(); + void receiveCommandMessages(); + void waitRobotConnections(); - void initializeSocket(int port); - void waitRobotConnectionsSocket(); - void waitRobotConnectionsSHM(); + void initializeSocket(int port); bool areAllRobotsReady() const; bool areAllRobotsConnected() const; @@ -80,8 +77,16 @@ class RobotManager { RobotManager(const RobotManager&) = delete; RobotManager& operator=(const RobotManager&) = delete; - std::atomic serverRunning_ = false; - std::thread serverThread_; + void sendStateMessagesSHM(); + void sendStateMessagesSocket(); + + void receiveCommandMessagesSHM(); + void receiveCommandMessagesSocket(); + + void waitRobotConnectionsSocket(); + void waitRobotConnectionsSHM(); + + std::string connectMode_; mutable std::mutex mutex_; std::vector> robots_; diff --git a/include/robots/Robot.h b/include/robots/Robot.h index ae08485..e854d8a 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -62,20 +62,13 @@ class Robot { virtual void receiveMessageSocket(const std::map& message) = 0; - // Publishes this robot's per-tick state to shared memory, for the case where - // circus and simbridge run on the same machine. Mirrors packMessage()'s role - // for the socket path; a robot that only needs the socket path implements - // this with an empty body. virtual void sendMessageSHM() = 0; - // Reads and applies the latest command from shared memory, if a new one has - // arrived since the last call. Returns true iff a new command was applied - // this call. Mirrors receiveMessageSocket()'s role for the socket path. virtual bool receiveMessageSHM() = 0; // True once this robot's simbridge process has announced itself via shared // memory (SimulationThread::waitRobotConnectionsSHM(), the "shm" connect-mode // counterpart to the socket handshake in waitRobotConnections()). - bool hasConnectSignal() const { + bool hasConnectSignalSHM() const { return std::filesystem::exists(receive_shm_path); } diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index fc88502..a5f83f7 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -227,34 +227,23 @@ void AppWindow::loadScene(const QString& yaml_file) { CircusNetwork::instance().init(); RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server - std::cout << "Starting containers..." << std::endl; - RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); - - std::cout << "Simulation starting with SHM connection mode..." << std::endl; - std::cout << "Connecting Robots..." << std::endl; - RobotManager::instance().waitRobotConnectionsSHM(); - - std::cout << "Waiting Robots are Ready..." << std::endl; - RobotManager::instance().receiveCommandMessagesSHM(); } else { // socket mode CircusNetwork::instance().init(); RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server RobotManager::instance().initializeSocket(frameworkCommunicationPort); - - std::cout << "Starting containers..." << std::endl; - RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); - - - std::cout << "Simulation starting with SOCKET connection mode..." << std::endl; - std::cout << "Connecting Robots..." << std::endl; - RobotManager::instance().waitRobotConnectionsSocket(); - std::cout << "Waiting Robots are Ready..." << std::endl; - RobotManager::instance().receiveCommandMessagesSocket(); - } + std::cout << "Starting containers..." << std::endl; + RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); + + std::cout << "Simulation starting with " << connectMode_ << " connection mode..." << std::endl; + std::cout << "Connecting Robots..." << std::endl; + RobotManager::instance().waitRobotConnections(); + + std::cout << "Waiting Robots are Ready..." << std::endl; + RobotManager::instance().receiveCommandMessages(); // Set initial simulation state (playing when scene is loaded) toolsPanel->setSimulationPlaying(true); diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index a61f13d..9b4958d 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -4,7 +4,7 @@ #include #include "Constants.h" -#include "Team.h" // needed for the forward declaration in the .h +#include "Team.h" #include "Utils.h" namespace spqr { @@ -69,6 +69,29 @@ void RobotManager::clear() { robots_.clear(); } +void RobotManager::sendStateMessages() { + if (connectMode_ == "shm") { + // std::cout << "[sendStateMessages] sending message with SHM" << std::endl; + sendStateMessagesSHM(); + } + else { + // std::cout << "[sendStateMessages] sending message with Socket" << std::endl; + sendStateMessagesSocket(); + } +} + +void RobotManager::receiveCommandMessages() { + if (connectMode_ == "shm") { + // std::cout << "[receiveCommandMessages] receiving message with SHM" << std::endl; + receiveCommandMessagesSHM(); + } + else { + // std::cout << "[receiveCommandMessages] receiving message with Socket" << std::endl; + receiveCommandMessagesSocket(); + } +} + + void RobotManager::sendStateMessagesSHM() { for (auto& r : robots_) { std::unique_lock lock(mutex_); @@ -79,18 +102,11 @@ void RobotManager::sendStateMessagesSHM() { void RobotManager::sendStateMessagesSocket() { for (auto& r : robots_) { std::unique_lock lock(mutex_); - // mutex_ is already held here, so look up the fd directly instead of - // going through getRobotFd() (which would try to re-lock the same - // non-recursive mutex_ and deadlock). auto fdIt = robotFdMap_.find(r->name); r->sendMessageSocket(fdIt != robotFdMap_.end() ? fdIt->second : -1); } } -// Shared-memory counterpart to receiveCommandMessagesSocket(): same purpose (block -// until every robot has a fresh command applied, retrying with a periodic -// notice if one is stalled) but polling r->receiveMessageSHM() per robot -// instead of poll()/read()/msgpack-unpack on the socket. void RobotManager::receiveCommandMessagesSHM() { int robot_size = robots_.size(); int done = 0; @@ -120,11 +136,6 @@ void RobotManager::receiveCommandMessagesSHM() { } if (done < robot_size) { if (std::chrono::steady_clock::now() - windowStart > std::chrono::milliseconds(2500)) { - // Re-send state to robots that haven't replied yet. Load-bearing for - // bootstrap: waitRobotConnections() sends state only once and run() - // (with its recurring sendStateMessagesSHM()) hasn't started yet, so - // without this retry a framework that subscribes late never receives - // state, never publishes a command, and startup deadlocks. std::unique_lock lock(mutex_); for (auto& r : robots_) { if (!pendingRobots.count(r->name)) @@ -159,12 +170,9 @@ void RobotManager::receiveCommandMessagesSocket() { for (auto& r : robots_) { if (pendingRobots.count(r->name)) { std::cout << "[receiveCommandMessagesSocket] Timeout, resending state to: " << r->name << std::endl; - // mutex_ is already held here, so look up the fd directly - // instead of going through getRobotFd() (which would try to - // re-lock the same non-recursive mutex_ and deadlock). + auto fdIt = robotFdMap_.find(r->name); r->sendMessageSocket(fdIt != robotFdMap_.end() ? fdIt->second : -1); - // r->sendMessageSHM(); } } continue; @@ -265,6 +273,15 @@ void RobotManager::initializeSocket(int port) { pollFds_.push_back({serverFd_, POLLIN, 0}); } +void RobotManager::waitRobotConnections() { + if(connectMode_ == "shm") { + waitRobotConnectionsSHM(); + } + else { + waitRobotConnectionsSocket(); + } +} + void RobotManager::waitRobotConnectionsSocket() { bool areAllConnected = false; while (!areAllConnected) { @@ -313,7 +330,7 @@ void RobotManager::waitRobotConnectionsSocket() { r->isConnected = true; r->sendMessageSocket(client_fd); std::cout << "Connected Robot (socket): " << robotName << "\n"; - std::cout << "Published initial state to " << robotName << " via shared memory" << std::endl; + std::cout << "Published initial state to " << robotName << " via socket" << std::endl; break; } } @@ -331,13 +348,6 @@ void RobotManager::waitRobotConnectionsSocket() { std::cout << "All Robots are connected!" << std::endl; } -// "shm" connect-mode counterpart to waitRobotConnectionsSocket(): instead of accepting -// a socket connection and reading the robot's name off it, this polls each -// still-pending robot's Robot::hasConnectSignal() (the presence of its -// _commands.shm segment, created by simbridge's BridgeNode constructor -// before it would otherwise dial the socket -- see Robot::connect_shm_path_). -// Selected exclusively via the --connect-mode flag (AppWindow.cpp); never run -// together with waitRobotConnectionsSocket() in the same startup. void RobotManager::waitRobotConnectionsSHM() { std::set pendingRobots; { @@ -352,7 +362,7 @@ void RobotManager::waitRobotConnectionsSHM() { { std::unique_lock lock(mutex_); for (auto& r : robots_) { - if (pendingRobots.count(r->name) && r->hasConnectSignal()) { + if (pendingRobots.count(r->name) && r->hasConnectSignalSHM()) { r->isConnected = true; r->sendMessageSHM(); pendingRobots.erase(r->name); @@ -394,6 +404,9 @@ std::shared_ptr RobotManager::create(const std::string& name, const std:: } void RobotManager::startContainers(const std::string& fwkCfgPath, const std::string& pathsCfgPath, const std::string& connectMode) { + // Save connection mode + connectMode_ = connectMode; + YAML::Node configRoot = loadYamlFile(fwkCfgPath.c_str()); if (!configRoot["image"]) diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 33ffa48..7881ffd 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -66,14 +66,12 @@ void SimulationThread::run() { // N.B.3: il bottleneck al momento sembra essere sendStateMessages+receiveCommandMessages; aumentando il numero di kControlDecimation steps, // ovviamente il sistema rallenta. Quello che non deve succedere è quello spiegato in N.B.1 // - // TODO: il problema da risolvere è trovare la combinazione giusta di parametri. Probabilmente una configurazione che vada sempre bene non c'è, - // anche perché dipendente dal numero di robot simulati e dal pc utilizzato. - // Idee per risolvere questo problema: - // - abbassare la frequenza del nodo di Brain (se la policy non dà problemi), - // - alzare il sim_dt (fino a un certo punto, altrimenti lo step di integrazione diventa una cattiva approssimazione) - // per abbassare kControlDecimation - // - entrambe, cioè trovare una formulazione adattiva che possa conciliare tutti questi aspetti - // + // NOTE: con la shm funziona più o meno in real-time. Usando la socket, conviene impostare il seguente profilo al momento: + // constexpr int kControlDecimation = 5; + // constexpr double kTimestepPolicy = 0.2; + // timestep (in SceneParser) = 0.004; + // update_rate (nodo Brain)= 5; + double sim_dt = model_->opt.timestep; constexpr int kControlDecimation = 10; @@ -84,7 +82,9 @@ void SimulationThread::run() { auto next_step_time = clock::now(); while (running_) { if (!paused_) { - auto stepsStart = clock::now(); + // DEBUG + // auto stepsStart = clock::now(); + for(int i=0; i(clock::now() - receiveStart).count(); - // std::cout << "[SimulationThread] receiveCommandMessagesSHM took " << receiveElapsedMs << " ms" << std::endl; } - // DEBUG: real time spent computing kControlDecimation physics - // steps, vs. the sim-time budget they represent. - double stepsElapsedMs = std::chrono::duration(clock::now() - stepsStart).count(); - double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; + // DEBUG: real time spent computing kControlDecimation physics steps, vs. the sim-time budget they represent. + // double stepsElapsedMs = std::chrono::duration(clock::now() - stepsStart).count(); + // double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; // std::cout << "[SimulationThread] " << kControlDecimation << " physics steps took " // << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; - // for each robot: - // send(state) // non-blocking - // recv(torques) // bloccante o con timeout - - next_step_time += std::chrono::duration_cast(std::chrono::duration(kTimestepPolicy)); std::this_thread::sleep_until(next_step_time); From 3ab52d144b587b2798a7ddf4dea5d64f421133d6 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Tue, 1 Sep 2026 00:57:03 +0200 Subject: [PATCH 16/25] merge of main into shm (something to be adjusted) --- include/frontend/tools_panel/ToolsPanelGrid.h | 14 +-- include/robots/BoosterT1.h | 20 ++++- include/robots/Robot.h | 9 +- include/sensors/CameraDepth.h | 17 ++-- include/sensors/CameraInfo.h | 85 +++++++++++++++++++ include/sensors/GroundRelativePosition.h | 67 +++++++++++++++ resources/robots/Booster-T1/instance.xml | 18 ++-- src/RobotManager.cpp | 3 +- src/SceneParser.cpp | 7 +- src/SimulationThread.cpp | 10 +-- 10 files changed, 217 insertions(+), 33 deletions(-) create mode 100644 include/sensors/CameraInfo.h create mode 100644 include/sensors/GroundRelativePosition.h diff --git a/include/frontend/tools_panel/ToolsPanelGrid.h b/include/frontend/tools_panel/ToolsPanelGrid.h index c50f49d..3c47740 100644 --- a/include/frontend/tools_panel/ToolsPanelGrid.h +++ b/include/frontend/tools_panel/ToolsPanelGrid.h @@ -485,7 +485,7 @@ class ToolsPanelGrid : public QWidget { auto it = sensors.find("joints"); if (it != sensors.end()) { Sensor* jointsSensor = it->second; - Eigen::Vector3d position = dynamic_cast(jointsSensor)->getPosition(); + Eigen::VectorXd position = dynamic_cast(jointsSensor)->getPosition(); plot->addDataPoint("head_yaw", position(0), simTime); plot->addDataPoint("head_pitch", position(1), simTime); plot->addDataPoint("shoulder_left_pitch", position(2), simTime); @@ -514,7 +514,7 @@ class ToolsPanelGrid : public QWidget { auto it = sensors.find("joints"); if (it != sensors.end()) { Sensor* jointsSensor = it->second; - Eigen::Vector3d velocity = dynamic_cast(jointsSensor)->getVelocity(); + Eigen::VectorXd velocity = dynamic_cast(jointsSensor)->getVelocity(); plot->addDataPoint("head_yaw", velocity(0), simTime); plot->addDataPoint("head_pitch", velocity(1), simTime); plot->addDataPoint("shoulder_left_pitch", velocity(2), simTime); @@ -543,7 +543,7 @@ class ToolsPanelGrid : public QWidget { auto it = sensors.find("joints"); if (it != sensors.end()) { Sensor* jointsSensor = it->second; - Eigen::Vector3d acceleration = dynamic_cast(jointsSensor)->getAcceleration(); + Eigen::VectorXd acceleration = dynamic_cast(jointsSensor)->getAcceleration(); plot->addDataPoint("head_yaw", acceleration(0), simTime); plot->addDataPoint("head_pitch", acceleration(1), simTime); plot->addDataPoint("shoulder_left_pitch", acceleration(2), simTime); @@ -572,7 +572,7 @@ class ToolsPanelGrid : public QWidget { auto it = sensors.find("joints"); if (it != sensors.end()) { Sensor* jointsSensor = it->second; - Eigen::Vector3d torque = dynamic_cast(jointsSensor)->getTorque(); + Eigen::VectorXd torque = dynamic_cast(jointsSensor)->getTorque(); plot->addDataPoint("head_yaw", torque(0), simTime); plot->addDataPoint("head_pitch", torque(1), simTime); plot->addDataPoint("shoulder_left_pitch", torque(2), simTime); @@ -655,9 +655,13 @@ class ToolsPanelGrid : public QWidget { int height = depthCamera->getHeight(); if (!depthData.empty() && width > 0 && height > 0) { + // Depth is uint16 millimetres; map the near 10 m to the + // full 8-bit range so the preview keeps usable contrast + constexpr uint32_t kPreviewRangeMm = 10000; std::vector depthImage(depthData.size()); for (size_t i = 0; i < depthData.size(); ++i) { - depthImage[i] = static_cast(depthData[i] / 256); // Scale 16-bit to 8-bit + const uint32_t scaled = static_cast(depthData[i]) * 255u / kPreviewRangeMm; + depthImage[i] = static_cast(scaled > 255u ? 255u : scaled); } imageTool->setImage(depthImage.data(), width, height, 1); } else { diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 2c79d75..7c298c0 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -21,7 +22,9 @@ #include "ipc/SharedMemoryWriter.h" #include "robots/Robot.h" #include "sensors/CameraDepth.h" +#include "sensors/CameraInfo.h" #include "sensors/CameraRGB.h" +#include "sensors/GroundRelativePosition.h" #include "sensors/Imu.h" #include "sensors/Joint.h" #include "sensors/Oracle.h" @@ -67,11 +70,13 @@ struct BoosterT1StateMeta { class BoosterT1 : public Robot { public: Pose* pose = nullptr; + GroundRelativePosition* headPose = nullptr; Imu* imu = nullptr; Joints* joints = nullptr; Oracle* oracle = nullptr; CameraRGB* rgbCamera; CameraDepth* depthCamera; + CameraInfo* rgbCameraInfo = nullptr; BoosterT1(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team) @@ -105,6 +110,8 @@ class BoosterT1 : public Robot { void bindMujoco(MujocoContext* mujCtx) override { pose = new Pose(mujCtx->model, mujCtx->data, (name + "_position").c_str(), (name + "_orientation").c_str()); + headPose = new GroundRelativePosition(mujCtx->model, mujCtx->data, (name + "_head_rgb_cam_site").c_str(), + GroundRelativePosition::TargetType::Site, pose); imu = new Imu(mujCtx->model, mujCtx->data, (name + "_linear-acceleration").c_str(), (name + "_angular-velocity").c_str()); joints = new Joints(mujCtx->model, mujCtx->data, joint_map); @@ -136,6 +143,7 @@ class BoosterT1 : public Robot { // 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()); + rgbCameraInfo = new CameraInfo(mujCtx->model, (name + "_rgb_cam").c_str()); // State and both camera frames share a single segment. The frames are // runtime-sized (resolution comes from the MuJoCo model) but fixed once @@ -144,10 +152,10 @@ class BoosterT1 : public Robot { const uint32_t width = static_cast(rgbCamera->getWidth()); const uint32_t height = static_cast(rgbCamera->getHeight()); const size_t rgbBytes = static_cast(width) * height * 3; - const size_t depthBytes = static_cast(width) * height * 1; + const size_t depthBytes = static_cast(width) * height * 2; state_writer_.configure(send_shm_path, rgbBytes + depthBytes, BoosterT1StateMeta{kBoosterT1SchemaId, static_cast(sizeof(BoosterT1SharedState)), - ImageMeta{width, height, 3}, ImageMeta{width, height, 1}}); + ImageMeta{width, height, 3}, ImageMeta{width, height, 2}}); command_reader_.configure(receive_shm_path); // Create Oracle with the pose and all robots @@ -196,9 +204,11 @@ class BoosterT1 : public Robot { std::map msg; msg["robot_name"] = msgpack::object(name, buffer_zone_); msg["pose"] = pose->serialize(buffer_zone_); + msg["head_pose"] = headPose->serialize(buffer_zone_); msg["imu"] = imu->serialize(buffer_zone_); msg["joints"] = joints->serialize(buffer_zone_); msg["oracle"] = oracle->serialize(buffer_zone_); + msg["camera_info"] = rgbCameraInfo->serialize(buffer_zone_); return msg; } @@ -209,7 +219,7 @@ class BoosterT1 : public Robot { // different tick -- which matters as soon as anything unprojects an image // detection using the pose at capture time. const auto& rgbFrame = rgbCamera->getImage(); - const auto& depthFrame = depthCamera->getDepth8bit(); + const auto& depthFrame = depthCamera->getDepth16UC1(); state_writer_.write(BoosterT1SharedState{pose->toSharedState(), imu->toSharedState(), joints->toSharedState(), oracle->toSharedState()}, {{rgbFrame.data(), rgbFrame.size()}, {depthFrame.data(), depthFrame.size()}}); @@ -218,10 +228,12 @@ class BoosterT1 : public Robot { std::map getSensors() override { std::map sensors; sensors["pose"] = pose; + sensors["head_pose"] = headPose; sensors["imu"] = imu; sensors["joints"] = joints; sensors["rgb_camera"] = rgbCamera; sensors["depth_camera"] = depthCamera; + sensors["camera_info"] = rgbCameraInfo; return sensors; } @@ -232,11 +244,13 @@ class BoosterT1 : public Robot { void update() override { pose->update(); + headPose->update(); imu->update(); joints->update(); oracle->update(); rgbCamera->update(); depthCamera->update(); + rgbCameraInfo->update(); } ~BoosterT1() = default; diff --git a/include/robots/Robot.h b/include/robots/Robot.h index e854d8a..de14ab8 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -31,7 +31,14 @@ class Robot { Robot(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition, const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr& team, const std::string& role = "Striker") - : name(name), type(type), number(number), initPosition(initPosition), initOrientation(initOrientation), colorName(colorName), team(team), role(role) { + : name(name), + type(type), + number(number), + initPosition(initPosition), + initOrientation(initOrientation), + colorName(colorName), + team(team), + role(role) { if (colorName == "red") { color = {130, 36, 51}; } else if (colorName == "blue") { diff --git a/include/sensors/CameraDepth.h b/include/sensors/CameraDepth.h index 2f802c1..d561d06 100644 --- a/include/sensors/CameraDepth.h +++ b/include/sensors/CameraDepth.h @@ -88,7 +88,9 @@ class CameraDepth : public Sensor { 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. + // Depth is stored as uint16 millimetres, the ROS 16UC1 convention consumers + // expect (raw / 1000 = metres). Saturates at 65535 mm ~= 65 m. + constexpr float kMillimetresPerMetre = 1000.0f; float max_u16 = static_cast(std::numeric_limits::max()); // Resample offscreen depth to camera resolution and convert to metric depth. @@ -104,8 +106,8 @@ class CameraDepth : public Sensor { float z_converted = (znear * zfar) / (zfar - z_raw * (zfar - znear)); depthNormalized[dstRow + x] = z_converted; - float normalizedDepth = std::clamp(z_converted / kDepthMaxMeters, 0.0f, 1.0f); - depth[dstRow + x] = static_cast(normalizedDepth * max_u16); + const float depthMillimetres = std::clamp(z_converted * kMillimetresPerMetre, 0.0f, max_u16); + depth[dstRow + x] = static_cast(depthMillimetres); } } @@ -134,13 +136,14 @@ class CameraDepth : public Sensor { return depth; } - std::vector getDepth8bit() const { + std::vector getDepth16UC1() const { std::lock_guard lock(depthMutex_); - std::vector depth8(depth.size()); + std::vector bytes(depth.size() * sizeof(uint16_t)); for (size_t i = 0; i < depth.size(); ++i) { - depth8[i] = static_cast(depth[i] >> 8); // Convert uint16 to uint8 + bytes[i * 2 + 0] = static_cast(depth[i] & 0xFF); + bytes[i * 2 + 1] = static_cast((depth[i] >> 8) & 0xFF); } - return depth8; + return bytes; } int getWidth() const { diff --git a/include/sensors/CameraInfo.h b/include/sensors/CameraInfo.h new file mode 100644 index 0000000..7cee4b5 --- /dev/null +++ b/include/sensors/CameraInfo.h @@ -0,0 +1,85 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "sensors/Sensor.h" + +namespace spqr { + +// Intrinsics parameters of MuJoCo camera. +// Values are derived once from cam_fovy/cam_resolution (ideal pinhole, no distortion, +// since MuJoCo's OpenGL renderer doesn't model lens distortion) and never change at runtime. +class CameraInfo : public Sensor { + public: + CameraInfo(mjModel* mujModel, const char* cameraName) { + int camId = mj_name2id(mujModel, mjOBJ_CAMERA, cameraName); + if (camId < 0) + throw std::runtime_error(std::string("Camera not found: ") + cameraName); + + width_ = mujModel->cam_resolution[2 * camId + 0]; + height_ = mujModel->cam_resolution[2 * camId + 1]; + + double fovyRad = mujModel->cam_fovy[camId] * M_PI / 180.0; + + fx_ = (height_ / 2.0) / std::tan(fovyRad / 2.0); + fy_ = fx_; + cx_ = width_ / 2.0; + cy_ = height_ / 2.0; + } + + void doUpdate() override {} + + msgpack::object doSerialize(msgpack::zone& z) override { + std::map data; + data["height"] = msgpack::object(height_, z); + data["width"] = msgpack::object(width_, z); + data["distortion_model"] = msgpack::object(std::string("plumb_bob"), z); + data["d"] = msgpack::object(std::vector{0.0, 0.0, 0.0, 0.0, 0.0}, z); + data["k"] = msgpack::object(std::vector{fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0}, z); + data["r"] = msgpack::object(std::vector{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}, z); + data["p"] = msgpack::object(std::vector{fx_, 0.0, cx_, 0.0, 0.0, fy_, cy_, 0.0, 0.0, 0.0, 1.0, 0.0}, z); + data["binning_x"] = msgpack::object(0, z); + data["binning_y"] = msgpack::object(0, z); + + std::map roi; + roi["x_offset"] = msgpack::object(0, z); + roi["y_offset"] = msgpack::object(0, z); + roi["height"] = msgpack::object(0, z); + roi["width"] = msgpack::object(0, z); + roi["do_rectify"] = msgpack::object(false, z); + data["roi"] = msgpack::object(roi, z); + + return msgpack::object(data, z); + } + + int getWidth() const { + return width_; + } + int getHeight() const { + return height_; + } + double getFx() const { + return fx_; + } + double getFy() const { + return fy_; + } + double getCx() const { + return cx_; + } + double getCy() const { + return cy_; + } + + private: + int width_, height_; + double fx_, fy_, cx_, cy_; +}; + +} // namespace spqr diff --git a/include/sensors/GroundRelativePosition.h b/include/sensors/GroundRelativePosition.h new file mode 100644 index 0000000..466116d --- /dev/null +++ b/include/sensors/GroundRelativePosition.h @@ -0,0 +1,67 @@ +#pragma once + +#include + +#include +#include + +#include "sensors/Pose.h" +#include "sensors/Sensor.h" + +namespace spqr { + +// Position of a body or site relative to the ground projection of a reference Pose. +// Reference frame: (trunk_x, trunk_y, 0) with yaw-only rotation of the trunk. +class GroundRelativePosition : public Sensor { + public: + enum class TargetType { Body, Site }; + + GroundRelativePosition(mjModel* mujModel, mjData* mujData, const char* name, TargetType type, Pose* referencePose) + : mujData_(mujData), type_(type), referencePose_(referencePose) { + if (type_ == TargetType::Site) + targetId_ = mj_name2id(mujModel, mjOBJ_SITE, name); + else + targetId_ = mj_name2id(mujModel, mjOBJ_BODY, name); + } + + void doUpdate() override { + Eigen::Vector3d targetPos; + if (type_ == TargetType::Site) + targetPos = Eigen::Vector3d(mujData_->site_xpos[3 * targetId_], mujData_->site_xpos[3 * targetId_ + 1], + mujData_->site_xpos[3 * targetId_ + 2]); + else + targetPos = Eigen::Vector3d(mujData_->xpos[3 * targetId_], mujData_->xpos[3 * targetId_ + 1], mujData_->xpos[3 * targetId_ + 2]); + + Eigen::Vector4d refQuatVec = referencePose_->getQuatOrientation(); + Eigen::Quaterniond refQuat(refQuatVec(0), refQuatVec(1), refQuatVec(2), refQuatVec(3)); + Eigen::Vector3d refPos = referencePose_->getPosition(); + + double w = refQuat.w(), x = refQuat.x(), y = refQuat.y(), z = refQuat.z(); + double yaw = std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)); + Eigen::Quaterniond groundQuat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + Eigen::Vector3d groundPos(refPos.x(), refPos.y(), 0.0); + + position_ = groundQuat.conjugate() * (targetPos - groundPos); + } + + msgpack::object doSerialize(msgpack::zone& z) override { + std::vector position_vec = {position_(0), position_(1), position_(2)}; + std::map data; + data["position"] = msgpack::object(position_vec, z); + return msgpack::object(data, z); + } + + Eigen::Vector3d getPosition() const { + return position_; + } + + private: + mjData* mujData_; + int targetId_; + TargetType type_; + Pose* referencePose_; + + Eigen::Vector3d position_; +}; + +} // namespace spqr diff --git a/resources/robots/Booster-T1/instance.xml b/resources/robots/Booster-T1/instance.xml index a429646..aa1b281 100644 --- a/resources/robots/Booster-T1/instance.xml +++ b/resources/robots/Booster-T1/instance.xml @@ -34,13 +34,15 @@ - - - - - - - + + + + + + + + + @@ -164,7 +166,7 @@ - + true diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 9b4958d..20d8ac8 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -397,7 +397,8 @@ std::shared_ptr RobotManager::create(const std::string& name, const std:: auto it = robotFactory.find(type); if (it != robotFactory.end()) { auto robot = it->second(name, type, number, pos, ori, colorName, team); - if (robot) robot->role = role; + if (robot) + robot->role = role; return robot; } return nullptr; diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index 48f79c1..aa76f8d 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -133,9 +133,10 @@ SceneParser::SceneParser(const string& yamlPath) { uint8_t robotNumber = robotNode["number"].as(); string robotName = robotNode["name"] ? robotNode["name"].as() : teamName + "_" + robotType + "_" + to_string(typeIndex++); // Role is taken from the scene if present; otherwise fall back to a number-based default. - string robotRole = robotNode["role"] - ? robotNode["role"].as() - : (robotNumber == 1) ? "Goalkeeper" : (robotNumber == 2) ? "Defender" : "Striker"; + string robotRole = robotNode["role"] ? robotNode["role"].as() : + (robotNumber == 1) ? "Goalkeeper" : + (robotNumber == 2) ? "Defender" : + "Striker"; Vector3d pos = Vector3d::Zero(); Vector3d ori = Vector3d::Zero(); diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 7881ffd..eff6530 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -83,7 +83,7 @@ void SimulationThread::run() { while (running_) { if (!paused_) { // DEBUG - // auto stepsStart = clock::now(); + auto stepsStart = clock::now(); for(int i=0; i(clock::now() - stepsStart).count(); - // double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; - // std::cout << "[SimulationThread] " << kControlDecimation << " physics steps took " - // << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; + double stepsElapsedMs = std::chrono::duration(clock::now() - stepsStart).count(); + double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; + std::cout << "[SimulationThread] " << kControlDecimation << " physics steps took " + << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; next_step_time += std::chrono::duration_cast(std::chrono::duration(kTimestepPolicy)); std::this_thread::sleep_until(next_step_time); From 7daf4b4b3e6c65d94a5eede49ae3c1b9f855c138 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 4 Sep 2026 01:48:19 +0200 Subject: [PATCH 17/25] reduced loop time separating images from state MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adesso con shm riusciamo a stare in realtime senza problemi (almeno con un robot solo). Ho separato l'invio e ricezione delle immagini dallo stato, poiché non serve inviarle ad ogni ciclo di update di decimazione, ma una sola volta per ciclo di running. Questo fa risparmiare parecchio tempo. Va refactorato un po' --- include/RobotManager.h | 6 ++- include/ipc/SharedMemoryReader.h | 46 +++++----------- include/ipc/SharedMemoryWriter.h | 91 ++++++++++---------------------- include/robots/BoosterK1.h | 2 +- include/robots/BoosterT1.h | 72 ++++++++++++++++--------- include/robots/Robot.h | 6 ++- src/RobotManager.cpp | 12 ++--- src/SimulationThread.cpp | 18 ++++--- 8 files changed, 116 insertions(+), 137 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 0476326..c9372a2 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -49,7 +49,9 @@ class RobotManager { int getRobotFd(const std::string& name) const; // -1 se sconosciuto void removeRobotFd(int fd); - void sendStateMessages(); + // publishImages: forwarded to each robot's sendMessageSHM() (ignored in + // socket mode, which never sends images) -- see Robot::sendMessageSHM(). + void sendStateMessages(bool publishImages); void receiveCommandMessages(); void waitRobotConnections(); @@ -77,7 +79,7 @@ class RobotManager { RobotManager(const RobotManager&) = delete; RobotManager& operator=(const RobotManager&) = delete; - void sendStateMessagesSHM(); + void sendStateMessagesSHM(bool publishImages); void sendStateMessagesSocket(); void receiveCommandMessagesSHM(); diff --git a/include/ipc/SharedMemoryReader.h b/include/ipc/SharedMemoryReader.h index 57cda0a..efc44f6 100644 --- a/include/ipc/SharedMemoryReader.h +++ b/include/ipc/SharedMemoryReader.h @@ -10,21 +10,20 @@ #include #include -#include "ipc/SharedMemoryWriter.h" // for NoSharedMemoryMeta +#include "ipc/SharedMemoryWriter.h" // for NoSharedMemoryMeta, ImageMeta namespace spqr { -// Reader counterpart to SharedMemoryWriter (external/circus/include/ipc/SharedMemoryWriter.h). +// Reader counterpart to SharedMemoryWriter (external/circus/include/ipc/SharedMemoryWriter.h). // Used when circus is the *reader* (e.g. commands written by simbridge). Must // stay in sync with whatever writes the segment: same Header layout, same // magic/version, same ring-buffer/atomic-seq protocol. Mirrors // external/simbridge/src/SharedMemoryReader.hpp -- kept as a separate copy on // purpose, same duplication discipline used for the image Header struct. -template +template class SharedMemoryReader { public: - static_assert(std::is_trivially_copyable_v, "SharedMemoryReader requires a trivially-copyable T"); - static_assert(std::is_trivially_copyable_v, "SharedMemoryReader requires a trivially-copyable Meta"); + static_assert(std::is_trivially_copyable_v, "SharedMemoryReader requires a trivially-copyable Meta"); SharedMemoryReader() = default; @@ -46,28 +45,17 @@ class SharedMemoryReader { return true; } - // Reads the head, and the trailing_bytes of opaque data after it if any, out - // of the same slot -- the composite counterpart to SharedMemoryWriter::write. - // The head is copied into a properly typed object rather than mapped in - // place, which keeps this free of aliasing and object-lifetime concerns. - // trailing_out/trailing_bytes may be omitted for a plain POD struct with no - // trailing data. - bool readLatest(T& head_out, uint8_t* trailing_out = nullptr, size_t trailing_bytes = 0) { + // Reads the latest slot into out, which must be exactly bytes long -- a + // size mismatch (e.g. a segment written with a different payload) is + // rejected rather than partially filled. + bool readLatest(void* out, size_t bytes) { if (!ensureMapped_()) { return false; } if (!header_) { return false; } - if (header_->magic != kMagic || header_->version != kVersion || header_->slot_count == 0 || header_->slot_bytes == 0) { - return false; - } - const size_t requested = sizeof(T) + trailing_bytes; - // Composite slots are padded up to an 8-byte boundary, so the requested - // size may fall short of the slot by at most that much; anything short by - // more, or asking for more than the slot holds, means a size/type mismatch - // (e.g. a segment written with a different T) rather than padding slack. - if (requested > header_->slot_bytes || header_->slot_bytes - requested >= 8) { + if (header_->magic != kMagic || header_->version != kVersion || header_->slot_count == 0 || header_->slot_bytes != bytes) { return false; } @@ -80,16 +68,10 @@ class SharedMemoryReader { const uint8_t* slots_base = static_cast(map_ptr_) + sizeof(Header); const uint8_t* src = slots_base + static_cast(slot_idx) * header_->slot_bytes; - std::memcpy(&head_out, src, sizeof(T)); - if (trailing_bytes > 0) { - std::memcpy(trailing_out, src + sizeof(T), trailing_bytes); - } + std::memcpy(out, src, bytes); - // Torn-read guard. Nothing stops the writer from lapping us mid-copy, and - // the copy is only getting longer now that images share the slot. The - // writer starts filling the slot for seq X before publishing X, so by the - // time seq has advanced by slot_count - 1 it may already be overwriting - // the slot we just read; discard rather than hand back spliced data. + // Torn-read guard. Nothing stops the writer from lapping us mid-copy; + // discard rather than hand back spliced data. // (With slot_count == 1 every read is unprotected and so always rejected.) const uint64_t seq_after = __atomic_load_n(&header_->seq, __ATOMIC_ACQUIRE); if (seq_after - seq_before >= header_->slot_count - 1) { @@ -112,8 +94,8 @@ class SharedMemoryReader { static constexpr uint32_t kMagic = 0x5348514d; // SHQM // Must match SharedMemoryWriter::kVersion; see the note there on why the - // composite layout required a bump rather than a silent format change. - static constexpr uint32_t kVersion = 2; + // flat-slot layout required a bump rather than a silent format change. + static constexpr uint32_t kVersion = 3; bool ensureMapped_() { if (map_ptr_) { diff --git a/include/ipc/SharedMemoryWriter.h b/include/ipc/SharedMemoryWriter.h index 6bb9e78..889dfd1 100644 --- a/include/ipc/SharedMemoryWriter.h +++ b/include/ipc/SharedMemoryWriter.h @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -16,47 +15,32 @@ namespace spqr { // No metadata (default): use when the reader already knows how to interpret -// the payload (e.g. a fixed compile-time state struct with no trailing data). +// the payload (e.g. a fixed compile-time state struct). struct NoSharedMemoryMeta {}; -// Metadata for a segment whose trailing region holds one or more camera frames: -// lets a reader discover each frame's shape at runtime, since the resolution -// comes from the MuJoCo model and so is not known at compile time. +// Metadata for a segment carrying one or more camera frames: lets a reader +// discover each frame's shape at runtime, since the resolution comes from the +// MuJoCo model and so is not known at compile time. struct ImageMeta { uint32_t width = 0; uint32_t height = 0; uint32_t channels = 0; }; -// One source region of a slot's trailing payload, for the composite write below. -struct SharedMemoryChunk { - const void* data = nullptr; - size_t bytes = 0; -}; - -// Lock-free single-writer/multi-reader shared-memory ring buffer for any -// trivially-copyable T, with an optional trivially-copyable Meta blob written +// Lock-free single-writer/multi-reader shared-memory ring buffer for a flat, +// runtime-sized payload, with an optional trivially-copyable Meta blob written // once at configure() time (e.g. image width/height/channels, needed by a // reader that doesn't otherwise know the payload's shape). // -// Shared memory is just shared pages, not a wire protocol: a trivially-copyable -// type's bytes are already valid in any process built from the same struct -// definition, so plain memcpy is sufficient (and fastest) -- no serialization -// step is needed or performed. -// -// A slot is one T (the head) optionally followed by a runtime-sized trailing -// region of opaque bytes. That covers payloads whose size is only known at -// startup -- camera frames, whose resolution comes from the MuJoCo model -- -// without giving up the compile-time type of the head: T stays POD, and the -// shape of the trailing region travels in Meta. Head and trailing are -// published under a single seq, so a reader never mixes a head from one tick -// with trailing bytes from another. trailing_bytes = 0 is the degenerate case -// of a plain POD message struct with no trailing data. -template +// Shared memory is just shared pages, not a wire protocol: bytes written by +// one process are already valid to any process that agrees on their meaning, +// so plain memcpy is sufficient (and fastest) -- no serialization step is +// needed or performed. Each slot holds exactly slot_bytes of opaque data, +// published under a single seq per write() call. +template class SharedMemoryWriter { public: - static_assert(std::is_trivially_copyable_v, "SharedMemoryWriter requires a trivially-copyable T"); - static_assert(std::is_trivially_copyable_v, "SharedMemoryWriter requires a trivially-copyable Meta"); + static_assert(std::is_trivially_copyable_v, "SharedMemoryWriter requires a trivially-copyable Meta"); SharedMemoryWriter() = default; @@ -64,18 +48,12 @@ class SharedMemoryWriter { close_(); } - // trailing_bytes: how many bytes of opaque data follow the T head in each - // slot (e.g. summed image sizes), fixed for the segment's lifetime; 0 for a - // plain POD message struct with no trailing data. - void configure(const std::string& path, size_t trailing_bytes = 0, Meta meta = {}, int ring_slots = 3) { + // slot_bytes: exact size in bytes of the payload every write() call must + // supply, fixed for the segment's lifetime. + void configure(const std::string& path, size_t slot_bytes, Meta meta = {}, int ring_slots = 3) { if (ring_slots <= 0) { throw std::invalid_argument("Invalid shared-memory configuration"); } - trailing_bytes_ = trailing_bytes; - // Pad so every slot start stays 8-aligned, keeping the head's doubles - // naturally aligned for a reader that maps the slot in place. - const size_t padded_trailing = (trailing_bytes + 7u) & ~static_cast(7u); - const size_t slot_bytes = sizeof(T) + padded_trailing; const size_t total_bytes = sizeof(Header) + static_cast(ring_slots) * slot_bytes; close_(); @@ -125,19 +103,11 @@ class SharedMemoryWriter { return header_ != nullptr; } - // Writes the head, followed by the trailing chunks (if any), into one slot - // published with a single seq bump -- so head and trailing are always from - // the same tick. The chunk sizes must add up to the trailing_bytes passed to - // configure(); a mismatch is dropped rather than writing a partial slot. - void write(const T& head, std::initializer_list trailing = {}) { - if (!header_) { - return; - } - size_t supplied = 0; - for (const SharedMemoryChunk& chunk : trailing) { - supplied += chunk.bytes; - } - if (supplied != trailing_bytes_) { + // Writes bytes into one slot, published with a single seq bump. bytes must + // equal the slot_bytes passed to configure(); a mismatch is dropped rather + // than writing a partial slot. + void write(const void* data, size_t bytes) { + if (!header_ || bytes != header_->slot_bytes) { return; } @@ -146,14 +116,7 @@ class SharedMemoryWriter { 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, &head, sizeof(T)); - dst += sizeof(T); - for (const SharedMemoryChunk& chunk : trailing) { - if (chunk.bytes > 0) { - std::memcpy(dst, chunk.data, chunk.bytes); - dst += chunk.bytes; - } - } + std::memcpy(dst, data, bytes); __atomic_store_n(&header_->seq, next_seq, __ATOMIC_RELEASE); } @@ -168,10 +131,11 @@ class SharedMemoryWriter { }; static constexpr uint32_t kMagic = 0x5348514d; // SHQM - // v2 added the composite head+trailing slot layout. The bump matters because - // Meta sits inside Header: a v1 peer would still match magic/version at - // offsets 0 and 4 but read slot_count/slot_bytes from the wrong offsets. - static constexpr uint32_t kVersion = 2; + // v3 dropped the compile-time-typed head + trailing-chunk composite layout + // (v2) in favor of one flat, runtime-sized slot per write() call. The bump + // matters so a stale peer built against the old combined state+images + // protocol can't be silently misread as speaking the new one. + static constexpr uint32_t kVersion = 3; void close_() { if (map_ptr_) { @@ -192,7 +156,6 @@ class SharedMemoryWriter { size_t map_bytes_ = 0; Header* header_ = nullptr; std::string path_; - size_t trailing_bytes_ = 0; // unpadded; slot_bytes rounds this up to 8 }; } // namespace spqr diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 7c44122..4623560 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -93,7 +93,7 @@ class BoosterK1 : public Robot { return msg; } - void sendMessageSHM() override { + void sendMessageSHM(bool publishImages) override { // Socket-only for now: BoosterK1 doesn't yet have a shared-memory state channel. } diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 7c298c0..6e62f07 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -53,16 +54,25 @@ struct BoosterT1SharedState { // reinterpret the bytes; checking this first turns that into a clean failure. constexpr uint32_t kBoosterT1SchemaId = 0xB0057E71; -// Describes the camera frames that ride along in the trailing region of the -// state slot, in the order sendMessageSHM() writes them. Per-robot by -// design: another robot type declares its own SharedState / -// StateMeta pair with however many streams it actually has (the -// mechanism is generic; only the payload is robot-specific). If a robot ever -// needs a stream count that varies at runtime, bound it the way OracleData -// bounds teammates: a fixed-capacity array plus a count. struct BoosterT1StateMeta { uint32_t schema_id = kBoosterT1SchemaId; uint32_t state_bytes = static_cast(sizeof(BoosterT1SharedState)); +}; + +// Identifies the image segment's layout, separately from the state segment's +// kBoosterT1SchemaId above -- state and camera frames now publish on their own +// segments (state every physics substep, images once per control step; see +// SimulationThread::run()), so each needs its own schema/meta pair. +constexpr uint32_t kBoosterT1ImageSchemaId = 0xB0057E72; + +// Describes the camera frames that ride in the image segment, in the order +// sendMessageSHM() writes them. Per-robot by design: another robot type +// declares its own ImageMeta with however many streams it actually has +// (the mechanism is generic; only the payload is robot-specific). If a robot +// ever needs a stream count that varies at runtime, bound it the way +// OracleData bounds teammates: a fixed-capacity array plus a count. +struct BoosterT1ImageMeta { + uint32_t schema_id = kBoosterT1ImageSchemaId; ImageMeta rgb; ImageMeta depth; }; @@ -145,17 +155,21 @@ class BoosterT1 : public Robot { depthCamera = new CameraDepth(mujCtx, (name + "_rgb_cam").c_str()); rgbCameraInfo = new CameraInfo(mujCtx->model, (name + "_rgb_cam").c_str()); - // State and both camera frames share a single segment. The frames are - // runtime-sized (resolution comes from the MuJoCo model) but fixed once - // known, so they fit the writer's trailing region, with their shape - // carried in the meta blob for the reader. + // State and camera frames now publish on separate segments: state every + // physics substep (small, needed fresh for low-level torque control), + // images once per control step (unchanged between substeps anyway -- + // rendering happens on the GUI thread on its own cadence). See + // sendMessageSHM() and SimulationThread::run(). + state_writer_.configure(send_shm_path, sizeof(BoosterT1SharedState), BoosterT1StateMeta{}); + const uint32_t width = static_cast(rgbCamera->getWidth()); const uint32_t height = static_cast(rgbCamera->getHeight()); const size_t rgbBytes = static_cast(width) * height * 3; const size_t depthBytes = static_cast(width) * height * 2; - state_writer_.configure(send_shm_path, rgbBytes + depthBytes, - BoosterT1StateMeta{kBoosterT1SchemaId, static_cast(sizeof(BoosterT1SharedState)), - ImageMeta{width, height, 3}, ImageMeta{width, height, 2}}); + imageBuffer_.resize(rgbBytes + depthBytes); + image_writer_.configure(shmFilePath_("images"), imageBuffer_.size(), + BoosterT1ImageMeta{kBoosterT1ImageSchemaId, ImageMeta{width, height, 3}, ImageMeta{width, height, 2}}); + command_reader_.configure(receive_shm_path); // Create Oracle with the pose and all robots @@ -187,7 +201,7 @@ class BoosterT1 : public Robot { bool receiveMessageSHM() override { JointTorques torques; - if (!command_reader_.readLatest(torques)) { + if (!command_reader_.readLatest(&torques, sizeof(torques))) { return false; } @@ -213,16 +227,22 @@ class BoosterT1 : public Robot { return msg; } - void sendMessageSHM() override { - // One slot, one seq: pose/imu/joints/oracle and both frames are published - // together, so a reader can never pair a frame with a pose from a - // different tick -- which matters as soon as anything unprojects an image - // detection using the pose at capture time. + void sendMessageSHM(bool publishImages) override { + const BoosterT1SharedState state{pose->toSharedState(), imu->toSharedState(), + joints->toSharedState(), oracle->toSharedState()}; + state_writer_.write(&state, sizeof(state)); + + // Camera frames don't change between physics substeps -- rendering + // happens on the GUI thread on its own cadence -- so only copy and + // publish them once per control step, not on every substep. + if (!publishImages) { + return; + } const auto& rgbFrame = rgbCamera->getImage(); const auto& depthFrame = depthCamera->getDepth16UC1(); - state_writer_.write(BoosterT1SharedState{pose->toSharedState(), imu->toSharedState(), - joints->toSharedState(), oracle->toSharedState()}, - {{rgbFrame.data(), rgbFrame.size()}, {depthFrame.data(), depthFrame.size()}}); + std::memcpy(imageBuffer_.data(), rgbFrame.data(), rgbFrame.size()); + std::memcpy(imageBuffer_.data() + rgbFrame.size(), depthFrame.data(), depthFrame.size()); + image_writer_.write(imageBuffer_.data(), imageBuffer_.size()); } std::map getSensors() override { @@ -264,8 +284,10 @@ class BoosterT1 : public Robot { std::unordered_map latestTorques; std::string shm_dir_; - SharedMemoryWriter state_writer_; - SharedMemoryReader> command_reader_; + SharedMemoryWriter state_writer_; + SharedMemoryWriter image_writer_; + std::vector imageBuffer_; // persistent rgb+depth concat buffer for image_writer_ + SharedMemoryReader<> command_reader_; }; } // namespace spqr diff --git a/include/robots/Robot.h b/include/robots/Robot.h index de14ab8..d2492b3 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -69,7 +69,11 @@ class Robot { virtual void receiveMessageSocket(const std::map& message) = 0; - virtual void sendMessageSHM() = 0; + // publishImages: whether this call should also publish this robot's + // camera frames (on the robots that have a camera SHM channel), not just + // its low-rate-independent state. See SimulationThread::run() for the + // substep-vs-control-step cadence this flag encodes. + virtual void sendMessageSHM(bool publishImages) = 0; virtual bool receiveMessageSHM() = 0; // True once this robot's simbridge process has announced itself via shared diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 20d8ac8..483169d 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -69,10 +69,10 @@ void RobotManager::clear() { robots_.clear(); } -void RobotManager::sendStateMessages() { +void RobotManager::sendStateMessages(bool publishImages) { if (connectMode_ == "shm") { // std::cout << "[sendStateMessages] sending message with SHM" << std::endl; - sendStateMessagesSHM(); + sendStateMessagesSHM(publishImages); } else { // std::cout << "[sendStateMessages] sending message with Socket" << std::endl; @@ -92,10 +92,10 @@ void RobotManager::receiveCommandMessages() { } -void RobotManager::sendStateMessagesSHM() { +void RobotManager::sendStateMessagesSHM(bool publishImages) { for (auto& r : robots_) { std::unique_lock lock(mutex_); - r->sendMessageSHM(); + r->sendMessageSHM(publishImages); } } @@ -141,7 +141,7 @@ void RobotManager::receiveCommandMessagesSHM() { if (!pendingRobots.count(r->name)) continue; std::cout << "[receiveCommandMessagesSHM] Timeout, resending state to: " << r->name << std::endl; - r->sendMessageSHM(); + r->sendMessageSHM(true); } windowStart = std::chrono::steady_clock::now(); } @@ -364,7 +364,7 @@ void RobotManager::waitRobotConnectionsSHM() { for (auto& r : robots_) { if (pendingRobots.count(r->name) && r->hasConnectSignalSHM()) { r->isConnected = true; - r->sendMessageSHM(); + r->sendMessageSHM(true); pendingRobots.erase(r->name); std::cout << "Connected Robot (shm): " << r->name << "\n"; } diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index eff6530..00aeaf8 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -83,7 +83,6 @@ void SimulationThread::run() { while (running_) { if (!paused_) { // DEBUG - auto stepsStart = clock::now(); for(int i=0; i(clock::now() - stepsStart).count(); + double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; + std::cout << "[SimulationThread] il singolo step " << i << " impiega " << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; + } // DEBUG: real time spent computing kControlDecimation physics steps, vs. the sim-time budget they represent. - double stepsElapsedMs = std::chrono::duration(clock::now() - stepsStart).count(); - double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; - std::cout << "[SimulationThread] " << kControlDecimation << " physics steps took " - << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; next_step_time += std::chrono::duration_cast(std::chrono::duration(kTimestepPolicy)); std::this_thread::sleep_until(next_step_time); From bcf43dd37ca46e4cd79a60ff2d15facb3e853810 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 4 Sep 2026 18:49:31 +0200 Subject: [PATCH 18/25] [refactoring] debug comment --- src/SimulationThread.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index 00aeaf8..8907ae6 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -83,6 +83,7 @@ void SimulationThread::run() { while (running_) { if (!paused_) { // DEBUG + // auto stepsStart = clock::now(); for(int i=0; i(clock::now() - stepsStart).count(); - double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; - std::cout << "[SimulationThread] il singolo step " << i << " impiega " << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; - } // DEBUG: real time spent computing kControlDecimation physics steps, vs. the sim-time budget they represent. + // double stepsElapsedMs = std::chrono::duration(clock::now() - stepsStart).count(); + // double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; + // std::cout << "[SimulationThread] tutto il ciclo impiega " << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; next_step_time += std::chrono::duration_cast(std::chrono::duration(kTimestepPolicy)); std::this_thread::sleep_until(next_step_time); From d1b095738e54ea6285e9c255f69dbb2334e8b688 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 11 Sep 2026 15:32:40 +0200 Subject: [PATCH 19/25] configuration of shm channels only in shm mode --- include/RobotManager.h | 2 +- include/robots/BoosterK1.h | 2 +- include/robots/BoosterT1.h | 33 ++++++++++++++++----------------- include/robots/Robot.h | 2 +- src/AppWindow.cpp | 4 ++-- src/RobotManager.cpp | 4 ++-- 6 files changed, 23 insertions(+), 24 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index c9372a2..2442bf7 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -60,7 +60,7 @@ class RobotManager { bool areAllRobotsReady() const; bool areAllRobotsConnected() const; - void bindMujoco(MujocoContext* mujContext); + void bindMujoco(MujocoContext* mujContext, std::string connectMode_); std::shared_ptr create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos, const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr team, diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 4623560..e3073d7 100644 --- a/include/robots/BoosterK1.h +++ b/include/robots/BoosterK1.h @@ -62,7 +62,7 @@ class BoosterK1 : public Robot { {JointValue::ANKLE_RIGHT_PITCH, name + "_Right_Ankle_Pitch"}, {JointValue::ANKLE_RIGHT_ROLL, name + "_Right_Ankle_Roll"}} {} - void bindMujoco(MujocoContext* mujCtx) override { + void bindMujoco(MujocoContext* mujCtx, std::string connectMode_) override { pose = new Pose(mujCtx->model, mujCtx->data, (name + "_position").c_str(), (name + "_orientation").c_str()); imu = new Imu(mujCtx->model, mujCtx->data, (name + "_linear-acceleration").c_str(), (name + "_angular-velocity").c_str()); joints = new Joints(mujCtx->model, mujCtx->data, joint_map); diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 6e62f07..63b4265 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -118,7 +118,7 @@ class BoosterT1 : public Robot { shm_dir_ = "/dev/shm/circus_ipc"; } - void bindMujoco(MujocoContext* mujCtx) override { + void bindMujoco(MujocoContext* mujCtx, std::string connectMode_) override { pose = new Pose(mujCtx->model, mujCtx->data, (name + "_position").c_str(), (name + "_orientation").c_str()); headPose = new GroundRelativePosition(mujCtx->model, mujCtx->data, (name + "_head_rgb_cam_site").c_str(), GroundRelativePosition::TargetType::Site, pose); @@ -154,26 +154,25 @@ class BoosterT1 : public Robot { // This avoids parallax between rgb_cam and depth_cam when unprojecting RGB detections. depthCamera = new CameraDepth(mujCtx, (name + "_rgb_cam").c_str()); rgbCameraInfo = new CameraInfo(mujCtx->model, (name + "_rgb_cam").c_str()); + // Create Oracle with the pose and all robots + oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); - // State and camera frames now publish on separate segments: state every - // physics substep (small, needed fresh for low-level torque control), - // images once per control step (unchanged between substeps anyway -- - // rendering happens on the GUI thread on its own cadence). See - // sendMessageSHM() and SimulationThread::run(). - state_writer_.configure(send_shm_path, sizeof(BoosterT1SharedState), BoosterT1StateMeta{}); + if(connectMode_ == "shm") { + const uint32_t width = static_cast(rgbCamera->getWidth()); + const uint32_t height = static_cast(rgbCamera->getHeight()); + const size_t rgbBytes = static_cast(width) * height * 3; + const size_t depthBytes = static_cast(width) * height * 2; + imageBuffer_.resize(rgbBytes + depthBytes); - const uint32_t width = static_cast(rgbCamera->getWidth()); - const uint32_t height = static_cast(rgbCamera->getHeight()); - const size_t rgbBytes = static_cast(width) * height * 3; - const size_t depthBytes = static_cast(width) * height * 2; - imageBuffer_.resize(rgbBytes + depthBytes); - image_writer_.configure(shmFilePath_("images"), imageBuffer_.size(), - BoosterT1ImageMeta{kBoosterT1ImageSchemaId, ImageMeta{width, height, 3}, ImageMeta{width, height, 2}}); + // Configuration of shm channels + image_writer_.configure(shmFilePath_("images"), imageBuffer_.size(), + BoosterT1ImageMeta{kBoosterT1ImageSchemaId, ImageMeta{width, height, 3}, ImageMeta{width, height, 2}}); + state_writer_.configure(send_shm_path, sizeof(BoosterT1SharedState), BoosterT1StateMeta{}); + + command_reader_.configure(receive_shm_path); - command_reader_.configure(receive_shm_path); + } - // Create Oracle with the pose and all robots - oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); } void receiveMessageSocket(const std::map& message) override { diff --git a/include/robots/Robot.h b/include/robots/Robot.h index d2492b3..b6263e8 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -55,7 +55,7 @@ class Robot { send_shm_path = spqr::sharedMemoryPath + name + "_state.shm"; } virtual ~Robot() = default; - virtual void bindMujoco(MujocoContext* mujContext) = 0; + virtual void bindMujoco(MujocoContext* mujContext, std::string connectMode_) = 0; virtual void update() = 0; virtual void sendMessageSocket(int fd) final { diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index a5f83f7..bfa9ea0 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -225,13 +225,13 @@ void AppWindow::loadScene(const QString& yaml_file) { std::filesystem::create_directories(shmDir); CircusNetwork::instance().init(); - RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server + RobotManager::instance().bindMujoco(mujContext.get(), connectMode_); // memo: this must be run before starting the communications server } else { // socket mode CircusNetwork::instance().init(); - RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server + RobotManager::instance().bindMujoco(mujContext.get(), connectMode_); // memo: this must be run before starting the communications server RobotManager::instance().initializeSocket(frameworkCommunicationPort); } diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 483169d..416d3f9 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -386,9 +386,9 @@ void RobotManager::waitRobotConnectionsSHM() { std::cout << "All Robots are connected!" << std::endl; } -void RobotManager::bindMujoco(MujocoContext* mujContext) { +void RobotManager::bindMujoco(MujocoContext* mujContext, std::string connectMode_) { for (std::shared_ptr r : robots_) - r->bindMujoco(mujContext); + r->bindMujoco(mujContext, connectMode_); } std::shared_ptr RobotManager::create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos, From 8e9ffc37e3604cd6f49f9bfa3a04c363ff62bf0e Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 11 Sep 2026 16:19:14 +0200 Subject: [PATCH 20/25] [refactoring] cambio image shm path --- include/robots/BoosterT1.h | 3 +-- include/robots/Robot.h | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 63b4265..792e967 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -165,10 +165,9 @@ class BoosterT1 : public Robot { imageBuffer_.resize(rgbBytes + depthBytes); // Configuration of shm channels - image_writer_.configure(shmFilePath_("images"), imageBuffer_.size(), + image_writer_.configure(image_shm_path, imageBuffer_.size(), BoosterT1ImageMeta{kBoosterT1ImageSchemaId, ImageMeta{width, height, 3}, ImageMeta{width, height, 2}}); state_writer_.configure(send_shm_path, sizeof(BoosterT1SharedState), BoosterT1StateMeta{}); - command_reader_.configure(receive_shm_path); } diff --git a/include/robots/Robot.h b/include/robots/Robot.h index b6263e8..9f896af 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -53,6 +53,7 @@ class Robot { // proves the robot process is up, with no dedicated connect channel needed. receive_shm_path = spqr::sharedMemoryPath + name + "_commands.shm"; send_shm_path = spqr::sharedMemoryPath + name + "_state.shm"; + image_shm_path = spqr::sharedMemoryPath + name + "_images.shm"; } virtual ~Robot() = default; virtual void bindMujoco(MujocoContext* mujContext, std::string connectMode_) = 0; @@ -92,6 +93,7 @@ class Robot { protected: std::string receive_shm_path; std::string send_shm_path; + std::string image_shm_path; public: From c8bd76df8e6ec521e52041093c1cb722dd0967d4 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 11 Sep 2026 17:13:14 +0200 Subject: [PATCH 21/25] head pose for shm connect --- include/robots/BoosterT1.h | 5 +++-- include/sensors/GroundRelativePosition.h | 13 +++++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 792e967..007261e 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -47,6 +47,7 @@ struct BoosterT1SharedState { ImuData imu; JointState joints; OracleData oracle; + GroundRelativePositionData headPose; }; // Identifies this exact state layout. Shared memory carries no schema, so a @@ -114,7 +115,6 @@ class BoosterT1 : public Robot { {JointValue::KNEE_RIGHT_PITCH, name + "_Right_Knee_Pitch"}, {JointValue::ANKLE_RIGHT_PITCH, name + "_Right_Ankle_Pitch"}, {JointValue::ANKLE_RIGHT_ROLL, name + "_Right_Ankle_Roll"}} { - // Where to put the images shm_dir_ = "/dev/shm/circus_ipc"; } @@ -227,7 +227,8 @@ class BoosterT1 : public Robot { void sendMessageSHM(bool publishImages) override { const BoosterT1SharedState state{pose->toSharedState(), imu->toSharedState(), - joints->toSharedState(), oracle->toSharedState()}; + joints->toSharedState(), oracle->toSharedState(), + headPose->toSharedState()}; state_writer_.write(&state, sizeof(state)); // Camera frames don't change between physics substeps -- rendering diff --git a/include/sensors/GroundRelativePosition.h b/include/sensors/GroundRelativePosition.h index 466116d..6141201 100644 --- a/include/sensors/GroundRelativePosition.h +++ b/include/sensors/GroundRelativePosition.h @@ -10,6 +10,11 @@ namespace spqr { + +struct GroundRelativePositionData { + double position[3] = {}; +}; + // Position of a body or site relative to the ground projection of a reference Pose. // Reference frame: (trunk_x, trunk_y, 0) with yaw-only rotation of the trunk. class GroundRelativePosition : public Sensor { @@ -51,6 +56,14 @@ class GroundRelativePosition : public Sensor { return msgpack::object(data, z); } + GroundRelativePositionData toSharedState() const { + GroundRelativePositionData data; + for (int i = 0; i < 3; ++i) { + data.position[i] = position_(i); + } + return data; + } + Eigen::Vector3d getPosition() const { return position_; } From b3348bb66916ce52f19475b55818479272149b9b Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 11 Sep 2026 18:21:19 +0200 Subject: [PATCH 22/25] typo --- resources/robots/Booster-T1/instance.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/resources/robots/Booster-T1/instance.xml b/resources/robots/Booster-T1/instance.xml index f84c3d3..0b3d9a8 100644 --- a/resources/robots/Booster-T1/instance.xml +++ b/resources/robots/Booster-T1/instance.xml @@ -166,7 +166,7 @@ - true + From 6ce5b560e8bc9bc6c9f22387ba9e09db1b500c34 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Fri, 18 Sep 2026 11:28:21 +0200 Subject: [PATCH 23/25] changed socket connection, using unpackers of msgpack. Now it faster --- include/ipc/utils.h | 65 ++++++++++++++++++++++++++++-------------- include/robots/Robot.h | 2 +- src/RobotManager.cpp | 33 ++++++--------------- 3 files changed, 53 insertions(+), 47 deletions(-) diff --git a/include/ipc/utils.h b/include/ipc/utils.h index 5483e1b..41d9774 100644 --- a/include/ipc/utils.h +++ b/include/ipc/utils.h @@ -3,36 +3,57 @@ #include #include #include +#include -namespace spqr { - -// TCP Communication: sends the size of the message first, then the message itself. -// Returns the number of bytes sent (excluding the size header), or -1 on error. +#include +#include -// 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 -inline ssize_t send_all(int fd, const 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; - } +namespace spqr { - 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 */ +// TCP Communication: sender-side counterpart to recv_latest below -- loops +// send() until the full buffer has been handed to the kernel. Writes no +// length header: recv_latest's persistent msgpack::unpacker tracks message +// boundaries itself (MessagePack values are self-delimiting), so nothing +// needs to declare how many bytes are coming. Returns the number of bytes +// sent, or -1 on error. +inline ssize_t send_unframed(int fd, const char* buf, size_t len) { + ssize_t total = 0; + while (static_cast(total) < len) { + ssize_t n = send(fd, buf + total, len - static_cast(total), 0); + if (n <= 0) { return -1; } total += n; - bytesleft -= n; } return total; } +// TCP Communication: reads whatever bytes are currently available on fd into +// the caller-owned, per-connection `unp` (kept alive across calls, e.g. in a +// std::unordered_map keyed by fd), then extracts the +// most recently completed message -- any older ones that had piled up in the +// same read (fast sender, slow reader) are discarded on purpose, since only +// the latest is ever still relevant. Unlike a length-prefixed protocol, this +// needs no length header: MessagePack values are self-delimiting, so `unp` +// tracks message boundaries -- and any partial trailing bytes -- itself. +// Returns 1 if `out` now holds a complete message, 0 if the read only added +// to a still-incomplete one, or -1 if the connection was closed/errored (the +// caller should close fd). +inline int recv_latest(int fd, msgpack::unpacker& unp, msgpack::object_handle& out, size_t maxChunk) { + unp.reserve_buffer(maxChunk); + ssize_t n = read(fd, unp.buffer(), unp.buffer_capacity()); + if (n <= 0) { + return -1; + } + unp.buffer_consumed(static_cast(n)); + + msgpack::object_handle oh; + bool gotOne = false; + while (unp.next(oh)) { + out = std::move(oh); + gotOne = true; + } + return gotOne ? 1 : 0; +} + } \ No newline at end of file diff --git a/include/robots/Robot.h b/include/robots/Robot.h index 9f896af..b2e8089 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -64,7 +64,7 @@ class Robot { msgpack::sbuffer sbuf; msgpack::pack(sbuf, message); if (sbuf.size() > 0) { - send_all(fd, sbuf.data(), sbuf.size()); + send_unframed(fd, sbuf.data(), sbuf.size()); } } diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 416d3f9..4f6cc70 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -6,6 +6,7 @@ #include "Constants.h" #include "Team.h" #include "Utils.h" +#include "ipc/utils.h" namespace spqr { @@ -183,10 +184,9 @@ void RobotManager::receiveCommandMessagesSocket() { // An event occured for the i-th fd if (pollFds_[i].revents & POLLIN) { int fd = pollFds_[i].fd; - msgpack::unpacker& unp = unpackers_[fd]; - unp.reserve_buffer(MAX_MSG_SIZE); - int n = read(fd, unp.buffer(), unp.buffer_capacity()); - if (n <= 0) { + msgpack::object_handle latest; + int status = recv_latest(fd, unpackers_[fd], latest, MAX_MSG_SIZE); + if (status < 0) { close(fd); pollFds_.erase(pollFds_.begin() + i); --i; @@ -194,21 +194,7 @@ void RobotManager::receiveCommandMessagesSocket() { removeRobotFd(fd); continue; } - unp.buffer_consumed(static_cast(n)); - - // Drain every complete message currently buffered (a single - // read() can return several concatenated messages if we were - // slow to read), keeping only the latest one -- older ones - // queued up behind it are superseded and discarded on - // purpose, never applied. - msgpack::object_handle oh; - msgpack::object_handle latest; - bool gotOne = false; - while (unp.next(oh)) { - latest = std::move(oh); - gotOne = true; - } - if (!gotOne) + if (status == 0) continue; // only a partial message so far; wait for more bytes auto data_map = latest.get().as>(); @@ -299,17 +285,16 @@ void RobotManager::waitRobotConnectionsSocket() { pollFds_.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); + msgpack::unpacker unp; + msgpack::object_handle oh; + int status = recv_latest(client_fd, unp, oh, MAX_MSG_SIZE); - if (n <= 0) { + if (status <= 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 From f55b003de663efb1f37db6467abf9da40e23ccc1 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Sun, 4 Oct 2026 02:01:29 +0200 Subject: [PATCH 24/25] cleanup shm + sim loop parameters in header + change of timestep --- include/AppWindow.h | 3 ++- include/SimulationThread.h | 4 ++++ include/robots/BoosterT1.h | 5 ----- resources/config/framework_config.yaml | 1 - src/AppWindow.cpp | 24 +++++++++++++++++++----- src/Container.cpp | 3 +-- src/RobotManager.cpp | 10 ++++++++++ src/SceneParser.cpp | 2 +- src/SimulationThread.cpp | 4 ---- 9 files changed, 37 insertions(+), 19 deletions(-) diff --git a/include/AppWindow.h b/include/AppWindow.h index 7f2755c..afbcac6 100644 --- a/include/AppWindow.h +++ b/include/AppWindow.h @@ -40,6 +40,8 @@ class AppWindow : public QMainWindow { void loadScene(const QString& yaml_file); static void signalHandler(int signal); + static void cleanupSharedMemory(); + QVBoxLayout* mainLayout = nullptr; QHBoxLayout* contentLayout = nullptr; QWidget* viewportContainer = nullptr; @@ -55,7 +57,6 @@ class AppWindow : public QMainWindow { std::string frameworkConfigPath_ = spqr::frameworkConfigPath; std::string pathsConfigPath_ = spqr::pathsConfigPath; - std::string sharedMemoryPath_ = spqr::sharedMemoryPath; std::string connectMode_ = "shm"; }; diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 32130e4..4751f77 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -29,6 +29,10 @@ class SimulationThread : public QThread { std::atomic running_; std::atomic paused_; int maxSimulationTime_ = -1; // -1 means no limit + + static constexpr int kControlDecimation = 5; + static constexpr double kTimestepPolicy = 0.02; + }; } // namespace spqr diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 007261e..81971d9 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -115,7 +115,6 @@ class BoosterT1 : public Robot { {JointValue::KNEE_RIGHT_PITCH, name + "_Right_Knee_Pitch"}, {JointValue::ANKLE_RIGHT_PITCH, name + "_Right_Ankle_Pitch"}, {JointValue::ANKLE_RIGHT_ROLL, name + "_Right_Ankle_Roll"}} { - shm_dir_ = "/dev/shm/circus_ipc"; } void bindMujoco(MujocoContext* mujCtx, std::string connectMode_) override { @@ -275,10 +274,6 @@ 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; diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 53a819c..2f9c6b9 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -8,4 +8,3 @@ volumes: - "/tools/vision:/app/maximus/tools/vision" - "/src/app/config:/app/maximus/config" - "/src/app/behaviors/trees:/app/maximus/behaviors" - - "/dev/shm/circus_ipc:/dev/shm/circus_ipc" diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index bfa9ea0..54da8bb 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -1,6 +1,7 @@ #include "AppWindow.h" #include +#include #include #include @@ -117,6 +118,14 @@ void AppWindow::openScene() { } } +void AppWindow::cleanupSharedMemory() { + std::error_code ec; + std::filesystem::remove_all(std::filesystem::path(sharedMemoryPath), ec); + if (ec) { + std::cerr << "Failed to clean " << sharedMemoryPath << ": " << ec.message() << std::endl; + } +} + void AppWindow::loadScene(const QString& yaml_file) { try { TeamManager::instance().clear(); @@ -216,13 +225,16 @@ void AppWindow::loadScene(const QString& yaml_file) { // 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. - if(connectMode_ == "shm") { - const std::filesystem::path shmDir(sharedMemoryPath_); - if (std::filesystem::exists(shmDir)) { - std::filesystem::remove_all(shmDir); + const std::filesystem::path shmDir(sharedMemoryPath); + cleanupSharedMemory(); + + std::error_code ec; + std::filesystem::create_directories(shmDir, ec); + if (ec || access(shmDir.c_str(), W_OK) != 0) { + throw std::runtime_error(shmDir.string() + " is not writable by the current user. Remove it (sudo rm -rf " + + shmDir.string() + ") and load the scene again."); } - std::filesystem::create_directories(shmDir); CircusNetwork::instance().init(); RobotManager::instance().bindMujoco(mujContext.get(), connectMode_); // memo: this must be run before starting the communications server @@ -353,6 +365,7 @@ void AppWindow::signalHandler(int signal) { std::cerr.flush(); TeamManager::instance().clear(); + cleanupSharedMemory(); std::cerr << "Cleanup complete. Re-raising signal." << std::endl; std::cerr.flush(); @@ -365,6 +378,7 @@ AppWindow::~AppWindow() { if (sim != nullptr && sim->isRunning()) sim->stop(); TeamManager::instance().clear(); + cleanupSharedMemory(); } } // namespace spqr diff --git a/src/Container.cpp b/src/Container.cpp index 5cee481..c124812 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -53,7 +53,6 @@ void Container::create(const std::shared_ptr& robot, const std::string& i 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}}, {{"Name", "rtprio"}, {"Soft", 99}, {"Hard", 99}}})}, @@ -86,7 +85,7 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "XAUTHORITY=/root/.Xauthority", "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", - "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc", // TODO: vedere se questo si può rimpiazzare con la costante + "CIRCUS_SHM_DIR=" + std::string(sharedMemoryPath), "CONNECT_MODE=" + connectMode, // SHM: questo serve per simbridge, per sapere che modalità attivare "JOYSTICK_DEVICE=" + envOrDefault("JOYSTICK_DEVICE", "/dev/input/js0"), "INFERENCE_BACKEND=" + envOrDefault("INFERENCE_BACKEND", "trt"), diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 4f6cc70..1dd97d6 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -438,6 +438,16 @@ void RobotManager::startContainers(const std::string& fwkCfgPath, const std::str } binds.push_back(v2); } + + // The shared-memory bind belongs to "shm" mode only, + // and must match the path AppWindow creates and cleans + if (connectMode == "shm") { + std::string shmPath(sharedMemoryPath); + if (!shmPath.empty() && shmPath.back() == '/') { + shmPath.pop_back(); + } + binds.push_back(shmPath + ":" + shmPath); + } for (std::shared_ptr r : robots_) { r->container = std::make_unique("CIRCUS_" + r->name + "_container"); r->container->create(r, image, binds, connectMode); diff --git a/src/SceneParser.cpp b/src/SceneParser.cpp index aa76f8d..69f759a 100644 --- a/src/SceneParser.cpp +++ b/src/SceneParser.cpp @@ -213,7 +213,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.002"; + option.append_attribute("timestep") = "0.004"; 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 8907ae6..f6a703a 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -74,10 +74,6 @@ void SimulationThread::run() { double sim_dt = model_->opt.timestep; - constexpr int kControlDecimation = 10; - constexpr double kTimestepPolicy = 0.02; - int stepsSinceLastControl = 0; - using clock = std::chrono::steady_clock; auto next_step_time = clock::now(); while (running_) { From ffd7a47c205c15c6b07bd8ca4edbc8f081532492 Mon Sep 17 00:00:00 2001 From: FlavioFoxes Date: Sun, 4 Oct 2026 02:07:45 +0200 Subject: [PATCH 25/25] pre commit --- include/RobotManager.h | 4 +-- include/SimulationThread.h | 1 - include/ipc/utils.h | 2 +- include/robots/BoosterT1.h | 12 +++------ include/robots/Robot.h | 5 ++-- include/sensors/GroundRelativePosition.h | 3 +-- src/AppWindow.cpp | 13 +++++----- src/Container.cpp | 2 +- src/RobotManager.cpp | 19 ++++++-------- src/SimulationThread.cpp | 33 ++++++++++++------------ 10 files changed, 42 insertions(+), 52 deletions(-) diff --git a/include/RobotManager.h b/include/RobotManager.h index 2442bf7..19d9041 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -46,7 +46,7 @@ class RobotManager { void clear(); void setRobotFd(const std::string& name, int fd); - int getRobotFd(const std::string& name) const; // -1 se sconosciuto + int getRobotFd(const std::string& name) const; // -1 se sconosciuto void removeRobotFd(int fd); // publishImages: forwarded to each robot's sendMessageSHM() (ignored in @@ -55,7 +55,7 @@ class RobotManager { void receiveCommandMessages(); void waitRobotConnections(); - void initializeSocket(int port); + void initializeSocket(int port); bool areAllRobotsReady() const; bool areAllRobotsConnected() const; diff --git a/include/SimulationThread.h b/include/SimulationThread.h index 4751f77..5074882 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -32,7 +32,6 @@ class SimulationThread : public QThread { static constexpr int kControlDecimation = 5; static constexpr double kTimestepPolicy = 0.02; - }; } // namespace spqr diff --git a/include/ipc/utils.h b/include/ipc/utils.h index 41d9774..dd0cb88 100644 --- a/include/ipc/utils.h +++ b/include/ipc/utils.h @@ -56,4 +56,4 @@ inline int recv_latest(int fd, msgpack::unpacker& unp, msgpack::object_handle& o return gotOne ? 1 : 0; } -} \ No newline at end of file +} // namespace spqr diff --git a/include/robots/BoosterT1.h b/include/robots/BoosterT1.h index 81971d9..c6f3e6f 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -114,8 +114,7 @@ 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"}} {} void bindMujoco(MujocoContext* mujCtx, std::string connectMode_) override { pose = new Pose(mujCtx->model, mujCtx->data, (name + "_position").c_str(), (name + "_orientation").c_str()); @@ -156,7 +155,7 @@ class BoosterT1 : public Robot { // Create Oracle with the pose and all robots oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); - if(connectMode_ == "shm") { + if (connectMode_ == "shm") { const uint32_t width = static_cast(rgbCamera->getWidth()); const uint32_t height = static_cast(rgbCamera->getHeight()); const size_t rgbBytes = static_cast(width) * height * 3; @@ -168,9 +167,7 @@ class BoosterT1 : public Robot { BoosterT1ImageMeta{kBoosterT1ImageSchemaId, ImageMeta{width, height, 3}, ImageMeta{width, height, 2}}); state_writer_.configure(send_shm_path, sizeof(BoosterT1SharedState), BoosterT1StateMeta{}); command_reader_.configure(receive_shm_path); - } - } void receiveMessageSocket(const std::map& message) override { @@ -225,9 +222,8 @@ class BoosterT1 : public Robot { } void sendMessageSHM(bool publishImages) override { - const BoosterT1SharedState state{pose->toSharedState(), imu->toSharedState(), - joints->toSharedState(), oracle->toSharedState(), - headPose->toSharedState()}; + const BoosterT1SharedState state{pose->toSharedState(), imu->toSharedState(), joints->toSharedState(), + oracle->toSharedState(), headPose->toSharedState()}; state_writer_.write(&state, sizeof(state)); // Camera frames don't change between physics substeps -- rendering diff --git a/include/robots/Robot.h b/include/robots/Robot.h index b2e8089..339dbc5 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -15,11 +15,11 @@ #include #include -#include "Container.h" #include "Constants.h" +#include "Container.h" #include "MujocoContext.h" -#include "sensors/Sensor.h" #include "ipc/utils.h" +#include "sensors/Sensor.h" #define MAX_MSG_SIZE 1048576 // 1MB namespace spqr { @@ -96,7 +96,6 @@ class Robot { std::string image_shm_path; public: - std::string name; std::string type; uint8_t number; diff --git a/include/sensors/GroundRelativePosition.h b/include/sensors/GroundRelativePosition.h index 6141201..3833e95 100644 --- a/include/sensors/GroundRelativePosition.h +++ b/include/sensors/GroundRelativePosition.h @@ -10,9 +10,8 @@ namespace spqr { - struct GroundRelativePositionData { - double position[3] = {}; + double position[3] = {}; }; // Position of a body or site relative to the ground projection of a reference Pose. diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index 54da8bb..19502a9 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -47,7 +47,7 @@ AppWindow::AppWindow(int& argc, char** argv) : QMainWindow() { else if (arg == "--paths" && i + 1 < argc) pathsConfigPath_ = argv[++i]; else if (arg == "--connect-mode" && i + 1 < argc) - connectMode_ = argv[++i]; // TODO: forse si può aggiungere come variabile d'ambiente?? + connectMode_ = argv[++i]; // TODO: forse si può aggiungere come variabile d'ambiente?? } resize(spqr::initialWindowWidth, spqr::initialWindowHeight); @@ -225,28 +225,27 @@ void AppWindow::loadScene(const QString& yaml_file) { // 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. - if(connectMode_ == "shm") { + if (connectMode_ == "shm") { const std::filesystem::path shmDir(sharedMemoryPath); cleanupSharedMemory(); std::error_code ec; std::filesystem::create_directories(shmDir, ec); if (ec || access(shmDir.c_str(), W_OK) != 0) { - throw std::runtime_error(shmDir.string() + " is not writable by the current user. Remove it (sudo rm -rf " - + shmDir.string() + ") and load the scene again."); + throw std::runtime_error(shmDir.string() + " is not writable by the current user. Remove it (sudo rm -rf " + shmDir.string() + + ") and load the scene again."); } CircusNetwork::instance().init(); RobotManager::instance().bindMujoco(mujContext.get(), connectMode_); // memo: this must be run before starting the communications server - } - else { // socket mode + } else { // socket mode CircusNetwork::instance().init(); RobotManager::instance().bindMujoco(mujContext.get(), connectMode_); // memo: this must be run before starting the communications server RobotManager::instance().initializeSocket(frameworkCommunicationPort); } - + std::cout << "Starting containers..." << std::endl; RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); diff --git a/src/Container.cpp b/src/Container.cpp index c124812..bf339e4 100644 --- a/src/Container.cpp +++ b/src/Container.cpp @@ -86,7 +86,7 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", "CIRCUS_SHM_DIR=" + std::string(sharedMemoryPath), - "CONNECT_MODE=" + connectMode, // SHM: questo serve per simbridge, per sapere che modalità attivare + "CONNECT_MODE=" + connectMode, // SHM: questo serve per simbridge, per sapere che modalità attivare "JOYSTICK_DEVICE=" + envOrDefault("JOYSTICK_DEVICE", "/dev/input/js0"), "INFERENCE_BACKEND=" + envOrDefault("INFERENCE_BACKEND", "trt"), "TASK_NAME=" + envOrDefault("TASK_NAME", "t1-velocity"), diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 1dd97d6..ebec21b 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -1,8 +1,9 @@ #include "RobotManager.h" -#include #include +#include + #include "Constants.h" #include "Team.h" #include "Utils.h" @@ -74,8 +75,7 @@ void RobotManager::sendStateMessages(bool publishImages) { if (connectMode_ == "shm") { // std::cout << "[sendStateMessages] sending message with SHM" << std::endl; sendStateMessagesSHM(publishImages); - } - else { + } else { // std::cout << "[sendStateMessages] sending message with Socket" << std::endl; sendStateMessagesSocket(); } @@ -85,14 +85,12 @@ void RobotManager::receiveCommandMessages() { if (connectMode_ == "shm") { // std::cout << "[receiveCommandMessages] receiving message with SHM" << std::endl; receiveCommandMessagesSHM(); - } - else { + } else { // std::cout << "[receiveCommandMessages] receiving message with Socket" << std::endl; receiveCommandMessagesSocket(); } } - void RobotManager::sendStateMessagesSHM(bool publishImages) { for (auto& r : robots_) { std::unique_lock lock(mutex_); @@ -260,10 +258,9 @@ void RobotManager::initializeSocket(int port) { } void RobotManager::waitRobotConnections() { - if(connectMode_ == "shm") { + if (connectMode_ == "shm") { waitRobotConnectionsSHM(); - } - else { + } else { waitRobotConnectionsSocket(); } } @@ -392,7 +389,7 @@ std::shared_ptr RobotManager::create(const std::string& name, const std:: void RobotManager::startContainers(const std::string& fwkCfgPath, const std::string& pathsCfgPath, const std::string& connectMode) { // Save connection mode connectMode_ = connectMode; - + YAML::Node configRoot = loadYamlFile(fwkCfgPath.c_str()); if (!configRoot["image"]) @@ -439,7 +436,7 @@ void RobotManager::startContainers(const std::string& fwkCfgPath, const std::str binds.push_back(v2); } - // The shared-memory bind belongs to "shm" mode only, + // The shared-memory bind belongs to "shm" mode only, // and must match the path AppWindow creates and cleans if (connectMode == "shm") { std::string shmPath(sharedMemoryPath); diff --git a/src/SimulationThread.cpp b/src/SimulationThread.cpp index f6a703a..41b0507 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -31,25 +31,26 @@ void SimulationThread::run() { // con il framework e con la sua frequenza di update (del nodo Brain) // Parametri: // kControlDecimation = numero di step da eseguire per ogni comando desiderato che arriva dal framework (posizione dei giunti desiderata) - // kTimestepPolicy = tempo (in secondi) corrispondente alla frequenza del nodo Brain del framework + // kTimestepPolicy = tempo (in secondi) corrispondente alla frequenza del nodo Brain del framework // (al momento 0.02 perché ho settato la frequenza di update del nodo Brain a 50Hz, quindi 1/50 = 0.02) // sim_dt = dt di quanto viene incrementata la simulazione ogni volta che viene chiamato mj_step (viene preso da SceneParser) // // Parametri del framework: // POLICY_DT = quanto viene aumentata la fase del gait ogni volta che viene pubblicato un comando (vale per le policy custom, - // e sarebbe buona pratica essere uguale "kTimestepPolicy", ma possono anche differire. La consequenza sarebbe una + // e sarebbe buona pratica essere uguale "kTimestepPolicy", ma possono anche differire. La consequenza sarebbe una // simulazione più lenta della realtà). Si trova al momento in Locomotion.h // // L'idea del funzionamento è la seguente: - // per ogni comando desiderato (che rappresenta la posizione desiderata dei giunti), l'idea è che quel comando verrà applicato più di una volta. + // per ogni comando desiderato (che rappresenta la posizione desiderata dei giunti), l'idea è che quel comando verrà applicato più di una volta. // In particolare, lo applicheremo "kControlDecimation" volte, e deve valere la relazione per cui: // POLICY_DT = kControlDecimation * sim_dt - // In questo modo, ogni volta che applichiamo un comando ad alto livello, teniamo conto che l'avanzamento totale della simulazione sarà di "POLICY_DT". - // + // In questo modo, ogni volta che applichiamo un comando ad alto livello, teniamo conto che l'avanzamento totale della simulazione sarà di + // "POLICY_DT". + // // Invece, ogni iterazione del ciclo interno "kControlDecimation" trasforma i comandi ad alto livello in torques a basso livello da dover // applicare alla simulazione per farla avanzare di un solo "sim_dt". Quando questi comandi sono stati applicati e la simulazione è avanzata, // il nuovo stato della simulazione deve essere inviato per poter calcolare i nuovi comandi a basso livello (i.e. "sendStateMessages"), e questi - // devono poter essere ricevuti (i.e. "receiveCommandMessages"). + // devono poter essere ricevuti (i.e. "receiveCommandMessages"). // In pratica, il loop "for" funziona come se ci fosse un PID ad alta frequenza che calcola le torques a partire dalle joints desiderate e dai // joint states attuali. // @@ -58,9 +59,9 @@ void SimulationThread::run() { // // N.B.1: in questa pipeline, è molto importante che il tempo di computazione dei "kControlDecimation" steps sia inferiore a "kTimestepPolicy", // perché altrimenti il nodo di Brain manda un nuovo comando desiderato mentre si sta ancora aggiornando la simulazione con il comando - // desiderato precedente. + // desiderato precedente. // - // N.B.2: POLICY_DT e kTimestepPolicy possono potenzialmente differire, il che significa che ogni volta che il nodo di Brain invia un comando + // N.B.2: POLICY_DT e kTimestepPolicy possono potenzialmente differire, il che significa che ogni volta che il nodo di Brain invia un comando // desiderato (cioè ogni "kTimestepPolicy" secondi), il comando è quello giusto per far avanzare la simulazione di "POLICY_DT" secondi // // N.B.3: il bottleneck al momento sembra essere sendStateMessages+receiveCommandMessages; aumentando il numero di kControlDecimation steps, @@ -71,7 +72,7 @@ void SimulationThread::run() { // constexpr double kTimestepPolicy = 0.2; // timestep (in SceneParser) = 0.004; // update_rate (nodo Brain)= 5; - + double sim_dt = model_->opt.timestep; using clock = std::chrono::steady_clock; @@ -80,8 +81,8 @@ void SimulationThread::run() { if (!paused_) { // DEBUG // auto stepsStart = clock::now(); - - for(int i=0; i(clock::now() - stepsStart).count(); // double stepsBudgetMs = kControlDecimation * sim_dt * 1000.0; - // std::cout << "[SimulationThread] tutto il ciclo impiega " << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << std::endl; + // std::cout << "[SimulationThread] tutto il ciclo impiega " << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << + // std::endl; next_step_time += std::chrono::duration_cast(std::chrono::duration(kTimestepPolicy)); std::this_thread::sleep_until(next_step_time);