fix(rmw_wait): keep the wait timeout's sub-millisecond resolution

rmw_unix_socket_cpp/DESIGN.md

@@ -534,7 +534,7 @@ Each `rmw_wait()` runs the same sequence on the calling thread:
 1. **Drain first.** Every subscription, service, and client socket is drained into a per-entity message queue before anything blocks. The receive sockets are `SOCK_DGRAM | SOCK_NONBLOCK`, so the drain loop calls `recv_from` repeatedly and stops cleanly on `EAGAIN` (nothing left to read). This step exists because data may have arrived between the previous `rmw_wait()` and this one; draining up front means such data is not missed.
 2. **Check the graph.** The wait set carries its context (stored at `rmw_create_wait_set`), so this step runs for every wait — including a wait set that holds only guard conditions, which is exactly the shape rclcpp's GraphListener uses. The check first drains the context's doorbell socket, then reads the registry's `generation` counter and compares it against the value cached on the context. If it moved, the graph changed: 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.
+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_pwait2()`, which takes the caller's timeout with nanosecond resolution so a sub-millisecond wait (the remainder of a 1 kHz timer period) is neither stretched nor cut to a whole millisecond. Where `epoll_pwait2()` is unavailable (glibc < 2.35, kernel < 5.11, or a seccomp profile that blocks it) it falls back to `epoll_wait()` with the timeout rounded up to the next millisecond. 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. The 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

rmw_unix_socket_cpp/src/rmw_wait.cpp

@@ -20,9 +20,11 @@
 
 #include <algorithm>
 #include <array>
+#include <atomic>
 #include <cerrno>
 #include <chrono>
 #include <cstring>
+#include <ctime>
 #include <limits>
 #include <map>
 #include <unordered_map>
@@ -50,6 +52,36 @@ static int64_t wall_now_ns()
     std::chrono::system_clock::now().time_since_epoch()).count();
 }
 
+// Block in epoll for up to `timeout_ns` nanoseconds; -1 blocks forever.
+// epoll_pwait2 (Linux 5.11, glibc 2.35) takes the timeout with nanosecond
+// resolution. epoll_wait only takes milliseconds, so it is the fallback where
+// epoll_pwait2 is missing: an older glibc at build time, or an older kernel
+// (ENOSYS) or a container seccomp profile that blocks it (EPERM) at run time.
+static int epoll_wait_ns(
+  int epfd, struct epoll_event * events, int max_events, int64_t timeout_ns)
+{
+#if defined(__GLIBC__) && (__GLIBC__ > 2 || (__GLIBC__ == 2 && __GLIBC_MINOR__ >= 35))
+  static std::atomic<bool> pwait2_unavailable{false};
+  if (!pwait2_unavailable.load(std::memory_order_relaxed)) {
+    struct timespec ts;
+    ts.tv_sec = static_cast<time_t>(timeout_ns / 1000000000);
+    ts.tv_nsec = static_cast<decltype(ts.tv_nsec)>(timeout_ns % 1000000000);
+    int n = epoll_pwait2(epfd, events, max_events, timeout_ns < 0 ? nullptr : &ts, nullptr);
+    if (n >= 0 || (errno != ENOSYS && errno != EPERM)) {
+      return n;
+    }
+    pwait2_unavailable.store(true, std::memory_order_relaxed);
+  }
+#endif
+  // Round up so the fallback never returns before the caller's deadline.
+  int timeout_ms = -1;
+  if (timeout_ns >= 0) {
+    timeout_ms = static_cast<int>(std::min<int64_t>(
+      (timeout_ns + 999999) / 1000000, std::numeric_limits<int>::max()));
+  }
+  return epoll_wait(epfd, events, max_events, timeout_ms);
+}
+
 // 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
@@ -470,19 +502,17 @@ rmw_ret_t rmw_wait(
     }
   }
 
-  // Compute timeout. -1 means block forever (epoll_wait sentinel).
-  int timeout_ms = -1;
+  // Compute timeout in nanoseconds. -1 means block forever.
+  int64_t timeout_ns = -1;
   if (wait_timeout) {
     // Accumulate in int64_t; RMW_DURATION_INFINITE (~9.2e12 ms) overflows int.
     int64_t ms = static_cast<int64_t>(wait_timeout->sec) * 1000 +
       static_cast<int64_t>(wait_timeout->nsec) / 1000000;
     if (ms > std::numeric_limits<int>::max()) {
-      timeout_ms = -1;  // Infinite (or beyond epoll's range) -> block forever
+      timeout_ns = -1;  // Infinite (or beyond epoll's range) -> block forever
     } else {
-      timeout_ms = static_cast<int>(ms);
-      if (timeout_ms == 0 && wait_timeout->nsec > 0) {
-        timeout_ms = 1;  // At least 1ms
-      }
+      timeout_ns = static_cast<int64_t>(wait_timeout->sec) * 1000000000 +
+        static_cast<int64_t>(wait_timeout->nsec);
     }
   }
 
@@ -495,9 +525,9 @@ rmw_ret_t rmw_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 bool infinite = (timeout_ns < 0);
   const int64_t caller_deadline_ns =
-    infinite ? 0 : steady_now_ns() + static_cast<int64_t>(timeout_ms) * 1000000;
+    infinite ? 0 : steady_now_ns() + timeout_ns;
   // 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
@@ -508,26 +538,24 @@ rmw_ret_t rmw_wait(
   {
     struct epoll_event ready_events[64];
     while (true) {
-      int block_ms = 0;
+      int64_t block_ns = 0;
       if (!poll_only) {
-        block_ms = -1;
+        block_ns = -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<int>(
-            std::min<int64_t>(rem_ms, std::numeric_limits<int>::max()));
+          block_ns = (rem_ns > 0) ? rem_ns : 0;
         }
         if ((graph_gc_unwired || !unarmed.empty()) &&
-          (block_ms < 0 || block_ms > 200))
+          (block_ns < 0 || block_ns > 200000000))
         {
           // 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;
+          block_ns = 200000000;  // 200 ms
         }
       }
-      int n = epoll_wait(ws_data->epoll_fd, ready_events, 64, block_ms);
+      int n = epoll_wait_ns(ws_data->epoll_fd, ready_events, 64, block_ns);
       if (n < 0) {
         if (errno == EINTR) {
           continue;

rmw_unix_socket_cpp/test/README.md

@@ -130,6 +130,8 @@ Tests the epoll-based wait set.
 | `WaitWithGuardCondition` | Pre-triggered guard condition causes immediate return with `RMW_RET_OK` | Validates the "already ready" fast path — avoids unnecessary epoll_wait when data is already available. |
 | `WaitTimeoutWhenNoData` | Returns `RMW_RET_TIMEOUT` after specified duration, guard condition nulled out | Validates the timeout path. Without this, the executor would hang indefinitely when no messages arrive. |
 | `WaitWithSubscription` | Publish → wait on subscription → returns ready → take succeeds | End-to-end: message arrives on socket → epoll detects it → drain into queue → subscription marked ready → take succeeds. Tests the full executor wakeup path. |
+| `WaitKeepsSubMillisecondTimeoutResolution` | 0.3 ms and 1.7 ms waits return `RMW_RET_TIMEOUT` no earlier than the timeout and less than 0.5 ms after it | rcl passes the time to the next timer in nanoseconds. Rounding it to whole milliseconds made a 1.7 ms wait end after 1 ms (a false timeout) and a 0.3 ms wait take 1 ms. |
+| `OneKilohertzWaitLoopStaysOnSchedule` | A 1 kHz loop of deadline-based `rmw_wait` calls wakes within 0.5 ms of each period in at least 95 % of 500 periods | This is what an executor does for a 1 kHz timer. With millisecond rounding the loop slid behind schedule and rclcpp skipped periods. |
 
 ### test_rmw_graph.cpp
 

rmw_unix_socket_cpp/test/test_rmw_wait.cpp

@@ -14,9 +14,11 @@
 
 #include "test_base.hpp"
 
+#include <algorithm>
 #include <atomic>
 #include <chrono>
 #include <cstring>
+#include <limits>
 #include <thread>
 #include <vector>
 
@@ -299,6 +301,87 @@ TEST_F(RmwUdsNodeTest, WaitBlocksForFullCallerTimeout)
   auto _r2 [[maybe_unused]] = rmw_destroy_guard_condition(gc);
 }
 
+TEST_F(RmwUdsNodeTest, WaitKeepsSubMillisecondTimeoutResolution)
+{
+  // Scenario: rcl hands rmw_wait the time left until the next timer with
+  // nanosecond resolution. Rounding it to whole milliseconds makes a short
+  // wait return early (1.7 ms -> 1 ms, a false timeout) or late (0.3 ms ->
+  // 1 ms, a late timer). The wait must last at least the caller's timeout and
+  // must not be stretched to the next millisecond.
+  auto * gc = rmw_create_guard_condition(&context);
+  ASSERT_NE(nullptr, gc);
+  auto * ws = rmw_create_wait_set(&context, 1);
+  ASSERT_NE(nullptr, ws);
+
+  for (const int64_t timeout_ns : {300000, 1700000}) {  // 0.3 ms and 1.7 ms
+    int64_t shortest_ns = std::numeric_limits<int64_t>::max();
+    for (int i = 0; i < 20; ++i) {
+      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, static_cast<uint64_t>(timeout_ns)};  // never triggered
+      auto t0 = std::chrono::steady_clock::now();
+      rmw_ret_t ret = rmw_wait(nullptr, &gcs, nullptr, nullptr, nullptr, ws, &timeout);
+      int64_t elapsed_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::steady_clock::now() - t0).count();
+
+      EXPECT_EQ(RMW_RET_TIMEOUT, ret);
+      EXPECT_GE(elapsed_ns, timeout_ns) <<
+        "rmw_wait returned TIMEOUT before the caller's " << timeout_ns << " ns deadline";
+      shortest_ns = std::min(shortest_ns, elapsed_ns);
+    }
+    // The shortest of 20 waits is immune to an occasional preemption.
+    EXPECT_LT(shortest_ns, timeout_ns + 500000) <<
+      "a " << timeout_ns << " ns wait took at least " << shortest_ns << " ns";
+  }
+
+  auto _r1 [[maybe_unused]] = rmw_destroy_wait_set(ws);
+  auto _r2 [[maybe_unused]] = rmw_destroy_guard_condition(gc);
+}
+
+TEST_F(RmwUdsNodeTest, OneKilohertzWaitLoopStaysOnSchedule)
+{
+  // Scenario: an executor running a 1 kHz timer calls rmw_wait with whatever
+  // is left of the current 1 ms period. If that remainder is rounded up to a
+  // whole millisecond, each wake is late by the part of the period already
+  // spent, so the loop slides behind schedule and rclcpp skips periods.
+  auto * gc = rmw_create_guard_condition(&context);
+  ASSERT_NE(nullptr, gc);
+  auto * ws = rmw_create_wait_set(&context, 1);
+  ASSERT_NE(nullptr, ws);
+
+  constexpr int kPeriods = 500;
+  constexpr int64_t kPeriodNs = 1000000;  // 1 kHz
+  auto now_ns = [] {
+      return std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::steady_clock::now().time_since_epoch()).count();
+    };
+
+  int late = 0;
+  int64_t next_ns = now_ns();
+  for (int i = 0; i < kPeriods; ++i) {
+    next_ns += kPeriodNs;
+    void * gc_array[1] = {gc->data};
+    rmw_guard_conditions_t gcs;
+    gcs.guard_conditions = gc_array;
+    gcs.guard_condition_count = 1;
+
+    const int64_t left_ns = std::max<int64_t>(0, next_ns - now_ns());
+    rmw_time_t timeout{0, static_cast<uint64_t>(left_ns)};  // never triggered
+    auto _r [[maybe_unused]] = rmw_wait(nullptr, &gcs, nullptr, nullptr, nullptr, ws, &timeout);
+    if (now_ns() - next_ns > kPeriodNs / 2) {
+      ++late;
+    }
+  }
+  EXPECT_LT(late, kPeriods / 20) <<
+    late << " of " << kPeriods << " periods woke more than 0.5 ms late";
+
+  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