diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index dc0a363..94e59e1 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/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..9cf8f0d --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,251 @@ +# Changelog + +All notable changes to `rmw_unix_socket_cpp` are documented here. + +The format follows [Keep a Changelog](https://keepachangelog.com/en/1.1.0/), +and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). + +## [0.5.0] - 2026-08-27 + +The wait/wakeup release. The 200 ms `rmw_wait` poll is gone, replaced by an +event-driven doorbell; TRANSIENT_LOCAL latched replay is rebuilt as a pull from +a per-publisher shared-memory ring; and the per-node graph guard conditions that +`rclcpp`'s `GraphListener` actually waits on are finally triggered. + +> **Upgrade together.** See [Upgrade notes](#upgrade-notes-050) — a mixed-build +> fleet can lose latched replay and registry wakeups during the rollout window. + +### Added + +- **Pull-based TRANSIENT_LOCAL replay** (#60). A latched publisher writes each + sample into a per-publisher shm ring (`tl_ring`, `qos.depth` per-record-seqlocked + slots) at publish time; a late-joining subscription reads that ring itself + inside `rmw_create_subscription`. The idle publisher is never woken, polled, or + rung. History is enqueued before the subscription handle returns, so it always + precedes live samples. Payloads over `TL_EMBED_CAP` (1 KiB) ride the existing + durable segments as 32-byte descriptors; ring bytes are capped at + `TL_RING_MAX_BYTES` (2 MiB). +- **`rmw_subscription_options_t::ignore_local_publications`** (#49). Previously + discarded at creation. Each context now draws a random 64-bit `context_id` in + `rmw_init` and embeds it in the trailing 8 bytes of every GID, so + `is_same_context()` answers from bytes already on the wire — no extra traffic, + no wire-format change. "Local" is the same `rmw_context_t`, per the rmw + contract, not merely the same process. +- **Event-driven registry wakeup (doorbell).** Each context binds a doorbell + socket at `rmw_init`; every registry mutation sends one octet to every + registered doorbell strictly *after* bumping the generation, and `rmw_wait` + drains its doorbell strictly *before* reading the generation. That ordering + pair makes a lost wakeup impossible. No new threads, no registry layout change. +- **Graph-change triggering of per-node guard conditions** (#43). +- CI: **Lyrical** added to the build & test matrix (#17); CI now also runs on + `devel` pushes and PRs. +- GitHub issue forms and a pull request template (#56). +- Tests: `test_rmw_wait.cpp` (new, +648 lines), `test_shm_transport.cpp` (+283), + substantial additions to `test_rmw_qos.cpp` (+873), `test_rmw_graph.cpp`, + `test_transport.cpp`, `test_rmw_pub_sub.cpp`, and + `test_rmw_service_client.cpp`. + +### Changed + +- **VOLATILE late joiners no longer receive latched history.** Pulled records + are filtered by durability — DDS-correct behaviour. The old push path replayed + to every subscriber on the topic regardless of its durability QoS. +- The `ENTRY_DOORBELL` slot is now registered **lazily**, on the first + `rmw_wait` holding a graph guard condition, so only graph-event consumers + (`wait_for_service`, `GraphListener`, rosbag2) pay for registry wakeups. + Doorbell-slot mutations no longer ring, which removes a K²/2 registration + mini-storm. +- Removed with the push machinery: `transient_local_pubs` (and its mutex), + `known_subscriber_paths`, `CachedMessage` / the heap message cache, + `transient_local_publish`'s replay loop, and the wait-side TL replay block. +- `registry`: `teardown_slot` now `shm_unlink`s `tl_`-prefixed slot paths, and + the orphan sweep gained a `ros2_uds_tl_` prefix pass. +- `DESIGN.md` documents the doorbell wakeup, the graph-event wiring, GID + composition and context identity, and the TL ring. + +### Fixed + +- **`rmw_wait` timeout contract.** The 200 ms poll bound made every wait return + `RMW_RET_TIMEOUT` at 200 ms regardless of the caller's deadline — a 600 ms wait + returned at 200 ms, and an infinite wait, which must never time out, returned + `TIMEOUT`. Idle processes also woke 5×/s. `RMW_RET_TIMEOUT` now surfaces only + at the caller's own deadline. +- **`rmw_wait` reported spurious timeouts** when a drain legitimately yielded + nothing (an `ignore_local_publications` drop, or a shm descriptor whose sender + is gone). +- **A wait set holding only guard conditions got a null context** and skipped + the registry check entirely — the shape a publish-only node's executor + produces. The wait set now stores its context at `rmw_create_wait_set`. +- **`rmw_wait` dispatched fds the current call never armed.** The armed-fd cache + survived across calls and was insert-only, so a guard condition owned by + another wait set could have its eventfd consumed with the trigger recorded + nowhere — a permanent lost wakeup that hung `wait_for_service` and + `GraphListener`. Failed `epoll_ctl` ADDs now degrade to polling instead of + being silently dropped. +- **Dead graph-guard trigger.** `rmw_wait` fired `ctx->graph_guard_condition`, a + context-level field that is declared and never assigned, so the branch was + dead code and the object `rcl` waits on was never triggered (#43). +- **Zero-length datagrams were never dequeued.** `recv_from` peeks to size its + buffer; a zero-length datagram made the peek return 0 and the function bailed + out. `MSG_PEEK` does not consume, so the socket stayed readable forever, + spinning every wait that polled that fd. +- **Junk datagrams are now consumed atomically.** The zero-length cleanup used a + blind 1-byte `recv` after a separate, non-atomic peek; under a + `MultiThreadedExecutor` a racing drain could consume the peeked datagram and + the 1-byte `recv` would dequeue a real message and silently discard it. +- **Truncated consumes are detected with `MSG_TRUNC` and dropped.** Pre-existing: + the consuming `recv` omitted `MSG_TRUNC`, so a concurrently-dequeued peek + followed by a larger datagram was silently truncated to the smaller buffer and + delivered as valid. +- **TL: a writer filling a slot the pull scan skipped** opened a sequence gap + with `overlapped` left false, defeating the contiguous-prefix trim. +- **TL: the replay watermark now stops at an unresolvable record.** `max_seq` was + fixed before the enqueue loop, so a record whose descriptor could no longer be + resolved was skipped while its sequence stayed under the watermark — the + sample was lost, not lapped. +- `rmw_init`'s stale sweep is now stamped, and the throttle-scope comments were + corrected. +- Docs: `TL_RING_MAX_BYTES` was documented as 1 MiB (it is 2 MiB), and the + durable-segment budgeting note understated tmpfs sizing for over-cap latched + samples (a retained 5 MiB sample costs 5 MiB, not "an inode and a couple of + pages"). Reported by Copilot on #60. + +### Performance + +- **`rmw_wait` drains only the fds `epoll` reported ready** (#57). It previously + made three passes over every entity per call — drain every socket, re-arm every + fd, drain every socket again — roughly 2150 syscalls for a 719-entity wait set, + nearly all returning `EAGAIN` or `EEXIST`. +- **The stale-slot sweep is throttled off the graph query path** (#58). + `query_all` ran `registry_cleanup_stale` before every graph query across 13 + call sites, so `rmw_get_node_names`, `rmw_count_publishers`, + `rmw_count_subscribers`, `rmw_get_topic_names_and_types` and the rest each + walked every live slot, copied 1164 bytes out of each, and `stat`ed + `/proc/`. +- **Launch arithmetic at N=200 nodes:** doorbell datagrams drop from ~100,300 + (19,900 of them from doorbell self-registration alone) to roughly the number of + graph-event consumers; wake-side registry copies go from + O(mutations × processes × slots) to zero for plain pub/sub processes. +- Latched-replay latency improves from ~20 ms (doorbell wake) to synchronous at + subscription creation. + +### Upgrade notes (0.5.0) + +- **Latched replay across a mixed-build pair is lost.** A 0.5.0 publisher no + longer push-replays and a pre-0.5.0 subscriber never pulls, so latched replay + between that pair does not work during a rolling upgrade — upgrade together + (same precedent as the shm payload flag). Old publisher + new subscriber keeps + working: an empty `socket_path` skips the pull and the old push path still + delivers. +- **Registry wakeups across a mixed-build fleet can be missed.** A build that + predates the doorbell bumps the generation but never rings. +- **Containers need `--shm-size` ≥ 1 GiB** to run the full test suite. The 64 MB + Docker default cannot hold the ~38 MB registry plus the payload rings. + +## [0.4.1] - 2026-07-23 + +### Reverted + +- `fix(rmw_wait): deliver TRANSIENT_LOCAL latched samples to late joiners of + idle publishers` (#37). The push-based approach woke the publisher process to + resend, and the doorbell broadcast it relied on did not scale — every registry + mutation rang every process, and every wake rescanned the registry, which + melted 200-node launches. Replaced in 0.5.0 by the pull-based ring (#60). + +## [0.4.0] - 2026-07-16 + +### Added + +- **Shared-memory ring transport for large topic payloads**, with CDR serialized + directly into the ring record. +- **Durable shm segments for large TRANSIENT_LOCAL messages**, including the + serialized-publish path. +- **Large service request/response payloads routed through shm.** +- `fork()`-based cross-process shm integration tests. + +### Changed + +- The inline-vs-shm send/receive decision unified into two helpers. +- `DESIGN.md` expanded into a fuller architecture document (#16); doc/comment + drift from the merge-readiness audit fixed. +- Repository prepared for open-source release (#15). + +### Fixed + +- Contained the inline-path resize; the ring inline fallback is now tested. +- Honest TRANSIENT_LOCAL publish return value. + +### Performance + +- Shorter `cache_mutex` hold, smaller ring floor, POD-keyed reader cache. + +## [0.3.0] - 2026-06-17 + +Audit hardening & registry/fan-out performance. + +### Added + +- TRANSIENT_LOCAL replay to late-joining subscribers (#2). +- README. + +### Fixed + +- Contained `fastcdr`/`std` exceptions in `serialize()` so none cross the + `extern "C"` boundary (#4). +- Publish a registry slot's state only after its payload commits under the + seqlock (#5). +- Deep-copy `security_options` in `rmw_init_options_copy` to prevent a + double-free (#6). +- Accumulate the `rmw_wait` timeout in `int64` so `RMW_DURATION_INFINITE` keeps + blocking (#8). +- Write `rmw_take_sequence` output at the taken cursor (#7). +- Surface `EMSGSIZE` as `RMW_RET_ERROR` on publish (#3). +- Assorted P2/P3 hardening and identifier checks, plus dead-code removal (#9). +- Probe `localhost_only` via include dirs rather than linked targets, so Rolling + configure stops aborting. + +### Performance + +- Bound query/cleanup scans with a shared high-water mark (#14). +- Filter registry slots by type before the per-slot snapshot copy (#11). +- Copy-on-write subscriber-path cache; prune the stale known-subscriber set on + graph change (#12). +- Cache `rmw_service_server_is_available` on the registry generation (#13). + +## [0.2.0] - 2026-05-11 + +Observability. + +### Changed + +- Failure paths routed through `rcutils` logging. +- `ament_target_dependencies` replaced with direct `target_link_libraries`. +- `ament_lint_auto` dropped from the test suite. + +### Fixed + +- CI: install `test_msgs` explicitly so Rolling stops silently skipping it; tell + `rosdep` to install `test_depend` packages; raise `/dev/shm` in the container + to 1 GiB. + +## [0.1.0] - 2026-05-11 + +Initial release: a ROS 2 RMW implementation over `AF_UNIX` datagram sockets with +a lock-free shared-memory registry. + +### Added + +- Lock-free registry via per-slot seqlock + atomic state. +- Cached discovery lookups and scale benchmarks. +- Build & test workflows for Rolling, Jazzy, and Kilted. + +### Fixed + +- Guard `rmw_init_options_t::localhost_only` behind a CMake probe. + +[0.5.0]: https://github.com/benaliabderrahmane/rmw_unix_socket_cpp/compare/v0.4.1...v0.5.0 +[0.4.1]: https://github.com/benaliabderrahmane/rmw_unix_socket_cpp/compare/v0.4.0...v0.4.1 +[0.4.0]: https://github.com/benaliabderrahmane/rmw_unix_socket_cpp/compare/v0.3.0...v0.4.0 +[0.3.0]: https://github.com/benaliabderrahmane/rmw_unix_socket_cpp/compare/v0.2.0...v0.3.0 +[0.2.0]: https://github.com/benaliabderrahmane/rmw_unix_socket_cpp/compare/v0.1.0...v0.2.0 +[0.1.0]: https://github.com/benaliabderrahmane/rmw_unix_socket_cpp/releases/tag/v0.1.0 diff --git a/README.md b/README.md index 0135d27..44de630 100644 --- a/README.md +++ b/README.md @@ -22,9 +22,9 @@ datagram sockets; the wait set is `epoll` + `eventfd`. ## Status -- ROS 2 **Jazzy**, **Kilted**, and **Rolling** — built and tested in CI. +- ROS 2 **Jazzy**, **Kilted**, **Rolling**, and **Lyrical** — built and tested in CI. - **Linux / x86_64 only** (uses `epoll`, `eventfd`, `/dev/shm`, `AF_UNIX`). -- Version `0.1.0`, Apache-2.0. +- Version `0.5.0`, Apache-2.0. ## Quick start diff --git a/rmw_unix_socket_cpp/CMakeLists.txt b/rmw_unix_socket_cpp/CMakeLists.txt index a8c2d34..c45a3c0 100644 --- a/rmw_unix_socket_cpp/CMakeLists.txt +++ b/rmw_unix_socket_cpp/CMakeLists.txt @@ -197,8 +197,12 @@ if(BUILD_TESTING) target_link_libraries(test_rmw_wait ${_rmw_test_libs} ${_test_msg_deps}) target_include_directories(test_rmw_wait PRIVATE src) - # RMW API: graph introspection - ament_add_gtest(test_rmw_graph test/test_rmw_graph.cpp) + # RMW API: graph introspection. registry.cpp is compiled in as well so the + # test can seed the shared registry directly; the rmw library does not export + # those internal symbols. Both copies act on the same shared memory; the only + # process-local state in registry.cpp is ring_doorbells' thread_local send-fd + # cache, one per copy, so the duplication is benign. + ament_add_gtest(test_rmw_graph test/test_rmw_graph.cpp src/registry.cpp) target_link_libraries(test_rmw_graph ${_rmw_test_libs} ${_test_msg_deps}) target_include_directories(test_rmw_graph PRIVATE src) diff --git a/rmw_unix_socket_cpp/DESIGN.md b/rmw_unix_socket_cpp/DESIGN.md index f818332..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`).** `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 — 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. @@ -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. -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`. +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`. ### 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), 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. + +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 @@ -582,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 @@ -634,13 +657,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. +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. -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. +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. -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. - -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 @@ -648,9 +669,19 @@ Every entity needs a globally unique identifier in the RMW GID format, which is - bytes 0–3: the process PID (`getpid()`) - bytes 4–7: a per-process atomic counter (`g_gid_counter`, incremented once per entity) -- bytes 8–15: zero +- bytes 8–15: the `context_id` of the `rmw_context_t` that owns the entity (`UdsContext::context_id`, see Context identity and `ignore_local_publications` below) + +PID plus counter is unique on a single host by construction: two different processes have different PIDs, and within one process the monotonic counter guarantees distinct GIDs across all publishers, subscriptions, services, and clients. Sixteen bytes fits the RMW storage size exactly, and the scheme has no external dependency. The GID is what the request/response routing matches on: the client stamps its GID into every request header, and the server matches that GID to choose the reply socket. + +### Context identity and `ignore_local_publications` + +`rmw_subscription_options_t::ignore_local_publications` asks the middleware not to deliver messages published from the same `rmw_context_t`, not merely the same process — a process can in principle hold more than one context, so "local" is defined at context granularity. Each context gets a `context_id`, generated once in `rmw_init()` and stored on `UdsContext`; every node created from that `rmw_context_t*` shares the same `UdsContext` instance (`rmw_create_node` reuses `context->impl` rather than allocating a new one), so by default — one process, one `rclcpp::init()`, one context — every publisher and subscription in that process shares the same `context_id` automatically, exactly like nodes share `rclcpp`'s global default context unless a caller explicitly builds a separate one. + +The id itself is drawn from the kernel's entropy source (`std::random_device`, two 32-bit reads combined into a `uint64_t`) rather than derived from `getpid()`. Unlike the GID's PID field, which only has to disambiguate entities within one process's own generated GIDs, the context id is compared *across* processes and potentially across containers, where PIDs are only unique within their own PID namespace — two containers on the same host (e.g. separate docker-compose services sharing `/dev/shm`) can observe the same PID at the same instant. With 64 random bits, an accidental collision between concurrently running contexts is astronomically unlikely regardless of process or container topology. + +Because `UdsGid::generate()` embeds the owning context's id in the trailing 8 bytes of every GID, and the GID is copied verbatim into `WireHeader::gid` on every datagram, a receiver can decide "did this come from my own context?" purely from bytes already on the wire, with no extra traffic. `is_same_context(hdr, context_id)` extracts those trailing 8 bytes and compares them; `drain_subscription()` and `drain_socket()` call it only when the subscription actually requested `ignore_local_publications`, so a subscription that leaves the option at its default (`false`) never pays for or triggers the check. + -This is unique on a single host by construction. Two different processes have different PIDs, and within one process the monotonic counter guarantees distinct GIDs across all publishers, subscriptions, services, and clients. Sixteen bytes fits the RMW storage size exactly, and the scheme has no external dependency. The GID is what the request/response routing matches on: the client stamps its GID into every request header, and the server matches that GID to choose the reply socket. ## Operational requirements @@ -668,6 +699,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 +735,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 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` 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/package.xml b/rmw_unix_socket_cpp/package.xml index d22b746..40422b3 100644 --- a/rmw_unix_socket_cpp/package.xml +++ b/rmw_unix_socket_cpp/package.xml @@ -2,7 +2,7 @@ rmw_unix_socket_cpp - 0.1.0 + 0.5.0 Lightweight RMW implementation for ROS 2 using Unix domain sockets. Designed for localhost-only communication with minimal resource usage. diff --git a/rmw_unix_socket_cpp/src/registry.cpp b/rmw_unix_socket_cpp/src/registry.cpp index b0187df..6ce7c75 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,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); + // 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); } } @@ -319,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); + } } } @@ -345,6 +364,9 @@ void registry_remove(RegistryHeader * header, int32_t index) } teardown_slot(slot); header->generation.fetch_add(1, std::memory_order_acq_rel); + 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 @@ -372,13 +394,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 +540,12 @@ void registry_cleanup_stale(RegistryHeader * header) teardown_slot(&slots[i]); header->generation.fetch_add(1, std::memory_order_acq_rel); + 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/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_client.cpp b/rmw_unix_socket_cpp/src/rmw_client.cpp index 4329866..bfb78f8 100644 --- a/rmw_unix_socket_cpp/src/rmw_client.cpp +++ b/rmw_unix_socket_cpp/src/rmw_client.cpp @@ -86,7 +86,7 @@ rmw_client_t * rmw_create_client( return nullptr; } - cli_data->gid.generate(); + cli_data->gid.generate(ctx->context_id); cli_data->service_name = service_name; cli_data->type_name = rmw_uds::make_ros_type_name(sc.service_namespace, sc.service_name); cli_data->qos = resolve_qos(qos_policies); diff --git a/rmw_unix_socket_cpp/src/rmw_graph.cpp b/rmw_unix_socket_cpp/src/rmw_graph.cpp index 4864a1e..9ce7bda 100644 --- a/rmw_unix_socket_cpp/src/rmw_graph.cpp +++ b/rmw_unix_socket_cpp/src/rmw_graph.cpp @@ -16,6 +16,7 @@ #include "registry.hpp" #include "types.hpp" +#include #include #include #include @@ -43,6 +44,48 @@ static rmw_uds::UdsContext * get_context(const rmw_node_t * node) return nd->context; } +static int64_t steady_now_ns() +{ + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(); +} + +// Shortest gap between two stale-slot sweeps in one context. +static constexpr int64_t CLEANUP_MIN_INTERVAL_NS = 1000000000; // 1 s + +// Reclaiming slots whose owner died is garbage collection: it walks every live +// entry and stats /proc/ for each one. Running that on every graph query +// made a single call cost tens of milliseconds on a large registry, and a tool +// polling the graph paid it several times a second whether or not anything had +// changed. Sweep at most once per interval instead. +// +// This does not weaken any guarantee. A graph query can already return an +// entity whose owner died a microsecond after the sweep that vetted it, so the +// answer was never a liveness statement to begin with; the throttle only +// widens a window that was always open. The observable cost is crash-detection +// 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: 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(); + int64_t last = ctx->last_cleanup_ns.load(std::memory_order_relaxed); + if (now - last < CLEANUP_MIN_INTERVAL_NS) { + return; + } + // Whoever wins the CAS sweeps; the others skip rather than queue up behind + // it, which is the pile-up this change exists to remove. + if (!ctx->last_cleanup_ns.compare_exchange_strong( + last, now, std::memory_order_relaxed, std::memory_order_relaxed)) + { + return; + } + rmw_uds::registry_cleanup_stale(header); +} + static std::vector query_all( rmw_uds::UdsContext * ctx, rmw_uds::RegistryEntryType type, @@ -53,7 +96,7 @@ static std::vector query_all( auto * header = rmw_uds::registry_header(ctx->registry_ptr); // Lock-free: cleanup_stale + query both use the per-slot seqlock protocol // and may safely run concurrently with other readers/writers. - rmw_uds::registry_cleanup_stale(header); + maybe_cleanup_stale(ctx, header); return rmw_uds::registry_query(header, type, topic, node_name, node_ns); } diff --git a/rmw_unix_socket_cpp/src/rmw_init.cpp b/rmw_unix_socket_cpp/src/rmw_init.cpp index 9d89780..dae3769 100644 --- a/rmw_unix_socket_cpp/src/rmw_init.cpp +++ b/rmw_unix_socket_cpp/src/rmw_init.cpp @@ -14,6 +14,8 @@ #include +#include + #include "identifier.hpp" #include "logging.hpp" #include "registry.hpp" @@ -129,6 +131,13 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) domain_id = 0; } ctx->domain_id = domain_id; + try { + ctx->context_id = rmw_uds::generate_context_id(); + } catch (...) { + delete ctx; + RMW_SET_ERROR_MSG("failed to generate context id"); + return RMW_RET_ERROR; + } // Ensure socket directory exists. ensure_socket_dir swallows mkdir errors and // only returns the path, so validate the directory here (in-scope) rather than @@ -165,6 +174,15 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) rmw_uds::registry_cleanup_stale(init_header); rmw_uds::cleanup_orphan_socket_files(domain_id); rmw_uds::shm_cleanup_orphan_segments(domain_id); + // Date the sweep, or the first graph query — typically moments later, + // during node discovery — repeats the full stat-every-slot pass this one + // just paid for. Also keeps 0 unreachable as a live sentinel (steady_clock + // starts at boot, so a process starting within the first second of uptime + // would otherwise misread 0 as "swept just now"). + ctx->last_cleanup_ns.store( + std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(), + std::memory_order_relaxed); } rmw_uds::warn_if_sysctl_buffers_undersized(); @@ -184,8 +202,30 @@ 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 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. + { + 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); + 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; + } + } + + // Read initial generation ctx->last_registry_generation.store( rmw_uds::registry_generation(header), std::memory_order_relaxed); @@ -195,6 +235,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) { + 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); delete ctx; @@ -220,6 +262,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); } + 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); delete ctx; @@ -255,6 +299,18 @@ 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. 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); + } 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_publisher.cpp b/rmw_unix_socket_cpp/src/rmw_publisher.cpp index 0f69b90..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 @@ -92,7 +92,7 @@ rmw_publisher_t * rmw_create_publisher( return nullptr; } - pub_data->gid.generate(); + pub_data->gid.generate(ctx->context_id); pub_data->topic_name = topic_name; pub_data->type_name = rmw_uds::make_ros_type_name( callbacks->message_namespace_, callbacks->message_name_); @@ -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_service.cpp b/rmw_unix_socket_cpp/src/rmw_service.cpp index 73155f6..b7ceeba 100644 --- a/rmw_unix_socket_cpp/src/rmw_service.cpp +++ b/rmw_unix_socket_cpp/src/rmw_service.cpp @@ -86,7 +86,7 @@ rmw_service_t * rmw_create_service( return nullptr; } - srv_data->gid.generate(); + srv_data->gid.generate(ctx->context_id); srv_data->service_name = service_name; srv_data->type_name = rmw_uds::make_ros_type_name(sc.service_namespace, sc.service_name); srv_data->qos = resolve_qos(qos_profile); diff --git a/rmw_unix_socket_cpp/src/rmw_subscription.cpp b/rmw_unix_socket_cpp/src/rmw_subscription.cpp index aa25009..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,12 +85,22 @@ 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)) { continue; // shm descriptor unresolvable (publisher gone / ring lapped) } + if (sub->ignore_local_publications && + rmw_uds::is_same_context(hdr, sub->context->context_id)) + { + continue; // ignore_local_publications: drop same-context publications + } + rmw_uds::ReceivedMessage msg; msg.header = hdr; msg.payload = std::move(payload); @@ -116,7 +146,6 @@ rmw_subscription_t * rmw_create_subscription( const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_options) { - (void)subscription_options; RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_support, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); @@ -140,7 +169,7 @@ rmw_subscription_t * rmw_create_subscription( return nullptr; } - sub_data->gid.generate(); + sub_data->gid.generate(ctx->context_id); sub_data->topic_name = topic_name; sub_data->type_name = rmw_uds::make_ros_type_name( callbacks->message_namespace_, callbacks->message_name_); @@ -150,6 +179,8 @@ rmw_subscription_t * rmw_create_subscription( sub_data->callbacks = callbacks; sub_data->context = ctx; sub_data->node = node_data; + sub_data->ignore_local_publications = subscription_options ? + subscription_options->ignore_local_publications : false; // Create and bind socket sub_data->socket_path = rmw_uds::make_socket_path(ctx->domain_id, "sub"); @@ -209,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 3ebc286..11a9486 100644 --- a/rmw_unix_socket_cpp/src/rmw_wait.cpp +++ b/rmw_unix_socket_cpp/src/rmw_wait.cpp @@ -13,20 +13,24 @@ // limitations under the License. #include "identifier.hpp" +#include "logging.hpp" #include "registry.hpp" #include "transport.hpp" #include "types.hpp" +#include +#include #include #include #include -#include #include -#include -#include +#include +#include +#include #include #include +#include #include #include "rmw/allocators.h" @@ -49,7 +53,10 @@ static int64_t wall_now_ns() // Drain a socket into a message queue (subscription, service, or client). // `shm_cache`/`domain_id` resolve large-payload descriptors: topic, request, // and response messages can all carry SHM_PAYLOAD_FLAG, so every caller passes -// its own reader cache. +// its own reader cache. If `ignore_local` is true, `context_id` is compared +// against the sender context id embedded in WireHeader::gid and matching +// same-context publications are dropped (subscriptions only; services/clients +// leave ignore_local=false to disable the check). static void drain_socket( int fd, std::mutex & queue_mutex, @@ -57,7 +64,10 @@ static void drain_socket( size_t max_depth, uint8_t expected_msg_type, rmw_uds::ShmReaderCache & shm_cache, - size_t domain_id) + size_t domain_id, + bool ignore_local = false, + uint64_t context_id = 0, + std::map, int64_t> * replay_watermarks = nullptr) { rmw_uds::WireHeader hdr; std::vector payload; @@ -67,10 +77,32 @@ 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) } + if (ignore_local && rmw_uds::is_same_context(hdr, context_id)) { + payload.clear(); + continue; // ignore_local_publications: drop same-context publications + } rmw_uds::ReceivedMessage msg; msg.header = hdr; @@ -103,6 +135,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) { @@ -160,161 +193,174 @@ rmw_ret_t rmw_wait( auto * ws_data = static_cast(wait_set->data); - // 1. Drain all sockets - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - if (!subscriptions->subscribers[i]) {continue;} - auto * sub = static_cast(subscriptions->subscribers[i]); - drain_socket(sub->socket_fd, sub->queue_mutex, sub->message_queue, - sub->queue_depth, 0, sub->shm_cache, sub->context->domain_id); - } - } - - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - if (!services->services[i]) {continue;} - auto * srv = static_cast(services->services[i]); - drain_socket(srv->socket_fd, srv->queue_mutex, srv->request_queue, 100, 1, - srv->shm_cache, srv->context->domain_id); - } - } - - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - if (!clients->clients[i]) {continue;} - auto * cli = static_cast(clients->clients[i]); - drain_socket(cli->socket_fd, cli->queue_mutex, cli->response_queue, 100, 2, - cli->shm_cache, cli->context->domain_id); - } - } - - // 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) { - 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; - { - 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); - } - } - 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()); + // 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; } } } - - // 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 + } + 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"); } } - // Trigger graph guard condition on the context - if (ctx->graph_guard_condition) { - auto _r [[maybe_unused]] = rmw_trigger_guard_condition(ctx->graph_guard_condition); - } } } - // 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 - // reused after its previous owner was closed gets freshly armed. The kernel - // auto-removes closed fds, so no EPOLL_CTL_DEL is needed. + 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)) { + 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 gc_lock(ctx->graph_gcs_mutex); + for (auto * gc : ctx->graph_gcs) { + auto _r [[maybe_unused]] = rmw_trigger_guard_condition(gc); + } + } + } + }; + run_generation_check(); + + // 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 + // an epoll result can mark the right entry in gc_triggered below. + std::unordered_map gc_index; + // Fds armed by THIS call: the caller's entities plus the doorbell. The + // dispatch loop never touches an entity outside this set — it may already + // be destroyed, or belong to a wait set that is actually waiting on it. + std::unordered_set armed_this_call; + // Entities epoll could not watch (e.g. watch exhaustion): drained directly + // each call below so they degrade to polling instead of vanishing. + std::vector unarmed; { struct epoll_event ev; std::memset(&ev, 0, sizeof(ev)); - auto register_fd = [&](int fd) { + auto register_fd = [&](int fd, uint8_t kind, void * entity, uint64_t uid) { if (fd < 0) {return;} + armed_this_call.insert(fd); + auto it = ws_data->armed.find(fd); + if (it != ws_data->armed.end() && it->second.uid == uid) { + return; // Already armed for this entity. + } ev.events = EPOLLIN; ev.data.fd = fd; - // EEXIST means the fd is already armed -> treat as success. + // EEXIST means the fd is already armed in the kernel -> treat as + // success so the entry still lands in `armed`. Returning here instead + // would leave the fd armed but unmapped, and every event on it would + // then be dropped by the lookup below. if (epoll_ctl(ws_data->epoll_fd, EPOLL_CTL_ADD, fd, &ev) != 0 && errno != EEXIST) { - // Other errors are ignored, as before. + // Without a watch this entity is never reported ready, and there is + // no unconditional pre-drain any more — a silent skip would leave it + // permanently invisible. Surface the error (epoll watch exhaustion + // is an operator-level sysctl problem) and fall back to polling. + RMW_UDS_LOG_ERROR_THROTTLE( + 1000, + "epoll_ctl ADD failed for fd %d: %s (errno=%d) — entity degraded " + "to per-wait polling", + fd, std::strerror(errno), errno); + armed_this_call.erase(fd); + unarmed.push_back(rmw_uds::ArmedEntry{kind, entity, uid}); + return; } + ws_data->armed[fd] = rmw_uds::ArmedEntry{kind, entity, uid}; }; if (subscriptions) { for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { if (!subscriptions->subscribers[i]) {continue;} auto * sub = static_cast(subscriptions->subscribers[i]); - register_fd(sub->socket_fd); + register_fd(sub->socket_fd, rmw_uds::ARMED_SUBSCRIPTION, sub, sub->uid); } } if (services) { for (size_t i = 0; i < services->service_count; ++i) { if (!services->services[i]) {continue;} auto * srv = static_cast(services->services[i]); - register_fd(srv->socket_fd); + register_fd(srv->socket_fd, rmw_uds::ARMED_SERVICE, srv, srv->uid); } } if (clients) { for (size_t i = 0; i < clients->client_count; ++i) { if (!clients->clients[i]) {continue;} auto * cli = static_cast(clients->clients[i]); - register_fd(cli->socket_fd); + register_fd(cli->socket_fd, rmw_uds::ARMED_CLIENT, cli, cli->uid); } } if (guard_conditions) { @@ -322,17 +368,58 @@ rmw_ret_t rmw_wait( if (!guard_conditions->guard_conditions[i]) {continue;} auto * gc = static_cast( guard_conditions->guard_conditions[i]); - register_fd(gc->eventfd_fd); + gc_index[gc] = i; + register_fd(gc->eventfd_fd, rmw_uds::ARMED_GUARD_CONDITION, gc, gc->uid); } } + // 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); } - // 3. Check if anything is already ready + // 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 3 + // reads every caller GC directly. + for (const auto & ue : unarmed) { + switch (ue.kind) { + case rmw_uds::ARMED_SUBSCRIPTION: { + 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->replayed_watermarks); + break; + } + case rmw_uds::ARMED_SERVICE: { + auto * srv = static_cast(ue.entity); + drain_socket(srv->socket_fd, srv->queue_mutex, srv->request_queue, 100, 1, + srv->shm_cache, srv->context->domain_id); + break; + } + case rmw_uds::ARMED_CLIENT: { + auto * cli = static_cast(ue.entity); + drain_socket(cli->socket_fd, cli->queue_mutex, cli->response_queue, 100, 2, + cli->shm_cache, cli->context->domain_id); + break; + } + default: + break; + } + } + + // 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 + // mutex and a deque test per entity, no syscalls. bool something_ready = false; - // Per-GC readiness from the step-3 consuming read, carried to step 5. One - // read drains the whole eventfd counter, so we never write it back (a - // non-atomic read-back would race a concurrent trigger and inflate it). + // Per-GC readiness from the consuming read below, carried to the output + // pass. One read drains the whole eventfd counter, so we never write it back + // (a non-atomic read-back would race a concurrent trigger and inflate it). std::vector gc_triggered; if (subscriptions) { @@ -383,76 +470,182 @@ rmw_ret_t rmw_wait( } } - // 4. If nothing ready, block with epoll - if (!something_ready) { - // Compute timeout. -1 means block forever (epoll_wait sentinel). - int timeout_ms = -1; - if (wait_timeout) { - // Accumulate in int64_t; RMW_DURATION_INFINITE (~9.2e12 ms) overflows int. - int64_t ms = static_cast(wait_timeout->sec) * 1000 + - static_cast(wait_timeout->nsec) / 1000000; - if (ms > std::numeric_limits::max()) { - timeout_ms = -1; // Infinite (or beyond epoll's range) -> block forever - } else { - timeout_ms = static_cast(ms); - if (timeout_ms == 0 && wait_timeout->nsec > 0) { - timeout_ms = 1; // At least 1ms - } + // Compute timeout. -1 means block forever (epoll_wait sentinel). + int timeout_ms = -1; + if (wait_timeout) { + // Accumulate in int64_t; RMW_DURATION_INFINITE (~9.2e12 ms) overflows int. + int64_t ms = static_cast(wait_timeout->sec) * 1000 + + static_cast(wait_timeout->nsec) / 1000000; + if (ms > std::numeric_limits::max()) { + timeout_ms = -1; // Infinite (or beyond epoll's range) -> block forever + } else { + timeout_ms = static_cast(ms); + if (timeout_ms == 0 && wait_timeout->nsec > 0) { + timeout_ms = 1; // At least 1ms } } + } - // 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 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; + // 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 + // a single syscall instead of one per entity. + const bool poll_only = something_ready; + { struct epoll_event ready_events[64]; - const int64_t deadline_ns = - (timeout_ms >= 0) ? steady_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 = 0; + if (!poll_only) { + block_ms = -1; + if (!infinite) { + const int64_t rem_ns = caller_deadline_ns - steady_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 ((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; + } } - 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; // Caller's deadline passed, or the poll-only pass found nothing. } - const int64_t rem_ns = deadline_ns - steady_now_ns(); - if (rem_ns <= 0) { - break; // Deadline passed -> timeout; fall through to drain. + // 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 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) { + const int rfd = ready_events[e].data.fd; + auto it = ws_data->armed.find(rfd); + if (it == ws_data->armed.end() || + armed_this_call.find(rfd) == armed_this_call.end()) + { + // Not armed by this call: the entity may belong to another wait set + // now, or may already be destroyed. Never touch it — consuming its + // event here would steal a wakeup the owning wait set never gets + // back, and dereferencing a destroyed entity is a use-after-free. + // Withdraw the fd so a readable level-triggered fd cannot spin this + // loop; a later call that waits on the entity re-arms it. + (void)epoll_ctl(ws_data->epoll_fd, EPOLL_CTL_DEL, rfd, nullptr); + if (it != ws_data->armed.end()) { + ws_data->armed.erase(it); + } + continue; // Not progress: nothing the caller waits on changed. + } + const rmw_uds::ArmedEntry & entry = it->second; + switch (entry.kind) { + case rmw_uds::ARMED_SUBSCRIPTION: { + 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->replayed_watermarks); + std::lock_guard lock(sub->queue_mutex); + if (!sub->message_queue.empty()) { + progressed = true; + } + break; + } + case rmw_uds::ARMED_SERVICE: { + auto * srv = static_cast(entry.entity); + drain_socket(srv->socket_fd, srv->queue_mutex, srv->request_queue, 100, 1, + srv->shm_cache, srv->context->domain_id); + std::lock_guard lock(srv->queue_mutex); + if (!srv->request_queue.empty()) { + progressed = true; + } + break; + } + case rmw_uds::ARMED_CLIENT: { + auto * cli = static_cast(entry.entity); + drain_socket(cli->socket_fd, cli->queue_mutex, cli->response_queue, 100, 2, + cli->shm_cache, cli->context->domain_id); + std::lock_guard lock(cli->queue_mutex); + if (!cli->response_queue.empty()) { + progressed = true; + } + break; + } + case rmw_uds::ARMED_GUARD_CONDITION: { + // Consume the trigger here and remember it by the GC's slot in the + // caller's array, so the output pass reports the right entry even + // though this read already emptied the eventfd. + auto * gc = static_cast(entry.entity); + uint64_t val; + const ssize_t r = read(gc->eventfd_fd, &val, sizeof(val)); + if (r == static_cast(sizeof(val))) { + progressed = true; + const auto gi = gc_index.find(gc); + if (gi != gc_index.end()) { + gc_triggered[gi->second] = true; + } + } + break; + } + case rmw_uds::ARMED_DOORBELL: + rang = true; + break; + default: + break; + } } - const int64_t rem_ms = rem_ns / 1000000; - remaining_ms = (rem_ms > 0) ? static_cast(rem_ms) : 1; // >=1ms while time remains - } - // No EPOLL_CTL_DEL needed — fds stay registered across calls. - - // Drain again after epoll - if (subscriptions) { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { - if (!subscriptions->subscribers[i]) {continue;} - auto * sub = static_cast(subscriptions->subscribers[i]); - drain_socket(sub->socket_fd, sub->queue_mutex, sub->message_queue, - sub->queue_depth, 0, sub->shm_cache, sub->context->domain_id); + if (rang) { + run_generation_check(); // Drains the doorbell, triggers graph GCs. } - } - if (services) { - for (size_t i = 0; i < services->service_count; ++i) { - if (!services->services[i]) {continue;} - auto * srv = static_cast(services->services[i]); - drain_socket(srv->socket_fd, srv->queue_mutex, srv->request_queue, 100, 1, - srv->shm_cache, srv->context->domain_id); + if (poll_only) { + if (n == 64) { + continue; // A full batch: more fds may be ready than one epoll_wait + // reports. Terminates because drained fds stop being ready. + } + break; // Non-blocking sweep: a single pass is all it is for. } - } - if (clients) { - for (size_t i = 0; i < clients->client_count; ++i) { - if (!clients->clients[i]) {continue;} - auto * cli = static_cast(clients->clients[i]); - drain_socket(cli->socket_fd, cli->queue_mutex, cli->response_queue, 100, 2, - cli->shm_cache, cli->context->domain_id); + if (progressed) { + break; // Something the caller waits on became ready. + } + if (!infinite && steady_now_ns() >= caller_deadline_ns) { + break; // Doorbell-only wake at the deadline -> timeout. + } + if (graph_gc_unwired || !unarmed.empty()) { + break; // Bounded degraded wait: surface the spurious wake so the + // 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. } // 5. Set output: ready entities stay, non-ready set to NULL @@ -478,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; @@ -521,7 +715,9 @@ rmw_ret_t rmw_wait( } } - if (!any_ready) { + // Only the caller's own deadline may report a timeout. Waking up and finding + // nothing to take is a spurious wake: report OK with every entry nulled. + if (!any_ready && !infinite && steady_now_ns() >= caller_deadline_ns) { return RMW_RET_TIMEOUT; } 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/transport.cpp b/rmw_unix_socket_cpp/src/transport.cpp index 9842a1b..662a5c2 100644 --- a/rmw_unix_socket_cpp/src/transport.cpp +++ b/rmw_unix_socket_cpp/src/transport.cpp @@ -200,67 +200,91 @@ bool recv_from( // Use a fixed buffer approach: header + payload up to recv buffer static thread_local std::vector recv_buf(256 * 1024); // 256KB initial - ssize_t n = recv(socket_fd, recv_buf.data(), recv_buf.size(), - MSG_DONTWAIT | MSG_PEEK | MSG_TRUNC); - - if (n < 0) { - // EAGAIN is the steady-state "nothing to read" case driven by wait() - // loops; staying silent is intentional. Anything else (EBADF, EINVAL, - // EINTR-after-shutdown) is worth surfacing. - if (errno != EAGAIN && errno != EWOULDBLOCK) { - RMW_UDS_LOG_WARN_THROTTLE( - 1000, - "UDS recv (peek) failed: %s (errno=%d)", - std::strerror(errno), errno); + // Junk datagrams (zero-length, runt) are consumed and skipped here rather + // than returned as false: every drain loop stops on the first false, so + // ending the call on consumed junk would strand real messages queued behind + // it until the next wake. False means the socket is genuinely empty. + while (true) { + ssize_t n = recv(socket_fd, recv_buf.data(), recv_buf.size(), + MSG_DONTWAIT | MSG_PEEK | MSG_TRUNC); + + if (n < 0) { + // EAGAIN is the steady-state "nothing to read" case driven by wait() + // loops; staying silent is intentional. Anything else (EBADF, EINVAL, + // EINTR-after-shutdown) is worth surfacing. + if (errno != EAGAIN && errno != EWOULDBLOCK) { + RMW_UDS_LOG_WARN_THROTTLE( + 1000, + "UDS recv (peek) failed: %s (errno=%d)", + std::strerror(errno), errno); + } + return false; } - return false; - } - if (n == 0) { - return false; - } - // Resize if needed - if (static_cast(n) > recv_buf.size()) { - recv_buf.resize(static_cast(n)); - } + // Resize if needed + if (static_cast(n) > recv_buf.size()) { + recv_buf.resize(static_cast(n)); + } - // Actually receive the message - n = recv(socket_fd, recv_buf.data(), recv_buf.size(), MSG_DONTWAIT); - if (n < 0) { - if (errno != EAGAIN && errno != EWOULDBLOCK) { + // Actually receive the message. Concurrent drains of the same fd race the + // peek, so only this recv decides what was dequeued: a zero-length + // datagram peeked above may be gone by now with a real message at the + // head, which is why the n == 0 case is handled after the consume — a + // blind fixed-size discard here could destroy that real message. + n = recv(socket_fd, recv_buf.data(), recv_buf.size(), MSG_DONTWAIT | MSG_TRUNC); + if (n < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK) { + RMW_UDS_LOG_WARN_THROTTLE( + 1000, "UDS recv failed: %s (errno=%d)", + std::strerror(errno), errno); + } + return false; + } + if (static_cast(n) > recv_buf.size()) { + // A datagram larger than the buffer raced in behind a smaller peek; the + // kernel discarded its tail, so the prefix must not be delivered as a + // complete message. RMW_UDS_LOG_WARN_THROTTLE( - 1000, "UDS recv failed: %s (errno=%d)", - std::strerror(errno), errno); + 5000, + "UDS recv: datagram truncated (%zd > %zu bytes) — dropped", + n, recv_buf.size()); + continue; + } + if (n == 0) { + // A zero-length datagram carries no WireHeader, so it is not a message. + // It was consumed just above: leaving it queued would keep the socket + // permanently readable, which spins every wait that polls this fd. + RMW_UDS_LOG_WARN_THROTTLE(5000, "UDS recv: zero-length datagram — dropped"); + continue; + } + if (n < static_cast(sizeof(WireHeader))) { + RMW_UDS_LOG_WARN_THROTTLE( + 5000, + "UDS recv: runt datagram (%zd bytes, expected >= %zu) — dropped", + n, sizeof(WireHeader)); + continue; } - return false; - } - if (n < static_cast(sizeof(WireHeader))) { - RMW_UDS_LOG_WARN_THROTTLE( - 5000, - "UDS recv: runt datagram (%zd bytes, expected >= %zu) — dropped", - n, sizeof(WireHeader)); - return false; - } - std::memcpy(&header_out, recv_buf.data(), sizeof(WireHeader)); - - size_t payload_len = static_cast(n) - sizeof(WireHeader); - if (payload_len != header_out.payload_size) { - // Mismatch — typically the sender's payload was larger than our - // recv buffer and the kernel truncated. Surface it so we can correlate - // with the corresponding sender-side EMSGSIZE. - RMW_UDS_LOG_WARN_THROTTLE( - 5000, - "UDS recv: payload size mismatch (got %zu, header says %u) — truncating", - payload_len, header_out.payload_size); - payload_len = std::min(payload_len, static_cast(header_out.payload_size)); - } + std::memcpy(&header_out, recv_buf.data(), sizeof(WireHeader)); - payload_out.assign( - recv_buf.data() + sizeof(WireHeader), - recv_buf.data() + sizeof(WireHeader) + payload_len); + size_t payload_len = static_cast(n) - sizeof(WireHeader); + if (payload_len != header_out.payload_size) { + // Mismatch — typically the sender's payload was larger than our + // recv buffer and the kernel truncated. Surface it so we can correlate + // with the corresponding sender-side EMSGSIZE. + RMW_UDS_LOG_WARN_THROTTLE( + 5000, + "UDS recv: payload size mismatch (got %zu, header says %u) — truncating", + payload_len, header_out.payload_size); + payload_len = std::min(payload_len, static_cast(header_out.payload_size)); + } - return true; + payload_out.assign( + recv_buf.data() + sizeof(WireHeader), + recv_buf.data() + sizeof(WireHeader) + payload_len); + + return true; + } } OutboundPayload shm_prepare_send( diff --git a/rmw_unix_socket_cpp/src/types.hpp b/rmw_unix_socket_cpp/src/types.hpp index fa40803..d503bbb 100644 --- a/rmw_unix_socket_cpp/src/types.hpp +++ b/rmw_unix_socket_cpp/src/types.hpp @@ -15,18 +15,20 @@ #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 +#include +#include #include "rmw/event_callback_type.h" #include "rmw/types.h" @@ -61,20 +63,54 @@ inline std::string make_ros_type_name(const char * ns, const char * name) return result; } +// Process-wide monotonic id, stamped on every entity that can be armed in a +// wait set. Never reused, so a wait set can tell "the same fd armed for the +// same entity" from "the same fd number handed to a different entity after a +// close". +inline uint64_t next_entity_uid() +{ + static std::atomic counter{1}; + return counter.fetch_add(1, std::memory_order_relaxed); +} + struct UdsGid { uint8_t data[RMW_GID_STORAGE_SIZE] = {}; - void generate() + // bytes 0-3: process PID. bytes 4-7: monotonic counter, unique per entity + // within this process. bytes 8-15: `context_id`, identifying the + // rmw_context_t that owns the entity this GID is generated for (see + // UdsContext::context_id), so any receiver can tell — purely from the + // bytes already on the wire — whether a message came from a publisher + // living in the same context as itself (ignore_local_publications). + void generate(uint64_t context_id) { std::memset(data, 0, sizeof(data)); - pid_t pid = getpid(); - uint32_t cnt = g_gid_counter.fetch_add(1, std::memory_order_relaxed); - std::memcpy(data, &pid, sizeof(pid)); - std::memcpy(data + sizeof(pid), &cnt, sizeof(cnt)); + static_assert(RMW_GID_STORAGE_SIZE == 16, "UdsGid assumes a 16-byte GID"); + static_assert(sizeof(pid_t) == 4, "UdsGid assumes a 32-bit pid_t"); + const uint32_t pid = static_cast(getpid()); + const uint32_t cnt = g_gid_counter.fetch_add(1, std::memory_order_relaxed); + std::memcpy(data + 0, &pid, sizeof(pid)); + std::memcpy(data + 4, &cnt, sizeof(cnt)); + std::memcpy(data + 8, &context_id, sizeof(context_id)); } }; +// A context id unique to this rmw_context_t. Deliberately NOT derived from +// getpid(): PIDs are only unique within a PID namespace, so two containers +// (e.g. separate docker-compose services) sharing the same machine/IPC can +// observe the same pid at the same time, which would make a pid-based id +// collide. Instead draw the id from the kernel's entropy source — with 64 +// random bits, an accidental collision between concurrently running contexts +// is astronomically unlikely regardless of process/container topology. +inline uint64_t generate_context_id() +{ + static std::random_device rd; + uint64_t hi = rd(); + uint64_t lo = rd(); + return (hi << 32) | lo; +} + // Wire header prepended to every datagram struct __attribute__((packed)) WireHeader { @@ -106,24 +142,59 @@ struct ReceivedMessage int64_t received_timestamp_ns; }; -// Forward declaration: publisher type defined below. -struct UdsPublisher; +// True if `hdr` was sent by a publisher created from `context_id` — i.e. the +// rmw_context_t that owns the comparing subscription. UdsGid::generate() +// embeds the sender's context id in the trailing 8 bytes of the GID, which is +// copied verbatim into WireHeader::gid on every datagram, so this implements +// the rmw "same context" contract (ignore_local_publications) exactly, with +// no extra wire traffic. +inline bool is_same_context(const WireHeader & hdr, uint64_t context_id) +{ + uint64_t sender_context_id; + std::memcpy(&sender_context_id, hdr.gid + sizeof(pid_t) + sizeof(uint32_t), sizeof(sender_context_id)); + return sender_context_id == context_id; +} // Per-context implementation data struct UdsContext { size_t domain_id = 0; + // Unique id for this rmw_context_t (see generate_context_id()), embedded in + // every GID generated by entities created under it. Assigned once in rmw_init. + uint64_t context_id = 0; int registry_fd = -1; void * registry_ptr = nullptr; size_t registry_size = 0; int send_socket_fd = -1; std::atomic is_shutdown{false}; std::atomic last_registry_generation{0}; - rmw_guard_condition_t * graph_guard_condition = nullptr; - // TRANSIENT_LOCAL publishers, for wait-side cache replay on graph change. - std::mutex transient_local_pubs_mutex; - std::vector transient_local_pubs; + // Last time this context swept the registry for slots whose owning process + // is gone (steady clock, ns). Keeps that sweep off the graph query path; see + // maybe_cleanup_stale in rmw_graph.cpp. Stamped by the rmw_init sweep too; + // the registry-full fallback sweep in registry_add does not participate. + std::atomic last_cleanup_ns{0}; + + // 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. 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), + // 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; }; // Node data @@ -136,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 { @@ -172,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 @@ -187,6 +249,7 @@ struct UdsPublisher // Subscription data struct UdsSubscription { + uint64_t uid = next_entity_uid(); UdsGid gid; std::string topic_name; std::string type_name; @@ -202,6 +265,17 @@ struct UdsSubscription int32_t registry_index = -1; UdsContext * context = nullptr; UdsNode * node = nullptr; + // 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; @@ -222,6 +296,7 @@ struct CachedClient // Service server data struct UdsService { + uint64_t uid = next_entity_uid(); UdsGid gid; std::string service_name; std::string type_name; @@ -257,6 +332,7 @@ struct UdsService // Service client data struct UdsClient { + uint64_t uid = next_entity_uid(); UdsGid gid; std::string service_name; std::string type_name; @@ -294,13 +370,47 @@ struct UdsClient // Guard condition data struct UdsGuardCondition { + // Wait-set arming identity; see next_entity_uid(). + uint64_t uid = next_entity_uid(); int eventfd_fd = -1; }; +// What a given epoll fd was armed for. Lets rmw_wait map an epoll result back +// to its owner without scanning the wait set, and skip the epoll_ctl when the +// fd is already armed for the same entity. +enum ArmedKind : uint8_t +{ + ARMED_SUBSCRIPTION = 0, + ARMED_SERVICE, + ARMED_CLIENT, + ARMED_GUARD_CONDITION, + ARMED_DOORBELL, +}; + +struct ArmedEntry +{ + uint8_t kind = ARMED_SUBSCRIPTION; // ArmedKind + void * entity = nullptr; // UdsSubscription * etc, for the drain + uint64_t uid = 0; // 0 for the doorbell, which has no entity +}; + // Wait set data 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; + // fd -> what it was armed for. Survives across rmw_wait calls, which is what + // lets the arming pass cost no syscall in the steady state. An entry is + // dispatched only when the current call armed its fd (rmw_wait's + // armed_this_call gate); a ready fd outside that set — an entity waited on + // elsewhere now, or destroyed — is EPOLL_CTL_DEL'd and erased without ever + // dereferencing `entity`. (close() alone does not guarantee epoll removal: + // a fork()ed child or dup() keeps the open file description alive.) A fd + // number reused by a new entity is re-armed because the uid differs. + std::unordered_map armed; }; } // namespace rmw_uds diff --git a/rmw_unix_socket_cpp/test/test_base.hpp b/rmw_unix_socket_cpp/test/test_base.hpp index 4d2032e..1fc37b1 100644 --- a/rmw_unix_socket_cpp/test/test_base.hpp +++ b/rmw_unix_socket_cpp/test/test_base.hpp @@ -31,13 +31,15 @@ class RmwUdsTestBase : public ::testing::Test protected: rmw_context_t context = rmw_get_zero_initialized_context(); rmw_init_options_t options = rmw_get_zero_initialized_init_options(); + // Unique domain to avoid collisions with running ROS systems. A subclass + // constructor may override it for tests that need a private registry. + size_t domain_id = 99; void SetUp() override { rcutils_allocator_t allocator = rcutils_get_default_allocator(); ASSERT_EQ(RMW_RET_OK, rmw_init_options_init(&options, allocator)); - // Use a unique domain to avoid collisions with running ROS systems - options.domain_id = 99; + options.domain_id = domain_id; ASSERT_EQ(RMW_RET_OK, rmw_init(&options, &context)); } diff --git a/rmw_unix_socket_cpp/test/test_rmw_graph.cpp b/rmw_unix_socket_cpp/test/test_rmw_graph.cpp index edd4eac..264a76f 100644 --- a/rmw_unix_socket_cpp/test/test_rmw_graph.cpp +++ b/rmw_unix_socket_cpp/test/test_rmw_graph.cpp @@ -14,9 +14,13 @@ #include "test_base.hpp" +#include #include #include +#include "../src/registry.hpp" +#include "../src/types.hpp" + #include "test_msgs/msg/basic_types.hpp" #include "rmw/get_topic_names_and_types.h" @@ -46,6 +50,90 @@ TEST_F(RmwUdsNodeTest, GetNodeNames) auto _r2 [[maybe_unused]] = rcutils_string_array_fini(&namespaces); } +// registry_cleanup_stale walks every live slot and stats /proc/ for each +// one. That is garbage collection, and it must not run on every graph query. +// +// Private domain: the assertions below are negatives on shared registry state, +// and domain 99 is one machine-global segment that every fixture-based test +// binary sweeps unthrottled at rmw_init — under parallel ctest that reclaims +// the ghost mid-test. (94-98 are taken by the other binaries, 99 by fixtures.) +class RmwUdsGraphThrottleTest : public RmwUdsNodeTest +{ +protected: + RmwUdsGraphThrottleTest() {domain_id = 93;} +}; + +TEST_F(RmwUdsGraphThrottleTest, GraphQueryThrottlesStaleCleanup) +{ + auto * nd = static_cast(node->data); + auto * header = rmw_uds::registry_header(nd->context->registry_ptr); + + // A PID that cannot be running, so cleanup_stale sees the entry as dead. + constexpr pid_t DEAD_PID = 2147483000; + auto add_ghost = [&](const char * name) { + rmw_uds::RegistryEntry e; + std::memset(&e, 0, sizeof(e)); + e.type = rmw_uds::ENTRY_NODE; + e.pid = DEAD_PID; + std::strncpy(e.node_name, name, sizeof(e.node_name) - 1); + return rmw_uds::registry_add(header, e); + }; + + auto graph_lists = [&](const char * name) { + rcutils_string_array_t names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t namespaces = rcutils_get_zero_initialized_string_array(); + EXPECT_EQ(RMW_RET_OK, rmw_get_node_names(node, &names, &namespaces)); + bool found = false; + for (size_t i = 0; i < names.size; ++i) { + if (std::string(names.data[i]) == name) { + found = true; + } + } + auto _r1 [[maybe_unused]] = rcutils_string_array_fini(&names); + auto _r2 [[maybe_unused]] = rcutils_string_array_fini(&namespaces); + return found; + }; + + auto steady_ns = [] { + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(); + }; + + // Age the stamp past the interval so the first query sweeps regardless of + // how recently rmw_init dated its own sweep. + nd->context->last_cleanup_ns.store( + steady_ns() - 2 * 1000000000LL, std::memory_order_relaxed); + const int32_t first = add_ghost("ghost_first"); + ASSERT_GE(first, 0); + EXPECT_FALSE(graph_lists("ghost_first")) << + "the first graph query should still reclaim dead slots"; + + // Re-arm the throttle deterministically rather than racing the 1 s interval + // against the wall clock (a slow/sanitized run can lose that race). + nd->context->last_cleanup_ns.store(steady_ns(), std::memory_order_relaxed); + + // A second dead slot added now must survive: the sweep is throttled, so the + // very next query does not pay for another full stat() pass. + const int32_t second = add_ghost("ghost_second"); + ASSERT_GE(second, 0); + EXPECT_TRUE(graph_lists("ghost_second")) << + "cleanup_stale ran again right away; the throttle is not in effect"; + + // And the sweep must resume once the interval has passed — "once per process + // lifetime" would satisfy the two assertions above. Age the stamp again + // instead of sleeping through the interval. + nd->context->last_cleanup_ns.store( + steady_ns() - 2 * 1000000000LL, std::memory_order_relaxed); + EXPECT_FALSE(graph_lists("ghost_second")) << + "the sweep never resumed after the throttle interval passed"; + + // A failed assertion above strands a ghost in the persistent segment; remove + // unconditionally (no-op for slots the sweeps already reclaimed — nothing + // else writes this private domain, so the indices cannot have been reused). + rmw_uds::registry_remove(header, first); + rmw_uds::registry_remove(header, second); +} + TEST_F(RmwUdsNodeTest, CountPublishersAndSubscribers) { auto * ts = rosidl_typesupport_cpp::get_message_type_support_handle< diff --git a/rmw_unix_socket_cpp/test/test_rmw_pub_sub.cpp b/rmw_unix_socket_cpp/test/test_rmw_pub_sub.cpp index bb01ff1..a8bfb13 100644 --- a/rmw_unix_socket_cpp/test/test_rmw_pub_sub.cpp +++ b/rmw_unix_socket_cpp/test/test_rmw_pub_sub.cpp @@ -343,3 +343,45 @@ TEST_F(PubSubTest, TakeSequenceSkipsMidBatchCorruptContiguously) ::close(send_fd); EXPECT_EQ(RMW_RET_OK, rmw_serialized_message_fini(&good_ser)); } + +// subscription_options.ignore_local_publications is enforced in +// drain_subscription() (rmw_subscription.cpp), which drops any message whose +// sender context id — embedded in WireHeader::gid by UdsGid::generate() — +// matches the subscription's owning rmw_context_t, before it reaches the queue. +TEST_F(PubSubTest, IgnoreLocalPublicationsDropsSameProcessMessage) +{ + auto pub_opts = rmw_get_default_publisher_options(); + pub = rmw_create_publisher(node, ts, "/ignore_local", &qos, &pub_opts); + ASSERT_NE(nullptr, pub); + + // Subscriber that wants to ignore messages from publishers in this process. + auto ignoring_sub_opts = rmw_get_default_subscription_options(); + ignoring_sub_opts.ignore_local_publications = true; + sub = rmw_create_subscription(node, ts, "/ignore_local", &qos, &ignoring_sub_opts); + ASSERT_NE(nullptr, sub); + + // Baseline subscriber with default options, same topic, same process: it + // must still receive the message so we know the publish itself worked. + auto default_sub_opts = rmw_get_default_subscription_options(); + rmw_subscription_t * baseline_sub = rmw_create_subscription( + node, ts, "/ignore_local", &qos, &default_sub_opts); + ASSERT_NE(nullptr, baseline_sub); + + test_msgs::msg::BasicTypes send_msg; + send_msg.int32_value = 123; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &send_msg, nullptr)); + + test_msgs::msg::BasicTypes recv_msg; + bool taken = false; + + EXPECT_EQ(RMW_RET_OK, rmw_take(baseline_sub, &recv_msg, &taken, nullptr)); + EXPECT_TRUE(taken) << "baseline subscriber (ignore_local_publications=false) " + "should still receive the same-process publication"; + + taken = false; + EXPECT_EQ(RMW_RET_OK, rmw_take(sub, &recv_msg, &taken, nullptr)); + EXPECT_FALSE(taken) << "ignore_local_publications=true must drop messages " + "published from within the same process/context"; + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_subscription(node, baseline_sub)); +} diff --git a/rmw_unix_socket_cpp/test/test_rmw_qos.cpp b/rmw_unix_socket_cpp/test/test_rmw_qos.cpp index 8695960..f2bd0da 100644 --- a/rmw_unix_socket_cpp/test/test_rmw_qos.cpp +++ b/rmw_unix_socket_cpp/test/test_rmw_qos.cpp @@ -14,11 +14,17 @@ #include "test_base.hpp" +#include +#include #include #include +#include +#include #include #include +#include +#include #include #include @@ -30,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 @@ -137,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"; @@ -198,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); @@ -266,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( @@ -280,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); @@ -289,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); } @@ -384,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); @@ -421,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( @@ -975,3 +868,705 @@ 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); +} + +// --- 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_rmw_service_client.cpp b/rmw_unix_socket_cpp/test/test_rmw_service_client.cpp index d99e2b0..298bdd7 100644 --- a/rmw_unix_socket_cpp/test/test_rmw_service_client.cpp +++ b/rmw_unix_socket_cpp/test/test_rmw_service_client.cpp @@ -240,6 +240,57 @@ TEST_F(ServiceClientTest, LargeRequestDeliveredThroughWaitDrain) EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); } +// Response delivery through rmw_wait: post-drain-only-ready-fds, a client +// socket is read only when epoll reports it in the wait, so the client arm of +// that dispatch is the sole path that makes a response visible to an executor. +// Drive it via a wait on the clients array rather than a direct take. +TEST_F(ServiceClientTest, ResponseDeliveredThroughWaitOnClient) +{ + srv = rmw_create_service(node, ts, "/wait_client_srv", &qos); + cli = rmw_create_client(node, ts, "/wait_client_srv", &qos); + ASSERT_NE(nullptr, srv); + ASSERT_NE(nullptr, cli); + + test_msgs::srv::BasicTypes::Request request; + request.int32_value = 55; + int64_t seq_id = 0; + EXPECT_EQ(RMW_RET_OK, rmw_send_request(cli, &request, &seq_id)); + + test_msgs::srv::BasicTypes::Request recv_request; + rmw_service_info_t request_header; + std::memset(&request_header, 0, sizeof(request_header)); + bool taken = false; + EXPECT_EQ(RMW_RET_OK, rmw_take_request(srv, &request_header, &recv_request, &taken)); + ASSERT_TRUE(taken); + + test_msgs::srv::BasicTypes::Response response; + response.int32_value = 110; + EXPECT_EQ(RMW_RET_OK, rmw_send_response(srv, &request_header.request_id, &response)); + + auto * ws = rmw_create_wait_set(&context, 1); + ASSERT_NE(nullptr, ws); + rmw_clients_t clients; + void * cli_array[1] = {cli->data}; + clients.clients = cli_array; + clients.client_count = 1; + rmw_time_t timeout{2, 0}; + ASSERT_EQ( + RMW_RET_OK, + rmw_wait(nullptr, nullptr, nullptr, &clients, nullptr, ws, &timeout)) << + "the response never made the client ready through rmw_wait"; + EXPECT_NE(nullptr, clients.clients[0]); + + test_msgs::srv::BasicTypes::Response recv_response; + rmw_service_info_t response_header; + std::memset(&response_header, 0, sizeof(response_header)); + taken = false; + EXPECT_EQ(RMW_RET_OK, rmw_take_response(cli, &response_header, &recv_response, &taken)); + ASSERT_TRUE(taken); + EXPECT_EQ(110, recv_response.int32_value); + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); +} + TEST_F(ServiceClientTest, SendResponseToGoneClientReturnsOk) { // A service that responds after its client has shut down must NOT return an diff --git a/rmw_unix_socket_cpp/test/test_rmw_wait.cpp b/rmw_unix_socket_cpp/test/test_rmw_wait.cpp index fe86ff3..66ca639 100644 --- a/rmw_unix_socket_cpp/test/test_rmw_wait.cpp +++ b/rmw_unix_socket_cpp/test/test_rmw_wait.cpp @@ -14,7 +14,11 @@ #include "test_base.hpp" +#include +#include #include +#include +#include #include "test_msgs/msg/basic_types.hpp" @@ -139,3 +143,647 @@ TEST_F(RmwUdsNodeTest, WaitWithSubscription) EXPECT_EQ(RMW_RET_OK, rmw_destroy_subscription(node, sub)); EXPECT_EQ(RMW_RET_OK, rmw_destroy_publisher(node, pub)); } + +// drain_socket() (rmw_wait.cpp) applies the same ignore_local_publications +// filter as drain_subscription(), so a subscription created with +// ignore_local_publications=true must not wake a wait set for a message +// published from within the same process/context. +TEST_F(RmwUdsNodeTest, WaitDoesNotWakeForIgnoredLocalPublication) +{ + auto * ts = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::BasicTypes>(); + + rmw_qos_profile_t qos; + std::memset(&qos, 0, sizeof(qos)); + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 10; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/wait_ignore_local", &qos, &pub_opts); + auto sub_opts = rmw_get_default_subscription_options(); + sub_opts.ignore_local_publications = true; + auto * sub = rmw_create_subscription(node, ts, "/wait_ignore_local", &qos, &sub_opts); + ASSERT_NE(nullptr, pub); + ASSERT_NE(nullptr, sub); + + auto * ws = rmw_create_wait_set(&context, 1); + ASSERT_NE(nullptr, ws); + + test_msgs::msg::BasicTypes msg; + msg.int32_value = 77; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &msg, nullptr)); + + rmw_subscriptions_t subscriptions; + void * sub_array[1] = {sub->data}; + subscriptions.subscribers = sub_array; + subscriptions.subscriber_count = 1; + + rmw_time_t timeout; + timeout.sec = 0; + timeout.nsec = 200000000; // 200 ms — no wakeup is expected + + rmw_ret_t ret = rmw_wait(&subscriptions, nullptr, nullptr, nullptr, nullptr, ws, &timeout); + EXPECT_EQ(RMW_RET_TIMEOUT, ret) << + "ignore_local_publications=true must not wake the wait set for a " + "same-context publication"; + EXPECT_EQ(nullptr, subscriptions.subscribers[0]); + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); + 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)); +} + +// A same-context publication dropped by ignore_local_publications wakes the +// epoll but leaves nothing to take. rmw_wait may return early, but only the +// caller's own deadline may produce RMW_RET_TIMEOUT. +TEST_F(RmwUdsNodeTest, IgnoredLocalPublicationIsNotReportedAsTimeout) +{ + auto * ts = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::BasicTypes>(); + + rmw_qos_profile_t qos; + std::memset(&qos, 0, sizeof(qos)); + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 10; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/ignore_local_no_timeout", &qos, &pub_opts); + auto sub_opts = rmw_get_default_subscription_options(); + sub_opts.ignore_local_publications = true; + auto * sub = rmw_create_subscription(node, ts, "/ignore_local_no_timeout", &qos, &sub_opts); + ASSERT_NE(nullptr, pub); + ASSERT_NE(nullptr, sub); + + auto * ws = rmw_create_wait_set(&context, 1); + ASSERT_NE(nullptr, ws); + + // Publish only once the wait is already blocked, so the drop happens on the + // post-epoll drain rather than the pre-epoll one. + std::thread publisher([&]() { + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + test_msgs::msg::BasicTypes m; + m.int32_value = 42; + rmw_publish(pub, &m, nullptr); + }); + + rmw_subscriptions_t subscriptions; + void * sub_array[1] = {sub->data}; + subscriptions.subscribers = sub_array; + subscriptions.subscriber_count = 1; + + rmw_time_t timeout; + timeout.sec = 2; + timeout.nsec = 0; + + auto t0 = std::chrono::steady_clock::now(); + rmw_ret_t ret = rmw_wait(&subscriptions, nullptr, nullptr, nullptr, nullptr, ws, &timeout); + auto elapsed_ms = std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count(); + publisher.join(); + + EXPECT_TRUE(ret != RMW_RET_TIMEOUT || elapsed_ms >= 1900) << + "rmw_wait reported RMW_RET_TIMEOUT after " << elapsed_ms << + " ms for a 2000 ms deadline"; + EXPECT_EQ(nullptr, subscriptions.subscribers[0]); + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_subscription(node, sub)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_publisher(node, pub)); +} + +// rmw_wait(..., nullptr) blocks indefinitely, so RMW_RET_TIMEOUT has no meaning +// for it: rclcpp's GraphListener treats that code as fatal. Dropping a +// same-context publication must not produce it. +TEST_F(RmwUdsNodeTest, InfiniteWaitNeverTimesOutOnIgnoredLocalPublication) +{ + auto * ts = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::BasicTypes>(); + + rmw_qos_profile_t qos; + std::memset(&qos, 0, sizeof(qos)); + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 10; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + + auto pub_opts = rmw_get_default_publisher_options(); + auto * pub = rmw_create_publisher(node, ts, "/ignore_local_infinite", &qos, &pub_opts); + auto sub_opts = rmw_get_default_subscription_options(); + sub_opts.ignore_local_publications = true; + auto * sub = rmw_create_subscription(node, ts, "/ignore_local_infinite", &qos, &sub_opts); + auto * gc = rmw_create_guard_condition(&context); // unblocks the wait on teardown + ASSERT_NE(nullptr, pub); + ASSERT_NE(nullptr, sub); + ASSERT_NE(nullptr, gc); + + auto * ws = rmw_create_wait_set(&context, 2); + ASSERT_NE(nullptr, ws); + + std::atomic returned{false}; + std::atomic ret{RMW_RET_OK}; + std::thread waiter([&]() { + rmw_subscriptions_t subs; + void * sub_array[1] = {sub->data}; + subs.subscribers = sub_array; + subs.subscriber_count = 1; + rmw_guard_conditions_t gcs; + void * gc_array[1] = {gc->data}; + gcs.guard_conditions = gc_array; + gcs.guard_condition_count = 1; + ret = rmw_wait(&subs, &gcs, nullptr, nullptr, nullptr, ws, nullptr); + returned = true; + }); + + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + test_msgs::msg::BasicTypes m; + m.int32_value = 7; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub, &m, nullptr)); + std::this_thread::sleep_for(std::chrono::milliseconds(400)); + + // Returning early with nothing ready is allowed; returning TIMEOUT is not. + if (returned.load()) { + EXPECT_NE(RMW_RET_TIMEOUT, ret.load()) << + "infinite rmw_wait returned RMW_RET_TIMEOUT after a dropped same-context " + "publication"; + } + + rmw_trigger_guard_condition(gc); + waiter.join(); + EXPECT_NE(RMW_RET_TIMEOUT, ret.load()); + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_guard_condition(gc)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_subscription(node, sub)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_publisher(node, pub)); +} + +// The armed-fd cache survives across rmw_wait calls. A guard condition armed +// by an earlier call but absent from THIS call's array (its node moved to +// another executor, its callback group is busy) must not have its trigger +// consumed by this wait: the wakeup belongs to whichever wait set holds the +// GC now, and consuming it here loses it forever. +TEST_F(RmwUdsNodeTest, WaitDoesNotStealTriggerOfGuardConditionNotWaitedOn) +{ + auto * g1 = rmw_create_guard_condition(&context); + auto * g2 = rmw_create_guard_condition(&context); + auto * ws = rmw_create_wait_set(&context, 2); + ASSERT_NE(nullptr, g1); + ASSERT_NE(nullptr, g2); + ASSERT_NE(nullptr, ws); + + // Arm both fds in ws (neither is triggered, so this times out). + { + void * both[2] = {g1->data, g2->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = both; + gcs.guard_condition_count = 2; + rmw_time_t t{0, 20000000}; // 20 ms + EXPECT_EQ( + RMW_RET_TIMEOUT, + rmw_wait(nullptr, &gcs, nullptr, nullptr, nullptr, ws, &t)); + } + + // Wait on g1 alone while g2 fires mid-wait. g2's still-armed fd reports + // ready in this wait set, but g2 is not in this call's array: the wait must + // neither consume the trigger nor end early on it. + std::thread trigger([&] { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + auto _r [[maybe_unused]] = rmw_trigger_guard_condition(g2); + }); + { + void * only_g1[1] = {g1->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = only_g1; + gcs.guard_condition_count = 1; + rmw_time_t t{0, 300000000}; // 300 ms + auto t0 = std::chrono::steady_clock::now(); + rmw_ret_t ret = rmw_wait(nullptr, &gcs, nullptr, nullptr, nullptr, ws, &t); + auto elapsed_ms = std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count(); + trigger.join(); + EXPECT_EQ(RMW_RET_TIMEOUT, ret) << + "a guard condition outside this call's array ended the wait"; + EXPECT_EQ(nullptr, gcs.guard_conditions[0]); + EXPECT_GE(elapsed_ms, 250) << "the wait did not re-block after g2 fired"; + } + + // The trigger must still be pending for a wait that DOES hold g2. + { + void * only_g2[1] = {g2->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = only_g2; + gcs.guard_condition_count = 1; + rmw_time_t t{0, 100000000}; // 100 ms + EXPECT_EQ( + RMW_RET_OK, + rmw_wait(nullptr, &gcs, nullptr, nullptr, nullptr, ws, &t)) << + "g2's trigger was consumed by a wait that was not waiting on it"; + EXPECT_NE(nullptr, gcs.guard_conditions[0]); + } + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_guard_condition(g1)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_guard_condition(g2)); +} + +// The armed cache is keyed by fd; the uid comparison is what tells "the same +// fd re-armed for the same entity" from "the fd number recycled to a new +// entity after a close". Recycle an eventfd number into a new GC on the same +// wait set: the new GC must be re-armed and still wake the wait. +TEST_F(RmwUdsNodeTest, RecycledFdNumberIsRearmedForNewEntity) +{ + auto * ws = rmw_create_wait_set(&context, 1); + ASSERT_NE(nullptr, ws); + + auto wait_on = [&](rmw_guard_condition_t * gc, rmw_time_t t) { + void * arr[1] = {gc->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = arr; + gcs.guard_condition_count = 1; + rmw_ret_t ret = rmw_wait(nullptr, &gcs, nullptr, nullptr, nullptr, ws, &t); + return ret == RMW_RET_OK && gcs.guard_conditions[0] != nullptr; + }; + + auto * g1 = rmw_create_guard_condition(&context); + ASSERT_NE(nullptr, g1); + const int fd1 = static_cast(g1->data)->eventfd_fd; + EXPECT_FALSE(wait_on(g1, rmw_time_t{0, 20000000})); // arms fd1 for g1 + EXPECT_EQ(RMW_RET_OK, rmw_destroy_guard_condition(g1)); + + // Provoke fd-number reuse: the kernel hands out the lowest free descriptor, + // so the next eventfd normally lands on fd1 at once. Keep non-matching + // candidates alive so retries do not just get the same number back. + rmw_guard_condition_t * g2 = nullptr; + std::vector decoys; + for (int i = 0; i < 32 && !g2; ++i) { + auto * cand = rmw_create_guard_condition(&context); + ASSERT_NE(nullptr, cand); + if (static_cast(cand->data)->eventfd_fd == fd1) { + g2 = cand; + } else { + decoys.push_back(cand); + } + } + for (auto * d : decoys) { + EXPECT_EQ(RMW_RET_OK, rmw_destroy_guard_condition(d)); + } + if (!g2) { + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); + GTEST_SKIP() << "eventfd number was not recycled; nothing to pin"; + } + + std::thread trigger([&] { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + auto _r [[maybe_unused]] = rmw_trigger_guard_condition(g2); + }); + const bool woke = wait_on(g2, rmw_time_t{2, 0}); + trigger.join(); + EXPECT_TRUE(woke) << + "a recycled fd number kept its stale arming; the new entity never wakes"; + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_guard_condition(g2)); +} + +// When an entity already holds queued-but-untaken data, the wait makes one +// non-blocking epoll pass instead of blocking: an entity whose data still +// sits unread in its socket must be reported in this same call, and the +// caller's timeout must not be consumed by a wait that already has work. +TEST_F(RmwUdsNodeTest, QueuedBacklogStillReportsSocketDataWithoutBlocking) +{ + auto * ts = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::BasicTypes>(); + rmw_qos_profile_t qos = rmw_qos_profile_default; + auto pub_opts = rmw_get_default_publisher_options(); + auto sub_opts = rmw_get_default_subscription_options(); + + auto * pub_x = rmw_create_publisher(node, ts, "/backlog_x", &qos, &pub_opts); + auto * sub_x = rmw_create_subscription(node, ts, "/backlog_x", &qos, &sub_opts); + auto * pub_y = rmw_create_publisher(node, ts, "/backlog_y", &qos, &pub_opts); + auto * sub_y = rmw_create_subscription(node, ts, "/backlog_y", &qos, &sub_opts); + ASSERT_NE(nullptr, pub_x); + ASSERT_NE(nullptr, sub_x); + ASSERT_NE(nullptr, pub_y); + ASSERT_NE(nullptr, sub_y); + + auto * ws = rmw_create_wait_set(&context, 2); + ASSERT_NE(nullptr, ws); + + // Two messages for X; wait on X alone so both land in X's queue, take one. + test_msgs::msg::BasicTypes m; + m.int32_value = 1; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub_x, &m, nullptr)); + m.int32_value = 2; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub_x, &m, nullptr)); + { + rmw_subscriptions_t subs; + void * arr[1] = {sub_x->data}; + subs.subscribers = arr; + subs.subscriber_count = 1; + rmw_time_t t{1, 0}; + ASSERT_EQ( + RMW_RET_OK, rmw_wait(&subs, nullptr, nullptr, nullptr, nullptr, ws, &t)); + } + test_msgs::msg::BasicTypes recv; + bool taken = false; + ASSERT_EQ(RMW_RET_OK, rmw_take(sub_x, &recv, &taken, nullptr)); + ASSERT_TRUE(taken); + + // One message for Y, left sitting in Y's socket (Y was never waited on). + m.int32_value = 3; + EXPECT_EQ(RMW_RET_OK, rmw_publish(pub_y, &m, nullptr)); + + // X's leftover queue makes the wait poll-only; Y's socket data must still be + // reported in this call, well before the 5 s deadline. + rmw_subscriptions_t subs; + void * arr[2] = {sub_x->data, sub_y->data}; + subs.subscribers = arr; + subs.subscriber_count = 2; + rmw_time_t t{5, 0}; + auto t0 = std::chrono::steady_clock::now(); + rmw_ret_t ret = rmw_wait(&subs, nullptr, nullptr, nullptr, nullptr, ws, &t); + auto elapsed_ms = std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count(); + + EXPECT_EQ(RMW_RET_OK, ret); + EXPECT_NE(nullptr, subs.subscribers[0]) << "X's queued backlog went unreported"; + EXPECT_NE(nullptr, subs.subscribers[1]) << + "Y's socket data was missed by the poll-only pass"; + EXPECT_LT(elapsed_ms, 1000) << + "a wait with queued work blocked instead of polling"; + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_subscription(node, sub_x)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_subscription(node, sub_y)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_publisher(node, pub_x)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_publisher(node, pub_y)); +} + +// A doorbell-only wake carries no caller-visible work: under continuous +// registry churn a bounded wait must keep re-blocking and still time out at +// the caller's deadline — not before it, and not spinning past it. +TEST_F(RmwUdsNodeTest, BoundedWaitUnderRegistryChurnTimesOutOnSchedule) +{ + // Register the context's doorbell first: it is lazily registered by the + // first wait whose set holds one of this context's graph guard conditions. + const rmw_guard_condition_t * graph_gc = rmw_node_get_graph_guard_condition(node); + ASSERT_NE(nullptr, graph_gc); + auto * ws_reg = rmw_create_wait_set(&context, 1); + ASSERT_NE(nullptr, ws_reg); + { + void * arr[1] = {graph_gc->data}; + rmw_guard_conditions_t gcs; + gcs.guard_conditions = arr; + gcs.guard_condition_count = 1; + rmw_time_t t{0, 20000000}; // 20 ms + auto _r [[maybe_unused]] = rmw_wait( + nullptr, &gcs, nullptr, nullptr, nullptr, ws_reg, &t); + } + + auto * ts = 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, "/churn_quiet", &qos, &sub_opts); + ASSERT_NE(nullptr, sub); + + std::atomic stop{false}; + std::thread churn([&] { + auto pub_opts = rmw_get_default_publisher_options(); + auto * ts_c = rosidl_typesupport_cpp::get_message_type_support_handle< + test_msgs::msg::BasicTypes>(); + rmw_qos_profile_t qos_c = rmw_qos_profile_default; + while (!stop.load()) { + auto * p = rmw_create_publisher(node, ts_c, "/churn_topic", &qos_c, &pub_opts); + if (p) { + auto _r [[maybe_unused]] = rmw_destroy_publisher(node, p); + } + } + }); + + auto * ws = rmw_create_wait_set(&context, 1); + ASSERT_NE(nullptr, ws); + rmw_subscriptions_t subs; + void * sub_arr[1] = {sub->data}; + subs.subscribers = sub_arr; + subs.subscriber_count = 1; + rmw_time_t t{0, 600000000}; // 600 ms, no traffic on the subscription + auto t0 = std::chrono::steady_clock::now(); + rmw_ret_t ret = rmw_wait(&subs, nullptr, nullptr, nullptr, nullptr, ws, &t); + auto elapsed_ms = std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count(); + stop.store(true); + churn.join(); + + EXPECT_EQ(RMW_RET_TIMEOUT, ret) << + "registry churn ended a wait with nothing ready"; + EXPECT_EQ(nullptr, subs.subscribers[0]); + EXPECT_GE(elapsed_ms, 550) << "churn wakes ate into the caller's deadline"; + EXPECT_LE(elapsed_ms, 1500) << "the wait overshot the deadline"; + + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_wait_set(ws_reg)); + EXPECT_EQ(RMW_RET_OK, rmw_destroy_subscription(node, sub)); +} 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); +} diff --git a/rmw_unix_socket_cpp/test/test_transport.cpp b/rmw_unix_socket_cpp/test/test_transport.cpp index 3ff1390..2fde129 100644 --- a/rmw_unix_socket_cpp/test/test_transport.cpp +++ b/rmw_unix_socket_cpp/test/test_transport.cpp @@ -14,10 +14,13 @@ #include +#include #include #include #include +#include #include +#include #include #include "../src/transport.hpp" @@ -100,6 +103,87 @@ TEST_F(TransportTest, RecvFromEmptyReturnsF) rmw_uds::close_socket(fd, path); } +TEST_F(TransportTest, RecvFromConsumesZeroLengthDatagram) +{ + auto path = rmw_uds::make_socket_path(domain_id, "zerolen"); + int recv_fd = rmw_uds::create_bound_socket(path); + ASSERT_GE(recv_fd, 0); + + int send_fd = rmw_uds::create_send_socket(); + ASSERT_GE(send_fd, 0); + + struct sockaddr_un addr; + std::memset(&addr, 0, sizeof(addr)); + addr.sun_family = AF_UNIX; + std::strncpy(addr.sun_path, path.c_str(), sizeof(addr.sun_path) - 1); + ASSERT_EQ( + 0, sendto( + send_fd, nullptr, 0, 0, + reinterpret_cast(&addr), sizeof(addr))); + + // A zero-length datagram carries no WireHeader, so it is not a message. + rmw_uds::WireHeader hdr; + std::vector payload; + EXPECT_FALSE(rmw_uds::recv_from(recv_fd, hdr, payload)); + + // It must still have been consumed. MSG_PEEK does not dequeue, so leaving it + // would keep the socket readable forever and spin every wait polling this fd. + char probe = 0; + errno = 0; + const ssize_t left = recv(recv_fd, &probe, sizeof(probe), MSG_DONTWAIT | MSG_PEEK); + EXPECT_EQ(-1, left) << "zero-length datagram was left queued on the socket"; + EXPECT_TRUE(errno == EAGAIN || errno == EWOULDBLOCK); + + close(send_fd); + rmw_uds::close_socket(recv_fd, path); +} + +// A junk datagram must not end a drain: with a zero-length datagram queued +// ahead of a real message, a single recv_from call must consume the junk and +// return the real message. Every drain loop stops on the first false, so +// returning false here would leave the real message stranded until the next +// wake. +TEST_F(TransportTest, RecvFromSkipsZeroLengthDatagramToRealMessage) +{ + auto path = rmw_uds::make_socket_path(domain_id, "zerolen_then_real"); + int recv_fd = rmw_uds::create_bound_socket(path); + ASSERT_GE(recv_fd, 0); + + int send_fd = rmw_uds::create_send_socket(); + ASSERT_GE(send_fd, 0); + + struct sockaddr_un addr; + std::memset(&addr, 0, sizeof(addr)); + addr.sun_family = AF_UNIX; + std::strncpy(addr.sun_path, path.c_str(), sizeof(addr.sun_path) - 1); + ASSERT_EQ( + 0, sendto( + send_fd, nullptr, 0, 0, + reinterpret_cast(&addr), sizeof(addr))); + + rmw_uds::WireHeader send_hdr; + std::memset(&send_hdr, 0, sizeof(send_hdr)); + send_hdr.sequence_number = 7; + std::vector payload = {9, 8, 7}; + send_hdr.payload_size = static_cast(payload.size()); + ASSERT_EQ( + rmw_uds::SendResult::Ok, + rmw_uds::send_to(send_fd, path, send_hdr, payload.data(), payload.size())); + + rmw_uds::WireHeader hdr; + std::vector recv_payload; + EXPECT_TRUE(rmw_uds::recv_from(recv_fd, hdr, recv_payload)) << + "the zero-length datagram terminated the drain with a real message queued"; + EXPECT_EQ(7, hdr.sequence_number); + EXPECT_EQ(payload, recv_payload); + + // Nothing left: the junk was consumed, not skipped over. + EXPECT_FALSE(rmw_uds::recv_from(recv_fd, hdr, recv_payload)); + + close(send_fd); + rmw_uds::close_socket(recv_fd, path); +} + TEST_F(TransportTest, MultipleMessages) { auto path = rmw_uds::make_socket_path(domain_id, "multi");