fix(rmw_wait): event-driven wakeup via a registry doorbell; remove the 200 ms poll

.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:

rmw_unix_socket_cpp/CMakeLists.txt

@@ -207,6 +207,11 @@ if(BUILD_TESTING)
   target_link_libraries(test_rmw_qos ${_rmw_test_libs} ${_test_msg_deps})
   target_include_directories(test_rmw_qos PRIVATE src)
 
+  # White-box: TL replay-cache pruning (deliberately inspects publisher internals)
+  ament_add_gtest(test_internal_publisher_cache test/test_internal_publisher_cache.cpp)
+  target_link_libraries(test_internal_publisher_cache ${_rmw_test_libs} ${_test_msg_deps})
+  target_include_directories(test_internal_publisher_cache PRIVATE src)
+
   # Integration: fork()-based two-process tests pushing >4 MB payloads across
   # a real PID boundary through the shared-memory path.
   ament_add_gtest(test_rmw_cross_process test/test_rmw_cross_process.cpp)

rmw_unix_socket_cpp/src/registry.cpp

@@ -21,14 +21,19 @@
 
 #include <fcntl.h>
 #include <sys/mman.h>
+#include <sys/socket.h>
 #include <sys/stat.h>
+#include <sys/un.h>
 #include <unistd.h>
 
 #include "logging.hpp"
 
 namespace rmw_uds
 {
 
+// Defined below; called after every generation bump (see its comment).
+static void ring_doorbells(RegistryHeader * header);
+
 // Lock-free atomics in shared memory require both lock-freedom AND
 // address-freedom. On every Linux target we support these hold; assert at
 // compile time so we fail loud on exotic platforms.
@@ -276,6 +281,7 @@ static int32_t try_add_once(RegistryHeader * header, const RegistryEntry & entry
              !header->high_water_slot.compare_exchange_weak(
                cur, want, std::memory_order_relaxed, std::memory_order_relaxed)) {}
       header->generation.fetch_add(1, std::memory_order_acq_rel);
+      ring_doorbells(header);  // strictly after the bump — see ring_doorbells
       return static_cast<int32_t>(i);
     }
   }
@@ -345,6 +351,7 @@ void registry_remove(RegistryHeader * header, int32_t index)
   }
   teardown_slot(slot);
   header->generation.fetch_add(1, std::memory_order_acq_rel);
+  ring_doorbells(header);  // strictly after the bump — see ring_doorbells
 }
 
 // Best-effort: stat /proc/<pid>. ENOENT means the PID is not in our
@@ -372,13 +379,96 @@ static const char * entry_type_name(uint8_t t)
     case ENTRY_SUBSCRIPTION: return "subscription";
     case ENTRY_SERVICE: return "service";
     case ENTRY_CLIENT: return "client";
+    case ENTRY_DOORBELL: return "doorbell";
     default: return "?";
   }
 }
 
+// Ring every registered doorbell (one octet, best-effort) so processes blocked
+// in rmw_wait re-check the registry. Called strictly AFTER a generation bump:
+// paired with rmw_wait draining its doorbell strictly BEFORE reading the
+// generation, every mutation either lands in the pre-block generation read or
+// leaves a queued datagram on a level-triggered fd — no lost wakeup. EAGAIN
+// means the peer already has a wakeup queued; other send errors mean a dead
+// peer whose slot will be reclaimed. Scans slots directly (not registry_query,
+// which calls back into cleanup and would recurse).
+static void ring_doorbells(RegistryHeader * header)
+{
+  // One ring socket per mutating thread, closed at thread exit. AF_UNIX
+  // datagrams stay charged to the SENDER's buffer until the receiver consumes
+  // them, so a peer that is slow to drain could exhaust this fd's budget and
+  // make sendto fail for every OTHER peer too; the recreate-on-EAGAIN below
+  // resets that budget. On a fresh fd, EAGAIN can only mean the destination's
+  // own queue is full — a wakeup is already pending there, so the drop is safe.
+  struct RingFd
+  {
+    int fd = -1;
+    ~RingFd() {if (fd >= 0) {close(fd);}}
+  };
+  static thread_local RingFd ring;
+  if (ring.fd < 0) {
+    ring.fd = socket(AF_UNIX, SOCK_DGRAM | SOCK_NONBLOCK | SOCK_CLOEXEC, 0);
+    if (ring.fd < 0) {
+      return;
+    }
+  }
+  auto * slots = registry_slots(header);
+  const uint32_t hi = header->high_water_slot.load(std::memory_order_acquire);
+  for (uint32_t i = 0; i < hi; ++i) {
+    if (slots[i].state.load(std::memory_order_acquire) !=
+      static_cast<uint8_t>(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<uint8_t>(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<const struct sockaddr *>(&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<const struct sockaddr *>(&addr), sizeof(addr));
+    }
+  }
+}
+
 void registry_cleanup_stale(RegistryHeader * header)
 {
   auto * slots = registry_slots(header);
+  bool reclaimed = false;
   // Scan only [0, high_water): over-scan is safe, under-scan is impossible.
   uint32_t hw = header->high_water_slot.load(std::memory_order_acquire);
   uint32_t max = header->max_entries;
@@ -435,6 +525,10 @@ void registry_cleanup_stale(RegistryHeader * header)
 
     teardown_slot(&slots[i]);
     header->generation.fetch_add(1, std::memory_order_acq_rel);
+    reclaimed = true;
+  }
+  if (reclaimed) {
+    ring_doorbells(header);  // strictly after the bump(s) — see ring_doorbells
   }
 }
 

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

rmw_unix_socket_cpp/src/rmw_init.cpp

@@ -184,8 +184,44 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
     "rmw_init: context up (domain_id=%zu, pid=%d)",
     domain_id, static_cast<int>(getpid()));
 
