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/AppWindow.h b/include/AppWindow.h index 8ceef82..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,6 +57,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/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/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 2eea743..19d9041 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,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 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, 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(); @@ -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 serverRunning_ = false; - std::thread serverThread_; + std::string connectMode_; 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 ae5d9b4..5074882 100644 --- a/include/SimulationThread.h +++ b/include/SimulationThread.h @@ -3,11 +3,6 @@ #include #include -#include -#include -#include - -#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 running_; std::atomic paused_; int maxSimulationTime_ = -1; // -1 means no limit - std::map entity_fd_map; - - std::function areAllRobotsReadyCallback_; - - // Socket for communication stuff - int server_fd; - std::vector> robots_; - std::vector fds; - mutable std::mutex mutex_; - - ssize_t send_all(int fd, char* buf, size_t len); - void sendStateMessages(); + static constexpr int kControlDecimation = 5; + static constexpr double kTimestepPolicy = 0.02; }; } // namespace spqr diff --git a/include/ipc/SharedMemoryReader.h b/include/ipc/SharedMemoryReader.h new file mode 100644 index 0000000..efc44f6 --- /dev/null +++ b/include/ipc/SharedMemoryReader.h @@ -0,0 +1,151 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "ipc/SharedMemoryWriter.h" // for NoSharedMemoryMeta, ImageMeta + +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 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(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, 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(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/sensors/ImageSharedMemoryWriter.h b/include/ipc/SharedMemoryWriter.h similarity index 54% rename from include/sensors/ImageSharedMemoryWriter.h rename to include/ipc/SharedMemoryWriter.h index 7abf3b6..889dfd1 100644 --- a/include/sensors/ImageSharedMemoryWriter.h +++ b/include/ipc/SharedMemoryWriter.h @@ -10,24 +10,50 @@ #include #include #include -#include +#include namespace spqr { -class ImageSharedMemoryWriter { +// No metadata (default): use when the reader already knows how to interpret +// the payload (e.g. a fixed compile-time state struct). +struct NoSharedMemoryMeta {}; + +// 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; +}; + +// 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: 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: - ImageSharedMemoryWriter() = default; + static_assert(std::is_trivially_copyable_v, "SharedMemoryWriter requires a trivially-copyable Meta"); + + SharedMemoryWriter() = default; - ~ImageSharedMemoryWriter() { + ~SharedMemoryWriter() { close_(); } - void configure(const std::string& path, int width, int height, int channels, int ring_slots = 3) { - if (width <= 0 || height <= 0 || channels <= 0 || ring_slots <= 0) { - throw std::invalid_argument("Invalid shared-memory image configuration"); + // 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"); } - // 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_(); @@ -39,21 +65,20 @@ class ImageSharedMemoryWriter { fd_ = open(path.c_str(), O_RDWR | O_CREAT, 0666); if (fd_ < 0) { - throw std::runtime_error("Failed to open shared image file: " + path); + 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 image file: " + path); + throw std::runtime_error("Failed to resize shared memory 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); + throw std::runtime_error("Failed to mmap shared memory file: " + path); } header_ = static_cast(map_ptr_); @@ -62,12 +87,13 @@ class ImageSharedMemoryWriter { 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_->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; @@ -77,11 +103,11 @@ class ImageSharedMemoryWriter { return header_ != nullptr; } - void write(const std::vector& frame) { - if (!header_) { - return; - } - if (frame.size() != header_->slot_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; } @@ -89,7 +115,8 @@ class ImageSharedMemoryWriter { 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()); + + std::memcpy(dst, data, bytes); __atomic_store_n(&header_->seq, next_seq, __ATOMIC_RELEASE); } @@ -97,16 +124,18 @@ class ImageSharedMemoryWriter { struct Header { uint32_t magic = 0; uint32_t version = 0; - uint32_t width = 0; - uint32_t height = 0; - uint32_t channels = 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; + // 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_) { diff --git a/include/ipc/utils.h b/include/ipc/utils.h new file mode 100644 index 0000000..dd0cb88 --- /dev/null +++ b/include/ipc/utils.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +namespace spqr { + +// 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; + } + 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; +} + +} // namespace spqr diff --git a/include/robots/BoosterK1.h b/include/robots/BoosterK1.h index 821d080..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); @@ -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) { @@ -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_); @@ -93,6 +93,15 @@ class BoosterK1 : public Robot { return msg; } + void sendMessageSHM(bool publishImages) override { + // Socket-only for now: BoosterK1 doesn't yet have a shared-memory state channel. + } + + bool receiveMessageSHM() 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 e0917bc..c6f3e6f 100644 --- a/include/robots/BoosterT1.h +++ b/include/robots/BoosterT1.h @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -18,12 +19,13 @@ #include #include "MujocoContext.h" +#include "ipc/SharedMemoryReader.h" +#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/ImageSharedMemoryWriter.h" #include "sensors/Imu.h" #include "sensors/Joint.h" #include "sensors/Oracle.h" @@ -34,6 +36,48 @@ namespace spqr { class Team; // Forward declaration +constexpr size_t kBoosterT1JointCount = 23; + +// 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). +struct BoosterT1SharedState { + PoseData pose; + ImuData imu; + JointState joints; + OracleData oracle; + GroundRelativePositionData headPose; +}; + +// 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; + +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; +}; + class BoosterT1 : public Robot { public: Pose* pose = nullptr; @@ -70,12 +114,9 @@ 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"}} { - // Where to put the images - shm_dir_ = "/dev/shm/circus_ipc"; - } + {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()); headPose = new GroundRelativePosition(mujCtx->model, mujCtx->data, (name + "_head_rgb_cam_site").c_str(), GroundRelativePosition::TargetType::Site, pose); @@ -111,19 +152,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()); - - // 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 is 16-bit (16UC1): two bytes per pixel, published without precision loss - depth_writer_.configure(shmFilePath_("depth"), width, height, 2); - // Create Oracle with the pose and all robots oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose); + + 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); + + // Configuration of shm channels + 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); + } } - 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"); @@ -146,7 +193,21 @@ class BoosterT1 : public Robot { } } - std::map sendMessage() override { + bool receiveMessageSHM() override { + JointTorques torques; + if (!command_reader_.readLatest(&torques, sizeof(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 packMessage() override { buffer_zone_.clear(); std::map msg; msg["robot_name"] = msgpack::object(name, buffer_zone_); @@ -157,13 +218,27 @@ class BoosterT1 : public Robot { msg["oracle"] = oracle->serialize(buffer_zone_); msg["camera_info"] = rgbCameraInfo->serialize(buffer_zone_); - // Write in the shared file the information - rgb_writer_.write(rgbCamera->getImage()); - depth_writer_.write(depthCamera->getDepth16UC1()); - return msg; } + void sendMessageSHM(bool publishImages) override { + 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 + // 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(); + 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 { std::map sensors; sensors["pose"] = pose; @@ -195,16 +270,14 @@ class BoosterT1 : public Robot { ~BoosterT1() = default; private: - std::string shmFilePath_(const std::string& camera) const { - return shm_dir_ + "/" + name + "_" + camera + ".shm"; - } - std::map joint_map; std::unordered_map latestTorques; std::string shm_dir_; - ImageSharedMemoryWriter rgb_writer_; - ImageSharedMemoryWriter depth_writer_; + 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 306ee93..339dbc5 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -8,14 +8,17 @@ #include #include +#include #include #include #include #include #include +#include "Constants.h" #include "Container.h" #include "MujocoContext.h" +#include "ipc/utils.h" #include "sensors/Sensor.h" #define MAX_MSG_SIZE 1048576 // 1MB @@ -43,15 +46,56 @@ 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. + 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) = 0; + virtual void bindMujoco(MujocoContext* mujContext, std::string connectMode_) = 0; virtual void update() = 0; - virtual void receiveMessage(const std::map& message) = 0; - virtual std::map sendMessage() = 0; + + virtual void sendMessageSocket(int fd) final { + auto message = packMessage(); + msgpack::sbuffer sbuf; + msgpack::pack(sbuf, message); + if (sbuf.size() > 0) { + send_unframed(fd, sbuf.data(), sbuf.size()); + } + } + + virtual void receiveMessageSocket(const std::map& message) = 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 + // memory (SimulationThread::waitRobotConnectionsSHM(), the "shm" connect-mode + // counterpart to the socket handshake in waitRobotConnections()). + bool hasConnectSignalSHM() const { + return std::filesystem::exists(receive_shm_path); + } + virtual std::map getSensors() = 0; virtual void applyCommands() = 0; + private: + virtual std::map packMessage() = 0; + + protected: + std::string receive_shm_path; + std::string send_shm_path; + 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 466116d..3833e95 100644 --- a/include/sensors/GroundRelativePosition.h +++ b/include/sensors/GroundRelativePosition.h @@ -10,6 +10,10 @@ 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 +55,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_; } 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..f27b83d 100644 --- a/include/sensors/Joint.h +++ b/include/sensors/Joint.h @@ -39,6 +39,26 @@ 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] = {}; +}; + +// 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::receiveMessageSocket() today. +template +struct JointTorques { + double torque[N] = {}; +}; + class Joints : public Sensor { public: Joints(mjModel* mujModel, mjData* mujData, std::map map) : mujModel(mujModel), mujData(mujData) { @@ -123,6 +143,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/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 846c254..19502a9 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -1,6 +1,7 @@ #include "AppWindow.h" #include +#include #include #include @@ -45,6 +46,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]; // TODO: forse si può aggiungere come variabile d'ambiente?? } resize(spqr::initialWindowWidth, spqr::initialWindowHeight); @@ -115,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(); @@ -214,24 +225,36 @@ 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") { + 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."); + } + + CircusNetwork::instance().init(); + RobotManager::instance().bindMujoco(mujContext.get(), connectMode_); // memo: this must be run before starting the communications server - CircusNetwork::instance().init(); - RobotManager::instance().bindMujoco(mujContext.get()); // memo: this must be run before starting the communications server - sim->initializeSocket(frameworkCommunicationPort); + } 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_); + RobotManager::instance().startContainers(frameworkConfigPath_, pathsConfigPath_, connectMode_); + std::cout << "Simulation starting with " << connectMode_ << " connection mode..." << std::endl; std::cout << "Connecting Robots..." << std::endl; - sim->waitRobotConnections(); + RobotManager::instance().waitRobotConnections(); std::cout << "Waiting Robots are Ready..." << std::endl; - sim->receiveCommandMessages(); + RobotManager::instance().receiveCommandMessages(); // Set initial simulation state (playing when scene is loaded) toolsPanel->setSimulationPlaying(true); @@ -341,6 +364,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(); @@ -353,6 +377,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 f5e0a4e..bf339e4 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); @@ -52,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}}})}, @@ -85,7 +85,8 @@ void Container::create(const std::shared_ptr& robot, const std::string& i "XAUTHORITY=/root/.Xauthority", "XDG_RUNTIME_DIR=/run/user/0", "ROBOT_STACK=booster", - "CIRCUS_IMAGE_SHM_DIR=/dev/shm/circus_ipc", + "CIRCUS_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"), "TASK_NAME=" + envOrDefault("TASK_NAME", "t1-velocity"), diff --git a/src/RobotManager.cpp b/src/RobotManager.cpp index 1c14959..ebec21b 100644 --- a/src/RobotManager.cpp +++ b/src/RobotManager.cpp @@ -1,10 +1,13 @@ #include "RobotManager.h" +#include + #include #include "Constants.h" -#include "Team.h" // needed for the forward declaration in the .h +#include "Team.h" #include "Utils.h" +#include "ipc/utils.h" namespace spqr { @@ -30,6 +33,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,9 +71,306 @@ void RobotManager::clear() { robots_.clear(); } -void RobotManager::bindMujoco(MujocoContext* mujContext) { +void RobotManager::sendStateMessages(bool publishImages) { + if (connectMode_ == "shm") { + // std::cout << "[sendStateMessages] sending message with SHM" << std::endl; + sendStateMessagesSHM(publishImages); + } 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(bool publishImages) { + for (auto& r : robots_) { + std::unique_lock lock(mutex_); + r->sendMessageSHM(publishImages); + } +} + +void RobotManager::sendStateMessagesSocket() { + for (auto& r : robots_) { + std::unique_lock lock(mutex_); + auto fdIt = robotFdMap_.find(r->name); + r->sendMessageSocket(fdIt != robotFdMap_.end() ? fdIt->second : -1); + } +} + +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)) { + 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(true); + } + 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; + + auto fdIt = robotFdMap_.find(r->name); + r->sendMessageSocket(fdIt != robotFdMap_.end() ? fdIt->second : -1); + } + } + 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::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; + unpackers_.erase(fd); + removeRobotFd(fd); + continue; + } + if (status == 0) + 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::waitRobotConnections() { + if (connectMode_ == "shm") { + waitRobotConnectionsSHM(); + } else { + waitRobotConnectionsSocket(); + } +} + +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 + msgpack::unpacker unp; + msgpack::object_handle oh; + int status = recv_latest(client_fd, unp, oh, MAX_MSG_SIZE); + + if (status <= 0) { + std::cerr << "Error reading the initial message.\n"; + // close(client_fd); + continue; + } + + 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 socket" << std::endl; + break; + } + } + } + if (areAllRobotsConnected()) { + areAllConnected = true; + break; + } + } + } + } + } + } + + std::cout << "All Robots are connected!" << std::endl; +} + +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->hasConnectSignalSHM()) { + r->isConnected = true; + r->sendMessageSHM(true); + 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, 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, @@ -65,7 +386,10 @@ 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) { + // Save connection mode + connectMode_ = connectMode; + YAML::Node configRoot = loadYamlFile(fwkCfgPath.c_str()); if (!configRoot["image"]) @@ -111,9 +435,19 @@ 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); + r->container->create(r, image, binds, connectMode); r->container->start(); } std::cout << "Containers started successfully!" << std::endl; @@ -136,4 +470,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/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 4453aa7..41b0507 100644 --- a/src/SimulationThread.cpp +++ b/src/SimulationThread.cpp @@ -1,6 +1,5 @@ #include "SimulationThread.h" -#include #include #include "GameController.h" @@ -8,226 +7,8 @@ namespace spqr { -SimulationThread::SimulationThread(const mjModel* model, mjData* data) : model_(model), data_(data), running_(true), paused_(false) {} - -// Source - https://stackoverflow.com/a -// Posted by Arun, modified by community. See post 'Timeline' for change history -// Retrieved 2026-01-12, License - CC BY-SA 3.0 -// TCP Communication, it sends before the size of the message and then the message itself -ssize_t SimulationThread::send_all(int fd, char* buf, size_t len) { - // First, send the size of the message - uint32_t msg_size = htonl(len); - ssize_t bytes_sent = send(fd, &msg_size, sizeof(msg_size), 0); - if (bytes_sent != sizeof(msg_size)) { - return -1; - } - - ssize_t total = 0; // how many bytes we've sent - size_t bytesleft = len; // how many we have left to send - ssize_t n = 0; - while (total < len) { - n = send(fd, buf + total, bytesleft, 0); - if (n == -1) { - /* print/log error details */ - return -1; - } - total += n; - bytesleft -= n; - } - return total; -} - -void SimulationThread::initializeSocket(int port) { - server_fd = socket(AF_INET, SOCK_STREAM, 0); - if (server_fd < 0) - throw std::runtime_error("Failed to create socket"); - - int opt = 1; - setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); - int send_buf_size = 1 * 1024 * 1024; - if (setsockopt(server_fd, SOL_SOCKET, SO_SNDBUF, &send_buf_size, sizeof(send_buf_size)) < 0) { - perror("setsockopt(SO_SNDBUF)"); - } - int recv_buf_size = 1 * 1024 * 1024; - if (setsockopt(server_fd, SOL_SOCKET, SO_RCVBUF, &recv_buf_size, sizeof(recv_buf_size)) < 0) { - perror("setsockopt(SO_RCVBUF)"); - } - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = INADDR_ANY; - address.sin_port = htons(port); - - robots_ = RobotManager::instance().getRobots(); - if (bind(server_fd, (struct sockaddr*)&address, sizeof(address)) < 0) { - perror("bind"); - throw std::runtime_error("Socket bind failed"); - } - if (listen(server_fd, robots_.size()) < 0) - throw std::runtime_error("Listen failed"); - - fds.push_back({server_fd, POLLIN, 0}); -} - -void SimulationThread::receiveCommandMessages() { - int robot_size = robots_.size(); - int done = 0; - // Track which robots have not yet replied this step - std::set pendingRobots; - for (auto& r : robots_) - pendingRobots.insert(r->name); - - int timeoutCount = 0; - while (done < robot_size) { - int ret = poll(fds.data(), fds.size(), 500); - if (ret <= 0) { - ++timeoutCount; - // Resend state only every 5 timeouts (2.5s) to avoid flooding the TCP buffer - if (timeoutCount % 5 != 0) - continue; - std::unique_lock lock(mutex_); - for (auto& r : robots_) { - if (pendingRobots.count(r->name)) { - std::cout << "[receiveCommandMessages] Timeout, resending state to: " << r->name << std::endl; - msgpack::sbuffer sbuf; - auto answ = r->sendMessage(); - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - int fd = entity_fd_map[r->name]; - if (fd) - send_all(fd, sbuf.data(), sbuf.size()); - else - std::cerr << "[receiveCommandMessages] No fd for: " << r->name << std::endl; - } - } - } - continue; - } - timeoutCount = 0; - - for (size_t i = 0; i < fds.size(); ++i) { - // An event occured for the i-th fd - if (fds[i].revents & POLLIN) { - char buffer[MAX_MSG_SIZE]; - int n = read(fds[i].fd, buffer, sizeof(buffer) - 1); - if (n <= 0) { - close(fds[i].fd); - fds.erase(fds.begin() + i); - --i; - continue; - } - - msgpack::object_handle oh = msgpack::unpack(buffer, n); - auto data_map = oh.get().as>(); - auto it = data_map.find("robot_name"); - if (it == data_map.end()) - continue; - - std::string messageRecipient = it->second.as(); - - { - std::unique_lock lock(mutex_); - for (auto& r : robots_) { - if (r->name == messageRecipient) { - if (!r->isReady) { - r->isReady = true; - std::cout << "Robot ready: " << r->name << std::endl; - if (RobotManager::instance().areAllRobotsReady()) { - emit allRobotsReadySignal(); - } - } - - r->receiveMessage(data_map); - pendingRobots.erase(messageRecipient); - ++done; - break; - } - } - } - } - } - } -} - -void SimulationThread::waitRobotConnections() { - bool areAllConnected = false; - while (!areAllConnected) { - int ret = poll(fds.data(), fds.size(), 100); - if (ret <= 0) - continue; // Timeout, skip iteration (timeout necessary to check whether serverRunning_ is - - for (size_t i = 0; i < fds.size(); ++i) { - // An event occured for the i-th fd - if (fds[i].revents & POLLIN) { - if (fds[i].fd == server_fd) { - if (!entity_fd_map["server"]) { - entity_fd_map["server"] = server_fd; - } - // The only event for the server is someone knocking - int client_fd = accept(server_fd, nullptr, nullptr); - if (client_fd >= 0) { - fds.push_back({client_fd, POLLIN, 0}); - - // Receive initial message with robot name - char buffer[MAX_MSG_SIZE]; - int n = read(client_fd, buffer, sizeof(buffer) - 1); - - if (n <= 0) { - std::cerr << "Error reading the initial message.\n"; - // close(client_fd); - continue; - } - - // unpack of the MsgPack message - msgpack::object_handle oh = msgpack::unpack(buffer, n); - msgpack::object obj = oh.get(); - - // First message is the robot name as a string - if (obj.type != msgpack::type::STR) { - std::cerr << "First message must be a string. Ignore it...\n"; - continue; - } - - std::string robotName = obj.as(); - entity_fd_map[robotName] = client_fd; - - // Send message with initial state - msgpack::sbuffer sbuf; - std::map answ; - bool answOk = false; - // Pack initial message - { - std::lock_guard lock(mutex_); - for (auto& r : robots_) { - if (r->name == robotName) { - r->isConnected = true; - answ = r->sendMessage(); - answOk = true; - break; - } - } - } - if (answOk) { - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - std::cout << "Connected Robot: " << robotName << "\n"; - std::cout << "Sending initial message to " << robotName << std::endl; - ssize_t bytes_sent = send_all(client_fd, sbuf.data(), sbuf.size()); - if (bytes_sent <= 0) { - perror("Error in sending initial message"); - } - } - } - if (RobotManager::instance().areAllRobotsConnected()) { - areAllConnected = true; - std::cout << "All Robots are connected!" << std::endl; - break; - } - } - } - } - } - } +SimulationThread::SimulationThread(const mjModel* model, mjData* data) : model_(model), data_(data), running_(true), paused_(false) { + RobotManager::instance().setAreAllRobotsReadyCallback([this]() { emit allRobotsReadySignal(); }); } /* @@ -246,38 +27,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.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 + // 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 + // + // 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; 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(); + // DEBUG + // auto stepsStart = clock::now(); + + for (int i = 0; i < kControlDecimation; ++i) { + 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; + } - std::memset(data_->xfrc_applied, 0, model_->nbody * 6 * sizeof(mjtNum)); + // Camera frames only change once per GUI-thread render cycle, so only + // publish them on the last substep of each control step rather than + // re-copying them on every one of the kControlDecimation substeps. + const bool publishImages = (i == kControlDecimation - 1); - if (maxSimulationTime_ > 0 && data_->time >= maxSimulationTime_) { - running_ = false; - emit maxSimulationTimeReached(); - break; + RobotManager::instance().sendStateMessages(publishImages); + RobotManager::instance().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] tutto il ciclo impiega " << stepsElapsedMs << " ms (sim-time budget " << stepsBudgetMs << " ms)" << + // std::endl; - // for each robot: - // send(state) // non-blocking - // recv(torques) // bloccante o con timeout - - sendStateMessages(); - receiveCommandMessages(); + next_step_time += std::chrono::duration_cast(std::chrono::duration(kTimestepPolicy)); + std::this_thread::sleep_until(next_step_time); - 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(); + 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)); @@ -287,32 +125,6 @@ void SimulationThread::run() { } } -void SimulationThread::sendStateMessages() { - for (auto& r : robots_) { - msgpack::sbuffer sbuf; - std::map answ; - bool answOk = false; - { - std::unique_lock lock(mutex_); - answ = r->sendMessage(); - answOk = true; - } - - if (answOk) { - msgpack::pack(sbuf, answ); - if (sbuf.size() > 0) { - int fd = entity_fd_map[r->name]; - if (!fd) - perror("file descriptor not existing"); - ssize_t bytes_sent = send_all(fd, sbuf.data(), sbuf.size()); - if (bytes_sent <= 0) { - perror("Sending message"); - } - } - } - } -} - void SimulationThread::stop() { running_ = false; wait();