diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6610537..5b3ffed 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,9 +2,9 @@ name: CI on: push: - branches: [main] + branches: [main, devel] pull_request: - branches: [main] + branches: [main, devel] workflow_dispatch: jobs: diff --git a/rmw_unix_socket_cpp/DESIGN.md b/rmw_unix_socket_cpp/DESIGN.md index f818332..0381999 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`).** `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. `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 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. **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. @@ -299,7 +299,7 @@ Every entity in the system claims exactly one slot in the shared-memory registry A slot is the `RegistryEntrySlot` struct mapped into shared memory (`registry.hpp`). It is plain data with no pointers, because pointers would be meaningless across processes that map the region at different addresses. Two fields at the front coordinate concurrent access, and the rest describe the endpoint: - `seq` (32-bit atomic) — the seqlock counter. Even means the payload is stable; odd means a writer is mid-update. -- `state` (8-bit atomic) — the entity kind and lifecycle marker: `ENTRY_EMPTY`, `ENTRY_NODE`, `ENTRY_PUBLISHER`, `ENTRY_SUBSCRIPTION`, `ENTRY_SERVICE`, `ENTRY_CLIENT`, plus the internal `ENTRY_RESERVED` value a writer sets to claim an empty slot before its payload is filled in. +- `state` (8-bit atomic) — the entity kind and lifecycle marker: `ENTRY_EMPTY`, `ENTRY_NODE`, `ENTRY_PUBLISHER`, `ENTRY_SUBSCRIPTION`, `ENTRY_SERVICE`, `ENTRY_CLIENT`, `ENTRY_DOORBELL` (one per context, holding the socket path of that process's wakeup doorbell — see The doorbell under Wait set; type-filtered queries never match it, so it is invisible to graph introspection), plus the internal `ENTRY_RESERVED` value a writer sets to claim an empty slot before its payload is filled in. - `pid` — the owning process ID. This is the liveness handle. Stale-entry cleanup reclaims a slot when `/proc/` no longer exists. - `gid[16]` — the RMW GID, the 16-byte unique identity of this endpoint. - `node_name[256]` and `node_namespace[256]` — which node owns this endpoint, used to answer graph queries. @@ -392,7 +392,7 @@ slots[i].state.store(static_cast(entry.type), std::memory_order_release The release store pairs with the acquire load every reader does on `state`. Any reader that now observes a real type (`ENTRY_NODE`, `ENTRY_PUBLISHER`, and so on) is guaranteed to also see the fully-written payload that was published before it. This store is the single point at which the slot becomes visible to discovery. Until it happens, the slot reads as `ENTRY_RESERVED` and is invisible. -**Bump the generation counter.** Finally the writer does `header->generation.fetch_add(1)`. The generation counter is the table's change signal. Publishers, services, and clients cache their lookup results and re-scan only when the generation moves, so bumping it here tells every cached reader that the graph changed and its cache is stale. This is what turns a new registration into a graph event without any push notification or daemon. +**Bump the generation counter, then ring the doorbells.** Finally the writer does `header->generation.fetch_add(1)`. The generation counter is the table's change signal. Publishers, services, and clients cache their lookup results and re-scan only when the generation moves, so bumping it here tells every cached reader that the graph changed and its cache is stale. Immediately after the bump, the writer sends one octet to every registered doorbell (`ring_doorbells`), which wakes any process blocked in `rmw_wait` so it re-reads the counter. The order is load-bearing: ring strictly after the bump, paired with the wait side draining its doorbell strictly before reading the counter, is what makes a lost wakeup impossible (see The doorbell under Wait set). This is what turns a new registration into a graph event without any daemon: the table is the state, the doorbell is the edge. **The slot index is remembered, so removal needs no scan.** `registry_add` returns the slot index, and the owning entity stores it. Removal (`registry_remove`) indexes straight to that slot and CASes its `state` back to `ENTRY_EMPTY`. There is no second scan to find the entry on the way out, which keeps teardown cheap and makes removal symmetric with the single-CAS claim used on the way in. @@ -532,16 +532,35 @@ 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 shared-memory registry holds a `generation` counter that is bumped whenever an endpoint is added or removed. The wait reads it once and compares it against the value cached on the context. If it moved, the graph changed, and the context's graph guard condition is triggered. This is a single atomic load from shared memory, which is why a daemon or cross-process push notification is not needed (see the graph guard condition design choice). -3. **Arm the fds.** Every entity fd and guard-condition eventfd 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 call `epoll_wait()` with the computed timeout. The thread is asleep in the kernel here, not spinning. `epoll_wait()` is retried on `EINTR` against a steady-clock deadline so a signal neither returns a false timeout nor busy-loops. +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. +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. `RMW_RET_TIMEOUT` therefore surfaces only at the caller's own deadline, and an infinite wait blocks until something the caller asked about is actually ready. `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. If nothing became ready, the call returns `RMW_RET_TIMEOUT`. ### Which guard condition the graph change wakes -There are two graph guard conditions in play, and the path between them matters. Each node creates its own guard condition in `rmw_create_node()` and stores it on the node (`UdsNode::graph_guard_condition`). `rmw_node_get_graph_guard_condition()` hands rcl that per-node object, so the per-node guard condition is what the executor's wait set actually watches for graph changes. +Each node creates its own graph guard condition in `rmw_create_node()` and stores it on the node (`UdsNode::graph_guard_condition`). `rmw_node_get_graph_guard_condition()` hands rcl that per-node object, so the per-node guard condition is what the executor's wait set actually watches for graph changes. -The trigger in step 2 above, however, fires `ctx->graph_guard_condition` (the context-level guard condition), not the per-node one. In the current code these are separate objects, and `UdsContext::graph_guard_condition` is never assigned, so the trigger does not reach the guard condition rcl is waiting on. A graph change is still observed, because every `rmw_wait()` re-reads the registry generation in step 2 and re-drains regardless of guard-condition state, so a wait already in progress or the next wait call picks up the change. The separate context-level trigger is therefore redundant rather than load-bearing. This is a wiring gap worth confirming against intent: if the context guard condition is meant to wake blocked waits on a graph change, it would need to be the node's guard condition (or be linked to it), and it would need to be assigned. +To reach it, the context keeps a mutex-guarded list of every node's graph guard condition. `rmw_create_node()` appends to the list as its last step (so no failure path has to undo it); `rmw_destroy_node()` removes the entry under the same mutex before destroying the guard condition, so the trigger loop can never fire a freed object. When step 2 of the wait observes a generation change, it triggers every guard condition in that list. A GraphListener blocked on a graph guard condition alone is woken by the doorbell (its fd is in the wait set's epoll), re-runs the check, triggers its own guard condition, and reports it ready — which is how `wait_for_service` and graph-change callbacks make progress with no data traffic at all. + +### The doorbell: cross-process wakeup without a daemon + +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. + +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. + +Best-effort has sharp edges, each handled explicitly: + +- **A full receiver queue is success.** `EAGAIN` because the destination's queue is full means wakeups are already pending there; dropping the octet loses nothing. +- **A slow peer must not mute the others.** `AF_UNIX` datagrams stay charged to the *sender's* buffer until the receiver consumes them, so one process that never drains (hung, or not yet waiting) could exhaust the ring socket's budget and make sends fail for every peer. On `EAGAIN` the ring socket is closed, recreated, and the send retried once: on a fresh socket, `EAGAIN` can only mean the benign case above. A test that wedges one participant and asserts a healthy one still gets latched delivery guards this. +- **A recycled slot must not receive the octet.** The ring loop re-validates the slot type inside its seqlock window, so a slot that was a doorbell at the start of the scan but has been reused by a data endpoint cannot be sent to. +- **The ring socket is per mutating thread** (thread-local, closed at thread exit), so ringing takes no lock and leaks nothing. + +Cleanup needs no new machinery: the doorbell is a PID-owned slot holding a socket path, so graceful shutdown removes it like any endpoint, and the existing stale-PID reaper reclaims it after a crash — slot teardown already unlinks the socket file. + +Two limits are accepted. A build that predates the doorbell bumps the generation but never rings, so a fleet running mixed old and new builds can miss wakeups during the upgrade window — upgrade the fleet together, as with any wire-format change. And a process whose threads are never inside `rmw_wait` at all (an executor wedged in a user callback, or a bare-rmw publisher that never waits) cannot observe anything, doorbell or not; that gap predates the doorbell and is unchanged. ### In-process concurrency @@ -634,13 +653,11 @@ Request and response are correlated by sequence number, and that correlation is ### The graph guard condition -ROS 2 lets a node block until the graph changes (a node, publisher, subscription, service, or client appears or disappears). The mechanism is a graph guard condition: rcl adds it to a wait set, and the RMW arranges for that wait set to wake when the graph moves. This RMW detects graph changes by polling, not by cross-process push, because pushing a notification between processes without a daemon would need per-process signals or pipes, which is exactly the complexity the design avoids. - -The signal source is the registry's `generation` counter. Every successful add, remove, and stale-slot reclaim does `generation.fetch_add(1)` after publishing its slot change. A single monotonic counter in shared memory is enough to mean "something in the graph changed" without saying what. +ROS 2 lets a node block until the graph changes (a node, publisher, subscription, service, or client appears or disappears). The mechanism is a graph guard condition: rcl adds it to a wait set, and the RMW arranges for that wait set to wake when the graph moves. -Each node owns its own graph guard condition, created at `rmw_node_create` as an `eventfd` (see the wait section) and stored on the node. `rmw_node_get_graph_guard_condition` hands that per-node guard condition to rcl, so the object rcl waits on is the node's. On every `rmw_wait` the context reads the current `generation` and compares it to the value it cached on the previous call (`UdsContext::last_registry_generation`). When the counter has moved, the context records the new value, so the change is observed exactly once. Because rcl calls `rmw_wait` continuously while a node is spinning, a graph change is observed within one wait cycle, and the cost of the check is a single atomic load from shared memory. +The signal source is the registry's `generation` counter. Every successful add, remove, and stale-slot reclaim does `generation.fetch_add(1)` after publishing its slot change, then rings every registered doorbell (see The doorbell under Wait set). A single monotonic counter in shared memory is enough to mean "something in the graph changed" without saying what; the doorbell is what carries that fact into a process that is asleep. -There is a known seam here, the same one described under the wait set. The per-node guard condition is what rcl receives and waits on, but the trigger call in `rmw_wait` targets `UdsContext::graph_guard_condition`, a separate field that is never assigned and so is always null. The detection of the generation change is correct and the cached generation is advanced, but the explicit eventfd write that would wake a node blocked on the graph guard alone does not currently fire through that field. In practice the wait set is driven by its other fds and the per-cycle generation check, so graph queries observe the change; wiring the trigger to the per-node guard condition rcl actually holds is a correctness gap worth closing. +Each node owns its own graph guard condition, created at `rmw_node_create` as an `eventfd` (see the wait section), stored on the node, and registered in the context's list of graph guard conditions. `rmw_node_get_graph_guard_condition` hands that per-node guard condition to rcl, so the object rcl waits on is the node's — and it is exactly the object `rmw_wait` triggers when it observes a generation change (`UdsContext::last_registry_generation` records the new value, so each change is observed once per context). The doorbell wakes the blocked wait, the generation check runs, the per-node guard conditions fire, and rcl's graph machinery proceeds — with no polling interval anywhere in the path. ### GID generation @@ -668,6 +685,8 @@ Docker requirements: - `--pid=host` — for cross-container stale-PID cleanup correctness - `-v /tmp/ros2_uds:/tmp/ros2_uds` — for socket file sharing +Files under `/tmp/ros2_uds//` must not be removed while their owning processes are alive: they are live sockets (data and doorbell), not temp files, and an external cleaner deleting one silently severs delivery or wakeups to that process. Exempt the directory from tmpfile sweepers, e.g. a `tmpfiles.d` drop-in containing `x /tmp/ros2_uds`. (Ubuntu's default configuration cleans `/tmp` only at boot, and Docker containers run no cleaner, so this bites mainly on hosts with a cron-driven `tmpwatch`/`tmpreaper`.) + ## Resource usage, limitations, and build ### Resource profile @@ -702,6 +721,10 @@ This RMW communicates only between processes on a single host. `AF_UNIX` sockets Each message must fit in a single datagram; the usable cap is roughly 400 KB on a stock kernel. See The per-message size cap (~400 KB) under Transport for the full derivation and the sysctl remedy. +#### 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. + #### Functions that return `RMW_RET_UNSUPPORTED` These functions are part of the RMW API but cannot be backed by a copy-based Unix-socket transport. Returning `RMW_RET_UNSUPPORTED` is the contract that tells rcl/rclcpp to skip the feature gracefully rather than fail. diff --git a/rmw_unix_socket_cpp/src/registry.cpp b/rmw_unix_socket_cpp/src/registry.cpp index b0187df..84080b3 100644 --- a/rmw_unix_socket_cpp/src/registry.cpp +++ b/rmw_unix_socket_cpp/src/registry.cpp @@ -21,7 +21,9 @@ #include #include +#include #include +#include #include #include "logging.hpp" @@ -29,6 +31,9 @@ namespace rmw_uds { +// Defined below; called after every generation bump (see its comment). +static void ring_doorbells(RegistryHeader * header); + // Lock-free atomics in shared memory require both lock-freedom AND // address-freedom. On every Linux target we support these hold; assert at // compile time so we fail loud on exotic platforms. @@ -276,6 +281,7 @@ 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 return static_cast(i); } } @@ -345,6 +351,7 @@ 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 } // Best-effort: stat /proc/. ENOENT means the PID is not in our @@ -372,13 +379,96 @@ static const char * entry_type_name(uint8_t t) case ENTRY_SUBSCRIPTION: return "subscription"; case ENTRY_SERVICE: return "service"; case ENTRY_CLIENT: return "client"; + case ENTRY_DOORBELL: return "doorbell"; default: return "?"; } } +// Ring every registered doorbell (one octet, best-effort) so processes blocked +// in rmw_wait re-check the registry. Called strictly AFTER a generation bump: +// paired with rmw_wait draining its doorbell strictly BEFORE reading the +// generation, every mutation either lands in the pre-block generation read or +// leaves a queued datagram on a level-triggered fd — no lost wakeup. EAGAIN +// means the peer already has a wakeup queued; other send errors mean a dead +// peer whose slot will be reclaimed. Scans slots directly (not registry_query, +// which calls back into cleanup and would recurse). +static void ring_doorbells(RegistryHeader * header) +{ + // One ring socket per mutating thread, closed at thread exit. AF_UNIX + // datagrams stay charged to the SENDER's buffer until the receiver consumes + // them, so a peer that is slow to drain could exhaust this fd's budget and + // make sendto fail for every OTHER peer too; the recreate-on-EAGAIN below + // resets that budget. On a fresh fd, EAGAIN can only mean the destination's + // own queue is full — a wakeup is already pending there, so the drop is safe. + struct RingFd + { + int fd = -1; + ~RingFd() {if (fd >= 0) {close(fd);}} + }; + static thread_local RingFd ring; + if (ring.fd < 0) { + ring.fd = socket(AF_UNIX, SOCK_DGRAM | SOCK_NONBLOCK | SOCK_CLOEXEC, 0); + if (ring.fd < 0) { + return; + } + } + auto * slots = registry_slots(header); + const uint32_t hi = header->high_water_slot.load(std::memory_order_acquire); + for (uint32_t i = 0; i < hi; ++i) { + if (slots[i].state.load(std::memory_order_acquire) != + static_cast(ENTRY_DOORBELL)) + { + continue; + } + // Seqlock snapshot of the socket path, re-validating the type INSIDE the + // seq window: without it, a remove + re-claim of this slot by a data + // endpoint between the fast-skip above and the copy could land the wake + // octet on a real data socket. A rewrite after this re-check still bumps + // seq, so the s1 comparison below rejects the torn copy. + char path[sizeof(slots[i].socket_path)]; + const uint32_t s1 = slots[i].seq.load(std::memory_order_acquire); + if (s1 & 1) { + continue; + } + if (slots[i].state.load(std::memory_order_acquire) != + static_cast(ENTRY_DOORBELL)) + { + continue; + } + std::memcpy(path, slots[i].socket_path, sizeof(path)); + if (slots[i].seq.load(std::memory_order_acquire) != s1 || path[0] == '\0') { + continue; + } + struct sockaddr_un addr; + std::memset(&addr, 0, sizeof(addr)); + addr.sun_family = AF_UNIX; + // memcpy of the measured length: addr is zeroed, so termination is free + // and -Wstringop-truncation stays quiet (path may fill all 108 bytes). + std::memcpy(addr.sun_path, path, strnlen(path, sizeof(addr.sun_path) - 1)); + const uint8_t octet = 1; + ssize_t sent = sendto( + ring.fd, &octet, 1, MSG_DONTWAIT | MSG_NOSIGNAL, + reinterpret_cast(&addr), sizeof(addr)); + if (sent < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) { + // Sender-side budget exhausted (some peer is slow to drain): reset the + // budget and retry once, so one undrained doorbell cannot mute rings to + // healthy peers. EAGAIN again on the fresh fd is the benign case. + close(ring.fd); + ring.fd = socket(AF_UNIX, SOCK_DGRAM | SOCK_NONBLOCK | SOCK_CLOEXEC, 0); + if (ring.fd < 0) { + return; + } + (void)sendto( + ring.fd, &octet, 1, MSG_DONTWAIT | MSG_NOSIGNAL, + reinterpret_cast(&addr), sizeof(addr)); + } + } +} + void registry_cleanup_stale(RegistryHeader * header) { auto * slots = registry_slots(header); + bool reclaimed = false; // Scan only [0, high_water): over-scan is safe, under-scan is impossible. uint32_t hw = header->high_water_slot.load(std::memory_order_acquire); uint32_t max = header->max_entries; @@ -435,6 +525,10 @@ void registry_cleanup_stale(RegistryHeader * header) teardown_slot(&slots[i]); header->generation.fetch_add(1, std::memory_order_acq_rel); + reclaimed = true; + } + if (reclaimed) { + ring_doorbells(header); // strictly after the bump(s) — see ring_doorbells } } diff --git a/rmw_unix_socket_cpp/src/registry.hpp b/rmw_unix_socket_cpp/src/registry.hpp index b4d9141..25c4b9d 100644 --- a/rmw_unix_socket_cpp/src/registry.hpp +++ b/rmw_unix_socket_cpp/src/registry.hpp @@ -44,6 +44,11 @@ enum RegistryEntryType : uint8_t ENTRY_SUBSCRIPTION, ENTRY_SERVICE, ENTRY_CLIENT, + // Per-context wakeup socket (see ring_doorbells in registry.cpp): rung with + // one octet after every registry mutation so a blocked rmw_wait re-checks + // the registry. Additive: no slot layout change, and type-filtered queries + // never match it, so it is invisible to graph introspection. + ENTRY_DOORBELL, // Transient claim state: a writer won the slot but has not yet committed its // payload. Readers treat it like ENTRY_EMPTY so they never observe a slot // before its payload is published. Never stored in a RegistryEntry; lives diff --git a/rmw_unix_socket_cpp/src/rmw_init.cpp b/rmw_unix_socket_cpp/src/rmw_init.cpp index 9d89780..9a3a281 100644 --- a/rmw_unix_socket_cpp/src/rmw_init.cpp +++ b/rmw_unix_socket_cpp/src/rmw_init.cpp @@ -184,8 +184,44 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) "rmw_init: context up (domain_id=%zu, pid=%d)", domain_id, static_cast(getpid())); - // Read initial generation 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). + { + const std::string ctl_path = rmw_uds::make_socket_path(domain_id, "ctl"); + ctx->doorbell_fd = rmw_uds::create_bound_socket(ctl_path); + if (ctx->doorbell_fd < 0) { + RMW_UDS_LOG_ERROR( + "rmw_init: failed to create doorbell socket (domain_id=%zu)", domain_id); + close(ctx->send_socket_fd); + rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size); + delete ctx; + 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 ctx->last_registry_generation.store( rmw_uds::registry_generation(header), std::memory_order_relaxed); @@ -195,6 +231,8 @@ 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); + close(ctx->doorbell_fd); close(ctx->send_socket_fd); rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size); delete ctx; @@ -220,6 +258,8 @@ 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); + close(ctx->doorbell_fd); close(ctx->send_socket_fd); rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size); delete ctx; @@ -255,6 +295,15 @@ 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. + 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); + } + if (ctx->doorbell_fd >= 0) { + close(ctx->doorbell_fd); + } if (ctx->send_socket_fd >= 0) { close(ctx->send_socket_fd); } diff --git a/rmw_unix_socket_cpp/src/rmw_node.cpp b/rmw_unix_socket_cpp/src/rmw_node.cpp index a788cdc..06f53f3 100644 --- a/rmw_unix_socket_cpp/src/rmw_node.cpp +++ b/rmw_unix_socket_cpp/src/rmw_node.cpp @@ -16,7 +16,9 @@ #include "registry.hpp" #include "types.hpp" +#include #include +#include #include "rcutils/strdup.h" #include "rmw/allocators.h" @@ -108,6 +110,13 @@ rmw_node_t * rmw_create_node( return nullptr; } + // Last step, so no failure path above needs to undo it: expose the graph GC + // to rmw_wait's generation check (triggered there on graph changes). + { + std::lock_guard lock(ctx->graph_gcs_mutex); + ctx->graph_gcs.push_back(graph_gc); + } + return node; } @@ -127,6 +136,15 @@ rmw_ret_t rmw_destroy_node(rmw_node_t * node) } if (node_data->graph_guard_condition) { + // Unpublish from the context BEFORE destroying, so rmw_wait can never + // trigger a freed guard condition (it holds the same mutex). + if (node_data->context) { + std::lock_guard lock(node_data->context->graph_gcs_mutex); + auto & gcs = node_data->context->graph_gcs; + gcs.erase( + std::remove(gcs.begin(), gcs.end(), node_data->graph_guard_condition), + gcs.end()); + } auto _r [[maybe_unused]] = rmw_destroy_guard_condition(node_data->graph_guard_condition); } diff --git a/rmw_unix_socket_cpp/src/rmw_wait.cpp b/rmw_unix_socket_cpp/src/rmw_wait.cpp index 6b0028b..2d80fdb 100644 --- a/rmw_unix_socket_cpp/src/rmw_wait.cpp +++ b/rmw_unix_socket_cpp/src/rmw_wait.cpp @@ -17,6 +17,7 @@ #include "transport.hpp" #include "types.hpp" +#include #include #include #include @@ -27,6 +28,7 @@ #include #include +#include #include #include "rmw/allocators.h" @@ -97,6 +99,7 @@ rmw_wait_set_t * rmw_create_wait_set(rmw_context_t * context, size_t max_conditi RMW_SET_ERROR_MSG("failed to allocate wait set data"); return nullptr; } + ws_data->context = reinterpret_cast(context->impl); ws_data->epoll_fd = epoll_create1(EPOLL_CLOEXEC); if (ws_data->epoll_fd < 0) { @@ -182,18 +185,26 @@ rmw_ret_t rmw_wait( } } - // 2. Check graph generation changes — trigger graph guard conditions - // We need to find the context from any available entity - rmw_uds::UdsContext * ctx = nullptr; - if (subscriptions && subscriptions->subscriber_count > 0 && subscriptions->subscribers[0]) { - ctx = static_cast(subscriptions->subscribers[0])->context; - } else if (services && services->service_count > 0 && services->services[0]) { - ctx = static_cast(services->services[0])->context; - } else if (clients && clients->client_count > 0 && clients->clients[0]) { - ctx = static_cast(clients->clients[0])->context; - } - - if (ctx && ctx->registry_ptr) { + // 2. 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. + rmw_uds::UdsContext * ctx = ws_data->context; + const int doorbell_fd = ctx ? ctx->doorbell_fd : -1; + auto run_generation_check = [&]() { + // Drain the doorbell strictly BEFORE reading the generation: paired with + // ring_doorbells running strictly AFTER the bump, a mutation either lands + // in this generation read or leaves a queued datagram that keeps the + // level-triggered fd readable — no lost wakeup. + if (doorbell_fd >= 0) { + uint8_t buf[16]; + while (recv(doorbell_fd, buf, sizeof(buf), MSG_DONTWAIT) > 0) { + } + } + 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)) { @@ -256,20 +267,19 @@ rmw_ret_t rmw_wait( } } - // Trigger all graph guard conditions in the guard_conditions list - if (guard_conditions) { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { - if (!guard_conditions->guard_conditions[i]) {continue;} - // We don't know which are graph GCs, so we just note the change - // The graph GC is triggered by the node itself + // 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); } } - // Trigger graph guard condition on the context - if (ctx->graph_guard_condition) { - auto _r [[maybe_unused]] = rmw_trigger_guard_condition(ctx->graph_guard_condition); - } } - } + }; + run_generation_check(); // Arm every entity fd with epoll on every wait. EPOLL_CTL_ADD is idempotent // here: a still-live fd returns EEXIST (already armed), while a fd number @@ -319,6 +329,8 @@ rmw_ret_t rmw_wait( register_fd(gc->eventfd_fd); } } + // The context's doorbell: rung by any process after a registry mutation. + register_fd(doorbell_fd); } // 3. Check if anything is already ready @@ -395,30 +407,57 @@ rmw_ret_t rmw_wait( } } - // Block, retrying on EINTR. A finite timeout uses a steady_clock deadline - // so a signal interruption neither returns TIMEOUT early nor busy-loops. + // 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. + const bool infinite = (timeout_ms < 0); + const int64_t caller_deadline_ns = + infinite ? 0 : now_ns() + static_cast(timeout_ms) * 1000000; struct epoll_event ready_events[64]; - const int64_t deadline_ns = - (timeout_ms >= 0) ? now_ns() + static_cast(timeout_ms) * 1000000 : 0; - int remaining_ms = timeout_ms; while (true) { - int n = epoll_wait(ws_data->epoll_fd, ready_events, 64, remaining_ms); - if (n >= 0) { - break; + int block_ms = -1; + if (!infinite) { + const int64_t rem_ns = caller_deadline_ns - now_ns(); + const int64_t rem_ms = (rem_ns > 0) ? (rem_ns + 999999) / 1000000 : 0; // ceil + block_ms = static_cast( + std::min(rem_ms, std::numeric_limits::max())); } - if (errno != EINTR) { + int n = epoll_wait(ws_data->epoll_fd, ready_events, 64, block_ms); + if (n < 0) { + if (errno == EINTR) { + continue; + } RMW_SET_ERROR_MSG("epoll_wait failed"); return RMW_RET_ERROR; } - if (timeout_ms < 0) { - continue; // Infinite wait: just re-block. + if (n == 0) { + break; // The caller's deadline passed -> timeout; fall through to drain. + } + bool only_doorbell = true; + bool rang = false; + for (int e = 0; e < n; ++e) { + if (ready_events[e].data.fd == doorbell_fd) { + rang = true; + } else { + only_doorbell = false; + } + } + if (rang) { + run_generation_check(); // Drains the doorbell, replays, triggers GCs. + } + if (!only_doorbell) { + break; // Something the caller waits on fired -> fall through to drain. } - const int64_t rem_ns = deadline_ns - now_ns(); - if (rem_ns <= 0) { - break; // Deadline passed -> timeout; fall through to drain. + if (!infinite && now_ns() >= caller_deadline_ns) { + break; // Doorbell-only wake at the deadline -> timeout. } - const int64_t rem_ms = rem_ns / 1000000; - remaining_ms = (rem_ms > 0) ? static_cast(rem_ms) : 1; // >=1ms while time remains + // Doorbell-only wake: re-block for the caller's remaining time. } // No EPOLL_CTL_DEL needed — fds stay registered across calls. diff --git a/rmw_unix_socket_cpp/src/types.hpp b/rmw_unix_socket_cpp/src/types.hpp index fa40803..dc3091b 100644 --- a/rmw_unix_socket_cpp/src/types.hpp +++ b/rmw_unix_socket_cpp/src/types.hpp @@ -119,7 +119,19 @@ struct UdsContext int send_socket_fd = -1; std::atomic is_shutdown{false}; std::atomic last_registry_generation{0}; - rmw_guard_condition_t * graph_guard_condition = nullptr; + + // 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). + int doorbell_fd = -1; + int32_t doorbell_registry_index = -1; + + // Per-node graph guard conditions (see rmw_node_get_graph_guard_condition), + // triggered from rmw_wait when the registry generation changes. Guarded by + // 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; @@ -301,6 +313,10 @@ struct UdsGuardCondition struct UdsWaitSet { int epoll_fd = -1; + // Set at rmw_create_wait_set. The top-of-wait replay/graph check needs the + // context even when the wait set holds only guard conditions (rclcpp's + // GraphListener), so it cannot be scavenged from the waited-on entities. + UdsContext * context = nullptr; }; } // namespace rmw_uds diff --git a/rmw_unix_socket_cpp/test/test_rmw_qos.cpp b/rmw_unix_socket_cpp/test/test_rmw_qos.cpp index 8695960..e2c9cad 100644 --- a/rmw_unix_socket_cpp/test/test_rmw_qos.cpp +++ b/rmw_unix_socket_cpp/test/test_rmw_qos.cpp @@ -14,6 +14,7 @@ #include "test_base.hpp" +#include #include #include #include @@ -975,3 +976,162 @@ TEST_F(QosTest, MultipleClientsOneService) auto _r2 [[maybe_unused]] = rmw_destroy_client(node, cli1); auto _r3 [[maybe_unused]] = rmw_destroy_service(node, srv); } + +TEST_F(QosTest, TransientLocalReplayReachesLateJoinerWhileWaitBlocked) +{ + // The subscriber joins AFTER the publisher's executor is already blocked in + // rmw_wait. A joining subscriber only bumps the shm generation counter, which + // signals no fd, so an idle rmw_wait(infinite) would block in epoll forever + // and never re-run the top-of-wait replay. The latched message must still + // reach the late joiner within a bounded time. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); + + // A service anchors ctx resolution inside rmw_wait, mirroring an idle node + // whose wait set holds only its services. + auto srv_ts = rosidl_typesupport_cpp::get_service_type_support_handle< + test_msgs::srv::BasicTypes>(); + auto svc_qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, RMW_QOS_POLICY_DURABILITY_VOLATILE); + auto * srv = rmw_create_service(node, srv_ts, "/idle_anchor", &svc_qos); + ASSERT_NE(nullptr, srv); + + // Guard condition only unblocks the executor thread on teardown so the test + // never hangs when the message never arrives (the failing case). + auto * gc = rmw_create_guard_condition(&context); + ASSERT_NE(nullptr, gc); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/idle_replay", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + // Publish before any subscriber exists; then the node goes idle. + test_msgs::msg::BasicTypes m; + m.int32_value = 7; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &m, nullptr)); + + auto * ws = rmw_create_wait_set(&context, 4); + ASSERT_NE(nullptr, ws); + + // Executor thread: spin rmw_wait with an INFINITE timeout, like an idle node. + std::atomic stop{false}; + std::thread executor( + [&] { + while (!stop.load()) { + void * srv_array[1] = {srv->data}; + rmw_services_t services; + services.services = srv_array; + services.service_count = 1; + void * gc_array[1] = {gc->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = gc_array; + gcs.guard_condition_count = 1; + auto _r [[maybe_unused]] = rmw_wait( + nullptr, &gcs, &services, nullptr, nullptr, ws, nullptr); + } + }); + + // Let the executor reach epoll and block before the subscriber joins. + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + + // Late joiner — created after the executor is already blocked in rmw_wait. + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription(node, ts, "/idle_replay", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + bool got = false; + for (int i = 0; i < 300 && !got; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + if (rmw_take(sub, &recv, &taken, nullptr) == RMW_RET_OK && taken && + recv.int32_value == 7) + { + got = true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + stop.store(true); + auto _t [[maybe_unused]] = rmw_trigger_guard_condition(gc); + executor.join(); + + EXPECT_TRUE(got) << "late joiner never received the latched message while the " + "publisher's executor was blocked in rmw_wait"; + + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _r2 [[maybe_unused]] = rmw_destroy_wait_set(ws); + auto _r3 [[maybe_unused]] = rmw_destroy_publisher(node, pub); + auto _r4 [[maybe_unused]] = rmw_destroy_guard_condition(gc); + auto _r5 [[maybe_unused]] = rmw_destroy_service(node, srv); +} + +TEST_F(QosTest, TransientLocalLateJoinerWhilePublisherProcessIdle) +{ + // Scenario: a latched (TRANSIENT_LOCAL) publisher lives in a process that + // is completely idle — its only executor thread is parked in an unbounded + // rmw_wait that contains no subscriptions, services, or clients. A + // subscriber that joins later must still receive the retained message. + // This is the user-visible bug: a latched topic on a quiet node never + // reaching late subscribers, however the process happens to be waiting. + auto qos = make_qos( + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 5); + + auto * gc = rmw_create_guard_condition(&context); + ASSERT_NE(nullptr, gc); + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/idle_replay_gc_only", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + test_msgs::msg::BasicTypes m; + m.int32_value = 9; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &m, nullptr)); + + auto * ws = rmw_create_wait_set(&context, 4); + ASSERT_NE(nullptr, ws); + + std::atomic stop{false}; + std::thread executor( + [&] { + while (!stop.load()) { + void * gc_array[1] = {gc->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = gc_array; + gcs.guard_condition_count = 1; + auto _r [[maybe_unused]] = rmw_wait( + nullptr, &gcs, nullptr, nullptr, nullptr, ws, nullptr); + } + }); + + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription(node, ts, "/idle_replay_gc_only", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + bool got = false; + for (int i = 0; i < 300 && !got; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + if (rmw_take(sub, &recv, &taken, nullptr) == RMW_RET_OK && taken && + recv.int32_value == 9) + { + got = true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + stop.store(true); + auto _t [[maybe_unused]] = rmw_trigger_guard_condition(gc); + executor.join(); + + EXPECT_TRUE(got) << "late joiner never received the latched message while the " + "publisher's process was idle"; + + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _r2 [[maybe_unused]] = rmw_destroy_wait_set(ws); + auto _r3 [[maybe_unused]] = rmw_destroy_publisher(node, pub); + auto _r4 [[maybe_unused]] = rmw_destroy_guard_condition(gc); +} diff --git a/rmw_unix_socket_cpp/test/test_rmw_wait.cpp b/rmw_unix_socket_cpp/test/test_rmw_wait.cpp index fe86ff3..b0166fa 100644 --- a/rmw_unix_socket_cpp/test/test_rmw_wait.cpp +++ b/rmw_unix_socket_cpp/test/test_rmw_wait.cpp @@ -14,7 +14,10 @@ #include "test_base.hpp" +#include +#include #include +#include #include "test_msgs/msg/basic_types.hpp" @@ -139,3 +142,201 @@ TEST_F(RmwUdsNodeTest, WaitWithSubscription) EXPECT_EQ(RMW_RET_OK, rmw_destroy_subscription(node, sub)); EXPECT_EQ(RMW_RET_OK, rmw_destroy_publisher(node, pub)); } + +TEST_F(RmwUdsNodeTest, NodeGraphGuardConditionTriggersOnGraphChange) +{ + // Scenario: graph-change notification. rclcpp's GraphListener (and + // wait_for_service, on_graph_change callbacks) blocks on the node's graph + // guard condition and relies on it firing when the ROS graph changes. + // Block on that guard condition alone, then create a subscription from + // another thread: the wait must wake with the guard condition ready, well + // before the timeout. Without this, wait_for_service can hang forever even + // though the service is up. + const rmw_guard_condition_t * graph_gc = rmw_node_get_graph_guard_condition(node); + ASSERT_NE(nullptr, graph_gc); + + auto * ws = rmw_create_wait_set(&context, 1); + ASSERT_NE(nullptr, ws); + + auto wait_on_graph_gc = [&](rmw_time_t timeout) { + void * gc_array[1] = {graph_gc->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = gc_array; + gcs.guard_condition_count = 1; + rmw_ret_t ret = rmw_wait(nullptr, &gcs, nullptr, nullptr, nullptr, ws, &timeout); + // Ready iff rmw_wait returned OK and kept the entry non-null. + return ret == RMW_RET_OK && gcs.guard_conditions[0] != nullptr; + }; + + // Settle first. The fixture's own node registration left an unconsumed + // registry generation edge, and a wait that starts on it reports the guard + // condition ready without ever blocking — which would let this test pass even + // with the wakeup path removed entirely. Consume pending edges until a wait + // genuinely blocks and times out. + bool settled = false; + for (int i = 0; i < 50 && !settled; ++i) { + settled = !wait_on_graph_gc(rmw_time_t{0, 20000000}); // 20 ms + } + ASSERT_TRUE(settled) << + "the graph guard condition never settled, so the wait below would not block"; + + // From here the wait can only be satisfied by the graph change made below. + std::atomic woke_ready{false}; + std::atomic blocked_ms{-1}; + std::thread waiter( + [&] { + auto t0 = std::chrono::steady_clock::now(); + bool ready = wait_on_graph_gc(rmw_time_t{3, 0}); + blocked_ms.store( + std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count()); + woke_ready.store(ready); + }); + + // Let the waiter reach epoll, then change the graph. + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto * ts_local = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::BasicTypes>(); + rmw_qos_profile_t qos = rmw_qos_profile_default; + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription(node, ts_local, "/graph_gc_probe", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + waiter.join(); + EXPECT_TRUE(woke_ready.load()) << + "the node's graph guard condition was not triggered by a graph change"; + // Proves the wake came from the graph change rather than from an edge that + // was already pending when the wait started. + EXPECT_GE(blocked_ms.load(), 150) << + "the wait did not block; it was already satisfied before the graph changed"; + EXPECT_LT(blocked_ms.load(), 3000) << "the wait ran to its timeout instead of waking"; + + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _r2 [[maybe_unused]] = rmw_destroy_wait_set(ws); +} + +TEST_F(RmwUdsNodeTest, WaitBlocksForFullCallerTimeout) +{ + // Scenario: the caller's timeout is a contract. Callers such as + // rclcpp::wait_for_message and WaitSet::wait treat an early RMW_RET_TIMEOUT + // as "nothing arrived in my window" — if rmw_wait returns before the + // caller's deadline, they misreport. With nothing ready, a 600 ms wait must + // block ~600 ms and only then return RMW_RET_TIMEOUT. + auto * gc = rmw_create_guard_condition(&context); + ASSERT_NE(nullptr, gc); + auto * ws = rmw_create_wait_set(&context, 1); + ASSERT_NE(nullptr, ws); + + void * gc_array[1] = {gc->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = gc_array; + gcs.guard_condition_count = 1; + + rmw_time_t timeout{0, 600000000}; // 600 ms, never triggered + auto t0 = std::chrono::steady_clock::now(); + rmw_ret_t ret = rmw_wait(nullptr, &gcs, nullptr, nullptr, nullptr, ws, &timeout); + auto elapsed_ms = std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count(); + + EXPECT_EQ(RMW_RET_TIMEOUT, ret); + EXPECT_GE(elapsed_ms, 550) << + "rmw_wait returned TIMEOUT before the caller's 600 ms deadline"; + EXPECT_LE(elapsed_ms, 1500) << "rmw_wait overshot the deadline"; + + auto _r1 [[maybe_unused]] = rmw_destroy_wait_set(ws); + auto _r2 [[maybe_unused]] = rmw_destroy_guard_condition(gc); +} + +TEST_F(RmwUdsNodeTest, LatchedTopicSurvivesAnUnresponsiveParticipant) +{ + // Scenario: one participant on the domain initializes but never services + // its wait loop (a hung or busy process). However much graph churn its + // unread notifications accumulate, the rest of the system must keep + // working: a latched (TRANSIENT_LOCAL) message published by an idle node + // must still reach a subscriber that joins after heavy churn. + rmw_init_options_t opts2 = rmw_get_zero_initialized_init_options(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + ASSERT_EQ(RMW_RET_OK, rmw_init_options_init(&opts2, allocator)); + opts2.domain_id = 99; // same domain as the fixture + rmw_context_t ctx2 = rmw_get_zero_initialized_context(); + ASSERT_EQ(RMW_RET_OK, rmw_init(&opts2, &ctx2)); // never waits, never drains + + auto * ts_local = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::BasicTypes>(); + rmw_qos_profile_t latched = rmw_qos_profile_default; + latched.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + latched.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + latched.depth = 5; + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher( + node, ts_local, "/unresponsive_latched", &latched, &pub_opts); + ASSERT_NE(nullptr, pub); + test_msgs::msg::BasicTypes m; + m.int32_value = 21; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &m, nullptr)); + + // The healthy participant's executor: idle, blocked, servicing its waits — + // exactly what a quiet production node does. + auto * gc = rmw_create_guard_condition(&context); + ASSERT_NE(nullptr, gc); + auto * ws = rmw_create_wait_set(&context, 4); + ASSERT_NE(nullptr, ws); + std::atomic stop{false}; + std::thread executor( + [&] { + while (!stop.load()) { + void * gc_array[1] = {gc->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = gc_array; + gcs.guard_condition_count = 1; + auto _r [[maybe_unused]] = rmw_wait( + nullptr, &gcs, nullptr, nullptr, nullptr, ws, nullptr); + } + }); + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + + // Heavy graph churn while the second participant stays unresponsive. The + // healthy executor keeps draining its own notifications throughout, so any + // per-sender resource pinned by the unresponsive peer stays pinned. + rmw_qos_profile_t qos = rmw_qos_profile_default; + for (int i = 0; i < 600; ++i) { + auto * p = rmw_create_publisher(node, ts_local, "/churn", &qos, &pub_opts); + ASSERT_NE(nullptr, p); + ASSERT_EQ(RMW_RET_OK, rmw_destroy_publisher(node, p)); + } + + // A late joiner after the churn must still receive the latched message. + auto sub_opts = rmw_get_default_subscription_options(); + auto * sub = rmw_create_subscription( + node, ts_local, "/unresponsive_latched", &latched, &sub_opts); + ASSERT_NE(nullptr, sub); + + bool got = false; + for (int i = 0; i < 300 && !got; ++i) { + test_msgs::msg::BasicTypes recv; + bool taken = false; + if (rmw_take(sub, &recv, &taken, nullptr) == RMW_RET_OK && taken && + recv.int32_value == 21) + { + got = true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + stop.store(true); + auto _t [[maybe_unused]] = rmw_trigger_guard_condition(gc); + executor.join(); + + EXPECT_TRUE(got) << + "a participant that never drains its notifications starved a healthy " + "idle publisher: the latched message never reached the late joiner"; + + auto _r1 [[maybe_unused]] = rmw_destroy_subscription(node, sub); + auto _r2 [[maybe_unused]] = rmw_destroy_wait_set(ws); + auto _r3 [[maybe_unused]] = rmw_destroy_publisher(node, pub); + auto _r4 [[maybe_unused]] = rmw_destroy_guard_condition(gc); + EXPECT_EQ(RMW_RET_OK, rmw_shutdown(&ctx2)); + EXPECT_EQ(RMW_RET_OK, rmw_context_fini(&ctx2)); + EXPECT_EQ(RMW_RET_OK, rmw_init_options_fini(&opts2)); +}