diff --git a/rmw_unix_socket_cpp/DESIGN.md b/rmw_unix_socket_cpp/DESIGN.md index bf564ff..21be2f7 100644 --- a/rmw_unix_socket_cpp/DESIGN.md +++ b/rmw_unix_socket_cpp/DESIGN.md @@ -16,7 +16,7 @@ The middleware has four components: a shared-memory discovery registry, an AF_UN **Serialization (CDR via fastcdr).** Messages are serialized to CDR using `fastcdr` driven by the `rosidl_typesupport_fastrtps_cpp` callbacks generated for each message type. This is the same encoding the default DDS-based RMWs use. Because the serialize and deserialize routines are compiled per message type, there is no runtime field walking. The Serialization section explains why an earlier introspection-based serializer was abandoned. -**Wait mechanism (`epoll` + `eventfd` + a doorbell).** `rmw_wait()` blocks on `epoll`, watching the receive-socket file descriptors (fds) of every subscription, service, and client in the wait set plus an `eventfd` per guard condition, plus one per-context doorbell socket that other processes ring after any registry mutation, so a blocked wait learns about graph changes without polling (see The doorbell under Wait set). `epoll` is used here rather than `poll`/`select`; the wait section explains why. There are no background receiver threads: all socket draining happens inside `rmw_wait()`, which the executor already calls in a tight loop. This keeps the data path single-threaded per process and avoids lock contention on internal queues. +**Wait mechanism (`epoll` + `eventfd` + a doorbell).** `rmw_wait()` blocks on `epoll`, watching the receive-socket file descriptors (fds) of every subscription, service, and client in the wait set plus an `eventfd` per guard condition, plus — in processes that consume graph events — one per-context doorbell socket that other processes ring after any registry mutation, so a blocked graph wait learns about changes without polling (see The doorbell under Wait set). `epoll` is used here rather than `poll`/`select`; the wait section explains why. There are no background receiver threads: all socket draining happens inside `rmw_wait()`, which the executor already calls in a tight loop. This keeps the data path single-threaded per process and avoids lock contention on internal queues. **End-to-end publish flow.** On `rmw_publish`, the node serializes the ROS message to CDR once — into a heap payload for small messages, or directly into the shared-memory ring record for large ones (see Staging and fanout). It fills in a fixed 37-byte wire header (sender GID, sequence number, send timestamp, payload size, and a type byte). It then reads the registry's `generation` counter and compares it to the value cached on the publisher. If the graph has not changed, the publisher reuses its cached list of subscriber socket paths and never touches the registry. Only when `generation` has moved does it re-scan the registry to rebuild that list. For each subscriber path it issues one `sendmsg` with gather I/O, so the header and payload become one datagram with no intermediate copy. Sends are non-blocking and best-effort, one kernel copy into each subscriber's socket buffer. Payloads of 64 KiB and above take a different route: the publisher stages the bytes once in its shared-memory ring and each subscriber receives only a 32-byte descriptor datagram (see Large payloads under Transport). On the receiving side, a later `rmw_wait()` drains the bound socket, splits the header from the payload, and queues the message; `rmw_take` then deserializes it and hands it to the subscription callback. In steady state, the only discovery cost per publish is a single atomic read of the generation counter. @@ -532,7 +532,7 @@ The ROS 2 executor finds out that work is ready by calling `rmw_wait()`. It hand Each `rmw_wait()` runs the same sequence on the calling thread: 1. **Drain first.** Every subscription, service, and client socket is drained into a per-entity message queue before anything blocks. The receive sockets are `SOCK_DGRAM | SOCK_NONBLOCK`, so the drain loop calls `recv_from` repeatedly and stops cleanly on `EAGAIN` (nothing left to read). This step exists because data may have arrived between the previous `rmw_wait()` and this one; draining up front means such data is not missed. -2. **Check the graph.** The wait set carries its context (stored at `rmw_create_wait_set`), so this step runs for every wait — including a wait set that holds only guard conditions, which is exactly the shape rclcpp's GraphListener uses. The check first drains the context's doorbell socket, then reads the registry's `generation` counter and compares it against the value cached on the context. If it moved, the graph changed: any `TRANSIENT_LOCAL` publishers replay their cached messages to newly-matched subscribers, and every node's graph guard condition is triggered (see the next subsection). The drain-before-read order is half of the lost-wakeup proof; the other half is on the registry's writer side. +2. **Check the graph.** The wait set carries its context (stored at `rmw_create_wait_set`), so this step runs for every wait — including a wait set that holds only guard conditions, which is exactly the shape rclcpp's GraphListener uses. The check first drains the context's doorbell socket, then reads the registry's `generation` counter and compares it against the value cached on the context. If it moved, the graph changed: every node's graph guard condition is triggered (see the next subsection). (`TRANSIENT_LOCAL` replay no longer runs here — it is pull-based, served inside `rmw_create_subscription` of the joining process.) The drain-before-read order is half of the lost-wakeup proof; the other half is on the registry's writer side. 3. **Arm the fds.** Every entity fd, guard-condition eventfd, and the context's doorbell fd is added to the epoll instance with `EPOLL_CTL_ADD`. This is idempotent across calls: a still-live fd returns `EEXIST` and is treated as already armed, and a fd number reused after its previous owner closed gets freshly armed. The kernel removes closed fds from an epoll set automatically, so there is no matching `EPOLL_CTL_DEL` and no per-call teardown. 4. **Check ready, then block only if needed.** If any queue already holds a message, or any guard-condition eventfd reads as triggered, the call skips blocking entirely and reports what is ready. Only when nothing is ready does it block in `epoll_wait()`. The thread is asleep in the kernel here, not spinning; there is no internal poll interval. A wake caused only by the doorbell is internal: the wait re-runs the graph check of step 2 and goes back to sleep for the caller's remaining time. `epoll_wait()` is retried on `EINTR` against the same deadline so a signal neither returns a false timeout nor busy-loops. 5. **Drain again and report.** After waking, the sockets are drained once more, then the output arrays are pruned: entities with no pending data are set to `NULL`, and the ones that are ready are left in place for the executor to service. `RMW_RET_TIMEOUT` is returned only once the caller's own deadline has passed. A wake that drained nothing to take returns `RMW_RET_OK` with every entry `NULL` instead: a drain can legitimately yield nothing — a subscription with `ignore_local_publications` set drops its own context's messages, and a descriptor whose sender is gone is dropped — and that is a spurious wake, not a timeout. An infinite wait therefore never reports `RMW_RET_TIMEOUT`. @@ -547,7 +547,7 @@ To reach it, the context keeps a mutex-guarded list of every node's graph guard A single atomic load can tell a *running* wait that the graph changed, but it cannot wake a *blocked* one: an mmap store is invisible to `epoll`. Something a mutation can touch must be a file descriptor in the sleeping process. The doorbell is that object, chosen over the alternatives (signals, cross-process eventfd passing, inotify, io_uring futex — each fails on hygiene, permissions, coverage, or container seccomp) because it reuses the one primitive this transport is already made of: an `AF_UNIX` datagram socket. -Each context binds one doorbell socket at `rmw_init` (a `ctl_*` file beside the data sockets) and registers it in the registry as `ENTRY_DOORBELL` — before taking its first generation snapshot, so no mutation can fall between the snapshot and the wiring. Every registry mutation (add, remove, stale-slot reclaim), after bumping the generation counter, sends one octet to every registered doorbell, non-blocking and best-effort. +Each context binds one doorbell socket at `rmw_init` (a `ctl_*` file beside the data sockets), but registers it in the registry as `ENTRY_DOORBELL` **lazily and interest-scoped**: on the first `rmw_wait` whose wait set contains one of the context's graph guard conditions — i.e. only in processes that actually consume graph events (`wait_for_service`, rclcpp's GraphListener, rosbag2 discovery). With TRANSIENT_LOCAL replay pull-based, those are the only processes a registry mutation needs to wake, so a fleet launch rings approximately as many doorbells as there are graph-event consumers, not mutations x processes — the broadcast storm that motivated this scoping. The lazy registration runs strictly before that wait's generation check (the same register-before-snapshot ordering the eager `rmw_init` recipe used), on an already-bound socket, and a failed registration (registry full) is retried on the next wait rather than latched. Every registry mutation (add, remove, stale-slot reclaim — except mutations of doorbell slots themselves, which have no wake consumers), after bumping the generation counter, sends one octet to every registered doorbell, non-blocking and best-effort. The correctness argument is one ordering pair. The writer rings strictly **after** the generation bump; the waiter drains its doorbell strictly **before** reading the generation. Any mutation therefore either lands in the generation value the waiter is about to read, or leaves a datagram queued on a level-triggered fd that makes the next `epoll_wait` return immediately. The datagram queues whether or not the target is currently blocked, so there is no check-then-block race to lose. The proof rests entirely on those two orderings; both call sites carry a comment saying so, and a regression test pins the behavior. @@ -601,7 +601,11 @@ The resolved profile is stored on the endpoint and copied into its registry slot `durability = VOLATILE` is the straightforward case: the publisher does not retain anything, so a subscription that joins after a message was published never sees it. -`durability = TRANSIENT_LOCAL` is implemented on the publisher side. A TRANSIENT_LOCAL publisher keeps its last `depth` messages in a local cache, trimmed the same way the subscriber queue is. When a new subscriber appears (detected because the registry generation counter moved), the publisher replays the cached messages to it. This is the one place `depth` controls a publisher-side structure rather than a subscriber-side one. +`durability = TRANSIENT_LOCAL` is **pull-based**, the same philosophy as discovery itself: the latched history lives in shared memory, and the party that wants it reads it. At publish time the publisher writes each sample into a per-publisher latched-cache segment in `/dev/shm` (`ros2_uds_tl___`), a fixed ring of `depth` per-record-seqlocked slots created *before* the publisher's registry slot and named *in* that slot (the previously unused publisher `socket_path` field). A late-joining TRANSIENT_LOCAL subscription, inside `rmw_create_subscription`, queries the registry for matching latched publishers, maps each cache read-only, snapshots the committed records (bounded per-slot retries — a publisher killed mid-write poisons one slot, never the history, and never hangs creation), validates the segment's embedded 16-byte GID against the slot it came from, and enqueues the history into its own queue before the handle is returned. The idle publisher is never woken, polled, or rung — replay costs it nothing, ever. + +Losslessness rests on a store-buffering fence pair: the publisher does *ring-write → `seq_cst` fence → fresh generation load → refresh subscriber cache → fan out*, the subscriber does *registry_add → `seq_cst` fence → pull*. A sample therefore either lands in a cache the pull observes, or its publisher observes the subscriber's registration and delivers it as a datagram; the overlap is deduplicated by a per-publisher sequence watermark recorded at pull time (sequence numbers are assigned inside the latch critical section, so ring order equals sequence order and "at or below the watermark" exactly means "pulled or lapped"). Payloads above a small embed cap (`TL_EMBED_CAP`, 1 KiB) are staged once into the existing durable segments and the slot carries the 32-byte descriptor; the ring's byte footprint is capped (`TL_RING_MAX_BYTES`, 2 MiB), so an extreme `depth` replays the newest samples that fit — a stated, accepted limit. If the cache cannot be created or a slot cannot be committed (`/dev/shm` unavailable or full), the publisher runs latch-less with a logged error: live delivery is unaffected, late joiners get nothing — there is no heap fallback to replay from anymore. `depth` still controls this one publisher-side structure. Two consequences are deliberate: a VOLATILE late joiner no longer receives latched history (the pull is durability-gated — the DDS-correct behaviour, where the old push replayed to every subscriber on the topic), and replay from an already-dead publisher's still-mapped cache remains possible until its slot is reaped, a mild extension of DDS writer-lifetime semantics. + +Three operational notes. **/dev/shm budget:** latched history that used to live on the heap now lives in tmpfs — budget roughly one latched ring per TRANSIENT_LOCAL publisher (sparse file up to `TL_RING_MAX_BYTES`; committed pages only for slots actually latched, so a chatty depth-1000 topic like `/rosout` converges toward ~1 MiB) plus one durable segment per over-cap latched sample currently live in a ring, each committing its **whole payload** up front (`posix_fallocate`, page-rounded — a retained 5 MiB sample costs 5 MiB, not an inode), so add the sum of the retained over-cap payloads, up to each publisher's effective depth. Docker's default 64 MB `--shm-size` is not enough at fleet scale: the registry alone is 38 MB — size the mount explicitly. **Dedup bound:** the per-publisher watermark claims only "pulled or already lapped out of the publisher's ring at pull time"; a subscriber that matches mid-burst may therefore not receive samples older than the newest ring-depth — exactly the history DDS would not owe it either — and when a publisher latches concurrently with the pull's scan, the pull keeps only the contiguous prefix of what it saw, letting the rest arrive as datagrams rather than risk the watermark swallowing a missed sample. **Mixed-build reaping:** an old binary's stale-slot reaper does not know the tl_ prefix, so it zeroes a dead new-binary publisher's slot without unlinking the cache segment; the leak is bounded and reclaimed by any new-binary process's `rmw_init` sweep. ### Reliability: accepted, not differentiated @@ -733,7 +737,7 @@ Each message must fit in a single datagram; the usable cap is roughly 400 KB on #### Notification requires a thread inside rmw_wait -Graph events and `TRANSIENT_LOCAL` late-joiner replay are serviced from inside `rmw_wait` (woken by the doorbell). A process none of whose threads ever enters `rmw_wait` — an executor wedged in a user callback, or a bare-rmw publisher that never waits — cannot replay its latched messages or observe graph changes until it next waits or publishes. Standard rclcpp nodes always have a waiting thread (the GraphListener), so this bites only unusual bare-rmw setups. +Graph events are serviced from inside `rmw_wait` (woken by the doorbell). A process none of whose threads ever enters `rmw_wait` — an executor wedged in a user callback — cannot observe graph changes until it next waits. `TRANSIENT_LOCAL` late-joiner replay no longer has this limit at all: the joining subscriber pulls the latched cache itself, so an idle, wedged, or never-waiting publisher process still replays. #### Functions that return `RMW_RET_UNSUPPORTED` diff --git a/rmw_unix_socket_cpp/src/registry.cpp b/rmw_unix_socket_cpp/src/registry.cpp index 84080b3..6ce7c75 100644 --- a/rmw_unix_socket_cpp/src/registry.cpp +++ b/rmw_unix_socket_cpp/src/registry.cpp @@ -281,7 +281,13 @@ static int32_t try_add_once(RegistryHeader * header, const RegistryEntry & entry !header->high_water_slot.compare_exchange_weak( cur, want, std::memory_order_relaxed, std::memory_order_relaxed)) {} header->generation.fetch_add(1, std::memory_order_acq_rel); - ring_doorbells(header); // strictly after the bump — see ring_doorbells + // Strictly after the bump — see ring_doorbells. Doorbell registration + // itself rings nobody: the slot is invisible to queries, no consumer + // exists for the wake, and ringing here would let K graph-waiting + // processes registering at launch produce a K^2/2 datagram mini-storm. + if (entry.type != ENTRY_DOORBELL) { + ring_doorbells(header); + } return static_cast(i); } } @@ -325,9 +331,16 @@ static void teardown_slot(RegistryEntrySlot * slot) slot->seq.fetch_add(1, std::memory_order_acq_rel); // Unlink outside the seqlock — filesystem op, doesn't need to be observable - // by other readers atomically. + // by other readers atomically. A TRANSIENT_LOCAL publisher slot names its + // latched-cache shm segment here (not a socket file), so a stale-PID reap + // reclaims the dead publisher's cache promptly instead of leaking it until + // the next rmw_init sweep. if (path_copy[0] != '\0') { - unlink(path_copy); + if (std::strncmp(path_copy, "/ros2_uds_tl_", 13) == 0) { + shm_unlink(path_copy); + } else { + unlink(path_copy); + } } } @@ -351,7 +364,9 @@ void registry_remove(RegistryHeader * header, int32_t index) } teardown_slot(slot); header->generation.fetch_add(1, std::memory_order_acq_rel); - ring_doorbells(header); // strictly after the bump — see ring_doorbells + if (st != ENTRY_DOORBELL) { // doorbell slots have no wake consumers + ring_doorbells(header); // strictly after the bump — see ring_doorbells + } } // Best-effort: stat /proc/. ENOENT means the PID is not in our @@ -525,7 +540,9 @@ void registry_cleanup_stale(RegistryHeader * header) teardown_slot(&slots[i]); header->generation.fetch_add(1, std::memory_order_acq_rel); - reclaimed = true; + if (expected != static_cast(ENTRY_DOORBELL)) { + reclaimed = true; // doorbell-only reclaims ring nobody + } } if (reclaimed) { ring_doorbells(header); // strictly after the bump(s) — see ring_doorbells diff --git a/rmw_unix_socket_cpp/src/rmw_graph.cpp b/rmw_unix_socket_cpp/src/rmw_graph.cpp index 0cdea93..9ce7bda 100644 --- a/rmw_unix_socket_cpp/src/rmw_graph.cpp +++ b/rmw_unix_socket_cpp/src/rmw_graph.cpp @@ -66,8 +66,9 @@ static constexpr int64_t CLEANUP_MIN_INTERVAL_NS = 1000000000; // 1 s // latency: reclaiming a dead process's slots — and with them the doorbell // rings and latched-cache teardown only a sweep produces — now lags up to one // interval per polling context. The full-registry path in registry_add still -// sweeps unconditionally, because there it is the last resort before entity -// creation fails. +// sweeps unconditionally: there it is the last resort before entity creation +// fails, and also a recurring retry path (a graph wait whose doorbell +// registration failed on a full registry retries registry_add every wait). static void maybe_cleanup_stale(rmw_uds::UdsContext * ctx, rmw_uds::RegistryHeader * header) { const int64_t now = steady_now_ns(); diff --git a/rmw_unix_socket_cpp/src/rmw_init.cpp b/rmw_unix_socket_cpp/src/rmw_init.cpp index 6a9ca30..dae3769 100644 --- a/rmw_unix_socket_cpp/src/rmw_init.cpp +++ b/rmw_unix_socket_cpp/src/rmw_init.cpp @@ -204,13 +204,16 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) auto * header = rmw_uds::registry_header(ctx->registry_ptr); - // Doorbell: bind + register BEFORE the first generation snapshot below, so - // no registry mutation can land in the gap between the snapshot and the - // wakeup wiring (a mutation after registration rings this socket; one - // before it is covered by the snapshot). + // Doorbell: bind the socket now so the fd is stable for every rmw_wait, but + // do NOT register the ENTRY_DOORBELL slot yet. Registration is lazy, from + // the first rmw_wait whose wait set holds a graph guard condition (see + // rmw_wait.cpp): with TRANSIENT_LOCAL replay pull-based, only graph-event + // consumers need registry wakeups, so plain pub/sub processes never ring — + // a fleet launch sends ~zero doorbell datagrams instead of mutations x + // processes. The lazy path replicates this bind-before-register ordering. { - const std::string ctl_path = rmw_uds::make_socket_path(domain_id, "ctl"); - ctx->doorbell_fd = rmw_uds::create_bound_socket(ctl_path); + ctx->doorbell_path = rmw_uds::make_socket_path(domain_id, "ctl"); + ctx->doorbell_fd = rmw_uds::create_bound_socket(ctx->doorbell_path); if (ctx->doorbell_fd < 0) { RMW_UDS_LOG_ERROR( "rmw_init: failed to create doorbell socket (domain_id=%zu)", domain_id); @@ -220,23 +223,6 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) RMW_SET_ERROR_MSG("failed to create doorbell socket"); return RMW_RET_ERROR; } - rmw_uds::RegistryEntry dentry; - std::memset(&dentry, 0, sizeof(dentry)); - dentry.type = rmw_uds::ENTRY_DOORBELL; - dentry.pid = getpid(); - std::strncpy(dentry.socket_path, ctl_path.c_str(), sizeof(dentry.socket_path) - 1); - ctx->doorbell_registry_index = rmw_uds::registry_add(header, dentry); - if (ctx->doorbell_registry_index < 0) { - RMW_UDS_LOG_ERROR( - "rmw_init: registry full — cannot register doorbell (domain_id=%zu)", domain_id); - close(ctx->doorbell_fd); - unlink(ctl_path.c_str()); - close(ctx->send_socket_fd); - rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size); - delete ctx; - RMW_SET_ERROR_MSG("registry full — cannot register doorbell"); - return RMW_RET_ERROR; - } } // Read initial generation @@ -249,7 +235,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) if (options->enclave) { enclave_copy = rcutils_strdup(options->enclave, options->allocator); if (!enclave_copy) { - rmw_uds::registry_remove(header, ctx->doorbell_registry_index); + unlink(ctx->doorbell_path.c_str()); // doorbell not yet registered close(ctx->doorbell_fd); close(ctx->send_socket_fd); rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size); @@ -276,7 +262,7 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) if (enclave_copy) { options->allocator.deallocate(enclave_copy, options->allocator.state); } - rmw_uds::registry_remove(header, ctx->doorbell_registry_index); + unlink(ctx->doorbell_path.c_str()); // doorbell not yet registered close(ctx->doorbell_fd); close(ctx->send_socket_fd); rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size); @@ -314,10 +300,13 @@ rmw_ret_t rmw_context_fini(rmw_context_t * context) auto * ctx = reinterpret_cast(context->impl); if (ctx) { // Doorbell teardown before the registry unmaps: registry_remove's slot - // teardown also unlinks the socket file. + // teardown also unlinks the socket file. When the doorbell was never + // lazily registered there is no slot, so unlink the socket file here. if (ctx->doorbell_registry_index >= 0 && ctx->registry_ptr) { auto * header = rmw_uds::registry_header(ctx->registry_ptr); rmw_uds::registry_remove(header, ctx->doorbell_registry_index); + } else if (!ctx->doorbell_path.empty()) { + unlink(ctx->doorbell_path.c_str()); } if (ctx->doorbell_fd >= 0) { close(ctx->doorbell_fd); diff --git a/rmw_unix_socket_cpp/src/rmw_publisher.cpp b/rmw_unix_socket_cpp/src/rmw_publisher.cpp index ff289b4..cccb665 100644 --- a/rmw_unix_socket_cpp/src/rmw_publisher.cpp +++ b/rmw_unix_socket_cpp/src/rmw_publisher.cpp @@ -19,7 +19,7 @@ #include "transport.hpp" #include "types.hpp" -#include +#include #include #include @@ -102,6 +102,22 @@ rmw_publisher_t * rmw_create_publisher( pub_data->context = ctx; pub_data->node = node_data; + // TRANSIENT_LOCAL: create the latched cache STRICTLY BEFORE registry_add, + // so a slot visible to any subscriber always names a mappable, validated + // segment. On failure the publisher runs latch-less (live delivery is + // unaffected, late joiners get no history) and the slot publishes an empty + // name, which pullers skip. + if (pub_data->qos.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { + if (!rmw_uds::tl_ring_create( + pub_data->tl_ring, ctx->domain_id, pub_data->gid.data, + pub_data->qos.depth)) + { + RMW_UDS_LOG_ERROR( + "latched cache unavailable for TRANSIENT_LOCAL topic '%s' — late " + "joiners will not receive retained samples", topic_name); + } + } + // Register in shared memory auto * header = rmw_uds::registry_header(ctx->registry_ptr); rmw_uds::RegistryEntry entry; @@ -113,6 +129,11 @@ rmw_publisher_t * rmw_create_publisher( std::strncpy(entry.node_namespace, node_data->ns.c_str(), sizeof(entry.node_namespace) - 1); std::strncpy(entry.topic_name, topic_name, sizeof(entry.topic_name) - 1); std::strncpy(entry.type_name, pub_data->type_name.c_str(), sizeof(entry.type_name) - 1); + // Publisher slots carried no socket_path until now; a TRANSIENT_LOCAL + // publisher publishes its latched-cache shm name here so late joiners can + // locate and pull the history themselves (see tl_ring_pull). + std::strncpy(entry.socket_path, pub_data->tl_ring.shm_name.c_str(), + sizeof(entry.socket_path) - 1); entry.qos_reliability = static_cast(pub_data->qos.reliability); entry.qos_durability = static_cast(pub_data->qos.durability); entry.qos_history = static_cast(pub_data->qos.history); @@ -126,6 +147,7 @@ rmw_publisher_t * rmw_create_publisher( "Increase REGISTRY_MAX_ENTRIES or check for slot leaks.", topic_name, node_data->ns.c_str(), node_data->name.c_str()); + rmw_uds::tl_ring_close(pub_data->tl_ring); delete pub_data; RMW_SET_ERROR_MSG("registry full — cannot create publisher"); return nullptr; @@ -134,6 +156,7 @@ rmw_publisher_t * rmw_create_publisher( auto * pub = rmw_publisher_allocate(); if (!pub) { rmw_uds::registry_remove(header, pub_data->registry_index); + rmw_uds::tl_ring_close(pub_data->tl_ring); delete pub_data; RMW_SET_ERROR_MSG("failed to allocate rmw_publisher_t"); return nullptr; @@ -145,11 +168,6 @@ rmw_publisher_t * rmw_create_publisher( pub->options = publisher_options ? *publisher_options : rmw_get_default_publisher_options(); pub->can_loan_messages = false; - if (pub_data->qos.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - std::lock_guard lock(ctx->transient_local_pubs_mutex); - ctx->transient_local_pubs.push_back(pub_data); - } - return pub; } @@ -163,18 +181,13 @@ rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) auto * pub_data = static_cast(publisher->data); if (pub_data) { - if (pub_data->context && - pub_data->qos.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) - { - auto * ctx = pub_data->context; - std::lock_guard lock(ctx->transient_local_pubs_mutex); - auto & v = ctx->transient_local_pubs; - v.erase(std::remove(v.begin(), v.end(), pub_data), v.end()); - } if (pub_data->context && pub_data->registry_index >= 0) { auto * header = rmw_uds::registry_header(pub_data->context->registry_ptr); rmw_uds::registry_remove(header, pub_data->registry_index); } + // Slot first, cache second: a puller that raced the removal either saw + // the slot and pulled a still-valid segment, or saw no slot at all. + rmw_uds::tl_ring_close(pub_data->tl_ring); rmw_uds::shm_writer_close(pub_data->shm_ring); delete pub_data; } @@ -188,87 +201,121 @@ rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) return RMW_RET_OK; } -// Cache a TRANSIENT_LOCAL message for late-joiner replay and send it now. -// Shared by rmw_publish and rmw_publish_serialized_message. Large payloads -// (>= SHM_PAYLOAD_THRESHOLD) are staged into a dedicated durable shm segment -// (shm_stage_durable) so the cached entry and its replays are a small -// descriptor rather than a datagram the kernel would reject; small payloads and -// the shm-unavailable fallback are cached inline. `payload` is taken by value: -// callers std::move a serialized vector in, so the common path is one move, and -// the inline branch keeps the bytes with no copy. Returns RMW_RET_ERROR if the -// live send of the current message hits the kernel size cap (EMSGSIZE), -// mirroring the non-TL path; replay of previously-cached messages to a new -// subscriber is best-effort and not reflected in the return code. +// Refresh the generation-keyed subscriber-path cache if `current_gen` moved, +// and return the (possibly shared) path list. The memoization invariant that +// keeps the pull-based replay proof sound: cached_generation only ever +// advances to a value loaded BEFORE the query ran, under sub_cache_mutex — +// so current_gen == cached_generation implies the cache already saw every +// subscriber whose registration that generation covers. +static std::shared_ptr> refresh_sub_paths( + rmw_uds::UdsPublisher * pub_data, + rmw_uds::RegistryHeader * header, + uint64_t current_gen) +{ + std::lock_guard lock(pub_data->sub_cache_mutex); + // Monotonic guard (not !=): a thread that loaded an older generation and + // lost the race must not move cached_generation backward — the invariant + // above is what the pull-based replay reasoning cites. A lower current_gen + // means the cached list was built by a newer query and is already a + // superset of the subscribers that generation covers. + if (current_gen > pub_data->cached_generation) { + auto subs = rmw_uds::registry_query( + header, rmw_uds::ENTRY_SUBSCRIPTION, pub_data->topic_name.c_str(), + nullptr, nullptr); + auto fresh = std::make_shared>(); + fresh->reserve(subs.size()); + for (const auto & s : subs) { + if (!s.socket_path.empty()) { + fresh->push_back(s.socket_path); + } + } + pub_data->cached_generation = current_gen; + pub_data->cached_subscriber_paths = std::move(fresh); + } + return pub_data->cached_subscriber_paths; +} + +// Latch a TRANSIENT_LOCAL sample into the pull cache and fan it out live. +// Shared by rmw_publish and rmw_publish_serialized_message. +// +// Ordering is the store-buffering pair that makes late-joiner replay +// lossless with no publisher-side wakeup (mirrored by rmw_create_subscription, +// which does registry_add -> seq_cst fence -> pull): +// 1. write the ring record (sequence number assigned under cache_mutex, so +// slot commit order == sequence order and the puller's watermark dedup +// is sound), +// 2. seq_cst fence, +// 3. FRESH generation load -> refresh the subscriber cache if stale, +// 4. fan out. +// If this publish misses a joining subscriber's registration, the fence pair +// guarantees the subscriber's pull sees the record; if it sees the +// registration, the sample goes out as a datagram; the overlap is deduped by +// the subscriber against its pull watermark. Reusing a generation loaded +// before step 1 would reopen the lost-sample window — never do that here. static rmw_ret_t transient_local_publish( rmw_uds::UdsPublisher * pub_data, rmw_uds::WireHeader hdr, - std::vector payload, - const std::shared_ptr> & sub_paths) + const std::vector & payload) { - // Build the cache entry — including the shm_stage_durable() syscalls - // (shm_open/posix_fallocate/mmap, up to milliseconds) — BEFORE taking - // cache_mutex. Staging reads only the payload, the immutable domain id, and a - // global atomic counter, none of it guarded by cache_mutex, so this is - // data-safe and keeps the multi-ms segment create off a lock that rmw_wait's - // replay holds under the process-wide transient_local_pubs_mutex. - rmw_uds::CachedMessage cached; - cached.header = hdr; - if (payload.size() >= rmw_uds::SHM_PAYLOAD_THRESHOLD) { - rmw_uds::ShmPayloadDescriptor desc; - auto seg = rmw_uds::shm_stage_durable( - pub_data->context->domain_id, payload.data(), payload.size(), desc); - if (seg) { - cached.header.msg_type |= rmw_uds::SHM_PAYLOAD_FLAG; - cached.header.payload_size = static_cast(sizeof(desc)); - cached.payload.assign( - reinterpret_cast(&desc), - reinterpret_cast(&desc) + sizeof(desc)); - cached.shm_seg = std::move(seg); - } else { - cached.payload = std::move(payload); // inline fallback + // Stage an over-cap payload BEFORE taking cache_mutex: staging is + // shm_open/fallocate/mmap (up to milliseconds), reads only the payload, + // the immutable domain id, and a global atomic counter — nothing + // cache_mutex guards. A staging failure just means this sample is not + // latched; the live path is unaffected. + rmw_uds::ShmPayloadDescriptor staged_desc; + std::unique_ptr staged_seg; + if (payload.size() > rmw_uds::TL_EMBED_CAP) { + staged_seg = rmw_uds::shm_stage_durable( + pub_data->context->domain_id, payload.data(), payload.size(), + staged_desc); + if (!staged_seg) { + RMW_UDS_LOG_WARN_THROTTLE( + 5000, + "latched sample (%zu bytes) could not be staged in shared memory — " + "not replayable to late joiners", + payload.size()); } - } else { - cached.payload = std::move(payload); } - - // Entries trimmed past qos.depth destruct (munmap + shm_unlink of their - // durable segment) after the lock is released, keeping that syscall off - // cache_mutex too. - std::deque evicted; + const bool staged = static_cast(staged_seg); + + // Latch, fence, refresh, and fan out under cache_mutex, so concurrent + // publishes on this publisher hit the wire in sequence order (the deleted + // push code held the same lock across its sends). The evicted slot's + // durable segment destructs (munmap + shm_unlink) after the lock drops. + std::unique_ptr evicted; + bool latched = false; bool config_error = false; { std::lock_guard lock(pub_data->cache_mutex); - pub_data->message_cache.push_back(std::move(cached)); - while (pub_data->message_cache.size() > pub_data->qos.depth) { - evicted.push_back(std::move(pub_data->message_cache.front())); - pub_data->message_cache.pop_front(); + hdr.sequence_number = + pub_data->sequence_number.fetch_add(1, std::memory_order_relaxed); + latched = rmw_uds::tl_ring_latch( + pub_data->tl_ring, hdr, payload.data(), payload.size(), + staged ? &staged_desc : nullptr, std::move(staged_seg), evicted); + std::atomic_thread_fence(std::memory_order_seq_cst); + + auto * header = rmw_uds::registry_header(pub_data->context->registry_ptr); + const uint64_t current_gen = rmw_uds::registry_generation(header); + auto sub_paths = refresh_sub_paths(pub_data, header, current_gen); + + // Live fan-out: payloads at or above the datagram threshold reuse the + // durable segment the latch just committed (same bytes, same + // descriptor). If the latch did NOT commit, the segment is already + // destroyed, so fall back to the inline send, which surfaces EMSGSIZE. + const uint8_t * wire_data = payload.data(); + size_t wire_size = payload.size(); + if (staged && latched && payload.size() >= rmw_uds::SHM_PAYLOAD_THRESHOLD) { + hdr.msg_type |= rmw_uds::SHM_PAYLOAD_FLAG; + hdr.payload_size = static_cast(sizeof(staged_desc)); + wire_data = reinterpret_cast(&staged_desc); + wire_size = sizeof(staged_desc); } - if (sub_paths) { - for (const auto & path : *sub_paths) { - if (pub_data->known_subscriber_paths.count(path) == 0) { - pub_data->known_subscriber_paths.insert(path); - for (size_t i = 0; i + 1 < pub_data->message_cache.size(); ++i) { - const auto & cm = pub_data->message_cache[i]; - rmw_uds::send_to( - pub_data->context->send_socket_fd, - path, cm.header, cm.payload.data(), cm.payload.size()); - } - } - } - } - - // Send the current message, read from the cached copy since payload was - // moved. Trimming never disturbs back(), so it is the message just pushed. - // Surface EMSGSIZE the way the non-TL path does so a latched payload the - // kernel rejects is not reported as a successful publish. - const auto & current = pub_data->message_cache.back(); if (sub_paths) { for (const auto & path : *sub_paths) { if (rmw_uds::send_to( pub_data->context->send_socket_fd, - path, current.header, current.payload.data(), current.payload.size()) - == rmw_uds::SendResult::ConfigError) + path, hdr, wire_data, wire_size) == rmw_uds::SendResult::ConfigError) { config_error = true; } @@ -298,63 +345,19 @@ rmw_ret_t rmw_publish( auto * pub_data = static_cast(publisher->data); - // Build wire header. payload_size is filled per-path below: serialization - // happens after the TRANSIENT_LOCAL fork so the non-latched path can - // serialize directly into the shm ring with no intermediate heap payload. + // Build wire header. payload_size is filled per-path below; the sequence + // number is assigned per-path too — the TRANSIENT_LOCAL path assigns it + // inside its latch critical section so slot commit order equals sequence + // order (see transient_local_publish). rmw_uds::WireHeader hdr; std::memset(&hdr, 0, sizeof(hdr)); std::memcpy(hdr.gid, pub_data->gid.data, sizeof(hdr.gid)); - hdr.sequence_number = pub_data->sequence_number.fetch_add(1, std::memory_order_relaxed); hdr.source_timestamp_ns = system_now_ns(); hdr.msg_type = 0; // topic message - // PERFORMANCE: only lock the registry when the graph generation has changed - // since we last cached the subscriber list. The hot path is purely local. - auto * header = rmw_uds::registry_header(pub_data->context->registry_ptr); - uint64_t current_gen = rmw_uds::registry_generation(header); - - std::shared_ptr> sub_paths; - { - std::lock_guard lock(pub_data->sub_cache_mutex); - if (current_gen != pub_data->cached_generation) { - auto subs = rmw_uds::registry_query( - header, rmw_uds::ENTRY_SUBSCRIPTION, pub_data->topic_name.c_str(), - nullptr, nullptr); - auto fresh = std::make_shared>(); - fresh->reserve(subs.size()); - for (const auto & s : subs) { - if (!s.socket_path.empty()) { - fresh->push_back(s.socket_path); - } - } - pub_data->cached_generation = current_gen; - pub_data->cached_subscriber_paths = std::move(fresh); - - // Prune known subscribers no longer present, against the freshly-built - // canonical list while STILL holding sub_cache_mutex (nested lock order - // sub_cache_mutex -> cache_mutex), so a concurrent refresh on another - // thread (e.g. the rmw_wait replay path) cannot make the pruning set - // stale and erase a still-live late-joiner. - if (pub_data->qos.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - std::lock_guard prune_lock(pub_data->cache_mutex); - std::set current( - pub_data->cached_subscriber_paths->begin(), - pub_data->cached_subscriber_paths->end()); - for (auto it = pub_data->known_subscriber_paths.begin(); - it != pub_data->known_subscriber_paths.end(); ) - { - it = (current.count(*it) == 0) ? - pub_data->known_subscriber_paths.erase(it) : std::next(it); - } - } - } - sub_paths = pub_data->cached_subscriber_paths; // refcount copy under lock - } - - // TRANSIENT_LOCAL: cache message and replay to late-joining subscribers. - // The replay cache owns the serialized bytes, so this path serializes into - // a heap payload as before (the durable segment is staged from it inside - // transient_local_publish). + // TRANSIENT_LOCAL: latch into the pull cache, then fan out (ordering + // documented at transient_local_publish). Serializes to a heap payload — + // the latch needs the bytes. if (pub_data->qos.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { std::vector payload; if (!rmw_uds::serialize(ros_message, pub_data->callbacks, payload)) { @@ -367,9 +370,18 @@ rmw_ret_t rmw_publish( return RMW_RET_ERROR; } hdr.payload_size = static_cast(payload.size()); - return transient_local_publish(pub_data, hdr, std::move(payload), sub_paths); + return transient_local_publish(pub_data, hdr, payload); } + hdr.sequence_number = + pub_data->sequence_number.fetch_add(1, std::memory_order_relaxed); + + // PERFORMANCE: only lock the registry when the graph generation has changed + // since we last cached the subscriber list. The hot path is purely local. + auto * header = rmw_uds::registry_header(pub_data->context->registry_ptr); + uint64_t current_gen = rmw_uds::registry_generation(header); + auto sub_paths = refresh_sub_paths(pub_data, header, current_gen); + // Non-latched path: serialize once, choosing the destination by size — a // large payload is written directly into the reserved ring record (no heap // payload, no staging copy) and only a descriptor crosses the socket; a @@ -429,62 +441,30 @@ rmw_ret_t rmw_publish_serialized_message( rmw_uds::WireHeader hdr; std::memset(&hdr, 0, sizeof(hdr)); std::memcpy(hdr.gid, pub_data->gid.data, sizeof(hdr.gid)); - hdr.sequence_number = pub_data->sequence_number.fetch_add(1, std::memory_order_relaxed); hdr.source_timestamp_ns = system_now_ns(); hdr.payload_size = static_cast(serialized_message->buffer_length); hdr.msg_type = 0; - // Reuse the cached subscriber list (refresh only on graph change) - auto * header = rmw_uds::registry_header(pub_data->context->registry_ptr); - uint64_t current_gen = rmw_uds::registry_generation(header); - std::shared_ptr> sub_paths; - { - std::lock_guard lock(pub_data->sub_cache_mutex); - if (current_gen != pub_data->cached_generation) { - auto subs = rmw_uds::registry_query( - header, rmw_uds::ENTRY_SUBSCRIPTION, pub_data->topic_name.c_str(), - nullptr, nullptr); - auto fresh = std::make_shared>(); - fresh->reserve(subs.size()); - for (const auto & s : subs) { - if (!s.socket_path.empty()) { - fresh->push_back(s.socket_path); - } - } - pub_data->cached_generation = current_gen; - pub_data->cached_subscriber_paths = std::move(fresh); - - // TRANSIENT_LOCAL: prune known subscribers no longer present, mirroring - // rmw_publish (nested lock order sub_cache_mutex -> cache_mutex) so a - // concurrent refresh cannot make the pruning set stale. - if (pub_data->qos.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - std::lock_guard prune_lock(pub_data->cache_mutex); - std::set current( - pub_data->cached_subscriber_paths->begin(), - pub_data->cached_subscriber_paths->end()); - for (auto it = pub_data->known_subscriber_paths.begin(); - it != pub_data->known_subscriber_paths.end(); ) - { - it = (current.count(*it) == 0) ? - pub_data->known_subscriber_paths.erase(it) : std::next(it); - } - } - } - sub_paths = pub_data->cached_subscriber_paths; - } - - // TRANSIENT_LOCAL: cache + replay through the same durable-shm path as + // TRANSIENT_LOCAL: latch + fan out through the same pull-cache path as // rmw_publish, so a serialized latched payload (including one above the - // datagram cap) reaches late joiners too. + // datagram cap) reaches late joiners too. Sequence number assigned inside + // the latch critical section (see transient_local_publish). if (pub_data->qos.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { return transient_local_publish( pub_data, hdr, std::vector( serialized_message->buffer, - serialized_message->buffer + serialized_message->buffer_length), - sub_paths); + serialized_message->buffer + serialized_message->buffer_length)); } + hdr.sequence_number = + pub_data->sequence_number.fetch_add(1, std::memory_order_relaxed); + + // Reuse the cached subscriber list (refresh only on graph change) + auto * header = rmw_uds::registry_header(pub_data->context->registry_ptr); + uint64_t current_gen = rmw_uds::registry_generation(header); + auto sub_paths = refresh_sub_paths(pub_data, header, current_gen); + // Large non-TL payloads: stage once into the cycling ring, fan out descriptors. rmw_uds::ShmPayloadDescriptor desc; auto wire = rmw_uds::shm_prepare_send( diff --git a/rmw_unix_socket_cpp/src/rmw_subscription.cpp b/rmw_unix_socket_cpp/src/rmw_subscription.cpp index 0c93eae..dc17142 100644 --- a/rmw_unix_socket_cpp/src/rmw_subscription.cpp +++ b/rmw_unix_socket_cpp/src/rmw_subscription.cpp @@ -19,6 +19,8 @@ #include "transport.hpp" #include "types.hpp" +#include +#include #include #include @@ -56,6 +58,24 @@ static int64_t system_now_ns() std::chrono::system_clock::now().time_since_epoch()).count(); } +// TRANSIENT_LOCAL dedup: true when this datagram's sample was already +// delivered by the creation-time pull — the sender's GID has a watermark and +// the sequence number is at or below it. Sequence numbers are assigned inside +// the publisher's latch critical section and the pull never extends the +// watermark across a scan-overlap gap, so anything <= the watermark was +// either pulled or already lapped out of the publisher's ring at pull time — +// history DDS would not owe a late joiner either. +static bool is_replayed_duplicate( + rmw_uds::UdsSubscription * sub, const rmw_uds::WireHeader & hdr) +{ + std::array key; + std::memcpy(key.data(), hdr.gid, key.size()); + std::lock_guard lock(sub->queue_mutex); + auto it = sub->replayed_watermarks.find(key); + return it != sub->replayed_watermarks.end() && + hdr.sequence_number <= it->second; +} + // Drain socket into message queue static void drain_subscription(rmw_uds::UdsSubscription * sub) { @@ -65,6 +85,10 @@ static void drain_subscription(rmw_uds::UdsSubscription * sub) while (rmw_uds::recv_from(sub->socket_fd, hdr, payload)) { if ((hdr.msg_type & ~rmw_uds::SHM_PAYLOAD_FLAG) != 0) {continue;} // Not a topic message + if (is_replayed_duplicate(sub, hdr)) { + continue; // already delivered by the creation-time pull + } + if (!rmw_uds::shm_resolve_incoming( sub->shm_cache, sub->context->domain_id, hdr, payload)) { @@ -216,6 +240,104 @@ rmw_subscription_t * rmw_create_subscription( *subscription_options : rmw_get_default_subscription_options(); sub->can_loan_messages = false; sub->is_cft_enabled = false; + + // TRANSIENT_LOCAL late-joiner replay, pull-based: this subscription reads + // each matched latched publisher's cache ITSELF — the publisher process is + // never woken, polled, or rung. The seq_cst fence after registry_add above + // mirrors the publisher's ring-write -> fence -> generation-load order + // (store-buffering pair): every latched sample is either in a cache this + // pull observes, or its publisher observed our registration and sends it + // as a datagram; the overlap is deduped by the per-publisher watermark. + // Runs to completion before the handle is returned, so pulled history + // always precedes live samples in the queue. + if (sub_data->qos.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { + std::atomic_thread_fence(std::memory_order_seq_cst); + auto pubs = rmw_uds::registry_query( + header, rmw_uds::ENTRY_PUBLISHER, sub_data->topic_name.c_str(), + nullptr, nullptr); + // Resolve pulled descriptors through a pull-local reader cache: durable + // segments are one-shot (fresh owner_id per sample), so retained + // mappings would never hit again — tear the cache down with the pull. + rmw_uds::ShmReaderCache pull_cache; + for (const auto & p : pubs) { + if (p.qos.durability != RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL || + p.socket_path.empty()) + { + continue; // volatile, ring-less, or old-binary publisher: no cache + } + if (sub_data->ignore_local_publications) { + rmw_uds::WireHeader gid_probe; + std::memcpy(gid_probe.gid, p.gid, sizeof(gid_probe.gid)); + if (rmw_uds::is_same_context(gid_probe, ctx->context_id)) { + continue; // same-context history is filtered exactly like live + } + } + std::vector records; + int64_t max_seq = 0; + bool overlapped = false; + if (!rmw_uds::tl_ring_pull( + p.socket_path, p.gid, sub_data->queue_depth, records, max_seq, + &overlapped)) + { + continue; // stale incarnation, empty cache, or poisoned writer: skip + } + if (overlapped) { + // A writer latched during the scan, so the snapshot is not + // point-in-time: a sequence gap may hide a sample the scan missed + // whose datagram is in flight to us (our slot was already visible). + // Keep only the contiguous prefix and let everything above the first + // gap arrive as datagrams — extending the watermark across the gap + // would silently drop the missed sample. Without overlap, a gap can + // only be a dead writer's poisoned slot, where the datagrams will + // never come: keep everything (partial history, documented). + size_t keep = records.size(); + for (size_t i = 1; i < records.size(); ++i) { + if (records[i].sequence_number != records[i - 1].sequence_number + 1) { + keep = i; + break; + } + } + if (keep < records.size()) { + records.resize(keep); + } + max_seq = records.empty() ? 0 : records.back().sequence_number; + if (records.empty()) { + continue; // nothing safely claimable; datagrams will deliver + } + } + const int64_t now_ns = system_now_ns(); + { + std::lock_guard lock(sub_data->queue_mutex); + for (auto & rec : records) { + rmw_uds::ReceivedMessage msg; + std::memcpy(&msg.header, rec.wire_header, sizeof(msg.header)); + msg.payload = std::move(rec.payload); + if (!rmw_uds::shm_resolve_incoming( + pull_cache, ctx->domain_id, msg.header, msg.payload)) + { + // The slot was overwritten and its segment unlinked between the + // scan and this resolve, so the sample is not delivered here. A + // watermark cannot express a hole: it stops below this sequence + // and the rest arrives as datagrams — claiming the sequence would + // drop the in-flight datagram carrying that very sample. + max_seq = rec.sequence_number - 1; + break; + } + msg.received_timestamp_ns = now_ns; + sub_data->message_queue.push_back(std::move(msg)); + while (sub_data->message_queue.size() > sub_data->queue_depth) { + sub_data->message_queue.pop_front(); + } + } + // Watermark AFTER a validated pull only — a publisher whose cache was + // not pulled must never have its datagrams dropped. + std::array key; + std::memcpy(key.data(), p.gid, key.size()); + sub_data->replayed_watermarks[key] = max_seq; + } + } + rmw_uds::shm_reader_close(pull_cache); + } return sub; } diff --git a/rmw_unix_socket_cpp/src/rmw_wait.cpp b/rmw_unix_socket_cpp/src/rmw_wait.cpp index 65af60e..11a9486 100644 --- a/rmw_unix_socket_cpp/src/rmw_wait.cpp +++ b/rmw_unix_socket_cpp/src/rmw_wait.cpp @@ -19,13 +19,13 @@ #include "types.hpp" #include +#include #include #include #include -#include #include -#include -#include +#include +#include #include #include @@ -66,7 +66,8 @@ static void drain_socket( rmw_uds::ShmReaderCache & shm_cache, size_t domain_id, bool ignore_local = false, - uint64_t context_id = 0) + uint64_t context_id = 0, + std::map, int64_t> * replay_watermarks = nullptr) { rmw_uds::WireHeader hdr; std::vector payload; @@ -76,6 +77,24 @@ static void drain_socket( payload.clear(); continue; } + if (replay_watermarks) { + // TRANSIENT_LOCAL dedup (subscriptions only): drop a datagram whose + // sample the creation-time pull already delivered. Checked before the + // descriptor resolve so a duplicate never maps a segment. + std::array key; + std::memcpy(key.data(), hdr.gid, key.size()); + bool dup = false; + { + std::lock_guard lock(queue_mutex); + auto it = replay_watermarks->find(key); + dup = it != replay_watermarks->end() && + hdr.sequence_number <= it->second; + } + if (dup) { + payload.clear(); + continue; + } + } if (!rmw_uds::shm_resolve_incoming(shm_cache, domain_id, hdr, payload)) { payload.clear(); continue; // shm descriptor unresolvable (sender gone / ring lapped) @@ -174,13 +193,70 @@ rmw_ret_t rmw_wait( auto * ws_data = static_cast(wait_set->data); - - // 1. Check graph generation changes — trigger graph guard conditions. - // The context comes from the wait set itself (set at rmw_create_wait_set): - // a guard-condition-only wait set (rclcpp's GraphListener) has no entity to - // scavenge it from, and this check must run for those waits too. Wrapped in - // a lambda so the step-4 loop can re-run it on each doorbell wake. + // 1. Graph-event wiring. The context comes from the wait set itself (set at + // rmw_create_wait_set): a guard-condition-only wait set (rclcpp's + // GraphListener) has no entity to scavenge it from, and this must run for + // those waits too. rmw_uds::UdsContext * ctx = ws_data->context; + + // Lazy doorbell registration: only graph-event consumers need registry + // wakeups (TRANSIENT_LOCAL replay is pull-based and needs no wake at all), + // so the ENTRY_DOORBELL slot is registered on the first wait whose set + // holds one of this context's graph guard conditions. Ordering mirrors the + // eager rmw_init recipe this replaces: the socket was bound at rmw_init, + // registration happens HERE — strictly before the generation check below — + // so a mutation either predates the registration (caught by the check) or + // rings the already-bound socket. Registration failure (registry full) + // leaves the flag unlatched and retries on the next wait; it never turns + // the wait into an error. + bool graph_gc_unwired = false; // graph GC waited on, doorbell unregistered + if (ctx && ctx->registry_ptr && guard_conditions && + !ctx->doorbell_registered.load(std::memory_order_acquire)) + { + bool has_graph_gc = false; + { + std::lock_guard gc_lock(ctx->graph_gcs_mutex); + for (size_t i = 0; + !has_graph_gc && i < guard_conditions->guard_condition_count; ++i) + { + for (auto * gc : ctx->graph_gcs) { + if (gc && gc->data == guard_conditions->guard_conditions[i]) { + has_graph_gc = true; + break; + } + } + } + } + if (has_graph_gc) { + std::lock_guard reg_lock(ctx->doorbell_reg_mutex); + if (!ctx->doorbell_registered.load(std::memory_order_relaxed)) { + auto * reg_header = rmw_uds::registry_header(ctx->registry_ptr); + rmw_uds::RegistryEntry dentry; + std::memset(&dentry, 0, sizeof(dentry)); + dentry.type = rmw_uds::ENTRY_DOORBELL; + dentry.pid = getpid(); + std::strncpy(dentry.socket_path, ctx->doorbell_path.c_str(), + sizeof(dentry.socket_path) - 1); + const int32_t idx = rmw_uds::registry_add(reg_header, dentry); + if (idx >= 0) { + ctx->doorbell_registry_index = idx; + ctx->doorbell_registered.store(true, std::memory_order_release); + } else { + // Registry full: this graph wait has no wakeup wiring. Returning an + // error would be executor-fatal, and blocking unbounded would lose + // graph events forever, so the block below is bounded instead: the + // wait degrades to a coarse retry loop (spurious OK wakes) until a + // slot frees up and registration succeeds. + graph_gc_unwired = true; + RMW_UDS_LOG_WARN_THROTTLE( + 5000, + "registry full — graph-event doorbell unregistered; graph waits " + "degrade to polling until a slot frees"); + } + } + } + } + const int doorbell_fd = ctx ? ctx->doorbell_fd : -1; auto run_generation_check = [&]() { // Drain the doorbell strictly BEFORE reading the generation: paired with @@ -195,87 +271,30 @@ rmw_ret_t rmw_wait( if (!(ctx && ctx->registry_ptr)) { return; } - auto * header = rmw_uds::registry_header(ctx->registry_ptr); - uint64_t gen = rmw_uds::registry_generation(header); - if (gen != ctx->last_registry_generation.load(std::memory_order_relaxed)) { - ctx->last_registry_generation.store(gen, std::memory_order_relaxed); - - // TRANSIENT_LOCAL late-joiner replay. Lock held across the loop — - // rmw_destroy_publisher takes the same mutex then deletes, so this - // is what keeps each pub pointer alive while we dereference it. - std::lock_guard tl_lock(ctx->transient_local_pubs_mutex); - for (auto * pub : ctx->transient_local_pubs) { - std::shared_ptr> sub_paths; + auto * header = rmw_uds::registry_header(ctx->registry_ptr); + uint64_t gen = rmw_uds::registry_generation(header); + if (gen != ctx->last_registry_generation.load(std::memory_order_relaxed)) { + ctx->last_registry_generation.store(gen, std::memory_order_relaxed); + + // Wake graph listeners: trigger every node's graph guard condition + // (rclcpp's GraphListener waits on these). rmw_destroy_node removes a + // node's GC from this list under the same mutex before destroying it, + // so a freed guard condition is never triggered. { - std::lock_guard sc_lock(pub->sub_cache_mutex); - if (rmw_uds::registry_generation(header) != pub->cached_generation) { - auto subs = rmw_uds::registry_query( - header, rmw_uds::ENTRY_SUBSCRIPTION, - pub->topic_name.c_str(), nullptr, nullptr); - auto fresh = std::make_shared>(); - fresh->reserve(subs.size()); - for (const auto & s : subs) { - if (!s.socket_path.empty()) { - fresh->push_back(s.socket_path); - } - } - pub->cached_generation = rmw_uds::registry_generation(header); - pub->cached_subscriber_paths = std::move(fresh); - - // Prune known subscribers no longer present, against the freshly- - // built canonical list while STILL holding sub_cache_mutex (nested - // lock order sub_cache_mutex -> cache_mutex), so a concurrent - // refresh (e.g. from rmw_publish) cannot make the pruning set stale - // and erase a still-live late-joiner. - std::lock_guard prune_lock(pub->cache_mutex); - std::set current( - pub->cached_subscriber_paths->begin(), - pub->cached_subscriber_paths->end()); - for (auto it = pub->known_subscriber_paths.begin(); - it != pub->known_subscriber_paths.end(); ) - { - it = (current.count(*it) == 0) ? - pub->known_subscriber_paths.erase(it) : std::next(it); - } + std::lock_guard gc_lock(ctx->graph_gcs_mutex); + for (auto * gc : ctx->graph_gcs) { + auto _r [[maybe_unused]] = rmw_trigger_guard_condition(gc); } - sub_paths = pub->cached_subscriber_paths; - } - std::lock_guard c_lock(pub->cache_mutex); - if (!sub_paths) { - continue; - } - for (const auto & path : *sub_paths) { - if (pub->known_subscriber_paths.count(path) != 0) { - continue; - } - pub->known_subscriber_paths.insert(path); - for (const auto & cm : pub->message_cache) { - rmw_uds::send_to( - ctx->send_socket_fd, - path, cm.header, cm.payload.data(), cm.payload.size()); - } - } - } - - // Wake graph listeners: trigger every node's graph guard condition - // (rclcpp's GraphListener waits on these). rmw_destroy_node removes a - // node's GC from this list under the same mutex before destroying it, - // so a freed guard condition is never triggered. - { - std::lock_guard gc_lock(ctx->graph_gcs_mutex); - for (auto * gc : ctx->graph_gcs) { - auto _r [[maybe_unused]] = rmw_trigger_guard_condition(gc); } } - } - }; + }; run_generation_check(); - // Arm every entity fd with epoll, but only when it is not already armed for - // this same entity. ws_data->armed remembers what each fd was armed for and - // survives across calls, so in the steady state this pass costs no syscall - // at all. A fd number reused after its previous owner closed carries a - // different uid and is re-armed. A ready fd is dispatched below only when + // 2. Arm every entity fd with epoll, but only when it is not already armed + // for this same entity. ws_data->armed remembers what each fd was armed for + // and survives across calls, so in the steady state this pass costs no + // syscall at all. A fd number reused after its previous owner closed carries + // a different uid and is re-armed. A ready fd is dispatched below only when // this call armed it (armed_this_call); stale entries are pruned there. // // gc_index maps a guard condition back to its slot in the caller's array, so @@ -353,15 +372,16 @@ rmw_ret_t rmw_wait( register_fd(gc->eventfd_fd, rmw_uds::ARMED_GUARD_CONDITION, gc, gc->uid); } } - // The context's doorbell: rung by any process after a registry mutation. - // It has no entity, so uid 0 (never handed out by next_entity_uid) arms it - // exactly once for the life of the wait set. + // The context's doorbell: rung by another process after a registry + // mutation, once this context lazily registers it (graph-GC waits only). + // It has no entity, so uid 0 (never handed out by next_entity_uid) arms + // it exactly once for the life of the wait set. register_fd(doorbell_fd, rmw_uds::ARMED_DOORBELL, nullptr, 0); } // Fallback for entities epoll cannot watch: drain their sockets directly so // the queue check below still sees their data, and bound the block below so - // the drain recurs. Guard-condition eventfds need no drain here — step 2 + // the drain recurs. Guard-condition eventfds need no drain here — step 3 // reads every caller GC directly. for (const auto & ue : unarmed) { switch (ue.kind) { @@ -369,7 +389,8 @@ rmw_ret_t rmw_wait( auto * sub = static_cast(ue.entity); drain_socket(sub->socket_fd, sub->queue_mutex, sub->message_queue, sub->queue_depth, 0, sub->shm_cache, sub->context->domain_id, - sub->ignore_local_publications, sub->context->context_id); + sub->ignore_local_publications, sub->context->context_id, + &sub->replayed_watermarks); break; } case rmw_uds::ARMED_SERVICE: { @@ -389,7 +410,7 @@ rmw_ret_t rmw_wait( } } - // 2. Check if anything is already ready, without blocking. drain_socket() + // 3. Check if anything is already ready, without blocking. drain_socket() // reads until EAGAIN, so an earlier wait can leave more messages queued than // rmw_take has consumed since. Those are invisible to epoll (their socket is // already empty), so the internal queues must be checked directly. This is a @@ -467,18 +488,18 @@ rmw_ret_t rmw_wait( // Block until something the caller waits on fires, or the caller's own // deadline. There is no internal poll: a registry mutation in any process - // rings this context's doorbell (ring_doorbells in registry.cpp), which - // wakes the epoll; the doorbell is drained, the registry re-checked - // (TRANSIENT_LOCAL late-joiner replay + graph guard conditions), and — if - // nothing the caller waits on became ready — the wait re-blocks. - // RMW_RET_TIMEOUT surfaces only at the caller's own deadline; an infinite - // wait never surfaces a synthetic timeout. EINTR re-enters the loop, so a - // signal neither returns TIMEOUT early nor busy-loops. + // rings this context's doorbell if it registered one (ring_doorbells in + // registry.cpp), which wakes the epoll; the doorbell is drained, the + // registry re-checked (graph guard conditions; TRANSIENT_LOCAL replay is + // pull-based), and — if nothing the caller waits on became ready — the wait + // re-blocks. RMW_RET_TIMEOUT surfaces only at the caller's own deadline; an + // infinite wait never surfaces a synthetic timeout. EINTR re-enters the + // loop, so a signal neither returns TIMEOUT early nor busy-loops. const bool infinite = (timeout_ms < 0); const int64_t caller_deadline_ns = infinite ? 0 : steady_now_ns() + static_cast(timeout_ms) * 1000000; - // 3. Block on epoll, then drain only the fds it reported ready. This is what - // keeps the wait O(ready) instead of O(entities in the wait set). When step 2 + // 4. Block on epoll, then drain only the fds it reported ready. This is what + // keeps the wait O(ready) instead of O(entities in the wait set). When step 3 // already found queued work we still make one non-blocking pass, so an entity // whose data is sitting unread in its socket is reported in this call rather // than the next one. That is what the old unconditional pre-drain bought, for @@ -496,10 +517,13 @@ rmw_ret_t rmw_wait( block_ms = static_cast( std::min(rem_ms, std::numeric_limits::max())); } - if (!unarmed.empty() && (block_ms < 0 || block_ms > 200)) { - // An entity epoll cannot watch: never block unbounded, or its event - // is silently lost forever. The early return is a spurious wake - // (OK, nothing ready) that lets the next rmw_wait re-drain. + if ((graph_gc_unwired || !unarmed.empty()) && + (block_ms < 0 || block_ms > 200)) + { + // No doorbell wiring (registry full), or an entity epoll cannot + // watch: never block unbounded, or its event is silently lost + // forever. The early return is a spurious wake (OK, nothing ready) + // that lets the next rmw_wait retry registration / re-drain. block_ms = 200; } } @@ -517,10 +541,11 @@ rmw_ret_t rmw_wait( // Only actual progress ends the wait: an entity the caller waits on now // holds data, or a guard condition fired. A wake alone is not enough. A // doorbell ring carries no caller work, and a datagram that drain_socket - // filters out (an ignored local publication, a foreign message type, an - // unresolvable shm descriptor) leaves the queue empty. Ending the wait on - // those would return RMW_RET_OK with nothing ready and wake the executor - // for every such message. + // filters out (an ignored local publication, a replayed-duplicate + // TRANSIENT_LOCAL sample, a foreign message type, an unresolvable shm + // descriptor) leaves the queue empty. Ending the wait on those would + // return RMW_RET_OK with nothing ready and wake the executor for every + // such message. bool progressed = false; bool rang = false; for (int e = 0; e < n; ++e) { @@ -547,7 +572,8 @@ rmw_ret_t rmw_wait( auto * sub = static_cast(entry.entity); drain_socket(sub->socket_fd, sub->queue_mutex, sub->message_queue, sub->queue_depth, 0, sub->shm_cache, sub->context->domain_id, - sub->ignore_local_publications, sub->context->context_id); + sub->ignore_local_publications, sub->context->context_id, + &sub->replayed_watermarks); std::lock_guard lock(sub->queue_mutex); if (!sub->message_queue.empty()) { progressed = true; @@ -598,7 +624,7 @@ rmw_ret_t rmw_wait( } } if (rang) { - run_generation_check(); // Drains the doorbell, replays, triggers GCs. + run_generation_check(); // Drains the doorbell, triggers graph GCs. } if (poll_only) { if (n == 64) { @@ -613,16 +639,16 @@ rmw_ret_t rmw_wait( if (!infinite && steady_now_ns() >= caller_deadline_ns) { break; // Doorbell-only wake at the deadline -> timeout. } - if (!unarmed.empty()) { + if (graph_gc_unwired || !unarmed.empty()) { break; // Bounded degraded wait: surface the spurious wake so the - // caller's next rmw_wait re-drains the unwatchable entities. + // caller's next rmw_wait retries registration / re-drains. } // Doorbell-only wake: re-block for the caller's remaining time. } // Fds stay registered across calls; the gate above prunes stale ones. } - // 4. Set output: ready entities stay, non-ready set to NULL + // 5. Set output: ready entities stay, non-ready set to NULL bool any_ready = false; if (subscriptions) { @@ -645,7 +671,8 @@ rmw_ret_t rmw_wait( guard_conditions->guard_conditions[i]); uint64_t val; // Consume a trigger that landed during the epoll block; combine with the - // step-3 read so a GC seen ready then stays reported without a read-back. + // earlier reads so a GC seen ready then stays reported without a + // read-back. ssize_t r = read(gc->eventfd_fd, &val, sizeof(val)); if (r == static_cast(sizeof(val)) || gc_triggered[i]) { any_ready = true; diff --git a/rmw_unix_socket_cpp/src/shm_transport.cpp b/rmw_unix_socket_cpp/src/shm_transport.cpp index 51d5036..df24f31 100644 --- a/rmw_unix_socket_cpp/src/shm_transport.cpp +++ b/rmw_unix_socket_cpp/src/shm_transport.cpp @@ -14,6 +14,7 @@ #include "shm_transport.hpp" +#include #include #include #include @@ -22,11 +23,13 @@ #include #include +#include #include #include #include #include "logging.hpp" +#include "types.hpp" // WireHeader (blitted into latched-cache slots) namespace rmw_uds { @@ -543,14 +546,18 @@ void shm_reader_close(ShmReaderCache & cache) void shm_cleanup_orphan_segments(size_t domain_id) { - // Name format (see make_segment_name, without the leading '/'): - // ros2_uds_data____ + // Name formats (without the leading '/'): + // ros2_uds_data____ (payload rings) + // ros2_uds_tl___ (latched caches) // Same dead-PID test as registry_cleanup_stale / cleanup_orphan_socket_files: // /proc only shows PIDs visible in our namespace, and a PID we cannot see // cannot be reached by our sockets either, so unlinking is safe. - char prefix[64]; - std::snprintf(prefix, sizeof(prefix), "ros2_uds_data_%zu_", domain_id); - const size_t prefix_len = std::strlen(prefix); + char data_prefix[64]; + std::snprintf(data_prefix, sizeof(data_prefix), "ros2_uds_data_%zu_", domain_id); + const size_t data_len = std::strlen(data_prefix); + char tl_prefix[64]; + std::snprintf(tl_prefix, sizeof(tl_prefix), "ros2_uds_tl_%zu_", domain_id); + const size_t tl_len = std::strlen(tl_prefix); DIR * dir = opendir("/dev/shm"); if (!dir) { @@ -558,7 +565,12 @@ void shm_cleanup_orphan_segments(size_t domain_id) } struct dirent * ent; while ((ent = readdir(dir)) != nullptr) { - if (std::strncmp(ent->d_name, prefix, prefix_len) != 0) { + size_t prefix_len; + if (std::strncmp(ent->d_name, data_prefix, data_len) == 0) { + prefix_len = data_len; + } else if (std::strncmp(ent->d_name, tl_prefix, tl_len) == 0) { + prefix_len = tl_len; + } else { continue; } char * end = nullptr; @@ -578,4 +590,359 @@ void shm_cleanup_orphan_segments(size_t domain_id) closedir(dir); } +// --------------------------------------------------------------------------- +// TRANSIENT_LOCAL latched cache (see shm_transport.hpp for the design notes). +// --------------------------------------------------------------------------- + +static size_t tl_slot_stride() +{ + return align_up(TL_SLOT_BYTES_UNALIGNED, SHM_RECORD_ALIGN); +} + +static uint8_t * tl_slot_at(const TlRingWriter & ring, uint32_t slot) +{ + return ring.base + SHM_RECORD_ALIGN /* header area */ + + static_cast(slot) * ring.slot_bytes; +} + +bool tl_ring_create( + TlRingWriter & ring, + size_t domain_id, + const uint8_t * gid16, + size_t depth) +{ + const size_t stride = tl_slot_stride(); + size_t slots = depth == 0 ? 1 : depth; + const size_t max_slots = TL_RING_MAX_BYTES / stride; + if (slots > max_slots) { + RMW_UDS_LOG_WARN_THROTTLE( + 5000, + "TRANSIENT_LOCAL depth %zu exceeds the latched-cache byte cap — " + "replaying the last %zu samples only", + depth, max_slots); + slots = max_slots; + } + + // Per-incarnation unique name, same counter+time recipe as create_segment: + // a recycled PID can never regenerate a dead publisher's cache name, so a + // puller can never alias a stale segment. The name is published in the + // registry slot, not derived, so uniqueness costs nothing. + const int32_t pid = getpid(); + auto now = std::chrono::steady_clock::now().time_since_epoch(); + uint32_t time_component = static_cast( + std::chrono::duration_cast(now).count() & 0xFFFF); + // Unlike the descriptor-borne ring owner_id (32-bit field), the tl name is + // a free-form string, so the full counter and the time salt are kept as + // separate components: the name-collision period is 2^32 creations, not + // 2^16 — a truncated counter could otherwise alias a LIVE same-process + // ring and unlink it via the EEXIST branch below. + const uint32_t owner_counter = + g_shm_owner_counter.fetch_add(1, std::memory_order_relaxed); + char name[96]; + std::snprintf(name, sizeof(name), "/ros2_uds_tl_%zu_%d_%u_%x", + domain_id, pid, owner_counter, time_component); + + int fd = shm_open(name, O_CREAT | O_EXCL | O_RDWR, 0644); + if (fd < 0 && errno == EEXIST) { + shm_unlink(name); + fd = shm_open(name, O_CREAT | O_EXCL | O_RDWR, 0644); + } + if (fd < 0) { + RMW_UDS_LOG_WARN_THROTTLE( + 5000, "shm_open('%s') failed: %s — latched replay disabled for this publisher", + name, std::strerror(errno)); + return false; + } + + // Sparse ftruncate, unlike create_segment's eager fallocate: an idle + // latched publisher must cost pages proportional to what it latched, not + // depth x slot_bytes. Slot ranges are committed by posix_fallocate at + // latch time, which turns a full /dev/shm into a clean no-latch there. + const size_t total = SHM_RECORD_ALIGN + slots * stride; + if (ftruncate(fd, static_cast(total)) != 0) { + RMW_UDS_LOG_WARN_THROTTLE( + 5000, "ftruncate('%s', %zu) failed: %s — latched replay disabled", + name, total, std::strerror(errno)); + close(fd); + shm_unlink(name); + return false; + } + + // The header page is written now, so commit it eagerly. + const int alloc_err = posix_fallocate(fd, 0, SHM_RECORD_ALIGN); + if (alloc_err != 0) { + RMW_UDS_LOG_WARN_THROTTLE( + 5000, "posix_fallocate('%s') failed: %s — latched replay disabled", + name, std::strerror(alloc_err)); + close(fd); + shm_unlink(name); + return false; + } + + void * base = mmap(nullptr, total, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + if (base == MAP_FAILED) { + RMW_UDS_LOG_WARN_THROTTLE( + 5000, "mmap('%s', %zu) failed: %s — latched replay disabled", + name, total, std::strerror(errno)); + close(fd); + shm_unlink(name); + return false; + } + + auto * header = reinterpret_cast(base); + header->magic = TL_RING_MAGIC; + header->version = TL_RING_VERSION; + std::memcpy(header->gid, gid16, sizeof(header->gid)); + header->slots = static_cast(slots); + header->slot_bytes = static_cast(stride); + + ring.fd = fd; + ring.base = static_cast(base); + ring.map_size = total; + ring.slots = static_cast(slots); + ring.slot_bytes = static_cast(stride); + ring.next_index = 1; + ring.shm_name = name; + ring.durable_segs.clear(); + ring.durable_segs.resize(slots); + return true; +} + +bool tl_ring_latch( + TlRingWriter & ring, + const WireHeader & hdr, + const uint8_t * payload, + size_t payload_size, + const ShmPayloadDescriptor * staged_desc, + std::unique_ptr staged_seg, + std::unique_ptr & evicted_out) +{ + if (!ring.base) { + return false; // ring-less publisher (creation failed) — no replay + } + if (!staged_desc && payload_size > TL_EMBED_CAP) { + // Over-cap payload whose caller-side staging failed: it cannot fit the + // fixed slot, and writing it anyway would run past the slot (and the + // mapping). The sample is simply not latched; live sends are unaffected. + return false; + } + + // Over-cap payloads arrive pre-staged by the caller (outside cache_mutex — + // staging is ms-scale syscalls); the slot then carries the 32-byte + // descriptor, exactly like a large sample on the wire. The segment becomes + // owned by this slot and is evicted (returned to the caller for unlinking + // outside the lock) when the slot is overwritten — an in-flight puller + // that already mapped it keeps a valid mapping; one that has not yet + // mapped gets ENOENT and skips, the documented lapped-record semantics. + WireHeader slot_hdr = hdr; + const uint8_t * slot_payload = payload; + size_t slot_payload_size = payload_size; + if (staged_desc) { + slot_hdr.msg_type |= SHM_PAYLOAD_FLAG; + slot_hdr.payload_size = static_cast(sizeof(*staged_desc)); + slot_payload = reinterpret_cast(staged_desc); + slot_payload_size = sizeof(*staged_desc); + } + + const uint64_t index = ring.next_index; + const uint32_t slot = static_cast((index - 1) % ring.slots); + uint8_t * slot_base = tl_slot_at(ring, slot); + + // Commit this slot's pages before writing: on a full tmpfs the deferred + // allocation would otherwise SIGBUS inside the memcpy below. + const off_t slot_off = static_cast(slot_base - ring.base); + const size_t record_bytes = + sizeof(ShmRecordHeader) + sizeof(WireHeader) + slot_payload_size; + const int alloc_err = posix_fallocate(ring.fd, slot_off, + static_cast(record_bytes)); + if (alloc_err != 0) { + RMW_UDS_LOG_WARN_THROTTLE( + 5000, "posix_fallocate(latched slot) failed: %s — sample not latched", + std::strerror(alloc_err)); + return false; + } + + auto * record = reinterpret_cast(slot_base); + // Per-record seqlock, absolute-index protocol shared with the payload ring: + // odd while the bytes are in flux, even once committed. exchange is an + // acq_rel RMW so the payload write cannot be reordered ahead of it. + record->seq.exchange( + static_cast(index * 2 - 1), std::memory_order_acq_rel); + record->payload_len = static_cast(slot_payload_size); + std::memcpy(slot_base + sizeof(ShmRecordHeader), &slot_hdr, sizeof(WireHeader)); + std::memcpy( + slot_base + sizeof(ShmRecordHeader) + sizeof(WireHeader), + slot_payload, slot_payload_size); + record->seq.store( + static_cast(index * 2), std::memory_order_release); + + // Evict the overwritten slot's durable segment only AFTER the new record is + // committed: until then a puller could still legitimately read the old one. + // The evicted handle goes back to the caller so its munmap + shm_unlink + // run after cache_mutex is released. + evicted_out = std::move(ring.durable_segs[slot]); + ring.durable_segs[slot] = std::move(staged_seg); + ring.next_index = index + 1; + return true; +} + +bool tl_ring_pull( + const std::string & shm_name, + const uint8_t * expected_gid16, + size_t max_records, + std::vector & records_out, + int64_t & max_seq_out, + bool * overlapped_out) +{ + max_seq_out = 0; + records_out.clear(); + if (overlapped_out) { + *overlapped_out = false; + } + if (shm_name.empty() || max_records == 0) { + return false; + } + + int fd = shm_open(shm_name.c_str(), O_RDONLY, 0); + if (fd < 0) { + return false; // publisher gone (or old binary): silent skip + } + struct stat st; + if (fstat(fd, &st) != 0 || + static_cast(st.st_size) < SHM_RECORD_ALIGN) + { + close(fd); + return false; + } + const size_t map_size = static_cast(st.st_size); + void * base = mmap(nullptr, map_size, PROT_READ, MAP_SHARED, fd, 0); + close(fd); // the mapping keeps the segment alive + if (base == MAP_FAILED) { + return false; + } + + // Validate the immutable header before trusting any geometry, mirroring + // map_segment: magic, version, creator GID against the slot the name came + // from, and slot geometry against the mapped size (never the live file). + const auto * header = reinterpret_cast(base); + const size_t stride = tl_slot_stride(); + bool ok = header->magic == TL_RING_MAGIC && + header->version == TL_RING_VERSION && + std::memcmp(header->gid, expected_gid16, 16) == 0 && + header->slot_bytes == stride && + header->slots > 0 && + static_cast(header->slots) <= + (map_size - SHM_RECORD_ALIGN) / stride; + if (!ok) { + munmap(base, map_size); + return false; // stale incarnation or malformed — skip silently + } + const uint32_t slots = header->slots; + const auto * area = static_cast(base) + SHM_RECORD_ALIGN; + + // Last seq the scan observed in EVERY slot, including the ones that yield + // no record (never written, or given up on mid-write). A writer filling + // one of those during the scan opens a sequence gap that the pulled + // records alone cannot reveal, so all slots are re-checked afterwards — + // pulled ones only would miss exactly that case (overlapped_out contract). + std::vector observed(slots, 0); + + for (uint32_t i = 0; i < slots; ++i) { + const auto * record = + reinterpret_cast(area + i * stride); + // Bounded per-slot seqlock snapshot (registry precedent): a slot mid- + // write is retried a few times then skipped — a publisher killed with a + // slot's seq odd poisons that slot alone, and subscription creation + // never spins on memory a dead writer owned. + for (int retry = 0; retry < 16; ++retry) { + const uint32_t s1 = record->seq.load(std::memory_order_acquire); + observed[i] = s1; + if (s1 == 0) { + break; // never written + } + if (s1 & 1u) { + sched_yield(); + continue; + } + const uint32_t len = record->payload_len; + if (len > TL_EMBED_CAP) { + // A committed record's length is always <= the cap (written inside + // the odd window), so this is a torn read from an in-flight write: + // retry like any other unstable snapshot rather than abandoning the + // slot. + sched_yield(); + continue; + } + TlPulledRecord rec; + std::memcpy(rec.wire_header, + reinterpret_cast(record) + sizeof(ShmRecordHeader), + sizeof(rec.wire_header)); + rec.payload.assign( + reinterpret_cast(record) + sizeof(ShmRecordHeader) + + sizeof(rec.wire_header), + reinterpret_cast(record) + sizeof(ShmRecordHeader) + + sizeof(rec.wire_header) + len); + // The fence keeps the copies above from sinking below the re-check on + // weakly-ordered CPUs (shm_fetch_payload precedent). + std::atomic_thread_fence(std::memory_order_acquire); + if (record->seq.load(std::memory_order_relaxed) != s1) { + sched_yield(); + continue; // overwritten mid-copy — retry this slot + } + std::memcpy(&rec.sequence_number, rec.wire_header + 16, sizeof(int64_t)); + if (rec.sequence_number > max_seq_out) { + max_seq_out = rec.sequence_number; + } + records_out.push_back(std::move(rec)); + break; + } + } + // Overlap detection: if any slot's seq moved since the scan observed it, a + // writer latched during the scan — the result is not a point-in-time + // snapshot, and a sequence gap in it may hide a sample the scan missed. + if (overlapped_out) { + for (uint32_t i = 0; i < slots; ++i) { + const auto * rec_hdr = + reinterpret_cast(area + i * stride); + if (rec_hdr->seq.load(std::memory_order_acquire) != observed[i]) { + *overlapped_out = true; + break; + } + } + } + munmap(base, map_size); + + // Oldest-first in sequence order; keep only the newest max_records so the + // subscriber's depth trim never fires (and never warns) on the backlog. + std::sort(records_out.begin(), records_out.end(), + [](const TlPulledRecord & a, const TlPulledRecord & b) { + return a.sequence_number < b.sequence_number; + }); + if (records_out.size() > max_records) { + records_out.erase( + records_out.begin(), + records_out.end() - static_cast(max_records)); + } + return !records_out.empty(); +} + +void tl_ring_close(TlRingWriter & ring) +{ + if (ring.base) { + munmap(ring.base, ring.map_size); + ring.base = nullptr; + } + if (ring.fd >= 0) { + close(ring.fd); + ring.fd = -1; + } + if (!ring.shm_name.empty()) { + shm_unlink(ring.shm_name.c_str()); + ring.shm_name.clear(); + } + ring.durable_segs.clear(); // dtors unlink the staged large payloads + ring.slots = 0; + ring.map_size = 0; +} + } // namespace rmw_uds diff --git a/rmw_unix_socket_cpp/src/shm_transport.hpp b/rmw_unix_socket_cpp/src/shm_transport.hpp index ca3599c..c47fd30 100644 --- a/rmw_unix_socket_cpp/src/shm_transport.hpp +++ b/rmw_unix_socket_cpp/src/shm_transport.hpp @@ -30,6 +30,9 @@ namespace rmw_uds { +// Defined in types.hpp (which includes this header); only referenced here. +struct WireHeader; + // Shared-memory payload path for large messages — topic payloads, service // requests, and service responses alike. // @@ -256,6 +259,147 @@ bool shm_fetch_payload( size_t domain_id, std::vector & payload_io); +// --------------------------------------------------------------------------- +// TRANSIENT_LOCAL latched cache: a per-publisher shm segment holding the last +// qos.depth latched samples, written at publish time and READ BY THE LATE +// JOINER ITSELF at rmw_create_subscription. The publisher process is never +// woken for replay — durability is pull-based, like discovery itself. The +// segment's name is published in the publisher's registry slot (socket_path, +// unused for publishers until now), so a subscriber locates it from the slot +// it already queried; the name carries a per-incarnation unique id, so a +// recycled PID can never alias a dead publisher's cache. +// --------------------------------------------------------------------------- + +// Latched payloads at or below this many bytes are embedded in the slot; +// larger ones are staged once via shm_stage_durable and the slot carries the +// 32-byte descriptor (resolved by the puller through shm_fetch_payload). +// Small keeps the fixed slot stride — and with it the ring's RAM commit — +// modest for chatty latched topics like /rosout (depth 1000). +static constexpr size_t TL_EMBED_CAP = 1024; + +// Hard byte ceiling for one publisher's latched ring (record area). A +// misconfigured depth cannot fallocate tens of MB: slots are clamped to +// whatever fits. Replay may then hold fewer than qos.depth samples — a +// stated, accepted limit (DESIGN, latched cache). 2 MiB admits the stock +// rosout profile (TRANSIENT_LOCAL, KEEP_LAST 1000: 1000 x 1088 B stride) +// without clamping; the file is sparse, so the ceiling costs nothing until +// slots are actually latched. +static constexpr size_t TL_RING_MAX_BYTES = 2 * 1024 * 1024; + +static constexpr uint32_t TL_RING_MAGIC = 0x4C544455; // "UDTL" +static constexpr uint32_t TL_RING_VERSION = 1; + +// First bytes of a latched-cache segment. Written once at creation, before +// the publisher's registry slot exists; immutable afterwards, so readers +// need no synchronization to validate it. The creator's full 16-byte GID is +// embedded so a puller can verify the segment belongs to the slot it derived +// the name from (stale-segment defense in depth; the unique name is the +// primary defense). +struct TlRingHeader +{ + uint32_t magic; + uint32_t version; + uint8_t gid[16]; + uint32_t slots; // slot count (qos.depth clamped by TL_RING_MAX_BYTES) + uint32_t slot_bytes; // stride of one slot +}; + +static_assert(sizeof(TlRingHeader) == 32, "TL ring header shm layout changed"); + +// One latched slot: ShmRecordHeader (seq + payload_len) followed by the +// sample's WireHeader (37 packed bytes) and the payload area (TL_EMBED_CAP +// bytes — inline CDR, or a 32-byte ShmPayloadDescriptor when the WireHeader +// carries SHM_PAYLOAD_FLAG). Slot i holds record n where n % slots == i; the +// record seq is the absolute 2n-1 (writing) / 2n (stable) protocol shared +// with the payload ring, so a reader detects both in-flight and lapped slots +// per record — a writer preempted or killed mid-write poisons exactly one +// slot, never the history. +static constexpr size_t TL_SLOT_BYTES_UNALIGNED = + sizeof(ShmRecordHeader) + 37 /* sizeof(WireHeader) */ + TL_EMBED_CAP; + +// Publisher-side latched-cache state (process-local; the mapped segment is +// the shared part). Guarded by UdsPublisher::cache_mutex; the sequence +// number is assigned under the same lock so slot commit order equals +// sequence order — the property that makes the puller's dedup watermark +// sound. durable_segs parallels the slots: it owns the DurableShmSegment a +// slot's descriptor points at, destroyed when that slot is overwritten. +struct TlRingWriter +{ + int fd = -1; + uint8_t * base = nullptr; + size_t map_size = 0; + uint32_t slots = 0; + uint32_t slot_bytes = 0; + uint64_t next_index = 1; // absolute record counter (drives per-slot seq) + std::string shm_name; + std::vector> durable_segs; +}; + +// One sample pulled out of a latched cache. +struct TlPulledRecord +{ + int64_t sequence_number; + uint8_t wire_header[37]; // verbatim WireHeader bytes + std::vector payload; // inline CDR or a descriptor (per msg_type) +}; + +// Create the latched-cache segment for a TRANSIENT_LOCAL publisher. Called +// BEFORE the publisher's registry_add, so a visible slot always names a +// mappable, validated segment. The file is sized sparsely (ftruncate); +// per-record ranges are committed by posix_fallocate at latch time, so an +// idle publisher's ring costs an inode and a page, not depth x slot_bytes of +// RAM. Returns false on failure — the publisher then runs latch-less (no +// replay) and its slot publishes an empty name. +bool tl_ring_create( + TlRingWriter & ring, + size_t domain_id, + const uint8_t * gid16, + size_t depth); + +// Latch one sample under the per-record seqlock. Caller holds the +// publisher's cache_mutex and has already assigned hdr.sequence_number under +// it. Payloads above TL_EMBED_CAP must be pre-staged BY THE CALLER (outside +// the lock — staging is shm_open/fallocate/mmap, up to milliseconds) via +// shm_stage_durable; pass its descriptor and segment here and the slot then +// carries the 32-byte descriptor with SHM_PAYLOAD_FLAG. On success the ring +// owns staged_seg (evicted with the slot); the previously-latched segment of +// the overwritten slot is returned in evicted_out so the caller can destroy +// it (munmap + shm_unlink) after releasing the lock. Returns false — and +// leaves staged_seg destroyed, nothing latched, live sends unaffected — when +// the ring is absent or the slot's pages cannot be committed (ENOSPC). +bool tl_ring_latch( + TlRingWriter & ring, + const WireHeader & hdr, + const uint8_t * payload, + size_t payload_size, + const ShmPayloadDescriptor * staged_desc, + std::unique_ptr staged_seg, + std::unique_ptr & evicted_out); + +// Map, validate, and snapshot a publisher's latched cache. expected_gid16 is +// the slot GID the name came from; a mismatched or malformed segment is +// skipped silently (stale incarnation). Records are returned sorted by +// sequence number, at most max_records newest. max_seq_out is the highest +// sequence number observed among stable records — the subscriber's dedup +// watermark. Per-slot seqlock reads are bounded (skip, never spin), so a +// publisher killed mid-write cannot hang subscription creation. +// *overlapped_out is set true when a writer latched DURING the scan (any +// slot's seq moved by the end — skipped slots included, since those are the +// ones a gap hides behind): the scan is then not a point-in-time snapshot +// and a sequence gap in the result may hide a sample the scan missed — the +// caller must not extend its dedup watermark across such a gap (the missed +// sample's datagram is in flight and must not be dropped). +bool tl_ring_pull( + const std::string & shm_name, + const uint8_t * expected_gid16, + size_t max_records, + std::vector & records_out, + int64_t & max_seq_out, + bool * overlapped_out); + +// Unmap + unlink the latched cache (publisher destruction / failed create). +void tl_ring_close(TlRingWriter & ring); + // Unmap + unlink the publisher's current segment (publisher destruction). void shm_writer_close(ShmRingWriter & ring); diff --git a/rmw_unix_socket_cpp/src/types.hpp b/rmw_unix_socket_cpp/src/types.hpp index 0b4e1d2..d503bbb 100644 --- a/rmw_unix_socket_cpp/src/types.hpp +++ b/rmw_unix_socket_cpp/src/types.hpp @@ -15,15 +15,16 @@ #ifndef RMW_UNIX_SOCKET_CPP__TYPES_HPP_ #define RMW_UNIX_SOCKET_CPP__TYPES_HPP_ +#include #include #include #include #include #include +#include #include #include #include -#include #include #include #include @@ -154,9 +155,6 @@ inline bool is_same_context(const WireHeader & hdr, uint64_t context_id) return sender_context_id == context_id; } -// Forward declaration: publisher type defined below. -struct UdsPublisher; - // Per-context implementation data struct UdsContext { @@ -179,9 +177,17 @@ struct UdsContext // Doorbell: a bound datagram socket other processes ring (one octet) after // any registry mutation, so a blocked rmw_wait re-checks the registry - // without polling. Registered in the registry as ENTRY_DOORBELL; the slot's - // teardown unlinks the socket file (graceful or via the stale-PID reaper). + // without polling. The socket is bound at rmw_init, but the ENTRY_DOORBELL + // slot is registered LAZILY, from the first rmw_wait whose wait set holds a + // graph guard condition: only graph-event consumers (wait_for_service, + // GraphListener, rosbag2) need registry wakeups now that TRANSIENT_LOCAL + // replay is pull-based, so plain pub/sub processes never register one and a + // fleet launch rings ~no doorbells. Guarded by doorbell_reg_mutex; the + // slot's teardown unlinks the socket file (graceful or stale-PID reaper). int doorbell_fd = -1; + std::string doorbell_path; + std::mutex doorbell_reg_mutex; + std::atomic doorbell_registered{false}; int32_t doorbell_registry_index = -1; // Per-node graph guard conditions (see rmw_node_get_graph_guard_condition), @@ -189,10 +195,6 @@ struct UdsContext // the mutex; rmw_destroy_node removes its entry before destroying the GC. std::mutex graph_gcs_mutex; std::vector graph_gcs; - - // TRANSIENT_LOCAL publishers, for wait-side cache replay on graph change. - std::mutex transient_local_pubs_mutex; - std::vector transient_local_pubs; }; // Node data @@ -205,19 +207,6 @@ struct UdsNode int32_t registry_index = -1; }; -// Cached message for TRANSIENT_LOCAL replay. For large payloads, `payload` -// holds a ShmPayloadDescriptor (header.msg_type carries SHM_PAYLOAD_FLAG) and -// `shm_seg` owns the durable segment the descriptor points at; the segment -// lives exactly as long as this cache entry, so it is replayable to late -// joiners and unlinked when the entry is evicted. Small payloads keep the -// inline bytes and leave shm_seg null. -struct CachedMessage -{ - WireHeader header; - std::vector payload; - std::unique_ptr shm_seg; -}; - // Publisher data struct UdsPublisher { @@ -241,10 +230,14 @@ struct UdsPublisher // wait hot path copies one refcount instead of N strings. Null until first refresh. std::shared_ptr> cached_subscriber_paths; - // TRANSIENT_LOCAL: cache of last N messages for late-joining subscribers + // TRANSIENT_LOCAL latched cache, pulled by late joiners themselves at + // rmw_create_subscription (see shm_transport.hpp). cache_mutex serializes + // latching; hdr.sequence_number is assigned under it so slot commit order + // equals sequence order, which is what makes the puller's watermark dedup + // sound. The segment is created BEFORE registry_add and its name published + // in the slot's socket_path; the idle publisher pays nothing, ever. std::mutex cache_mutex; - std::deque message_cache; - std::set known_subscriber_paths; // subs we've already replayed to + TlRingWriter tl_ring; // Large payloads: per-publisher /dev/shm ring (created lazily on the first // payload >= SHM_PAYLOAD_THRESHOLD). shm_mutex serializes staging when @@ -275,6 +268,14 @@ struct UdsSubscription // rmw_subscription_options_t::ignore_local_publications, copied at creation // time (used by drain_subscription()/drain_socket()). bool ignore_local_publications = false; + // TRANSIENT_LOCAL dedup: highest sequence number pulled from each latched + // publisher's cache at creation, keyed by the FULL 16-byte GID (the trailing + // context_id bytes are what distinguish a respawned publisher under a + // recycled pid — anything less would blackhole its fresh samples). Frozen at + // pull time; the drains drop an inbound datagram whose (gid, seq) is at or + // below its watermark. Guarded by queue_mutex. One small entry per latched + // publisher ever pulled — subscription-lifetime state, never pruned. + std::map, int64_t> replayed_watermarks; // Callback support std::mutex callback_mutex; rmw_event_callback_t on_new_message_cb = nullptr; diff --git a/rmw_unix_socket_cpp/test/test_rmw_qos.cpp b/rmw_unix_socket_cpp/test/test_rmw_qos.cpp index e2c9cad..f2bd0da 100644 --- a/rmw_unix_socket_cpp/test/test_rmw_qos.cpp +++ b/rmw_unix_socket_cpp/test/test_rmw_qos.cpp @@ -14,12 +14,17 @@ #include "test_base.hpp" +#include #include #include #include +#include +#include #include #include +#include +#include #include #include @@ -31,6 +36,8 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rosidl_typesupport_cpp/service_type_support.hpp" +#include + #include "types.hpp" class QosTest : public RmwUdsNodeTest @@ -138,14 +145,6 @@ TEST_F(QosTest, TransientLocalLargeMessageUsesDurableShm) EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); auto * pub_data = static_cast(pub->data); - { - std::lock_guard lock(pub_data->cache_mutex); - ASSERT_EQ(1u, pub_data->message_cache.size()); - const auto & cm = pub_data->message_cache.back(); - EXPECT_NE(nullptr, cm.shm_seg) - << "a large TRANSIENT_LOCAL message must be cached in a durable shm segment"; - EXPECT_TRUE(cm.header.msg_type & rmw_uds::SHM_PAYLOAD_FLAG); - } EXPECT_EQ(nullptr, pub_data->shm_ring.base) << "TRANSIENT_LOCAL must use a durable segment, never the cycling ring"; @@ -199,13 +198,6 @@ TEST_F(QosTest, TransientLocalHugeMessageLateJoiner) } EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &big, nullptr)); - auto * pub_data = static_cast(pub->data); - { - std::lock_guard lock(pub_data->cache_mutex); - ASSERT_EQ(1u, pub_data->message_cache.size()); - EXPECT_NE(nullptr, pub_data->message_cache.back().shm_seg); - } - // Late joiner, then a small publish to trigger replay of the cached 5 MB msg. auto sub_opts = rmw_get_default_subscription_options(); auto * sub = rmw_create_subscription(node, seq_ts, "/latched_huge", &qos, &sub_opts); @@ -267,11 +259,14 @@ TEST_F(QosTest, TransientLocalPublishReturnsErrorOnEMSGSIZE) auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); } -TEST_F(QosTest, TransientLocalLargeMessageInlineFallbackWhenShmUnavailable) +TEST_F(QosTest, TransientLocalLargeMessageNotLatchedWhenShmUnavailable) { - // When shm staging is unavailable, a large latched payload falls back to - // caching inline (no durable segment, no SHM_PAYLOAD_FLAG) and is still - // delivered byte-equal to a late joiner. The test seam forces the failure. + // When shm staging is unavailable, a large latched payload cannot enter the + // pull cache (there is nowhere durable to put it): the publish still + // returns OK, an EXISTING subscriber still receives the sample live as an + // inline datagram, and a late joiner gets nothing — the documented + // degradation of pull-based replay, replacing the old inline-heap fallback + // that the deleted push machinery could still replay. auto seq_ts = rosidl_typesupport_cpp::get_message_type_support_handle< test_msgs::msg::UnboundedSequences>(); auto qos = make_qos( @@ -281,6 +276,11 @@ TEST_F(QosTest, TransientLocalLargeMessageInlineFallbackWhenShmUnavailable) auto * pub = rmw_create_publisher(node, seq_ts, "/tl_fallback", &qos, &pub_opts); ASSERT_NE(nullptr, pub); + // Existing subscriber: must receive the sample live despite shm being down. + auto sub_opts = rmw_get_default_subscription_options(); + auto * live_sub = rmw_create_subscription(node, seq_ts, "/tl_fallback", &qos, &sub_opts); + ASSERT_NE(nullptr, live_sub); + setenv("RMW_UDS_TEST_FORCE_SHM_FAILURE", "1", 1); test_msgs::msg::UnboundedSequences msg; msg.uint8_values.resize(100 * 1024); @@ -290,30 +290,22 @@ TEST_F(QosTest, TransientLocalLargeMessageInlineFallbackWhenShmUnavailable) EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); unsetenv("RMW_UDS_TEST_FORCE_SHM_FAILURE"); // reset before it leaks to other tests - auto * pub_data = static_cast(pub->data); - { - std::lock_guard lock(pub_data->cache_mutex); - ASSERT_EQ(1u, pub_data->message_cache.size()); - const auto & cm = pub_data->message_cache.back(); - EXPECT_EQ(nullptr, cm.shm_seg) - << "shm forced unavailable — the large payload must be cached inline"; - EXPECT_FALSE(cm.header.msg_type & rmw_uds::SHM_PAYLOAD_FLAG); - } - - auto sub_opts = rmw_get_default_subscription_options(); - auto * sub = rmw_create_subscription(node, seq_ts, "/tl_fallback", &qos, &sub_opts); - ASSERT_NE(nullptr, sub); - test_msgs::msg::UnboundedSequences trigger; - trigger.uint8_values = {5, 6, 7}; - EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &trigger, nullptr)); - test_msgs::msg::UnboundedSequences recv; bool taken = false; - EXPECT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); - ASSERT_TRUE(taken) << "late joiner must receive the inline-fallback message"; + EXPECT_EQ(RMW_RET_OK, rmw_take(live_sub, &recv, &taken, nullptr)); + ASSERT_TRUE(taken) << "existing subscriber must receive the inline live send"; EXPECT_EQ(msg.uint8_values, recv.uint8_values); - auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + // Late joiner: the sample was never latched, so nothing is replayed. + auto * late_sub = rmw_create_subscription(node, seq_ts, "/tl_fallback", &qos, &sub_opts); + ASSERT_NE(nullptr, late_sub); + taken = false; + EXPECT_EQ(RMW_RET_OK, rmw_take(late_sub, &recv, &taken, nullptr)); + EXPECT_FALSE(taken) << + "a sample that could not be latched must not reach a late joiner"; + + auto _r0 [[maybe_unused]] = rmw_destroy_subscription(node, late_sub); + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, live_sub); auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); } @@ -385,12 +377,6 @@ TEST_F(QosTest, TransientLocalSerializedLargeMessageLateJoiner) // Publish BEFORE any subscriber — must be cached in a durable segment. EXPECT_EQ(RMW_RET_OK, rmw_publish_serialized_message(pub, &serialized, nullptr)); - auto * pub_data = static_cast(pub->data); - { - std::lock_guard lock(pub_data->cache_mutex); - ASSERT_EQ(1u, pub_data->message_cache.size()); - EXPECT_NE(nullptr, pub_data->message_cache.back().shm_seg); - } auto sub_opts = rmw_get_default_subscription_options(); auto * sub = rmw_create_subscription(node, ts, "/tl_serialized_huge", &qos, &sub_opts); @@ -422,100 +408,6 @@ TEST_F(QosTest, TransientLocalSerializedLargeMessageLateJoiner) auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); } -TEST_F(QosTest, KnownSubscriberPathsPrunedOnChurn) -{ - // The publisher's known_subscriber_paths must not accumulate dead entries as - // subscribers churn: each create/destroy bumps the registry generation, and a - // restarted subscriber gets a brand-new unique socket path. Without pruning on - // refresh the set is insert-only and grows by one per churned subscriber. - auto qos = make_qos( - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); - - auto pub_opts = rmw_get_default_publisher_options(); - auto * pub = rmw_create_publisher(node, ts, "/churn", &qos, &pub_opts); - ASSERT_NE(nullptr, pub); - - // Seed the cache so there is something to replay. - test_msgs::msg::BasicTypes seed; - seed.int32_value = 1; - EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &seed, nullptr)); - - auto sub_opts = rmw_get_default_subscription_options(); - constexpr int kChurn = 8; - for (int i = 0; i < kChurn; ++i) { - // New sub bumps generation -> next publish refreshes + records this sub. - auto * sub = rmw_create_subscription(node, ts, "/churn", &qos, &sub_opts); - ASSERT_NE(nullptr, sub); - test_msgs::msg::BasicTypes m; - m.int32_value = i + 2; - EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &m, nullptr)); - // Destroy bumps generation again; next publish refreshes + prunes the gone sub. - auto _r [[maybe_unused]] = rmw_destroy_subscription(node, sub); - EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &m, nullptr)); - } - - // After the loop every churned sub is destroyed, so known should be empty. - auto * pub_data = static_cast(pub->data); - size_t known_size = 0; - { - std::lock_guard lock(pub_data->cache_mutex); - known_size = pub_data->known_subscriber_paths.size(); - } - // With the prune: tracks only live subs (0 here). Without it: grows to kChurn. - EXPECT_LE(known_size, 1u) - << "known_subscriber_paths leaked dead entries: size=" << known_size; - - auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); -} - -TEST_F(QosTest, TransientLocalSerializedKnownSubscriberPathsPrunedOnChurn) -{ - // Same prune guarantee as KnownSubscriberPathsPrunedOnChurn, but driven - // through rmw_publish_serialized_message, which carries its own copy of the - // prune-on-refresh logic. Guards against that copy silently diverging: without - // the prune the serialized path's known_subscriber_paths grows by one per - // churned subscriber. - auto qos = make_qos( - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); - - auto pub_opts = rmw_get_default_publisher_options(); - auto * pub = rmw_create_publisher(node, ts, "/churn_serialized", &qos, &pub_opts); - ASSERT_NE(nullptr, pub); - - uint8_t bytes[] = {1, 2, 3, 4, 5, 6, 7, 8}; - rmw_serialized_message_t msg; - msg.buffer = bytes; - msg.buffer_length = sizeof(bytes); - msg.buffer_capacity = sizeof(bytes); - msg.allocator = rcutils_get_default_allocator(); - - // Seed the cache so there is something to replay. - EXPECT_EQ(RMW_RET_OK, rmw_publish_serialized_message(pub, &msg, nullptr)); - - auto sub_opts = rmw_get_default_subscription_options(); - constexpr int kChurn = 8; - for (int i = 0; i < kChurn; ++i) { - auto * sub = rmw_create_subscription(node, ts, "/churn_serialized", &qos, &sub_opts); - ASSERT_NE(nullptr, sub); - EXPECT_EQ(RMW_RET_OK, rmw_publish_serialized_message(pub, &msg, nullptr)); - auto _r [[maybe_unused]] = rmw_destroy_subscription(node, sub); - EXPECT_EQ(RMW_RET_OK, rmw_publish_serialized_message(pub, &msg, nullptr)); - } - - auto * pub_data = static_cast(pub->data); - size_t known_size = 0; - { - std::lock_guard lock(pub_data->cache_mutex); - known_size = pub_data->known_subscriber_paths.size(); - } - EXPECT_LE(known_size, 1u) - << "serialized-path known_subscriber_paths leaked dead entries: size=" << known_size; - - auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); -} - TEST_F(QosTest, TransientLocalCacheDepthEnforced) { auto qos = make_qos( @@ -1135,3 +1027,546 @@ TEST_F(QosTest, TransientLocalLateJoinerWhilePublisherProcessIdle) auto _r3 [[maybe_unused]] = rmw_destroy_publisher(node, pub); auto _r4 [[maybe_unused]] = rmw_destroy_guard_condition(gc); } + +// --- Pull-based TRANSIENT_LOCAL replay (latched-cache pull) --- + +TEST_F(QosTest, TransientLocalPullDeliversWithNoWaitAnywhere) +{ + // The strongest form of the idle-publisher scenario: NO thread in this + // process ever enters rmw_wait, so there is no doorbell drain and no + // wait-side replay. A late joiner must still receive the latched history, + // immediately, because it pulls the publisher's latched cache itself at + // rmw_create_subscription. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/pull_no_wait", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + for (int i = 1; i <= 3; ++i) { + test_msgs::msg::BasicTypes msg; + msg.int32_value = i * 10; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); + } + + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription(node, ts, "/pull_no_wait", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + // No sleeps, no rmw_wait: the history must already be in the queue. + int got = 0; + for (int i = 0; i < 5; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + if (!taken) {break;} + EXPECT_EQ((got + 1) * 10, recv.int32_value); // oldest-first, in seq order + ++got; + } + EXPECT_EQ(3, got) << + "late joiner did not receive the latched history without a wait cycle"; + + auto _p1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _p2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); +} + +TEST_F(QosTest, TransientLocalPullNoDuplicateWithLivePublish) +{ + // Publish latched history, join, then publish one live sample. The + // subscriber must see each sample exactly once: the pull covers the history, + // the datagram covers the live sample, and the per-publisher sequence + // watermark dedups any overlap. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 10); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/pull_dedup", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + for (int i = 1; i <= 3; ++i) { + test_msgs::msg::BasicTypes msg; + msg.int32_value = i; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); + } + + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription(node, ts, "/pull_dedup", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + test_msgs::msg::BasicTypes live; + live.int32_value = 4; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &live, nullptr)); + + // Drain with retries: the live datagram needs a moment to land. + std::vector seen; + for (int i = 0; i < 200 && seen.size() < 4; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + if (taken) { + seen.push_back(recv.int32_value); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + } + ASSERT_EQ(4u, seen.size()) << "expected the 3 latched + 1 live samples"; + for (int i = 0; i < 4; ++i) { + EXPECT_EQ(i + 1, seen[i]); // exactly once each, in order + } + // And nothing further: no duplicate from replay-vs-datagram overlap. + { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + EXPECT_FALSE(taken) << "duplicate sample delivered"; + } + + auto _p1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _p2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); +} + +TEST_F(QosTest, TransientLocalPullRespectsIgnoreLocal) +{ + // Same context, ignore_local_publications=true: the pulled history must be + // filtered exactly like the datagram path filters live samples, or a + // transform/republish node feeds back its own latched output. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/pull_ignore_local", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + test_msgs::msg::BasicTypes msg; + msg.int32_value = 42; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); + + auto sub_opts = rmw_get_default_subscription_options(); + sub_opts.ignore_local_publications = true; + auto * sub = rmw_create_subscription(node, ts, "/pull_ignore_local", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + EXPECT_FALSE(taken) << + "same-context latched history delivered despite ignore_local_publications"; + + auto _p1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _p2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); +} + +TEST_F(QosTest, TransientLocalPullSubscriberChurnRedelivers) +{ + // Destroy + recreate a subscription on the same topic: the NEW subscription + // is a new endpoint and must receive the full latched history again (its own + // pull), exactly once. This replaces the old known_subscriber_paths pruning + // tests, which pinned the deleted push-side machinery. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/pull_churn", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + test_msgs::msg::BasicTypes msg; + msg.int32_value = 5; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); + + auto sub_opts = rmw_get_default_subscription_options(); + for (int round = 0; round < 3; ++round) { + auto * sub = rmw_create_subscription(node, ts, "/pull_churn", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + EXPECT_TRUE(taken) << "round " << round << ": latched history not redelivered"; + if (taken) { + EXPECT_EQ(5, recv.int32_value); + } + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + EXPECT_FALSE(taken) << "round " << round << ": history delivered twice"; + + auto _p [[maybe_unused]] = rmw_destroy_subscription(node, sub); + } + + auto _p2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); +} + +TEST_F(QosTest, TransientLocalConcurrentPublishChurnStress) +{ + // Merge gate for the pull design: subscriptions churn while a publisher + // thread latches continuously. Every subscription must observe its + // publisher's samples exactly once (no pull/datagram duplicate, no + // watermark-swallowed sample) and in sequence order — the pull runs + // concurrently with ring overwrites, so this exercises the per-record + // seqlock snapshot and the store-buffering fence pair under real load. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 10); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/pull_stress", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + std::atomic stop{false}; + std::atomic published{0}; + std::thread publisher_thread( + [&] { + int32_t v = 0; + while (!stop.load()) { + test_msgs::msg::BasicTypes m; + m.int32_value = ++v; + if (rmw_publish(pub, &m, nullptr) != RMW_RET_OK) { + break; + } + published.store(v); + } + }); + // A fatal assertion below returns from the test with publisher_thread + // still joinable, which would std::terminate the whole process instead of + // reporting the failure — stop and join on every exit path. + struct JoinGuard + { + std::atomic & stop_flag; + std::thread & thread; + ~JoinGuard() + { + stop_flag.store(true); + if (thread.joinable()) { + thread.join(); + } + } + } join_guard{stop, publisher_thread}; + + // Gate round 0 on the pipeline being live: with zero publishes completed, + // the first churn round's pull finds an empty ring and its take loop could + // expire before the first sample lands — a scheduler flake, not a defect. + for (int i = 0; i < 2000 && published.load() == 0; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_GT(published.load(), 0) << "publisher thread never published"; + + // Churn: each round joins mid-stream, drains for a moment, and must see a + // strictly increasing, duplicate-free value sequence. + auto sub_opts = rmw_get_default_subscription_options(); + for (int round = 0; round < 20; ++round) { + auto * sub = rmw_create_subscription(node, ts, "/pull_stress", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + int32_t last = 0; + int received = 0; + for (int i = 0; i < 40; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + if (!taken) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + EXPECT_GT(recv.int32_value, last) + << "round " << round << ": duplicate or out-of-order sample " + << recv.int32_value << " after " << last; + last = recv.int32_value; + ++received; + } + EXPECT_GT(received, 0) << "round " << round << ": no samples at all"; + auto _r [[maybe_unused]] = rmw_destroy_subscription(node, sub); + } + + stop.store(true); + if (publisher_thread.joinable()) { + publisher_thread.join(); // JoinGuard then finds nothing left to do + } + + // Quiesced late joiner: must receive exactly the newest depth samples, in + // order — the latched history and nothing else. + const int32_t total = published.load(); + ASSERT_GE(total, 20); + auto * final_sub = rmw_create_subscription(node, ts, "/pull_stress", &qos, &sub_opts); + ASSERT_NE(nullptr, final_sub); + std::vector tail; + for (int i = 0; i < 15; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(final_sub, &recv, &taken, nullptr)); + if (!taken) {break;} + tail.push_back(recv.int32_value); + } + ASSERT_EQ(10u, tail.size()) << "expected exactly depth latched samples"; + for (size_t i = 0; i < tail.size(); ++i) { + EXPECT_EQ(total - 9 + static_cast(i), tail[i]) + << "latched history is not the newest-depth suffix in order"; + } + + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, final_sub); + auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); +} + +TEST_F(QosTest, TransientLocalWatermarkDropsForgedDuplicate) +{ + // Deterministic dedup coverage: after the pull sets the watermark, a + // datagram carrying the publisher's GID with a sequence number at or below + // the watermark must be dropped by the drain — and a genuine live sample + // must still get through, proving the drop is not vacuous. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 10); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/wm_forge", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + for (int i = 1; i <= 3; ++i) { + test_msgs::msg::BasicTypes m; + m.int32_value = i; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &m, nullptr)); + } + + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription(node, ts, "/wm_forge", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + // Drain the pulled history (seqs 1..3, watermark = 3). + for (int i = 0; i < 3; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + ASSERT_TRUE(taken); + } + + // Forge a duplicate the way the pull/live overlap would produce one: the + // publisher's GID, sequence 2, sent straight to the subscription socket. + auto * pub_data = static_cast(pub->data); + auto * sub_data = static_cast(sub->data); + rmw_uds::WireHeader forged; + std::memset(&forged, 0, sizeof(forged)); + std::memcpy(forged.gid, pub_data->gid.data, sizeof(forged.gid)); + forged.sequence_number = 2; + forged.msg_type = 0; + uint8_t junk[8] = {0}; + forged.payload_size = sizeof(junk); + // Raw sendto (send_to is not exported from the shared library): one + // datagram of WireHeader + payload to the subscription's bound socket, + // exactly what a publisher's fan-out produces. + { + struct sockaddr_un addr; + std::memset(&addr, 0, sizeof(addr)); + addr.sun_family = AF_UNIX; + std::strncpy(addr.sun_path, sub_data->socket_path.c_str(), + sizeof(addr.sun_path) - 1); + std::vector dgram(sizeof(forged) + sizeof(junk)); + std::memcpy(dgram.data(), &forged, sizeof(forged)); + std::memcpy(dgram.data() + sizeof(forged), junk, sizeof(junk)); + ASSERT_EQ(static_cast(dgram.size()), + sendto(sub_data->context->send_socket_fd, dgram.data(), dgram.size(), + 0, reinterpret_cast(&addr), sizeof(addr))); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + EXPECT_FALSE(taken) << "forged duplicate (seq <= watermark) was delivered"; + + // The watermark must not eat genuine live traffic. + test_msgs::msg::BasicTypes live; + live.int32_value = 44; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &live, nullptr)); + bool got_live = false; + for (int i = 0; i < 100 && !got_live; ++i) { + taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + if (taken && recv.int32_value == 44) {got_live = true;} + if (!taken) {std::this_thread::sleep_for(std::chrono::milliseconds(2));} + } + EXPECT_TRUE(got_live) << "live sample after the watermark was not delivered"; + + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); +} + +TEST_F(QosTest, ReplayWatermarkStopsAtAnUnresolvableRecord) +{ + // An over-cap latched record whose durable segment is unlinked after the + // scan copied its descriptor cannot be resolved, so it is never delivered. + // The watermark must not claim its sequence: the same sample's live + // datagram is inline at this size and may be in flight, and dedup would + // drop it — losing the sample outright. Unlinking the segments before the + // pull stands in for the concurrent overwrite that produces this state. + auto seq_ts = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::UnboundedSequences>(); + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); + + char prefix[64]; + std::snprintf(prefix, sizeof(prefix), "ros2_uds_data_%zu_%d_", + static_cast(node->context->actual_domain_id), + static_cast(getpid())); + auto data_segments = [&prefix]() { + std::set names; + DIR * dir = opendir("/dev/shm"); + if (dir == nullptr) { + return names; + } + while (auto * entry = readdir(dir)) { + if (std::strncmp(entry->d_name, prefix, std::strlen(prefix)) == 0) { + names.insert(std::string("/") + entry->d_name); + } + } + closedir(dir); + return names; + }; + const auto before = data_segments(); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, seq_ts, "/wm_unresolvable", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + test_msgs::msg::UnboundedSequences msg; + msg.uint8_values.resize(4 * 1024); // over TL_EMBED_CAP: staged durably + for (int i = 1; i <= 3; ++i) { // sequences 1..3 + msg.uint8_values[0] = static_cast(i); + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); + } + + size_t evicted = 0; + for (const auto & name : data_segments()) { + if (before.count(name) == 0) { + shm_unlink(name.c_str()); + ++evicted; + } + } + ASSERT_GT(evicted, 0u) << "no durable segment was staged for the latched samples"; + + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription(node, seq_ts, "/wm_unresolvable", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + auto * pub_data = static_cast(pub->data); + auto * sub_data = static_cast(sub->data); + std::array key; + std::memcpy(key.data(), pub_data->gid.data, key.size()); + { + std::lock_guard lock(sub_data->queue_mutex); + EXPECT_EQ(0, sub_data->replayed_watermarks[key]) + << "the watermark claimed sequences the replay never delivered"; + } + + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); +} + +TEST_F(QosTest, TransientLocalTwoPublishersBothHistoriesPulled) +{ + // Two latched publishers on one topic: a late joiner must receive both + // histories exactly once (independent per-GID watermarks), then one live + // sample from each. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 10); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub_a = rmw_create_publisher(node, ts, "/two_pubs", &qos, &pub_opts); + auto * pub_b = rmw_create_publisher(node, ts, "/two_pubs", &qos, &pub_opts); + ASSERT_NE(nullptr, pub_a); + ASSERT_NE(nullptr, pub_b); + + for (int i = 1; i <= 2; ++i) { + test_msgs::msg::BasicTypes m; + m.int32_value = 100 + i; // A: 101, 102 + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub_a, &m, nullptr)); + m.int32_value = 200 + i; // B: 201, 202 + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub_b, &m, nullptr)); + } + + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription(node, ts, "/two_pubs", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + std::vector seen; + for (int i = 0; i < 6; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + if (!taken) {break;} + seen.push_back(recv.int32_value); + } + std::sort(seen.begin(), seen.end()); + const std::vector expect = {101, 102, 201, 202}; + EXPECT_EQ(expect, seen) << "both publishers' histories, exactly once"; + + test_msgs::msg::BasicTypes m; + m.int32_value = 103; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub_a, &m, nullptr)); + m.int32_value = 203; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub_b, &m, nullptr)); + seen.clear(); + for (int i = 0; i < 200 && seen.size() < 2; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub, &recv, &taken, nullptr)); + if (taken) { + seen.push_back(recv.int32_value); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } + std::sort(seen.begin(), seen.end()); + const std::vector expect_live = {103, 203}; + EXPECT_EQ(expect_live, seen) << "one live sample from each publisher"; + + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub_a); + auto _r3 [[maybe_unused]] = rmw_destroy_publisher(node, pub_b); +} + +TEST_F(QosTest, TransientLocalMidBandPayloadLatchedAndLive) +{ + // The mid band — above the embed cap (1 KiB), below the datagram shm + // threshold (64 KiB) — rides a durable descriptor in the latched slot but + // an inline datagram on the wire. Both consumers must get byte-equal data. + auto seq_ts = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::UnboundedSequences>(); + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, seq_ts, "/mid_band", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + auto sub_opts = rmw_get_default_subscription_options(); + auto * live_sub = rmw_create_subscription(node, seq_ts, "/mid_band", &qos, &sub_opts); + ASSERT_NE(nullptr, live_sub); + + test_msgs::msg::UnboundedSequences msg; + msg.uint8_values.resize(4 * 1024); // squarely in the mid band + for (size_t i = 0; i < msg.uint8_values.size(); ++i) { + msg.uint8_values[i] = static_cast((i * 13 + 3) & 0xFF); + } + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); + + test_msgs::msg::UnboundedSequences recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(live_sub, &recv, &taken, nullptr)); + ASSERT_TRUE(taken) << "live mid-band sample not delivered inline"; + EXPECT_EQ(msg.uint8_values, recv.uint8_values); + + auto * late_sub = rmw_create_subscription(node, seq_ts, "/mid_band", &qos, &sub_opts); + ASSERT_NE(nullptr, late_sub); + taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(late_sub, &recv, &taken, nullptr)); + ASSERT_TRUE(taken) << "mid-band latched sample not pulled by the late joiner"; + EXPECT_EQ(msg.uint8_values, recv.uint8_values); + + auto _r0 [[maybe_unused]] = rmw_destroy_subscription(node, late_sub); + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, live_sub); + auto _r2 [[maybe_unused]] = rmw_destroy_publisher(node, pub); +} diff --git a/rmw_unix_socket_cpp/test/test_shm_transport.cpp b/rmw_unix_socket_cpp/test/test_shm_transport.cpp index 44e8018..664551a 100644 --- a/rmw_unix_socket_cpp/test/test_shm_transport.cpp +++ b/rmw_unix_socket_cpp/test/test_shm_transport.cpp @@ -14,16 +14,21 @@ #include +#include +#include #include #include +#include #include #include +#include #include #include #include #include "../src/shm_transport.hpp" +#include "../src/types.hpp" // WireHeader, for the tl_ring latched cache class ShmTransportTest : public ::testing::Test { @@ -307,3 +312,281 @@ TEST_F(ShmTransportTest, CommitOverReservationIsRejected) ASSERT_TRUE(rmw_uds::shm_fetch_payload(cache, domain_id, wire)); EXPECT_EQ(payload, wire); } + +// --- TRANSIENT_LOCAL latched cache (tl_ring) --- + +static rmw_uds::WireHeader make_tl_header(int64_t seq) +{ + rmw_uds::WireHeader hdr; + std::memset(&hdr, 0, sizeof(hdr)); + hdr.gid[0] = 0xAB; + hdr.gid[8] = 0xCD; // context-id byte: part of the full-GID validation + hdr.sequence_number = seq; + hdr.msg_type = 0; + return hdr; +} + +TEST_F(ShmTransportTest, TlRingLatchAndPullRoundTrip) +{ + rmw_uds::TlRingWriter ring; + uint8_t gid[16] = {0xAB, 0, 0, 0, 0, 0, 0, 0, 0xCD, 0, 0, 0, 0, 0, 0, 0}; + ASSERT_TRUE(rmw_uds::tl_ring_create(ring, domain_id, gid, 5)); + + std::unique_ptr evicted; + for (int64_t i = 1; i <= 8; ++i) { // 8 > depth 5: oldest three lap out + auto hdr = make_tl_header(i); + std::vector payload(64, static_cast(i)); + hdr.payload_size = static_cast(payload.size()); + ASSERT_TRUE(rmw_uds::tl_ring_latch( + ring, hdr, payload.data(), payload.size(), nullptr, nullptr, evicted)); + EXPECT_EQ(nullptr, evicted); // embedded records own no durable segment + } + + std::vector records; + int64_t max_seq = 0; + bool overlapped = true; + ASSERT_TRUE(rmw_uds::tl_ring_pull( + ring.shm_name, gid, 10, records, max_seq, &overlapped)); + EXPECT_FALSE(overlapped); // no concurrent writer in this test + EXPECT_EQ(8, max_seq); + ASSERT_EQ(5u, records.size()); + for (size_t i = 0; i < records.size(); ++i) { + EXPECT_EQ(static_cast(4 + i), records[i].sequence_number); + EXPECT_EQ(static_cast(4 + i), records[i].payload.at(0)); + } + + // Wrong expected GID (stale-segment defense): the pull must refuse. + uint8_t wrong_gid[16] = {0xAB, 0, 0, 0, 0, 0, 0, 0, 0xEE, 0, 0, 0, 0, 0, 0, 0}; + EXPECT_FALSE(rmw_uds::tl_ring_pull( + ring.shm_name, wrong_gid, 10, records, max_seq, nullptr)); + + // Capture the name BEFORE close (close clears it): this drives the pull + // through shm_open ENOENT — the destroyed-publisher path — instead of the + // empty-name early return, verifying close actually unlinked the segment. + const std::string name = ring.shm_name; + rmw_uds::tl_ring_close(ring); + EXPECT_FALSE(rmw_uds::tl_ring_pull(name, gid, 10, records, max_seq, nullptr)); +} + +TEST_F(ShmTransportTest, TlRingPullSkipsPoisonedSlotAndReturnsPromptly) +{ + // A publisher killed mid-latch leaves one slot's seqlock odd forever. The + // pull must skip exactly that slot after bounded retries — never spin, and + // never discard the rest of the history. + rmw_uds::TlRingWriter ring; + uint8_t gid[16] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}; + ASSERT_TRUE(rmw_uds::tl_ring_create(ring, domain_id, gid, 4)); + + std::unique_ptr evicted; + for (int64_t i = 1; i <= 4; ++i) { + auto hdr = make_tl_header(i); + std::vector payload(32, static_cast(i)); + hdr.payload_size = static_cast(payload.size()); + ASSERT_TRUE(rmw_uds::tl_ring_latch( + ring, hdr, payload.data(), payload.size(), nullptr, nullptr, evicted)); + } + + // Poison slot 1 (record seq 2) the way a SIGKILL mid-write would: odd seq. + auto * record = reinterpret_cast( + ring.base + 64 /* header area */ + 1 * ring.slot_bytes); + record->seq.store(2 * 2 - 1, std::memory_order_release); + + const auto t0 = std::chrono::steady_clock::now(); + std::vector records; + int64_t max_seq = 0; + bool overlapped = true; + ASSERT_TRUE(rmw_uds::tl_ring_pull( + ring.shm_name, gid, 10, records, max_seq, &overlapped)); + EXPECT_FALSE(overlapped) << "a dead writer's poisoned slot is not overlap"; + const auto elapsed = std::chrono::steady_clock::now() - t0; + + EXPECT_LT( + std::chrono::duration_cast(elapsed).count(), 500) + << "pull must not spin on a dead writer's odd seqlock"; + ASSERT_EQ(3u, records.size()) << "only the poisoned slot may be skipped"; + EXPECT_EQ(1, records[0].sequence_number); + EXPECT_EQ(3, records[1].sequence_number); + EXPECT_EQ(4, records[2].sequence_number); + EXPECT_EQ(4, max_seq); + + rmw_uds::tl_ring_close(ring); +} + +TEST_F(ShmTransportTest, TlRingPullFlagsOverlapWhenASkippedSlotIsFilled) +{ + // The dedup watermark is sound only if a sequence gap in the pulled result + // implies overlapped: on overlap the subscriber keeps just the contiguous + // prefix, and without it extends the watermark across the gap — dropping + // the missing sample's in-flight datagram. Slots the scan SKIPS (never + // written, or given up on after the bounded retries) produce no record, so + // a writer filling one during the scan has to be detected all the same. + // + // The interleaving is forced, not raced: both threads share one CPU, so the + // latching thread runs only when the scan yields — which it does at the + // poisoned slot, after it has already passed the empty slots 2-5. + cpu_set_t original; + CPU_ZERO(&original); + if (sched_getaffinity(0, sizeof(original), &original) != 0) { + GTEST_SKIP() << "CPU affinity unavailable"; + } + cpu_set_t single; + CPU_ZERO(&single); + for (int cpu = 0; cpu < CPU_SETSIZE; ++cpu) { + if (CPU_ISSET(cpu, &original)) { + CPU_SET(cpu, &single); + break; + } + } + if (sched_setaffinity(0, sizeof(single), &single) != 0) { + GTEST_SKIP() << "cannot pin to a single CPU"; + } + + uint8_t gid[16] = {0x5A, 1, 2, 3, 4, 5, 6, 7, 0xC3, 9, 10, 11, 12, 13, 14, 15}; + bool exercised = false; + for (int attempt = 0; attempt < 20 && !exercised; ++attempt) { + rmw_uds::TlRingWriter latched; + ASSERT_TRUE(rmw_uds::tl_ring_create(latched, domain_id, gid, 8)); + std::unique_ptr evicted; + for (int64_t i = 1; i <= 2; ++i) { // slots 0,1 — the cursor stops at 2 + auto hdr = make_tl_header(i); + std::vector payload(32, static_cast(i)); + hdr.payload_size = static_cast(payload.size()); + ASSERT_TRUE(rmw_uds::tl_ring_latch( + latched, hdr, payload.data(), payload.size(), nullptr, nullptr, evicted)); + } + // Slot 6 odd, exactly as sample 7 mid-write leaves it: the scan stalls + // there, having already passed slots 2-5 as never-written. + auto * poisoned = reinterpret_cast( + latched.base + 64 /* header area */ + 6 * latched.slot_bytes); + poisoned->seq.store(7 * 2 - 1, std::memory_order_release); + + std::atomic go{false}; + std::thread latcher([&latched, &go]() { + while (!go.load(std::memory_order_acquire)) { + } + std::unique_ptr ev; + for (int64_t i = 3; i <= 8; ++i) { // slots 2-7; sample 7 clears slot 6 + auto hdr = make_tl_header(i); + std::vector payload(32, static_cast(i)); + hdr.payload_size = static_cast(payload.size()); + (void)rmw_uds::tl_ring_latch( + latched, hdr, payload.data(), payload.size(), nullptr, nullptr, ev); + } + }); + + std::vector records; + int64_t max_seq = 0; + bool overlapped = false; + go.store(true, std::memory_order_release); + const bool pulled = rmw_uds::tl_ring_pull( + latched.shm_name, gid, 10, records, max_seq, &overlapped); + latcher.join(); + + bool gap = false; + for (size_t i = 1; i < records.size(); ++i) { + if (records[i].sequence_number != records[i - 1].sequence_number + 1) { + gap = true; + break; + } + } + if (pulled && gap) { + exercised = true; + EXPECT_TRUE(overlapped) + << "a gap opened by skipping a slot the writer then filled must read " + << "as overlap, or the watermark swallows the missing sample"; + } + rmw_uds::tl_ring_close(latched); + } + (void)sched_setaffinity(0, sizeof(original), &original); + if (!exercised) { + GTEST_SKIP() << "the scan never observed a writer-created gap"; + } +} + +TEST_F(ShmTransportTest, TlRingDepthClampAndNewestSuffix) +{ + // A depth beyond the byte cap clamps to the computed slot count (the stock + // rosout depth of 1000 must fit un-clamped — that is what sized the cap), + // and a fully-lapped ring replays exactly the newest slot-count suffix. + const size_t stride = 64; // SHM_RECORD_ALIGN + const size_t slot_bytes = + (sizeof(rmw_uds::ShmRecordHeader) + 37 + rmw_uds::TL_EMBED_CAP + stride - 1) & + ~(stride - 1); + const size_t expect_max = rmw_uds::TL_RING_MAX_BYTES / slot_bytes; + ASSERT_GE(expect_max, 1000u) << "stock rosout depth must fit the byte cap"; + + rmw_uds::TlRingWriter ring; + uint8_t gid[16] = {9, 9, 9, 9, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}; + ASSERT_TRUE(rmw_uds::tl_ring_create(ring, domain_id, gid, 10000)); + EXPECT_EQ(expect_max, ring.slots) << "depth 10000 must clamp to the byte cap"; + + // rosout-shaped depth: no clamp. + rmw_uds::TlRingWriter rosout_ring; + ASSERT_TRUE(rmw_uds::tl_ring_create(rosout_ring, domain_id, gid, 1000)); + EXPECT_EQ(1000u, rosout_ring.slots); + rmw_uds::tl_ring_close(rosout_ring); + + std::unique_ptr evicted; + const int64_t total = static_cast(ring.slots) + 5; // lap by 5 + for (int64_t i = 1; i <= total; ++i) { + auto hdr = make_tl_header(i); + uint8_t byte = static_cast(i & 0xFF); + hdr.payload_size = 1; + ASSERT_TRUE(rmw_uds::tl_ring_latch(ring, hdr, &byte, 1, nullptr, nullptr, evicted)); + } + std::vector records; + int64_t max_seq = 0; + ASSERT_TRUE(rmw_uds::tl_ring_pull( + ring.shm_name, gid, total + 10, records, max_seq, nullptr)); + EXPECT_EQ(total, max_seq); + ASSERT_EQ(static_cast(ring.slots), records.size()); + EXPECT_EQ(total - static_cast(ring.slots) + 1, + records.front().sequence_number); + EXPECT_EQ(total, records.back().sequence_number); + rmw_uds::tl_ring_close(ring); +} + +TEST_F(ShmTransportTest, TlRingEmbedCapBoundary) +{ + // Exactly TL_EMBED_CAP embeds; one byte over must be pre-staged durably by + // the caller. An off-by-one here would make tl_ring_pull treat the record + // length as torn and silently drop the latched sample. + rmw_uds::TlRingWriter ring; + uint8_t gid[16] = {7, 7, 7, 7, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 0}; + ASSERT_TRUE(rmw_uds::tl_ring_create(ring, domain_id, gid, 4)); + + std::unique_ptr evicted; + std::vector at_cap(rmw_uds::TL_EMBED_CAP, 0x5A); + auto hdr1 = make_tl_header(1); + hdr1.payload_size = static_cast(at_cap.size()); + ASSERT_TRUE(rmw_uds::tl_ring_latch( + ring, hdr1, at_cap.data(), at_cap.size(), nullptr, nullptr, evicted)); + + std::vector over_cap(rmw_uds::TL_EMBED_CAP + 1, 0xA5); + rmw_uds::ShmPayloadDescriptor desc; + auto seg = rmw_uds::shm_stage_durable( + domain_id, over_cap.data(), over_cap.size(), desc); + ASSERT_NE(nullptr, seg); + auto hdr2 = make_tl_header(2); + hdr2.payload_size = static_cast(over_cap.size()); + ASSERT_TRUE(rmw_uds::tl_ring_latch( + ring, hdr2, over_cap.data(), over_cap.size(), &desc, std::move(seg), evicted)); + + std::vector records; + int64_t max_seq = 0; + ASSERT_TRUE(rmw_uds::tl_ring_pull(ring.shm_name, gid, 10, records, max_seq, nullptr)); + ASSERT_EQ(2u, records.size()); + EXPECT_EQ(at_cap, records[0].payload); // embedded, byte-equal + // Over-cap record carries the 32-byte descriptor + SHM flag; resolve it + // through the same fetch path the subscription pull uses. + EXPECT_EQ(sizeof(rmw_uds::ShmPayloadDescriptor), records[1].payload.size()); + rmw_uds::WireHeader hdr_out; + std::memcpy(&hdr_out, records[1].wire_header, sizeof(hdr_out)); + EXPECT_TRUE(hdr_out.msg_type & rmw_uds::SHM_PAYLOAD_FLAG); + rmw_uds::ShmReaderCache local_cache; + std::vector resolved = records[1].payload; + ASSERT_TRUE(rmw_uds::shm_fetch_payload(local_cache, domain_id, resolved)); + EXPECT_EQ(over_cap, resolved); + rmw_uds::shm_reader_close(local_cache); + rmw_uds::tl_ring_close(ring); +}