diff --git a/.agent/work-plans/issue-10/plan.md b/.agent/work-plans/issue-10/plan.md new file mode 100644 index 0000000..1ee4e79 --- /dev/null +++ b/.agent/work-plans/issue-10/plan.md @@ -0,0 +1,195 @@ +# Plan: Bridge wedges (reader thread blocked, Recv-Q backup) when remote subscriber dies + +## Issue + +https://github.com/rolker/udp_bridge/issues/10 + +## Context + +The receive path runs entirely in `socket_drain_group_` (MutuallyExclusive): +`spin_once` (10 ms timer) → `recvfrom` loop → `decode` → `decodeData` → +`publisher->publish()` at `src/udp_bridge.cpp:776`. The destination publisher +is created **RELIABLE** + `KEEP_LAST(1)` (`qos_resolution.h` — a deliberate +`rmw_zenoh_cpp` 0.2.9 graph-visibility workaround; design intent is +`BEST_AVAILABLE`). When a local subscriber (e.g. CAMP) dies *uncleanly*, DDS +keeps it matched until liveliness times out, and a RELIABLE `publish()` blocks +waiting for a history slot. Because that publish is on the **MutuallyExclusive** +drain group, the blocked call stops `spin_once` from re-running → `recvfrom` +stalls → kernel `Recv-Q` backs up (361 KB observed) → forwarded topics go +silent. Four occurrences on 2026-04-27; one masked an RTK alert by freezing +`/diagnostics` stale on the operator side. + +`qos_design.md` (lines 36-39, 89-96, 305-309) already documents this mechanism +and rules out the QoS knobs as the fix: `BEST_EFFORT` would break CAMP's +RELIABLE subscribers (operator goes dark), `BEST_AVAILABLE` is blocked by the +rmw bug, and even BEST_AVAILABLE behaves RELIABLE for the local hop when a +RELIABLE subscriber matches — so it can still block. The fix must be +**transport-level, independent of QoS and rmw**: the drain thread must never +make a call that can block on a single subscriber. + +`publish()` is not the *only* rmw call on the drain path. `decodeData` also +runs, on the same thread: `create_generic_publisher()` (first-arrival, line +759 — the existing comment at 733-737 calls this "exactly the wedge mechanism") +and `sendBridgeInfo()` (first-arrival, line 773 — a graph query + two-mutex +snapshot + UDP send). Both can block on rmw discovery. So the decoupling must +move *all* of first-arrival create + bridge-info + publish off the drain +thread, not just the `publish()` call — otherwise the first message on any new +topic can still wedge the reader. The drain thread's only remaining +`decodeData` work is the (CPU-bound) `MessageInternal` deserialize + building +the inner `SerializedMessage`. + +## Approach + +Decouple the whole rmw-touching tail of `decodeData` from the socket-drain +thread via a dedicated **publish-worker thread** fed by a bounded queue. The +drain thread deserializes the `MessageInternal`, builds the inner +`SerializedMessage`, and enqueues a self-contained item; the worker does +lookup-or-create publisher + first-arrival bridge-info + `publish()`. A stalled +publish (or a blocking first-arrival create) then blocks only the worker — +`spin_once` keeps draining the socket. This realizes the package's documented +"best-effort with loss reduction" model on the local hop: when the local +publish path stalls, we drop republished messages rather than backing up the +wire reader. + +1. **`PublishQueue` component** (`include/udp_bridge/publish_queue.h`, + header-only). A bounded MPSC queue of `PublishItem` + `{topic, datatype, reliability, durability, history_depth, SerializedMessage}` + plus one worker thread. The worker is parameterized by an injected **sink** + `std::function` — keeps all rmw/`UDPBridge` coupling out + of the component so it unit-tests with a fake sink (a deliberately-blocking + sink proves the non-stall property; a recording sink proves FIFO). `push()` + never blocks; the bound is in **bytes** (`PublishItem::byte_size()`, since a + reassembled image is large) and on overflow it drops the **oldest** item(s) + and increments an atomic dropped counter. `start()` launches the worker, + `stop()` signals + joins; `push()` after `stop()` is a no-op (drop), so it is + safe to call from any thread at any lifecycle phase. `run()` wraps the sink + call in a last-resort `catch(...)` barrier so a throwing sink can never + `std::terminate` the process (review finding — see Implementation Notes). +2. **Wire into `UDPBridge`** — own a `PublishQueue` member. The injected sink + is a member lambda (`publishItem`) that runs the existing three-phase + lookup-or-create, the first-arrival `sendBridgeInfo()`, and + `publisher->publish()` — that block moves out of `decodeData` into the sink, + wrapped in a `try/catch` that logs via `RCLCPP_ERROR` (mirroring `decode()`'s + catch-all; the work previously ran under that barrier on the executor + thread). `decodeData` keeps only the deserialize + inner-`SerializedMessage` + build, then `publish_queue_.push(std::move(item))`. Lifecycle: `configure()` + the queue in `on_configure`, `start()` in `on_activate`, `stop()`+join in + `on_deactivate` (so the worker runs **only while ACTIVE** — matching the + state-guard discipline of the other publish paths, and never publishing on + the non-lifecycle destination publishers while INACTIVE), with a backstop + `stop()` in `on_cleanup` and the destructor. The join gates teardown: + destination publishers are **non-lifecycle** `GenericPublisher`s, so the + worker must be joined before the node/`publishers_` are torn down (the member + is declared last for the destructor-ordering backstop). Re-entrancy: an + activate→deactivate→activate cycle stops+restarts a fresh worker. +3. **Bound + observability** — byte budget as a ROS parameter + (`publish_queue_max_bytes`, declared via `declareIfMissing`); register a + single global drop diagnostic via the existing `syncDiagnosticTasks()` / + `diagnostic_task_names_` pattern so a stalled local subscriber surfaces as a + WARN instead of silently. (Drop count is a global tally, not per-remote.) +4. **Unit test** (`test/test_publish_queue.cpp`) — the non-stall property at the + component level, which is also the bug: with a sink that blocks on a latch + (simulating a stuck `publish()`), assert `push()` still returns promptly and + the producer is never blocked; FIFO order via a recording sink; drop-oldest + + counter when over the byte budget; oversize-item enqueue-then-evict; + push-after-stop / push-before-start no-op; clean `stop()`/join while the sink + is blocked; and a throwing sink does not kill the worker. Register via + `add_udp_bridge_gtest` in `CMakeLists.txt`. +5. **End-to-end reproduction** — the kill-a-subscriber-mid-forward repro + (acceptance #1) belongs in the issue #18 bench harness (PR #27 draft, which + already targets the "wedge" failure mode). Coordinate rather than duplicate: + this PR lands the component + the blocking-sink non-stall unit test; + cross-link #18 for the live-DDS integration repro. (Residual: no test in + *this* PR exercises a real dying DDS subscriber — the blocking-sink test is + the proxy; see Open Questions.) +6. **Docs** — add a "Receive-path publish decoupling" subsection to + `qos_design.md`: why the wedge is fixed at the transport layer (not QoS) and + holds regardless of the RELIABLE/BEST_AVAILABLE default, and that queue drops + are **unrecoverable** loss *after* successful wire delivery — distinct from, + and not recoverable by, the resend layer (which operates pre-`decodeData`). + Also update the `socket_drain_group_` invariant comment in + `udp_bridge_node.cpp` (which currently predicts this exact "move publish work + to a separate group via an internal queue" change). + +## Files to Change + +| File | Change | +|------|--------| +| `udp_bridge/include/udp_bridge/publish_queue.h` | New header-only bounded publish-worker queue (`PublishItem`, byte-bounded, injected sink, atomic drop counter) | +| `udp_bridge/src/udp_bridge.cpp` | Move first-arrival create + `sendBridgeInfo` + `publish` into the queue sink lambda (`publishItem`, wrapped in try/catch+log); `decodeData` deserializes + enqueues; `configure` queue in `on_configure`, `start` in `on_activate`, `stop`+join in `on_deactivate` (backstop in `on_cleanup`); `declareIfMissing("publish_queue_max_bytes")`; drop diagnostic via `syncDiagnosticTasks` | +| `udp_bridge/include/udp_bridge/udp_bridge.h` | `PublishQueue` member + `publish_queue_max_bytes_`; the sink helper decl | +| `udp_bridge/src/udp_bridge_node.cpp` | Update the `socket_drain_group_` invariant comment (it predicts this change) | +| `udp_bridge/test/test_publish_queue.cpp` | New unit test (blocking-sink non-stall, FIFO, byte-budget drop-oldest, join-while-blocked) | +| `udp_bridge/CMakeLists.txt` | Register `test_publish_queue` via `add_udp_bridge_gtest` | +| `udp_bridge/doc/qos_design.md` | Document transport-level decoupling as the #10 fix + unrecoverable-drop note | + +## Principles Self-Check + +| Principle | Consideration | +|---|---| +| Test what breaks | Unit-test the non-stall/drop invariants — the exact failure seen 4× in the field — not framework glue | +| A change includes its consequences | `qos_design.md` updated; diagnostic surfaces drops; #18 coordination noted, not deferred silently | +| Only what's needed | One worker + bounded queue; watchdog (issue fix-shape #2) deferred — decoupling removes the blocking site, making Recv-Q polling redundant | +| Capture decisions | Rationale (why transport-level, not QoS) recorded in `qos_design.md`, not just the diff | +| Human control & transparency | Drop count is observable via diagnostics; queue bound is a tunable parameter | + +## ADR Compliance + +| ADR | Triggered | How addressed | +|---|---|---| +| ADR-0008 (ROS 2 conventions) | Yes | Worker thread is internal; lifecycle start/stop follows LifecycleNode activate/deactivate contract; QoS contract unchanged | +| ADR-0013 (progress.md vocabulary) | Yes | `progress.md` gets a `## Plan Authored` entry | + +## Consequences + +| If we change... | Also update... | Included? | +|---|---|---| +| publish path moves off drain thread | `qos_design.md` receive-path description | Yes | +| add `publish_queue_max` param | parameter docs / README if it lists params | Yes (verify during impl) | +| reader-thread non-stall property | issue #18 bench harness adds the e2e wedge repro | No — cross-linked follow-up in #18 | + +## Decisions (resolved) + +- **Cross-topic isolation scope → single worker** (user, 2026-05-25). One worker + thread + bounded queue this PR; it fixes the reported reader-thread wedge (all + four field occurrences). Per-publisher isolation (so a stalled subscriber on + one topic can't head-of-line-block others) is deferred to a follow-up issue, + to be filed when this PR lands. The bounded-queue drop counter + diagnostic + make the head-of-line residual observable in the meantime. + +## Open Questions + +- **E2E repro home.** Default: the kill-subscriber integration repro lands in + the #18 bench harness (PR #27), which already targets the "wedge" failure + mode — not a standalone launch_test here. Flag if you'd rather it live in + this repo's tests. + +## Estimated Scope + +Single PR for the component + unit test + wiring + docs. The end-to-end +reproduction (acceptance #1) is coordinated into issue #18's harness. + +## Implementation Notes + +Rationale-bearing pivots from the local `/review-code` pass (Deep tier; +Claude + Copilot adversarial agreed), beyond what's visible in the diff: + +- **Worker exception barrier.** The sink (`publishItem`) can throw — + `create_generic_publisher` on an unknown/cross-version `datatype`, + `publish`, and `sendBridgeInfo`'s `ConnectionException` on socket errors. + Before #10 this work ran inside `decode()`'s catch-all on the executor + thread (logged + dropped). A plain `std::thread` has no such backstop, so an + uncaught throw would `std::terminate` the bridge — a regression. Fixed in two + layers: `publishItem` catches + logs via `RCLCPP_ERROR` (informative, has the + node logger); `PublishQueue::run` has a last-resort `catch(...)` so the + worker can never terminate the process even if a future sink forgets. +- **Worker runs only while ACTIVE.** Moved `start()` to `on_activate` and added + `stop()` to `on_deactivate` (was: start in `on_configure`). Otherwise the + worker could publish queued items after deactivation onto the + lifecycle-gated `bridge_info`/`topic_statistics` publishers (warn + drop) and + diverge from the state-guard discipline every other publish path follows. +- **Shutdown-join bound.** `stop()` discards the backlog and waits at most for + one in-flight publish to return (the rmw `max_blocking_time`), not the whole + queue — so leaving ACTIVE / cleanup can't hang on the backlog. A single + genuinely-infinite `publish()` is the inherent limit of joining a thread + blocked in a syscall; documented, not engineered around. diff --git a/.agent/work-plans/issue-10/progress.md b/.agent/work-plans/issue-10/progress.md new file mode 100644 index 0000000..44c7ae6 --- /dev/null +++ b/.agent/work-plans/issue-10/progress.md @@ -0,0 +1,111 @@ +--- +issue: 10 +--- + +# Issue #10 — Bridge wedges (reader thread blocked, Recv-Q backup) when remote subscriber dies + +## Plan Authored +**Status**: complete +**When**: 2026-05-25 18:30 -0400 +**By**: Claude Code Agent (Claude Opus 4.7 (1M context)) + +**Plan**: `.agent/work-plans/issue-10/plan.md` at `44a81ad` +**PR**: https://github.com/rolker/udp_bridge/pull/28 (`[PLAN]` prefix) +**Phases**: single + +### Open questions +- [x] Cross-topic isolation scope — **decided: single publish-worker this PR; per-publisher isolation deferred to a follow-up issue** (user, 2026-05-25). +- [ ] Confirm the kill-subscriber end-to-end reproduction (acceptance #1) lands in the issue #18 bench harness (PR #27), not a standalone launch_test here. + +## Plan Review +**Status**: complete — verdict **revise**, revisions applied +**When**: 2026-05-25 18:30 -0400 +**By**: Claude Code Agent (Claude Opus 4.7 (1M context)) — fresh-context internal review (pre-Copilot) + +**Plan**: `.agent/work-plans/issue-10/plan.md` at `078f794` + +Findings, all incorporated into the revised plan: +- [x] **B1** (blocking): `create_generic_publisher()` (first-arrival) is also on the drain thread and can block on rmw → move the whole first-arrival create + bridge-info + publish into the worker sink, not just `publish()`. +- [x] **B2**: first-arrival `sendBridgeInfo()` also inline on drain → folded into B1's sink move. +- [x] **B3/B4** (lifecycle): construct/start queue in `on_configure`, `stop()`+join in `on_cleanup`; `push()` no-op after stop; join gates post-deactivation publishes (destination publishers are non-lifecycle); cleanup→configure re-creates a fresh worker. +- [x] **S2**: bound the queue by **bytes**, not message count (reassembled images are large). +- [x] **N1/N2**: injected `std::function` sink seam → unit-test non-stall with a blocking fake sink (the proxy for a dying DDS subscriber). +- [x] **S1/S3/S4/N3**: doc unrecoverable drop; diagnostic via `syncDiagnosticTasks`; `declareIfMissing`; update `udp_bridge_node.cpp` invariant comment. + +## Implementation +**Status**: complete +**When**: 2026-05-25 19:48 -0400 +**By**: Claude Code Agent (Claude Opus 4.7 (1M context)) + +**Branch**: feature/issue-10 at `558bebf` +Implemented per plan: `PublishQueue` component + `decodeData`/`publishItem` rewiring + lifecycle wiring + diagnostic + `qos_design.md` + `udp_bridge_node.cpp` comment. Build clean; full suite **94 tests, 0 failures** (8 new `PublishQueue` tests). + +## Local Review (Pre-Push) +**Status**: complete +**When**: 2026-05-25 19:48 -0400 +**By**: Claude Code Agent (Claude Opus 4.7 (1M context)) +**Verdict**: changes-requested (all resolved) + +**Branch**: feature/issue-10 at `558bebf` +**Mode**: pre-push +**Depth**: Deep (reason: concurrency + lifecycle in a core comms package) +**Must-fix**: 1 | **Suggestions**: 2 + +### Findings +- [x] (must-fix) Worker `run()` had no exception barrier — a throwing sink (`create_generic_publisher`/`publish`/`sendBridgeInfo`) → `std::terminate`, a regression vs `decode()`'s old catch-all. Fixed: `publishItem` try/catch+log + `run()` `catch(...)` backstop + `ThrowingSinkDoesNotKillWorker` test — `publish_queue.h`, `udp_bridge.cpp` `publishItem` +- [x] (suggestion) Worker published while INACTIVE — moved `start`→`on_activate`, `stop`→`on_deactivate` — `udp_bridge.cpp` `on_activate`/`on_deactivate` +- [x] (suggestion) New concurrent outbound-send caller + shutdown-join bound documented — `udp_bridge_node.cpp` callback-group comment +- [x] (static) cppcheck: all findings on pre-existing/context lines (not changed lines) — dropped per silence filter + +## External Review +**Status**: complete +**When**: 2026-05-25 20:14 -0400 +**By**: Claude Code Agent (Claude Opus 4.7 (1M context)) + +**PR**: #28 at `e37e733` +**Reviews**: 1 review (Copilot bot), 2 valid, 0 false positives +**CI**: copilot-pull-request-reviewer success (repo has no build/test CI; local build + 94-test suite is the gate) + +### Actions +- [x] Fix: `test_publish_queue.cpp` `wait_until_blocked()` — bounded 2s deadline + `ASSERT_LT` (commit `241f2e5`). +- [x] Fix: `udp_bridge.cpp` on_configure — upper-bound clamp (2 GiB) + warning for `publish_queue_max_bytes` (commit `241f2e5`). + +## Local Review (Pre-Push) +**Status**: complete +**When**: 2026-05-25 20:14 -0400 +**By**: Claude Code Agent (Claude Opus 4.7 (1M context)) +**Verdict**: approved + +**Branch**: feature/issue-10 at `241f2e5` +**Mode**: pre-push +**Depth**: Light (reason: two small low-risk follow-up fixes; bulk already Deep-reviewed) +**Must-fix**: 0 | **Suggestions**: 0 + +### Findings +- [ ] No issues found. LGTM. (cppcheck: only a cross-TU `unusedStructMember` false positive on `publish_queue_max_bytes_`; Copilot: "No issues." Build clean, 94 tests pass.) + +## External Review +**Status**: complete +**When**: 2026-05-25 21:31 -0400 +**By**: Claude Code Agent (Claude Opus 4.7 (1M context)) + +**PR**: #28 at `c655350` +**Reviews**: 2 reviews (Copilot bot) — fresh re-review at HEAD; 1 valid, 0 false positives; 2 prior comments now addressed/stale +**CI**: copilot-pull-request-reviewer success + +### Actions +- [x] Fix: `publish_queue.h` `start()` — `running_ = true` now set after the `std::thread` is constructed (commit `329d308`; re-reviewed clean — cppcheck + Copilot "No issues"). Build clean, 94 tests pass. +- [x] (addressed `241f2e5`) `wait_until_blocked()` bounded wait — confirmed in re-review as stale. +- [x] (addressed `241f2e5`) `publish_queue_max_bytes` upper clamp — confirmed in re-review as stale. + +## External Review +**Status**: complete +**When**: 2026-05-26 00:20 -0400 +**By**: Claude Code Agent (Claude Opus 4.7 (1M context)) + +**PR**: #28 at `ce13281` +**Reviews**: 3 (Copilot bot) — fresh re-review at current HEAD found 0 comments; all prior findings stale/resolved +**CI**: copilot-pull-request-reviewer success + +### Actions +- [x] No outstanding review comments. All three rounds resolved (`241f2e5`, `329d308`); current-HEAD re-review clean. PR ready to merge. diff --git a/udp_bridge/CMakeLists.txt b/udp_bridge/CMakeLists.txt index bef9422..d17f346 100644 --- a/udp_bridge/CMakeLists.txt +++ b/udp_bridge/CMakeLists.txt @@ -143,6 +143,9 @@ if(BUILD_TESTING) # through the same helper so it gets UDP_BRIDGE_BUILD_TESTING too — # required for in-package ABI alignment per the helper's contract. add_udp_bridge_gtest(test_giveup_diagnostic test/test_giveup_diagnostic.cpp) + # test_publish_queue (issue #10) exercises the bounded publish-worker + # queue in isolation via an injected sink — no UDPBridge / node needed. + add_udp_bridge_gtest(test_publish_queue test/test_publish_queue.cpp) endif() ament_package() diff --git a/udp_bridge/doc/qos_design.md b/udp_bridge/doc/qos_design.md index df6e272..e58c45b 100644 --- a/udp_bridge/doc/qos_design.md +++ b/udp_bridge/doc/qos_design.md @@ -43,6 +43,51 @@ endpoints decide what contract they want with each other. It is not the bridge's job to assert a contract about the wire that the wire cannot honor. +## Receive-path publish decoupling (issue #10) + +The wedge described above is **not** fixed by any QoS choice, and the fix is +deliberately independent of the reliability policy: + +- `BEST_EFFORT` on the destination publisher would refuse `RELIABLE` + subscribers (CAMP goes dark — see the worked example below), so it is not + an option. +- `BEST_AVAILABLE` is the design intent but (a) is currently blocked by the + `rmw_zenoh_cpp` 0.2.9 graph-visibility bug, and (b) still behaves + `RELIABLE` *for the local hop* whenever a `RELIABLE` subscriber matches — + so it can still block on a dead-but-still-matched subscriber. + +So the wedge is fixed at the **transport layer instead**: the rmw-touching +tail of `decodeData` — find-or-create the destination publisher, the +first-arrival `sendBridgeInfo`, and `publish()` — no longer runs on the +socket-drain thread. `decodeData` deserializes the `MessageInternal` and +enqueues a `PublishItem` on a bounded, single-worker `PublishQueue` +(`include/udp_bridge/publish_queue.h`); the worker does the create/publish. +A `RELIABLE` publish stalling on an uncleanly-dead subscriber — or a +first-arrival `create_generic_publisher()` blocking on rmw discovery — now +blocks only the worker. `recvfrom` keeps draining; the kernel `Recv-Q` does +not back up. This holds regardless of whether the destination publisher is +`RELIABLE` (current operational default) or `BEST_AVAILABLE` (design intent). + +**Queue drops are unrecoverable, and distinct from wire loss.** When a local +subscriber stalls the publish path long enough to exceed the queue's byte +budget (`publish_queue_max_bytes`), the queue drops the **oldest** items +(KEEP_LAST(1)-consistent: newest wins) and counts them in a diagnostic. This +is loss that happens *after* successful wire delivery and reassembly, so the +resend layer — which operates on packet numbers *upstream* of `decodeData` — +cannot recover it. It is the local-hop expression of the same +"best-effort with loss reduction" model: the bridge would rather drop a +republished message than let one stalled subscriber wedge the reader for +every topic. The drop count surfaces as a `publish queue` diagnostic +(WARN on recent drops) so a stalled subscriber is visible to operators +rather than silent. + +A single worker means a stalled publish can still delay *other* republished +topics queued behind it (head-of-line) until they age out of the byte +budget — bounded and observable, but not isolated. Per-publisher isolation +(a worker per topic, so a dead costmap subscriber can't delay video) is a +deferred follow-up; the reported field wedges were all the reader-thread +wedge this decoupling removes. + ## How `BEST_AVAILABLE` matching works `BEST_AVAILABLE` is a special QoS value (see @@ -305,8 +350,9 @@ runtime. - Issue [#11](https://github.com/rolker/udp_bridge/issues/11) — robustness changes that introduced this design. - Issue [#10](https://github.com/rolker/udp_bridge/issues/10) — bridge - wedge whose hypothesized mechanism (RELIABLE publisher stalling on a - dead subscriber) motivated the reliability policy. + wedge (RELIABLE publisher stalling on a dead subscriber) that motivated + the reliability policy and is fixed by the receive-path publish + decoupling described above. - Issue [#9](https://github.com/rolker/udp_bridge/issues/9) — resend amplification, the package's actual loss-reduction layer. - Plan: `.agent/work-plans/PLAN_ISSUE-11.md`. diff --git a/udp_bridge/include/udp_bridge/publish_queue.h b/udp_bridge/include/udp_bridge/publish_queue.h new file mode 100644 index 0000000..b65f3a5 --- /dev/null +++ b/udp_bridge/include/udp_bridge/publish_queue.h @@ -0,0 +1,215 @@ +#ifndef UDP_BRIDGE_PUBLISH_QUEUE_H +#define UDP_BRIDGE_PUBLISH_QUEUE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/serialized_message.hpp" + +namespace udp_bridge +{ + +/// One republish job handed from the socket-drain thread to the publish +/// worker. It carries everything the worker needs to find-or-create the +/// destination publisher and publish, so that NO rmw work (publisher +/// creation, graph queries, publish) happens on the socket-drain thread. +/// See issue #10: a RELIABLE publish() stalling on an uncleanly-dead +/// subscriber — or a first-arrival create_generic_publisher() blocking on +/// rmw discovery — must never stall recvfrom. +struct PublishItem +{ + std::string topic; + std::string datatype; + std::string reliability; + std::string durability; + uint32_t history_depth = 0; + rclcpp::SerializedMessage message; + + /// Approximate footprint for the queue's byte budget. The serialized + /// payload dominates; the small string fields are included so an + /// empty-payload message still counts a nonzero amount. + size_t byte_size() const + { + return message.size() + + topic.size() + datatype.size() + + reliability.size() + durability.size(); + } +}; + +/// Bounded, drop-oldest publish queue served by a single worker thread. +/// +/// Decouples the rmw-touching republish tail of UDPBridge::decodeData from +/// the socket-drain thread (issue #10). push() never blocks; the bound is in +/// bytes (a reassembled image is large), and when a new item would exceed the +/// budget the oldest queued item(s) are dropped (KEEP_LAST semantics — newest +/// wins, matching the bridge's documented best-effort-with-loss-reduction +/// model; see doc/qos_design.md) and counted. The actual publish work is an +/// injected sink, so the component carries no dependency on UDPBridge / rclcpp +/// publishers and is unit-testable with a fake sink: a blocking sink proves +/// the non-stall property, a recording sink proves FIFO order. +class PublishQueue +{ +public: + using Sink = std::function; + + PublishQueue() = default; + ~PublishQueue() { stop(); } + + PublishQueue(const PublishQueue&) = delete; + PublishQueue& operator=(const PublishQueue&) = delete; + + /// Set the worker's sink and byte budget. Call once before start() (e.g. + /// in on_configure); changing these while running is not supported. + void configure(Sink sink, size_t max_bytes) + { + sink_ = std::move(sink); + max_bytes_ = max_bytes; + } + + /// Launch the worker thread. A no-op if already running. + void start() + { + std::lock_guard lock(mutex_); + if(running_) + return; + stop_requested_ = false; + // Construct the worker BEFORE marking running_: if std::thread throws + // (std::system_error on resource exhaustion), running_ stays false, so the + // queue is cleanly not-running — push() drops, start() is retryable — + // rather than stuck running_=true with no worker to drain. run() never + // reads running_ (only stop_requested_ / the queue), so this ordering is + // behaviour-neutral on the success path. + worker_ = std::thread(&PublishQueue::run, this); + running_ = true; + } + + /// Signal the worker to finish and join it. Any items still queued are + /// discarded (best-effort: prompt, bounded shutdown — the join waits at + /// most for one in-flight sink call to return, not for the whole backlog + /// to drain through a possibly-stalled sink). After stop(), push() is a + /// no-op until the next start(). Safe to call repeatedly and from the + /// destructor. + void stop() + { + { + std::lock_guard lock(mutex_); + if(!running_) + return; + stop_requested_ = true; + } + cv_.notify_all(); + if(worker_.joinable()) + worker_.join(); + std::lock_guard lock(mutex_); + running_ = false; + queue_.clear(); + queued_bytes_ = 0; + } + + /// Enqueue a job. Never blocks. Drops the oldest queued item(s) if adding + /// this one would exceed the byte budget (a single item larger than the + /// whole budget is still enqueued so it is not silently lost). A no-op + /// (counted as a drop) if the worker is not running, so push() is safe to + /// call from any thread at any lifecycle phase. + void push(PublishItem&& item) + { + const size_t item_bytes = item.byte_size(); + { + std::lock_guard lock(mutex_); + if(stop_requested_ || !running_) + { + dropped_count_.fetch_add(1, std::memory_order_relaxed); + return; + } + while(!queue_.empty() && queued_bytes_ + item_bytes > max_bytes_) + { + queued_bytes_ -= queue_.front().byte_size(); + queue_.pop_front(); + dropped_count_.fetch_add(1, std::memory_order_relaxed); + } + queued_bytes_ += item_bytes; + queue_.push_back(std::move(item)); + } + cv_.notify_one(); + } + + /// Total items dropped due to overflow or push-after-stop. + uint64_t dropped_count() const + { + return dropped_count_.load(std::memory_order_relaxed); + } + + /// Current queued depth (items). For diagnostics / tests. + size_t size() const + { + std::lock_guard lock(mutex_); + return queue_.size(); + } + +private: + void run() + { + for(;;) + { + PublishItem item; + { + std::unique_lock lock(mutex_); + cv_.wait(lock, [this]{ return stop_requested_ || !queue_.empty(); }); + if(stop_requested_) + return; // prompt shutdown; remaining items are discarded in stop() + item = std::move(queue_.front()); + queue_.pop_front(); + queued_bytes_ -= item.byte_size(); + } + // The sink runs WITHOUT the lock held: a blocking publish here stalls + // only this worker thread, never push() or the socket-drain thread. + // + // The try/catch is a last-resort barrier so a throwing sink can never + // std::terminate the process. The sink (UDPBridge::publishItem) is + // expected to catch and log its own exceptions — it has the node + // logger; this layer does not — but create_generic_publisher (unknown / + // cross-version datatype), publish(), and sendBridgeInfo() + // (ConnectionException on socket errors) can all throw, and before + // issue #10 this work ran inside the socket-drain executor thread whose + // catch-all logged + dropped bad packets instead of tearing the node + // down. A plain std::thread has no such backstop, so we add one here. + if(sink_) + { + try + { + sink_(std::move(item)); + } + catch(...) + { + // Swallow and continue: the worker must never terminate the + // process over one bad item. Informative logging belongs to the + // sink (which has the logger); reaching here means the sink let an + // exception escape, which it shouldn't. + } + } + } + } + + mutable std::mutex mutex_; + std::condition_variable cv_; + std::deque queue_; + size_t queued_bytes_ = 0; + size_t max_bytes_ = 0; + Sink sink_; + std::thread worker_; + bool running_ = false; + bool stop_requested_ = false; + std::atomic dropped_count_ {0}; +}; + +} // namespace udp_bridge + +#endif diff --git a/udp_bridge/include/udp_bridge/udp_bridge.h b/udp_bridge/include/udp_bridge/udp_bridge.h index 9653ddc..e01b64c 100644 --- a/udp_bridge/include/udp_bridge/udp_bridge.h +++ b/udp_bridge/include/udp_bridge/udp_bridge.h @@ -26,6 +26,7 @@ #include "giveup_diagnostic.h" #include "packet.h" #include "defragmenter.h" +#include "udp_bridge/publish_queue.h" #include "udp_bridge/types.h" #include "udp_bridge/wrapped_packet.h" //#include "std_msgs/msg/int32.hpp" @@ -107,7 +108,21 @@ class UDPBridge: public rclcpp_lifecycle::LifecycleNode /// Decodes data from a remote subscription received over the UDP link. /// @param message bytes representing a serialized MessageInternal message /// @param source_info info about the packet sender + /// + /// Runs on the socket-drain thread. It only deserializes the outer + /// MessageInternal and enqueues a PublishItem on publish_queue_; all + /// rmw-touching work (find-or-create the destination publisher, + /// first-arrival sendBridgeInfo, publish) happens on the publish worker + /// via publishItem(). See issue #10 — the drain thread must not make a + /// call that can block on a single subscriber. void decodeData(std::vector const &message, const SourceInfo& source_info); + + /// publish_queue_ sink: runs on the publish worker thread. Finds or + /// creates the destination GenericPublisher (first-arrival also triggers + /// sendBridgeInfo) and publishes. Any blocking here (RELIABLE publish to a + /// dead-but-matched subscriber; first-arrival rmw discovery) stalls only + /// the worker, never the socket-drain thread. + void publishItem(PublishItem&& item); /// Decodes topic info from remote. void decodeBridgeInfo(std::vector const &message, const SourceInfo& source_info); @@ -188,6 +203,14 @@ class UDPBridge: public rclcpp_lifecycle::LifecycleNode void diagnoseRemoteGiveups(const std::string& remote_name, diagnostic_updater::DiagnosticStatusWrapper& stat); + /// Populate the publish-queue DiagnosticStatus: queued depth + total + /// drops. WARNs when drops increased since the previous tick — drops mean + /// a local destination subscriber stalled the publish path long enough to + /// overflow the byte budget, so republished data was lost (unrecoverable; + /// distinct from wire loss — see doc/qos_design.md). Registered once in + /// on_configure (a single global task, not per-remote). + void diagnosePublishQueue(diagnostic_updater::DiagnosticStatusWrapper& stat); + /// Lifecycle-safe wrapper around `declare_parameter`. Parameters /// declared during `on_configure` persist across a /// cleanup→configure transition (`on_cleanup` does not @@ -354,6 +377,33 @@ class UDPBridge: public rclcpp_lifecycle::LifecycleNode // re-configure cycles don't accumulate stale handles. rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_handle_; + + // Byte budget for publish_queue_ (issue #10). Declared as a ROS + // parameter in on_configure (declareIfMissing) so deployments can size + // it without rebuild; set once per configure and read by configure(). + size_t publish_queue_max_bytes_ {kDefaultPublishQueueMaxBytes}; + static constexpr size_t kDefaultPublishQueueMaxBytes = 64u * 1024u * 1024u; + static constexpr size_t kMinPublishQueueMaxBytes = 1u * 1024u * 1024u; + // Sanity ceiling (2 GiB). Both-bounds clamping matches the port / + // maximum_packet_size / history_depth pattern and keeps the int64 → size_t + // cast in on_configure well within range on 32-bit size_t platforms. + static constexpr size_t kMaxPublishQueueMaxBytes = 2ull * 1024u * 1024u * 1024u; + + // Last publish-queue drop total observed by diagnosePublishQueue, so the + // diagnostic can WARN on *recent* drops (increase since last tick) rather + // than latching WARN forever after a single historical drop. Touched only + // from the diagnostic callback (periodic_group_, mutually exclusive). + uint64_t last_reported_publish_drops_ {0}; + + // Decouples the rmw-touching tail of decodeData from the socket-drain + // thread (issue #10). configure()'d in on_configure, start()'ed in + // on_activate, stop()+join()'ed in on_deactivate (so it runs only while + // ACTIVE) — with a backstop stop() in on_cleanup. Declared LAST so that on + // the non-lifecycle teardown path (node destroyed without on_cleanup) its + // destructor (stop() + join()) runs before publishers_ / subscribers_ / + // remote_nodes_ are destroyed — the worker's sink (publishItem / + // sendBridgeInfo) touches all three. + PublishQueue publish_queue_; }; } // namespace udp_bridge diff --git a/udp_bridge/src/udp_bridge.cpp b/udp_bridge/src/udp_bridge.cpp index f6d093d..1e5f772 100644 --- a/udp_bridge/src/udp_bridge.cpp +++ b/udp_bridge/src/udp_bridge.cpp @@ -171,6 +171,40 @@ UDPBridge::CallbackReturn UDPBridge::on_configure(const rclcpp_lifecycle::State "resend_giveup thresholds: warn=" << resend_giveup_warn_rate_per_s_ << "/s, error=" << resend_giveup_error_rate_per_s_ << "/s"); + // Publish-queue byte budget (issue #10). Bounds the memory the publish + // worker may hold when a local destination subscriber stalls; once the + // queued bytes would exceed this, the oldest republished messages are + // dropped (see PublishQueue / doc/qos_design.md). ROS 2 parameters are + // int64; clamp to both a sane floor and ceiling before the size_t cast (the + // same both-bounds discipline used for port / maximum_packet_size / + // history_depth above) so a tiny/negative value can't make the queue drop + // everything, and a huge value can't invite unbounded memory growth or wrap + // the int64 → size_t cast on 32-bit size_t platforms. + declareIfMissing("publish_queue_max_bytes", + static_cast(kDefaultPublishQueueMaxBytes)); + { + int64_t configured = get_parameter("publish_queue_max_bytes").as_int(); + if(configured < static_cast(kMinPublishQueueMaxBytes)) + { + RCLCPP_WARN_STREAM(get_logger(), + "publish_queue_max_bytes " << configured << " is below " + << kMinPublishQueueMaxBytes << "; clamping to " + << kMinPublishQueueMaxBytes); + configured = static_cast(kMinPublishQueueMaxBytes); + } + else if(configured > static_cast(kMaxPublishQueueMaxBytes)) + { + RCLCPP_WARN_STREAM(get_logger(), + "publish_queue_max_bytes " << configured << " is above " + << kMaxPublishQueueMaxBytes << "; clamping to " + << kMaxPublishQueueMaxBytes); + configured = static_cast(kMaxPublishQueueMaxBytes); + } + publish_queue_max_bytes_ = static_cast(configured); + } + RCLCPP_INFO_STREAM(get_logger(), + "publish_queue_max_bytes: " << publish_queue_max_bytes_); + on_set_parameters_handle_ = add_on_set_parameters_callback( [this](const std::vector& params) -> rcl_interfaces::msg::SetParametersResult @@ -428,24 +462,57 @@ UDPBridge::CallbackReturn UDPBridge::on_configure(const rclcpp_lifecycle::State diagnostic_updater_ = std::make_unique(this); diagnostic_updater_->setHardwareID(name_); syncDiagnosticTasks(); + // Single global publish-queue diagnostic (issue #10). diagnostic_updater_ + // is freshly created each on_configure (reset in on_cleanup), so adding + // the task here once per configure cannot double-register. + diagnostic_updater_->add("udp_bridge " + name_ + ": publish queue", + [this](diagnostic_updater::DiagnosticStatusWrapper& stat) + { + diagnosePublishQueue(stat); + }); diagnostic_timer_ = create_wall_timer( 1s, std::bind(&UDPBridge::diagnosticTick, this), periodic_group_); + // Configure the publish worker (issue #10) but don't start it yet — the + // worker runs only while the node is ACTIVE (started in on_activate, + // stopped in on_deactivate), so it never publishes during the INACTIVE + // state, matching the state-guard discipline of the other publish paths + // (spin_once / statsReportCallback / bridgeInfoCallback). + publish_queue_.configure( + [this](PublishItem&& item){ publishItem(std::move(item)); }, + publish_queue_max_bytes_); + return LifecycleNode::on_configure(state); } UDPBridge::CallbackReturn UDPBridge::on_activate(const rclcpp_lifecycle::State & state) { + // Start the publish worker (issue #10) only for the ACTIVE state, so it + // processes (and publishes) exactly while spin_once is enqueuing. start() + // is a no-op if already running. + publish_queue_.start(); return LifecycleNode::on_activate(state); } UDPBridge::CallbackReturn UDPBridge::on_deactivate(const rclcpp_lifecycle::State & state) { + // Stop + join the publish worker on leaving ACTIVE so no republish runs + // while INACTIVE. stop() discards any still-queued items (best-effort) and + // joins; the join waits at most for one in-flight publish to return (the + // rmw max_blocking_time), not the whole backlog. A subsequent on_activate + // restarts a fresh worker. + publish_queue_.stop(); return LifecycleNode::on_deactivate(state); } UDPBridge::CallbackReturn UDPBridge::on_cleanup(const rclcpp_lifecycle::State & state) { + // Backstop stop + join of the publish worker (issue #10). Normally it was + // already stopped in on_deactivate (cleanup follows deactivate); stop() is + // idempotent. Belt-and-suspenders for a configure→cleanup path that never + // activated (worker never started → no-op) and so the worker is provably + // joined before anything here touches the maps its sink uses. + publish_queue_.stop(); diagnostic_timer_.reset(); diagnostic_updater_.reset(); diagnostic_task_names_.clear(); @@ -720,60 +787,99 @@ void UDPBridge::decodeData(std::vector const &message, const SourceInfo (void)source_info; auto outer_message = deserialize(message); - rclcpp::SerializedMessage serialized_message; - serialized_message.reserve(outer_message.data.size()); - memcpy(serialized_message.get_rcl_serialized_message().buffer, outer_message.data.data(), outer_message.data.size()); - serialized_message.get_rcl_serialized_message().buffer_length = outer_message.data.size(); - - // publish, but first advertise if publisher not present - auto topic = outer_message.destination_topic; - if(topic.empty()) - topic = outer_message.source_topic; - - // Three-phase lookup so create_generic_publisher() runs WITHOUT - // publishers_mutex_ held. Holding the lock across rmw discovery / - // type-support resolution would block every other receive-decode - // path on first-arrival creation — exactly the wedge mechanism this - // commit is hardening against. - rclcpp::GenericPublisher::SharedPtr publisher; - bool first_arrival = false; - - // Phase 1: check map under lock. - { - std::lock_guard lock(publishers_mutex_); - auto it = publishers_.find(topic); - if(it != publishers_.end()) - publisher = it->second; - } + // Socket-drain thread does only the CPU-bound deserialize + payload copy, + // then hands everything rmw-touching to the publish worker. No publisher + // create / graph query / publish runs here — see issue #10 and + // publishItem(). The drain thread must never make a call that can block + // on a single subscriber. + PublishItem item; + item.message.reserve(outer_message.data.size()); + memcpy(item.message.get_rcl_serialized_message().buffer, + outer_message.data.data(), outer_message.data.size()); + item.message.get_rcl_serialized_message().buffer_length = outer_message.data.size(); + + // destination_topic falls back to source_topic when unset. + item.topic = outer_message.destination_topic; + if(item.topic.empty()) + item.topic = outer_message.source_topic; + item.datatype = outer_message.datatype; + item.reliability = outer_message.reliability; + item.durability = outer_message.durability; + item.history_depth = outer_message.history_depth; + + publish_queue_.push(std::move(item)); +} - // Phase 2: if missing, do the slow rmw call without holding the lock. - if(!publisher) +void UDPBridge::publishItem(PublishItem&& item) +{ + // Runs on the publish worker (issue #10). Find-or-create the destination + // publisher (first arrival also sends bridge info) and publish. Any + // blocking here — a RELIABLE publish() to a dead-but-still-matched + // subscriber, or first-arrival rmw discovery in create_generic_publisher() + // — stalls only this worker, never the socket-drain thread. + // + // Catch + log here, mirroring UDPBridge::decode's catch-all: before #10 + // this work ran inside decode() on the socket-drain executor thread, whose + // catch-all logged and dropped bad packets instead of tearing the node + // down. The worker is a plain std::thread (PublishQueue has a last-resort + // barrier too, but it can't log), and every step here can throw — + // create_generic_publisher on an unknown / cross-version datatype, publish + // from the rcl layer, and sendBridgeInfo's ConnectionException on socket + // errors. Reproduce that survivable-error contract here. + try { - // Resolve per-topic QoS from MessageInternal (sender-advertised), - // falling back to package defaults when fields are empty/zero — - // see udp_bridge/doc/qos_design.md for the contract. - auto qos = resolveDestinationPublisherQos( - outer_message.reliability, - outer_message.durability, - outer_message.history_depth); - auto new_publisher = create_generic_publisher(topic, outer_message.datatype, qos); - - // Phase 3: re-acquire and try-emplace. If another thread beat us - // (raced and inserted first), we use theirs and discard ours. + // Three-phase lookup so create_generic_publisher() runs WITHOUT + // publishers_mutex_ held; holding the lock across rmw discovery / + // type-support resolution would block every concurrent publisher lookup. + // (A single worker today means no concurrent publishItem; the pattern is + // retained for the planned per-publisher-worker evolution — see the #10 + // plan's deferred cross-topic-isolation follow-up.) + rclcpp::GenericPublisher::SharedPtr publisher; + bool first_arrival = false; + + // Phase 1: check map under lock. { std::lock_guard lock(publishers_mutex_); - auto [it, inserted] = publishers_.try_emplace(topic, new_publisher); - publisher = it->second; - first_arrival = inserted; + auto it = publishers_.find(item.topic); + if(it != publishers_.end()) + publisher = it->second; } - } - // sendBridgeInfo and publish run without publishers_mutex_ held — - // both are slow operations that should not block the publishers_ map. - if(first_arrival) - sendBridgeInfo(); + // Phase 2: if missing, do the slow rmw call without holding the lock. + if(!publisher) + { + // Resolve per-topic QoS from MessageInternal (sender-advertised), + // falling back to package defaults when fields are empty/zero — + // see udp_bridge/doc/qos_design.md for the contract. + auto qos = resolveDestinationPublisherQos( + item.reliability, + item.durability, + item.history_depth); + auto new_publisher = create_generic_publisher(item.topic, item.datatype, qos); + + // Phase 3: re-acquire and try-emplace. If a future multi-worker setup + // raced and inserted first, we use theirs and discard ours. + { + std::lock_guard lock(publishers_mutex_); + auto [it, inserted] = publishers_.try_emplace(item.topic, new_publisher); + publisher = it->second; + first_arrival = inserted; + } + } + + // sendBridgeInfo and publish run without publishers_mutex_ held — both + // are slow operations that should not block the publishers_ map. + if(first_arrival) + sendBridgeInfo(); - publisher->publish(serialized_message); + publisher->publish(item.message); + } + catch(const std::exception& e) + { + RCLCPP_ERROR_STREAM(get_logger(), + "publish worker: dropping message for '" << item.topic + << "' (type '" << item.datatype << "'): " << e.what()); + } } void UDPBridge::decodeBridgeInfo(std::vector const &message, const SourceInfo& source_info) @@ -1861,6 +1967,36 @@ void UDPBridge::diagnoseRemoteGiveups(const std::string& remote_name, stat.summary(diag.level, summary.str()); } +void UDPBridge::diagnosePublishQueue(diagnostic_updater::DiagnosticStatusWrapper& stat) +{ + // dropped_count() is atomic; depth takes the queue's brief internal lock. + // last_reported_publish_drops_ is touched only here (periodic_group_ is + // MutuallyExclusive), so the "recent" delta needs no extra synchronization. + const uint64_t dropped = publish_queue_.dropped_count(); + const uint64_t depth = static_cast(publish_queue_.size()); + const uint64_t recent = dropped - last_reported_publish_drops_; + last_reported_publish_drops_ = dropped; + + stat.add("queued_items", depth); + stat.add("dropped_total", dropped); + stat.add("dropped_since_last_tick", recent); + stat.add("max_bytes", static_cast(publish_queue_max_bytes_)); + + // WARN on *recent* drops rather than latching forever after one historical + // drop. A drop means a local destination subscriber stalled the publish + // path long enough to overflow the byte budget — republished data was lost + // (unrecoverable; the resend layer operates upstream of decodeData and + // cannot recover it — see doc/qos_design.md). + if(recent > 0) + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, + "publish queue dropping (" + std::to_string(recent) + + " since last tick) — local subscriber stalled?"); + else + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, + "queued " + std::to_string(depth) + ", " + std::to_string(dropped) + + " dropped total"); +} + // void UDPBridge::maximumPacketSizeCallback(const std_msgs::Int32::ConstPtr& msg) // { // max_packet_size_ = msg->data; diff --git a/udp_bridge/src/udp_bridge_node.cpp b/udp_bridge/src/udp_bridge_node.cpp index 1ba294f..8400d44 100644 --- a/udp_bridge/src/udp_bridge_node.cpp +++ b/udp_bridge/src/udp_bridge_node.cpp @@ -4,19 +4,21 @@ // // socket_drain_group_ (MutuallyExclusive) // Owns: spin_timer_ and all work executed inline from spin_once() -// (recvfrom, decode, decodeData, the per-topic generic-publisher -// publish, and the resend / cleanup tail loops). Decoded -// destination-side publishes currently happen on this hot path — if -// a destination publisher stalls under back-pressure, the socket -// drain stalls with it. A future change can move the publish work -// to a separate group via an internal queue; see qos_design.md -// for the destination-publisher reliability policy that bounds -// this exposure under the BEST_AVAILABLE design intent. The -// current operational default is RELIABLE (rmw_zenoh_cpp 0.2.9 -// graph-visibility workaround) which does NOT bound this exposure -// — a RELIABLE destination publisher stalled on a dead subscriber -// buffers and can wedge the drain path. The risk is accepted -// until BEST_AVAILABLE returns. +// (recvfrom, decode, decodeData, and the resend / cleanup tail loops). +// As of issue #10, decodeData does NOT publish on this thread: it only +// deserializes the outer MessageInternal and enqueues a PublishItem on +// UDPBridge::publish_queue_. The rmw-touching republish work +// (find-or-create the destination publisher, first-arrival +// sendBridgeInfo, publish) runs on the publish worker thread +// (UDPBridge::publishItem). This is what keeps a stalled destination +// publisher — e.g. a RELIABLE publisher buffering for a dead-but- +// still-matched subscriber, the wedge mechanism in #10 — from stalling +// the socket drain: the worker absorbs the block, recvfrom keeps +// draining, and the bounded queue drops oldest under back-pressure. +// See qos_design.md for the destination-publisher reliability policy +// (RELIABLE operational default under rmw_zenoh_cpp 0.2.9; BEST_AVAILABLE +// design intent) — the publish decoupling fixes the wedge independently +// of which reliability is in effect. // Invariant: this group must never be starved. Anything that runs // here is hot-path; if it blocks, the kernel SO_RCVBUF (500 KB) fills // and packets are silently dropped — the wedge symptom in #10. @@ -52,6 +54,25 @@ // work that must not race the hot path but can be safely serialized // against itself. // +// publish worker (UDPBridge::publish_queue_'s std::thread — issue #10) +// NOT an executor callback group: a single std::thread owned by +// PublishQueue, running only while the node is ACTIVE (started in +// on_activate, stopped+joined in on_deactivate). Runs UDPBridge:: +// publishItem — find-or-create the destination publisher, first-arrival +// sendBridgeInfo, and publish — work that decodeData used to do inline on +// socket_drain_group_. Moving it here is what stops a stalled destination +// publisher from wedging the drain (the #10 fix). Being outside the +// callback-group model, it is a new concurrent caller of the publishers_ +// map and the outbound-send path, so it was checked against the PR #12 / +// #16 audit: it takes publishers_mutex_ for the three-phase lookup like +// decodeData did; sendBridgeInfo takes its own deadlock-avoiding +// scoped_lock(subscribers_mutex_, remote_nodes_mutex_), so two concurrent +// senders (worker + a periodic-group sendBridgeInfo) can't deadlock; and +// concurrent recvfrom (drain) / sendto (worker) on the one UDP fd is safe. +// publishItem catches + logs its own exceptions (mirroring decode's +// catch-all) and PublishQueue::run has a last-resort barrier, so a bad +// packet can't terminate the process. +// // Shared state crossing groups is guarded by mutexes (publishers_, // subscribers_, remote_nodes_, pending_connections_, Connection::sent_packets_, // statistics). See PR #12 / issue #11 for the full audit. diff --git a/udp_bridge/test/test_publish_queue.cpp b/udp_bridge/test/test_publish_queue.cpp new file mode 100644 index 0000000..ca47f42 --- /dev/null +++ b/udp_bridge/test/test_publish_queue.cpp @@ -0,0 +1,318 @@ +// Unit tests for udp_bridge::PublishQueue (issue #10). +// +// The queue decouples the rmw-touching republish tail of decodeData from the +// socket-drain thread so a stalled publish() can't wedge recvfrom. These +// tests exercise that contract in isolation via an injected sink — no node, +// no DDS. The blocking-sink test is the in-process proxy for the field bug +// (a RELIABLE publish stalling on an uncleanly-dead subscriber); the live-DDS +// kill-subscriber reproduction lives in the issue #18 bench harness. + +#include "udp_bridge/publish_queue.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std::chrono_literals; +using udp_bridge::PublishItem; +using udp_bridge::PublishQueue; + +namespace +{ + +// Build a PublishItem whose byte_size() is (payload_bytes + topic.size()). +// We never read the payload — only its accounted size matters here. +PublishItem makeItem(const std::string& topic, size_t payload_bytes) +{ + PublishItem item; + item.topic = topic; + if(payload_bytes > 0) + { + item.message.reserve(payload_bytes); + item.message.get_rcl_serialized_message().buffer_length = payload_bytes; + } + return item; +} + +// A sink that blocks on a latch from its first invocation until release(). +// Declared so a test can construct it BEFORE the PublishQueue, guaranteeing +// the queue (and its worker) is destroyed first — i.e. the latch outlives the +// worker that waits on it. Always release() before the test ends. +struct BlockingSink +{ + std::mutex m; + std::condition_variable cv; + bool released = false; + std::atomic calls {0}; + + void operator()(PublishItem&&) + { + calls.fetch_add(1); + std::unique_lock lock(m); + cv.wait(lock, [this]{ return released; }); + } + + void release() + { + { + std::lock_guard lock(m); + released = true; + } + cv.notify_all(); + } + + // Wait (bounded) until the worker is parked inside the sink (has popped an + // item). Fails fast with a clear message instead of spinning forever if the + // worker never runs (a regression, start() failure, or scheduling stall) — + // an unbounded spin would otherwise surface only as an opaque colcon test + // timeout. + void wait_until_blocked() + { + const auto deadline = std::chrono::steady_clock::now() + 2s; + while(calls.load() == 0) + { + ASSERT_LT(std::chrono::steady_clock::now(), deadline) + << "publish worker never entered the sink"; + std::this_thread::sleep_for(1ms); + } + } +}; + +} // namespace + +// The core invariant and the bug: push() must never block on a stalled sink. +// With the worker parked inside a never-returning sink, a burst of pushes must +// still return effectively immediately. +TEST(PublishQueue, PushDoesNotBlockOnStalledSink) +{ + BlockingSink sink; + PublishQueue q; + q.configure([&sink](PublishItem&& i){ sink(std::move(i)); }, + /*max_bytes=*/ 1024u * 1024u); + q.start(); + + // Occupy the worker: it pops this item and blocks in the sink. + q.push(makeItem("prime", 64)); + sink.wait_until_blocked(); + + // Now the consumer is wedged. Pushing must not be. + const auto t0 = std::chrono::steady_clock::now(); + for(int i = 0; i < 200; ++i) + q.push(makeItem("t", 64)); + const auto elapsed = std::chrono::steady_clock::now() - t0; + + // 200 pushes against a wedged consumer in well under a second (real cost is + // microseconds; the bound is generous to avoid CI flakiness). + EXPECT_LT(elapsed, 1s); + + sink.release(); // let the worker drain + the dtor join cleanly +} + +// FIFO order through the worker. +TEST(PublishQueue, PreservesFifoOrder) +{ + std::mutex order_mutex; + std::vector order; + + // Gate the worker so all items are queued before any is processed; once + // opened, every sink call returns immediately, so processing is in queue + // (FIFO) order. + std::mutex gate_mutex; + std::condition_variable gate_cv; + bool open = false; + + PublishQueue q; + q.configure( + [&](PublishItem&& item) + { + { + std::unique_lock lock(gate_mutex); + gate_cv.wait(lock, [&]{ return open; }); + } + std::lock_guard lock(order_mutex); + order.push_back(item.topic); + }, + /*max_bytes=*/ 1024u * 1024u); + q.start(); + + const std::vector topics = {"a", "b", "c", "d", "e"}; + for(const auto& t : topics) + q.push(makeItem(t, 16)); + + { + std::lock_guard lock(gate_mutex); + open = true; + } + gate_cv.notify_all(); + + // Wait (bounded) for all items to be processed before stopping — stop() + // discards anything still queued. + const auto deadline = std::chrono::steady_clock::now() + 2s; + for(;;) + { + { + std::lock_guard lock(order_mutex); + if(order.size() == topics.size()) + break; + } + ASSERT_LT(std::chrono::steady_clock::now(), deadline) << "worker did not drain in time"; + std::this_thread::sleep_for(1ms); + } + q.stop(); + + EXPECT_EQ(order, topics); +} + +// Over the byte budget, push() drops the oldest queued item(s) and counts them. +TEST(PublishQueue, DropsOldestOverByteBudget) +{ + BlockingSink sink; + // Budget holds three 101-byte items (303) but not four (404). + PublishQueue q; + q.configure([&sink](PublishItem&& i){ sink(std::move(i)); }, /*max_bytes=*/ 350u); + q.start(); + + // Park the worker so nothing drains; the queue then exercises pure push + // overflow logic. The prime item (byte_size 101) is popped, not queued. + q.push(makeItem("p", 100)); // topic "p" (1) + 100 = 101 bytes + sink.wait_until_blocked(); + + // Each item is 101 bytes ("i" + 100). Push five. + for(int i = 0; i < 5; ++i) + q.push(makeItem("i", 100)); + + // Budget 350 holds 3 (303); the 4th and 5th each evict the oldest. + EXPECT_EQ(q.size(), 3u); + EXPECT_EQ(q.dropped_count(), 2u); + + sink.release(); +} + +// A single item larger than the whole budget is still enqueued (never silently +// lost), and the next push evicts it to make room. +TEST(PublishQueue, OversizeItemEnqueuedThenEvicted) +{ + BlockingSink sink; + PublishQueue q; + q.configure([&sink](PublishItem&& i){ sink(std::move(i)); }, /*max_bytes=*/ 100u); + q.start(); + + q.push(makeItem("p", 64)); // prime → worker blocks + sink.wait_until_blocked(); + + q.push(makeItem("big", 500)); // 503 bytes > budget, but queue empty → enqueued + EXPECT_EQ(q.size(), 1u); + EXPECT_EQ(q.dropped_count(), 0u); + + q.push(makeItem("big2", 500)); // evicts the first oversize item + EXPECT_EQ(q.size(), 1u); + EXPECT_EQ(q.dropped_count(), 1u); + + sink.release(); +} + +// push() after stop() is a no-op (counted as a drop), so it is safe to call at +// any lifecycle phase — including the deactivate teardown window. +TEST(PublishQueue, PushAfterStopIsDropped) +{ + PublishQueue q; + q.configure([](PublishItem&&){}, /*max_bytes=*/ 1024u); + q.start(); + q.stop(); + + q.push(makeItem("x", 16)); + EXPECT_EQ(q.size(), 0u); + EXPECT_EQ(q.dropped_count(), 1u); +} + +// push() before the worker is ever started is also a safe no-op drop. +TEST(PublishQueue, PushBeforeStartIsDropped) +{ + PublishQueue q; + q.configure([](PublishItem&&){}, /*max_bytes=*/ 1024u); + q.push(makeItem("x", 16)); + EXPECT_EQ(q.size(), 0u); + EXPECT_EQ(q.dropped_count(), 1u); +} + +// stop() joins cleanly even while the worker is blocked in the sink — it waits +// for the one in-flight sink call to return, then returns. This bounds +// shutdown to a single sink call (in the field, the rmw publish's +// max_blocking_time) rather than the whole backlog. +TEST(PublishQueue, StopWhileSinkBlockedJoinsAfterSinkReturns) +{ + BlockingSink sink; + PublishQueue q; + q.configure([&sink](PublishItem&& i){ sink(std::move(i)); }, /*max_bytes=*/ 1024u); + q.start(); + + q.push(makeItem("p", 16)); + sink.wait_until_blocked(); + + std::atomic stop_done {false}; + std::thread stopper([&]{ q.stop(); stop_done.store(true); }); + + // While the sink is blocked, stop() cannot complete. + std::this_thread::sleep_for(50ms); + EXPECT_FALSE(stop_done.load()); + + // Releasing the sink lets the in-flight call return; stop() then joins. + sink.release(); + stopper.join(); + EXPECT_TRUE(stop_done.load()); +} + +// A throwing sink must not terminate the worker: PublishQueue::run wraps the +// sink call in a last-resort barrier (issue #10 — the sink, UDPBridge:: +// publishItem, can throw from create_generic_publisher / publish / +// sendBridgeInfo, and a plain std::thread would otherwise std::terminate). +// The worker must keep draining subsequent items. +TEST(PublishQueue, ThrowingSinkDoesNotKillWorker) +{ + std::mutex m; + std::vector processed; + + PublishQueue q; + q.configure( + [&](PublishItem&& item) + { + if(item.topic == "boom") + throw std::runtime_error("sink failure"); + std::lock_guard lock(m); + processed.push_back(item.topic); + }, + /*max_bytes=*/ 1024u * 1024u); + q.start(); + + q.push(makeItem("boom", 16)); // sink throws — must be swallowed + q.push(makeItem("after", 16)); // worker must survive and process this + + const auto deadline = std::chrono::steady_clock::now() + 2s; + for(;;) + { + { + std::lock_guard lock(m); + if(!processed.empty()) + break; + } + ASSERT_LT(std::chrono::steady_clock::now(), deadline) + << "worker did not survive the throwing sink"; + std::this_thread::sleep_for(1ms); + } + + std::lock_guard lock(m); + ASSERT_EQ(processed.size(), 1u); + EXPECT_EQ(processed.front(), "after"); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}