Shared-memory communication option

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
 

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,6 +57,7 @@ class AppWindow : public QMainWindow {
 
         std::string frameworkConfigPath_ = spqr::frameworkConfigPath;
         std::string pathsConfigPath_ = spqr::pathsConfigPath;
+        std::string connectMode_ = "shm";
 };
 
 }  // namespace spqr

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

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>& robot, const std::string& image, const std::vector<std::string>& binds);
+        void create(const std::shared_ptr<Robot>& robot, const std::string& image, const std::vector<std::string>& binds,
+                    const std::string& connectMode = "shm");
 
         void start();
         void stop();

include/RobotManager.h

@@ -8,12 +8,16 @@
 #include <yaml-cpp/yaml.h>
 
 #include <Eigen/Eigen>
+#include <chrono>
+#include <map>
 #include <memory>
 #include <msgpack.hpp>
 #include <msgpack/v3/object_fwd_decl.hpp>
 #include <mutex>
+#include <set>
 #include <string>
 #include <thread>
+#include <unordered_map>
 #include <vector>
 
 #include "Constants.h"
@@ -41,16 +45,29 @@ 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);
+
+        // 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();
+
+        void initializeSocket(int port);
+
         bool areAllRobotsReady() const;
         bool areAllRobotsConnected() const;
 
-        void bindMujoco(MujocoContext* mujContext);
+        void bindMujoco(MujocoContext* mujContext, std::string connectMode_);
 
         std::shared_ptr<Robot> 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> 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<void()> cb);
         void applyCommands();
@@ -62,13 +79,23 @@ class RobotManager {
         RobotManager(const RobotManager&) = delete;
         RobotManager& operator=(const RobotManager&) = delete;
 
-        ssize_t send_all(int fd, char* buf, size_t len);
+        void sendStateMessagesSHM(bool publishImages);
+        void sendStateMessagesSocket();
+
+        void receiveCommandMessagesSHM();
+        void receiveCommandMessagesSocket();
+
+        void waitRobotConnectionsSocket();
+        void waitRobotConnectionsSHM();
 
-        std::atomic<bool> serverRunning_ = false;
-        std::thread serverThread_;
+        std::string connectMode_;
 
         mutable std::mutex mutex_;
         std::vector<std::shared_ptr<Robot>> robots_;
+        std::map<std::string, int> robotFdMap_;
+        int serverFd_ = -1;
+        std::vector<pollfd> pollFds_;
+        std::unordered_map<int, msgpack::unpacker> unpackers_;
         std::function<void()> areAllRobotsReadyCallback_;
 
         using RobotCreator = std::function<std::shared_ptr<Robot>(const std::string&, const std::string&, uint8_t, const Eigen::Vector3d&,

include/SimulationThread.h

@@ -3,11 +3,6 @@
 #include <mujoco/mujoco.h>
 
 #include <QThread>
-#include <map>
-#include <mutex>
-#include <vector>
-
-#include "robots/Robot.h"
 
 namespace spqr {
 
@@ -22,9 +17,6 @@ class SimulationThread : public QThread {
         void play();
         bool isPaused();
         void setMaxSimulationTime(int maxTime);
-        void initializeSocket(int port);
-        void waitRobotConnections();
-        void receiveCommandMessages();
 
     signals:
         void stepCompleted();
@@ -37,19 +29,9 @@ class SimulationThread : public QThread {
         std::atomic<bool> running_;
         std::atomic<bool> paused_;
         int maxSimulationTime_ = -1;  // -1 means no limit
-        std::map<std::string, int> entity_fd_map;
-
-        std::function<void()> areAllRobotsReadyCallback_;
-
-        // Socket for communication stuff
-        int server_fd;
-        std::vector<std::shared_ptr<Robot>> robots_;
-        std::vector<pollfd> fds;
-        mutable std::mutex mutex_;
-
-        ssize_t send_all(int fd, char* buf, size_t len);
 
-        void sendStateMessages();
+        static constexpr int kControlDecimation = 5;
+        static constexpr double kTimestepPolicy = 0.02;
 };
 
 }  // namespace spqr

include/ipc/SharedMemoryReader.h

@@ -0,0 +1,151 @@
+#pragma once
+
+#include <fcntl.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+#include <cstdint>
+#include <cstring>
+#include <string>
+#include <type_traits>
+
+#include "ipc/SharedMemoryWriter.h"  // for NoSharedMemoryMeta, ImageMeta
+
+namespace spqr {
+
+// Reader counterpart to SharedMemoryWriter<Meta> (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 <typename Meta = NoSharedMemoryMeta>
+class SharedMemoryReader {
+    public:
+        static_assert(std::is_trivially_copyable_v<Meta>, "SharedMemoryReader<Meta> 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;
+        }
+
+        // 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 != bytes) {
+                return false;
+            }
+
+            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<uint32_t>(seq_before % header_->slot_count);
+            const uint8_t* slots_base = static_cast<const uint8_t*>(map_ptr_) + sizeof(Header);
+            const uint8_t* src = slots_base + static_cast<size_t>(slot_idx) * header_->slot_bytes;
+
+            std::memcpy(out, src, bytes);
+
+            // 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) {
+                return false;
+            }
+
+            last_seq_ = seq_before;
+            return true;
+        }
+
+    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
+        // Must match SharedMemoryWriter::kVersion; see the note there on why the
+        // flat-slot layout required a bump rather than a silent format change.
+        static constexpr uint32_t kVersion = 3;
+
+        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<off_t>(sizeof(Header))) {
+                close_();
+                return false;
+            }
+
+            map_bytes_ = static_cast<size_t>(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<const Header*>(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