-  // Read initial generation
   auto * header = rmw_uds::registry_header(ctx->registry_ptr);
+
+  // Doorbell: bind + register BEFORE the first generation snapshot below, so
+  // no registry mutation can land in the gap between the snapshot and the
+  // wakeup wiring (a mutation after registration rings this socket; one
+  // before it is covered by the snapshot).
+  {
+    const std::string ctl_path = rmw_uds::make_socket_path(domain_id, "ctl");
+    ctx->doorbell_fd = rmw_uds::create_bound_socket(ctl_path);
+    if (ctx->doorbell_fd < 0) {
+      RMW_UDS_LOG_ERROR(
+        "rmw_init: failed to create doorbell socket (domain_id=%zu)", domain_id);
+      close(ctx->send_socket_fd);
+      rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size);
+      delete ctx;
+      RMW_SET_ERROR_MSG("failed to create doorbell socket");
+      return RMW_RET_ERROR;
+    }
+    rmw_uds::RegistryEntry dentry;
+    std::memset(&dentry, 0, sizeof(dentry));
+    dentry.type = rmw_uds::ENTRY_DOORBELL;
+    dentry.pid = getpid();
+    std::strncpy(dentry.socket_path, ctl_path.c_str(), sizeof(dentry.socket_path) - 1);
+    ctx->doorbell_registry_index = rmw_uds::registry_add(header, dentry);
+    if (ctx->doorbell_registry_index < 0) {
+      RMW_UDS_LOG_ERROR(
+        "rmw_init: registry full — cannot register doorbell (domain_id=%zu)", domain_id);
+      close(ctx->doorbell_fd);
+      unlink(ctl_path.c_str());
+      close(ctx->send_socket_fd);
+      rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size);
+      delete ctx;
+      RMW_SET_ERROR_MSG("registry full — cannot register doorbell");
+      return RMW_RET_ERROR;
+    }
+  }
+
+  // Read initial generation
   ctx->last_registry_generation.store(
     rmw_uds::registry_generation(header), std::memory_order_relaxed);
 
@@ -195,6 +231,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
   if (options->enclave) {
     enclave_copy = rcutils_strdup(options->enclave, options->allocator);
     if (!enclave_copy) {
+      rmw_uds::registry_remove(header, ctx->doorbell_registry_index);
+      close(ctx->doorbell_fd);
       close(ctx->send_socket_fd);
       rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size);
       delete ctx;
@@ -220,6 +258,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
     if (enclave_copy) {
       options->allocator.deallocate(enclave_copy, options->allocator.state);
     }
+    rmw_uds::registry_remove(header, ctx->doorbell_registry_index);
+    close(ctx->doorbell_fd);
     close(ctx->send_socket_fd);
     rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size);
     delete ctx;
@@ -255,6 +295,15 @@ rmw_ret_t rmw_context_fini(rmw_context_t * context)
 
   auto * ctx = reinterpret_cast<rmw_uds::UdsContext *>(context->impl);
   if (ctx) {
+    // Doorbell teardown before the registry unmaps: registry_remove's slot
+    // teardown also unlinks the socket file.
+    if (ctx->doorbell_registry_index >= 0 && ctx->registry_ptr) {
+      auto * header = rmw_uds::registry_header(ctx->registry_ptr);
+      rmw_uds::registry_remove(header, ctx->doorbell_registry_index);
+    }
+    if (ctx->doorbell_fd >= 0) {
+      close(ctx->doorbell_fd);
+    }
     if (ctx->send_socket_fd >= 0) {
       close(ctx->send_socket_fd);
     }

rmw_unix_socket_cpp/src/rmw_node.cpp

@@ -16,7 +16,9 @@
 #include "registry.hpp"
 #include "types.hpp"
 
+#include <algorithm>
 #include <cstring>
+#include <mutex>
 
 #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<std::mutex> 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<std::mutex> 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);
     }