diff --git a/.github/workflows/host_sanitizers.yml b/.github/workflows/host_sanitizers.yml new file mode 100644 index 0000000000..8163aaf5bd --- /dev/null +++ b/.github/workflows/host_sanitizers.yml @@ -0,0 +1,119 @@ +name: Host sanitizers (TSan / ASan) + +# Build the host library + pc test suite under ThreadSanitizer and +# AddressSanitizer and run the standalone (non-interop) tests. The RTPS stack +# is heavily concurrent (transport worker pool, socket reactor, protocol +# scheduler, deferred dispatchers), and most defects found in review have been +# data races, deadlocks, and use-after-frees - exactly the classes these +# sanitizers detect mechanically. Runs whenever RTPS or its concurrency +# building blocks change. + +# Minimal token scope: build + run tests, never writes. +permissions: + contents: read + +on: + pull_request: + paths: + - "components/rtps/**" + - "components/socket/**" + - "components/thread_pool/**" + - "components/task/**" + - "components/timer/**" + - "components/cdr/**" + - "lib/espp.cmake" + - "pc/tests/rtps_*" + - "pc/tests/socket_reactor.cpp" + - "pc/tests/thread_pool.cpp" + - ".github/workflows/host_sanitizers.yml" + workflow_dispatch: + +# Supersede in-progress runs (same rationale as rtps_interop.yml): keyed by +# workflow + PR number; never runs on push to main, so cancel is always safe. +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +jobs: + sanitize: + runs-on: ubuntu-latest + timeout-minutes: 45 + strategy: + fail-fast: false + matrix: + include: + - sanitizer: thread + cflags: "-fsanitize=thread -fno-omit-frame-pointer" + ldflags: "-fsanitize=thread" + - sanitizer: address + cflags: "-fsanitize=address -fno-omit-frame-pointer" + ldflags: "-fsanitize=address" + name: ${{ matrix.sanitizer }} + steps: + - uses: actions/checkout@v7 + with: + submodules: "recursive" + + - name: Build library (${{ matrix.sanitizer }}) + run: | + cmake -S lib -B lib/build \ + -DCMAKE_BUILD_TYPE=RelWithDebInfo \ + -DESPP_INSTALL=ON \ + -DESPP_BUILD_PYTHON=OFF \ + -DCMAKE_INSTALL_PREFIX="$PWD/install" \ + -DCMAKE_C_FLAGS="${{ matrix.cflags }}" \ + -DCMAKE_CXX_FLAGS="${{ matrix.cflags }}" \ + -DCMAKE_EXE_LINKER_FLAGS="${{ matrix.ldflags }}" \ + -DCMAKE_SHARED_LINKER_FLAGS="${{ matrix.ldflags }}" + cmake --build lib/build --target install --parallel 4 + + - name: Build pc tests (${{ matrix.sanitizer }}) + run: | + cmake -S pc -B pc/build \ + -DCMAKE_BUILD_TYPE=RelWithDebInfo \ + -DCMAKE_PREFIX_PATH="$PWD/install" \ + -DCMAKE_CXX_FLAGS="${{ matrix.cflags }}" \ + -DCMAKE_EXE_LINKER_FLAGS="${{ matrix.ldflags }}" + cmake --build pc/build --parallel 4 + + - name: Run standalone tests under ${{ matrix.sanitizer }} sanitizer + run: | + # halt_on_error=0: report every finding, fail via the exit code. + # detect_leaks=0: the engine deliberately holds pooled/static + # allocations for its lifetime; LSan end-of-process reports would be + # noise. ASan still catches use-after-free / overflow. + # + # The lock-order (deadlock) detector is ENABLED. Its first runs found + # four real ABBA cycles, all now fixed: checkAndResetHeartbeats + # agent->participant order, addBuiltInEndpoints lock scope, + # executeCallbacks snapshot-invoke, and StatefulReader delivering + # user callbacks under a leaf delivery mutex instead of the proxies + # mutex (which user callbacks could cycle back into via the + # facade/SEDP lock chain). + export TSAN_OPTIONS="second_deadlock_stack=1" + export ASAN_OPTIONS="detect_leaks=0" + fails=0 + # The interop client/server binaries need a FastDDS/ROS 2 peer (the + # docker matrix covers them); everything else runs standalone. The + # generous per-test timeout absorbs the sanitizer's slowdown; a hang + # (e.g. a deadlock TSan cannot see) still fails the job. + for t in pc/build/rtps_* pc/build/socket_reactor pc/build/thread_pool; do + [ -x "$t" ] || continue + name=$(basename "$t") + case "$name" in *interop*) continue;; esac + echo "::group::$name" + if timeout 300 "$t"; then + echo "PASS: $name" + else + rc=$? + echo "FAIL: $name (exit $rc)" + fails=$((fails+1)) + fi + echo "::endgroup::" + done + echo "==================== ${{ matrix.sanitizer }} summary ====================" + if [ "$fails" -ne 0 ]; then + echo "FAILED: $fails test(s) under ${{ matrix.sanitizer }} sanitizer" + exit 1 + fi + echo "ALL PASS under ${{ matrix.sanitizer }} sanitizer" diff --git a/.gitignore b/.gitignore index f3de3877c1..176a1dac14 100644 --- a/.gitignore +++ b/.gitignore @@ -64,3 +64,8 @@ docs/ # Local example-build helper script (not part of the repo). build_examples.sh + +# local sanitizer / variant build trees (host TSan/ASan, static-storage validation) +lib/build-*/ +pc/build-*/ +install-*/ diff --git a/components/rtps/CMakeLists.txt b/components/rtps/CMakeLists.txt index c09058553a..2b453eb699 100644 --- a/components/rtps/CMakeLists.txt +++ b/components/rtps/CMakeLists.txt @@ -20,7 +20,7 @@ idf_component_register( INCLUDE_DIRS "include" REQUIRES - base_component cdr task thread_pool socket + base_component cdr task thread_pool timer socket ) # Select the RTPS static-limits profile from Kconfig (see Kconfig in this @@ -40,6 +40,118 @@ endif() # limits profile never silently switches the MCU to heap-backed history. The # limits headers no longer define RTPS_STORAGE_DYNAMIC themselves; it is set here # on ESP and defaulted on in config.hpp for host/PC builds. +# Per-limit capacity overrides (Kconfig "Custom capacity overrides"): a +# nonzero value overrides that single cap of the selected profile via its +# RTPS_CFG_* macro. PUBLIC so application translation units see the same +# values as the engine (the pools are sized in this component's sources). +set(RTPS_LIMIT_KNOBS + NUM_STATELESS_WRITERS + NUM_STATELESS_READERS + NUM_STATEFUL_WRITERS + NUM_STATEFUL_READERS + MAX_NUM_PARTICIPANTS + NUM_WRITERS_PER_PARTICIPANT + NUM_READERS_PER_PARTICIPANT + NUM_WRITER_PROXIES_PER_READER + NUM_READER_PROXIES_PER_WRITER + MAX_NUM_UNMATCHED_REMOTE_WRITERS + MAX_NUM_UNMATCHED_REMOTE_READERS + MAX_NUM_READER_CALLBACKS + HISTORY_SIZE_STATELESS + HISTORY_SIZE_STATEFUL + MAX_TYPENAME_LENGTH + MAX_TOPICNAME_LENGTH + MAX_NUM_UDP_CONNECTIONS +) +foreach(knob ${RTPS_LIMIT_KNOBS}) + if(DEFINED CONFIG_RTPS_LIMIT_${knob} AND NOT "${CONFIG_RTPS_LIMIT_${knob}}" STREQUAL "" AND NOT "${CONFIG_RTPS_LIMIT_${knob}}" STREQUAL "0") + # Builtin-aware minima (0 stays the "use profile default" sentinel): the + # discovery endpoints draw from these pools, so a nonzero value below the + # reservation would crash startup (the second SEDP allocation returns null) + # or silently omit the builtins (per-participant caps < 3). + if((knob STREQUAL "NUM_STATEFUL_WRITERS" OR knob STREQUAL "NUM_STATEFUL_READERS") + AND CONFIG_RTPS_LIMIT_${knob} LESS 2) + message(FATAL_ERROR + "CONFIG_RTPS_LIMIT_${knob}=${CONFIG_RTPS_LIMIT_${knob}} is below the minimum of 2 " + "(2 slots are reserved for the builtin SEDP endpoints)") + endif() + if((knob STREQUAL "NUM_WRITERS_PER_PARTICIPANT" OR knob STREQUAL "NUM_READERS_PER_PARTICIPANT") + AND CONFIG_RTPS_LIMIT_${knob} LESS 3) + message(FATAL_ERROR + "CONFIG_RTPS_LIMIT_${knob}=${CONFIG_RTPS_LIMIT_${knob}} is below the minimum of 3 " + "(the 3 builtin discovery endpoints count against the per-participant cap)") + endif() + if(knob STREQUAL "MAX_NUM_UDP_CONNECTIONS" AND CONFIG_RTPS_LIMIT_${knob} LESS 4) + message(FATAL_ERROR + "CONFIG_RTPS_LIMIT_${knob}=${CONFIG_RTPS_LIMIT_${knob}} is below the minimum of 4 " + "(2 shared multicast + 2 unicast channels for a single participant)") + endif() + target_compile_definitions(${COMPONENT_LIB} PUBLIC "RTPS_CFG_${knob}=${CONFIG_RTPS_LIMIT_${knob}}") + set(RTPS_EFFECTIVE_${knob} ${CONFIG_RTPS_LIMIT_${knob}}) + endif() +endforeach() + +# Cross-limit capacity check (mirrors lib/espp.cmake): every participant +# consumes 1 stateless writer/reader + 2 stateful writers/readers from the +# GLOBAL pools for its builtin discovery endpoints. Effective value = the +# Kconfig override if nonzero, else the selected profile's default. +if(CONFIG_RTPS_LIMITS_PROFILE_HOST) + set(_rtps_defaults 8 16 16 32 32 24) +elseif(CONFIG_RTPS_LIMITS_PROFILE_HOST_LARGE) + set(_rtps_defaults 32 64 64 128 128 72) +else() # embedded (default) + set(_rtps_defaults 1 5 5 5 5 10) +endif() +list(GET _rtps_defaults 0 _rtps_d_participants) +list(GET _rtps_defaults 1 _rtps_d_stateless_w) +list(GET _rtps_defaults 2 _rtps_d_stateless_r) +list(GET _rtps_defaults 3 _rtps_d_stateful_w) +list(GET _rtps_defaults 4 _rtps_d_stateful_r) +list(GET _rtps_defaults 5 _rtps_d_channels) +foreach(pair + "MAX_NUM_PARTICIPANTS;_rtps_d_participants" + "NUM_STATELESS_WRITERS;_rtps_d_stateless_w" + "NUM_STATELESS_READERS;_rtps_d_stateless_r" + "NUM_STATEFUL_WRITERS;_rtps_d_stateful_w" + "NUM_STATEFUL_READERS;_rtps_d_stateful_r" + "MAX_NUM_UDP_CONNECTIONS;_rtps_d_channels") + list(GET pair 0 _rtps_k) + list(GET pair 1 _rtps_dvar) + if(NOT DEFINED RTPS_EFFECTIVE_${_rtps_k}) + set(RTPS_EFFECTIVE_${_rtps_k} ${${_rtps_dvar}}) + endif() +endforeach() +math(EXPR _rtps_need_stateful "2 * ${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS}") +if(RTPS_EFFECTIVE_NUM_STATELESS_WRITERS LESS RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS + OR RTPS_EFFECTIVE_NUM_STATELESS_READERS LESS RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS + OR RTPS_EFFECTIVE_NUM_STATEFUL_WRITERS LESS _rtps_need_stateful + OR RTPS_EFFECTIVE_NUM_STATEFUL_READERS LESS _rtps_need_stateful) + message(FATAL_ERROR + "RTPS limits cannot host the participant budget: MAX_NUM_PARTICIPANTS=" + "${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS} needs >= ${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS} " + "stateless writers/readers (have ${RTPS_EFFECTIVE_NUM_STATELESS_WRITERS}/" + "${RTPS_EFFECTIVE_NUM_STATELESS_READERS}) and >= ${_rtps_need_stateful} stateful " + "writers/readers (have ${RTPS_EFFECTIVE_NUM_STATEFUL_WRITERS}/" + "${RTPS_EFFECTIVE_NUM_STATEFUL_READERS}) for the builtin discovery endpoints " + "(1 stateless W/R + 2 stateful W/R per participant). Raise the pool overrides " + "(menuconfig 'Custom capacity overrides') or lower MAX_NUM_PARTICIPANTS.") +endif() +# Channel-pool constraint (mirrors lib/espp.cmake): 2 shared multicast + 2 +# unicast channels per participant are permanently bound from the same +# MAX_NUM_UDP_CONNECTIONS pool that runtime dedicated endpoint ports draw +# from; without this check a combination can pass the endpoint-pool math yet +# createParticipant() still fails on channels first. +math(EXPR _rtps_need_channels "2 + 2 * ${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS}") +if(RTPS_EFFECTIVE_MAX_NUM_UDP_CONNECTIONS LESS _rtps_need_channels) + message(FATAL_ERROR + "RTPS limits cannot host the participant budget: MAX_NUM_PARTICIPANTS=" + "${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS} needs >= ${_rtps_need_channels} transport channels " + "(2 shared multicast + 2 unicast per participant) but MAX_NUM_UDP_CONNECTIONS is " + "${RTPS_EFFECTIVE_MAX_NUM_UDP_CONNECTIONS}. Raise MAX_NUM_UDP_CONNECTIONS via menuconfig " + "'Custom capacity overrides' (leave headroom for dedicated endpoint ports, which draw " + "from the same pool at runtime) or lower MAX_NUM_PARTICIPANTS.") +endif() + if(CONFIG_RTPS_STORAGE_DYNAMIC) target_compile_definitions(${COMPONENT_LIB} PUBLIC RTPS_STORAGE_DYNAMIC) endif() diff --git a/components/rtps/Kconfig b/components/rtps/Kconfig index d220ba0180..fde2777580 100644 --- a/components/rtps/Kconfig +++ b/components/rtps/Kconfig @@ -12,6 +12,14 @@ menu "RTPS" storage. These caps are pure capacity limits and do NOT change any bytes on the wire. + Note: the builtin discovery endpoints draw from the same pools, so + the USABLE per-participant counts are lower than the raw caps: + 1 stateless writer + 1 stateless reader are reserved for SPDP, and + 2 stateful writers + 2 stateful readers for SEDP. E.g. the embedded + profile's NUM_STATELESS_WRITERS=5 leaves 4 usable BEST_EFFORT + writers. add_writer()/add_reader() name the exhausted pool and its + cap when a creation fails. + config RTPS_LIMITS_PROFILE_EMBEDDED bool "embedded (tight MCU caps)" help @@ -33,6 +41,143 @@ menu "RTPS" only appropriate on targets with plenty of RAM. endchoice + menu "Custom capacity overrides (advanced)" + # Each option below overrides ONE capacity cap of the selected limits + # profile (0 = keep the profile's default). This gives fine-grained + # control - e.g. raise only NUM_STATELESS_WRITERS instead of paying the + # RAM for a whole relaxed profile. All values are capacity-only and do + # NOT change any bytes on the wire. The builtin discovery endpoints + # consume slots from these same pools (see the profile help above), so + # size them as "usable + builtins". For a FULLY custom profile, a + # source-level escape hatch also exists: define RTPS_CONFIG_HEADER to + # your own header path. + config RTPS_LIMIT_NUM_STATELESS_WRITERS + int "NUM_STATELESS_WRITERS (0 = profile default)" + default 0 + range 0 255 + help + Stateless (BEST_EFFORT) writer pool; 1 slot is reserved for the builtin SPDP writer. 0 keeps the selected profile's value. + + config RTPS_LIMIT_NUM_STATELESS_READERS + int "NUM_STATELESS_READERS (0 = profile default)" + default 0 + range 0 255 + help + Stateless (BEST_EFFORT) reader pool; 1 slot is reserved for the builtin SPDP reader. 0 keeps the selected profile's value. + + config RTPS_LIMIT_NUM_STATEFUL_WRITERS + int "NUM_STATEFUL_WRITERS (0 = profile default)" + default 0 + range 0 255 + help + Stateful (RELIABLE) writer pool; 2 slots are reserved for the builtin SEDP writers. 0 keeps the selected profile's value. + + config RTPS_LIMIT_NUM_STATEFUL_READERS + int "NUM_STATEFUL_READERS (0 = profile default)" + default 0 + range 0 255 + help + Stateful (RELIABLE) reader pool; 2 slots are reserved for the builtin SEDP readers. 0 keeps the selected profile's value. + + config RTPS_LIMIT_MAX_NUM_PARTICIPANTS + int "MAX_NUM_PARTICIPANTS (0 = profile default)" + default 0 + range 0 255 + help + Maximum domain participants per process. 0 keeps the selected profile's value. + + config RTPS_LIMIT_NUM_WRITERS_PER_PARTICIPANT + int "NUM_WRITERS_PER_PARTICIPANT (0 = profile default)" + default 0 + range 0 255 + help + Per-participant writer cap; the 3 builtin discovery writers count against it. 0 keeps the selected profile's value. + + config RTPS_LIMIT_NUM_READERS_PER_PARTICIPANT + int "NUM_READERS_PER_PARTICIPANT (0 = profile default)" + default 0 + range 0 255 + help + Per-participant reader cap; the 3 builtin discovery readers count against it. 0 keeps the selected profile's value. + + config RTPS_LIMIT_NUM_WRITER_PROXIES_PER_READER + int "NUM_WRITER_PROXIES_PER_READER (0 = profile default)" + default 0 + range 0 255 + help + Remote writers a single reader can match. 0 keeps the selected profile's value. + + config RTPS_LIMIT_NUM_READER_PROXIES_PER_WRITER + int "NUM_READER_PROXIES_PER_WRITER (0 = profile default)" + default 0 + range 0 255 + help + Remote readers a single writer can match. 0 keeps the selected profile's value. + + config RTPS_LIMIT_MAX_NUM_UNMATCHED_REMOTE_WRITERS + int "MAX_NUM_UNMATCHED_REMOTE_WRITERS (0 = profile default)" + default 0 + range 0 255 if RTPS_LIMITS_PROFILE_EMBEDDED + range 0 65535 + help + Registry of discovered-but-unmatched remote writers. 0 keeps the selected profile's value. + + config RTPS_LIMIT_MAX_NUM_UNMATCHED_REMOTE_READERS + int "MAX_NUM_UNMATCHED_REMOTE_READERS (0 = profile default)" + default 0 + range 0 255 if RTPS_LIMITS_PROFILE_EMBEDDED + range 0 65535 + help + Registry of discovered-but-unmatched remote readers. 0 keeps the selected profile's value. + + config RTPS_LIMIT_MAX_NUM_READER_CALLBACKS + int "MAX_NUM_READER_CALLBACKS (0 = profile default)" + default 0 + range 0 255 + help + Callback registrations per reader. 0 keeps the selected profile's value. + + config RTPS_LIMIT_HISTORY_SIZE_STATELESS + int "HISTORY_SIZE_STATELESS (0 = profile default)" + default 0 + range 0 255 + help + History depth (samples) per stateless endpoint. 0 keeps the selected profile's value. + + config RTPS_LIMIT_HISTORY_SIZE_STATEFUL + int "HISTORY_SIZE_STATEFUL (0 = profile default)" + default 0 + range 0 255 + help + History depth (samples) per stateful endpoint. 0 keeps the selected profile's value. + + config RTPS_LIMIT_MAX_TYPENAME_LENGTH + int "MAX_TYPENAME_LENGTH (0 = profile default)" + default 0 + range 0 255 + help + Maximum DDS type-name length. 0 keeps the selected profile's value. + + config RTPS_LIMIT_MAX_TOPICNAME_LENGTH + int "MAX_TOPICNAME_LENGTH (0 = profile default)" + default 0 + range 0 255 + help + Maximum DDS topic-name length. 0 keeps the selected profile's value. + + config RTPS_LIMIT_MAX_NUM_UDP_CONNECTIONS + int "MAX_NUM_UDP_CONNECTIONS (0 = profile default)" + default 0 + range 0 255 + help + Transport channel pool: 2 shared multicast channels + 2 unicast + channels per participant are permanently bound, and dedicated + endpoint ports draw from the same pool at runtime. Must be at + least 2 + 2*MAX_NUM_PARTICIPANTS (cross-checked at build time). + 0 keeps the selected profile's value. + + endmenu + config RTPS_STORAGE_DYNAMIC bool "Use dynamic (heap) history/queue storage" default n diff --git a/components/rtps/README.md b/components/rtps/README.md index 39287a0133..92a7c87a22 100644 --- a/components/rtps/README.md +++ b/components/rtps/README.md @@ -66,7 +66,7 @@ flowchart TD direction TB TR["rtps::EsppTransport"] SOCK["espp::UdpSocket × N ports"] - REACT["espp::SocketReactor → espp::ThreadPool"] + REACT["espp::SocketReactor → espp::ThreadPool (QosBand priority)"] CDR["espp::cdr (reflection CDR/XCDR)"] TR --> SOCK --> REACT end @@ -168,6 +168,89 @@ via `include/rtps/config.hpp`. The domain id, announcement/heartbeat periods, and pool sizes live in the profile headers. +### Per-limit capacity overrides + +Every capacity cap in the profile headers can be raised (or lowered) +**individually** on top of the selected profile - so a system that only needs +more BEST_EFFORT writers does not have to pay the RAM for a whole relaxed +profile. Each cap `NAME` is overridable via an `RTPS_CFG_` compile +definition (see the `RTPS_CFG_*` blocks in `include/rtps/config_*.hpp` for the +full knob list): + +- **ESP-IDF**: menuconfig, `RTPS -> Custom capacity overrides (advanced)` - + each option overrides one cap; `0` keeps the profile default. +- **Host (espp.cmake / lib build)**: pass a semicolon list, e.g. + `-DRTPS_LIMIT_OVERRIDES="NUM_STATELESS_WRITERS=16;HISTORY_SIZE_STATEFUL=20"`. + +The overrides must be applied when **compiling the rtps sources** (the pools +are sized inside the library); both mechanisms above do this and propagate the +same values to consumer translation units. Defining `RTPS_CFG_*` for only a +consumer TU (e.g. before including the headers in application code) would +silently disagree with the library and must be avoided. All caps are +capacity-only and change no bytes on the wire. + +Note the builtin discovery endpoints consume slots from the same pools: 1 +stateless writer + 1 stateless reader for SPDP and 2 stateful writers + 2 +stateful readers for SEDP (which also count against the per-participant caps) - +so size pools as "usable + builtins". `add_writer()`/`add_reader()` report the +bound limits, the reserved slots, and the usable counts when a creation fails. + +For a **fully custom profile**, the source-level escape hatch is defining +`RTPS_CONFIG_HEADER` to your own header path (it replaces the profile header +entirely). + +--- + +## Priority scheduling (bands, dedicated ports, DSCP) + +Every transport channel is dispatched through the `SocketReactor`/`ThreadPool` +at a **priority band** (`espp::QosBand`). Defaults: metatraffic (SPDP/SEDP +discovery) at `High` — so discovery stays responsive when user traffic backs the +pool up — and the shared user channels at `Normal`. Both are configurable +(`RtpsParticipant::Config::metatraffic_band` / `user_traffic_band`); apart from +the two default changes below, an unconfigured participant behaves exactly as +before: + +1. **Metatraffic elevation** — discovery dispatches at `High` instead of + `Normal` (above). +2. **Bounded transport pool queue** — the transport's worker-pool queue is now + bounded (64 jobs) instead of unbounded. Under sustained overload a + submission is rejected rather than growing an unbounded heap backlog; + rejection is a real backpressure signal that the reactor (re-arm the socket + on the next `select()`) and the deferred/guaranteed retry paths recover + from without loss. This changes behavior only under extreme overload, where + the previous unbounded queue would have grown memory without bound. + +Since all of a participant's user traffic shares one user-unicast port, +per-endpoint priority uses **dedicated ports**: give a writer/reader config a +non-default `band` (or a `dscp`) and the endpoint gets its own unicast port — +allocated deterministically at `7400 + 250*domain + 100 + n`, probing at most +16 consecutive candidates per request (reuse-disabled bind) from an advancing +cursor; if the whole window is occupied the endpoint falls back to the shared +user port and the next request resumes past the window — whose socket runs at +the endpoint's band and is +optionally DSCP-marked (`espp::Dscp`, e.g. `Dscp::Ef`; the endpoint also sends +from this socket, so the marking applies to its outgoing traffic). The +endpoint's SEDP announcement carries the dedicated port as its standard +per-endpoint unicast locator (`PID_UNICAST_LOCATOR`), which FastDDS/ROS 2 honor +— the wire format is unchanged, only the announced port value differs. + +Dedicated ports are **rationed** (`Config::max_prioritized_endpoint_ports`, +default 4; each is one fd, and lwIP on ESP32 has ~10 total with 4 already used +by the participant). Past the cap — or with +`Config::enable_dedicated_endpoint_ports = false` — a banded endpoint logs a +warning and falls back to the shared port; banded *readers* then get +**deferred banded dispatch**: samples are queued (bounded, 32/reader) and the +callback is re-submitted to the transport pool at the reader's band, one +in-flight delivery per reader, preserving per-reader order. `Normal` endpoints +keep the original inline delivery path. + +`ServiceConfig`/`ActionConfig` accept the same `band`/`dscp`: a service applies +them to both of its endpoints (request + reply); an action passes them to all +of its underlying service/topic endpoints (note a ROS action server is ~8 +endpoints — more than the default ration, so most fall back to deferred +dispatch unless the cap is raised). + --- ## ESPP component dependencies diff --git a/components/rtps/idf_component.yml b/components/rtps/idf_component.yml index b9a5018e8e..472671de04 100644 --- a/components/rtps/idf_component.yml +++ b/components/rtps/idf_component.yml @@ -22,4 +22,5 @@ dependencies: espp/cdr: '>=1.0' espp/task: '>=1.0' espp/thread_pool: '>=1.0' + espp/timer: '>=1.0' espp/socket: '>=1.0' diff --git a/components/rtps/include/rtps/communication/EsppTransport.hpp b/components/rtps/include/rtps/communication/EsppTransport.hpp index ecb2a952ca..70bc2bf8c6 100644 --- a/components/rtps/include/rtps/communication/EsppTransport.hpp +++ b/components/rtps/include/rtps/communication/EsppTransport.hpp @@ -24,21 +24,38 @@ This file is part of the espp embeddedRTPS port. #define RTPS_ESPPTRANSPORT_H #include "base_component.hpp" +#include "dscp.hpp" +#include "qos_band.hpp" #include "rtps/common/types.hpp" #include "rtps/communication/PacketInfo.hpp" #include "rtps/config.hpp" #include "socket_reactor.hpp" #include "thread_pool.hpp" +#include "timer.hpp" #include "udp_socket.hpp" #include +#include #include #include +#include #include #include namespace rtps { +/// Per-channel scheduling options applied when a transport channel's socket is +/// registered on the reactor (see EsppTransport::ensureReceivePort). +struct ChannelOptions { + /// Priority band the reactor dispatches this socket's datagrams at (see + /// espp::QosBand / espp::SocketReactor). Normal = pre-band behavior. + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP code point set on the socket (marks traffic SENT from this + /// channel; see espp::Socket::set_dscp()). Best-effort: unsupported stacks + /// simply ignore it. + std::optional dscp{}; +}; + class EsppTransport : public espp::BaseComponent { public: using RxCallback = ReceiveCallback; @@ -49,19 +66,68 @@ class EsppTransport : public espp::BaseComponent { /// Ensure a receive channel exists for the port. Unicast ports are bound /// with address/port reuse DISABLED so an in-use port fails loudly (the /// Domain then probes the next participant id); multicast ports keep reuse - /// enabled so multiple processes on one host can share them. - bool ensureReceivePort(Ip4Port_t receivePort, bool is_multicast); - /// Tear down the receive channel for a port (used to unwind a partially - /// successful unicast port probe). + /// enabled so multiple processes on one host can share them. The options + /// only apply when the channel is newly created (an existing channel keeps + /// its original band/dscp). + bool ensureReceivePort(Ip4Port_t receivePort, bool is_multicast, + const ChannelOptions &options = {}); + /// Tear down the receive channel for a port (unwinds a partially successful + /// unicast port probe; also releases a deleted endpoint's dedicated port). + /// The channel slot is freed immediately; the underlying socket is RETIRED + /// (kept alive for any in-flight reactor dispatch that references it) and + /// destroyed - closing the fd and unbinding the port - as soon as the + /// reactor confirms the registration is fully removed, usually immediately + /// (see m_retiredSockets). bool releaseReceivePort(Ip4Port_t receivePort); + + /// Number of retired sockets still awaiting their removal-completion + /// destruction (see m_retiredSockets). Normally 0 shortly after a release; + /// exposed for tests/diagnostics. + std::size_t retiredSocketCount() const; bool joinMultiCastGroup(const Ip4AddressBytes &addr) const; void sendPacket(PacketInfo &info); /// Submit asynchronous protocol work (e.g. a writer's progress()) onto the /// transport's shared worker pool - the same pool the reactor dispatches - /// received datagrams on. Non-blocking; returns false (and logs) when the + /// received datagrams on - at the given priority band (Normal preserves the + /// pre-band FIFO behavior). Non-blocking; returns false (and logs) when the /// pool queue is full or stopped. - bool submit(std::function job); + bool submit(std::function job, espp::QosBand band = espp::QosBand::Normal); + + /// Submit work that MUST eventually run - e.g. a writer's progress(): the + /// pool queue is bounded, and a silently rejected progress submission would + /// strand unsent samples (a lone best-effort DATA has no heartbeat/acknack + /// path to recover it). On rejection the poke is PARKED per producer \p key + /// (the writer): each key holds one job + band and a count of owed runs, and + /// a retry timer re-submits them until the pool accepts. This is lossless + /// (nothing is dropped) and bounded by the number of producers, not by the + /// number of pokes - key by the producer so repeated pokes coalesce into its + /// count instead of growing an unbounded/dropping list. Each owed run maps + /// one-to-one to a progress() call, preserving the one-poke/one-sample + /// semantics. Jobs must remain safe to run until stop() (engine endpoints + /// are pooled and outlive the transport's stop, per the submit() contract). + void submitGuaranteed(const void *key, std::function job, + espp::QosBand band = espp::QosBand::Normal); + + /// Like submitGuaranteed(), but with DRAIN semantics: a parked entry keeps a + /// pending-count of at most ONE, because the job itself re-arms (resubmits) + /// while the producer still has unsent data. Used by the writers' progress() + /// pokes: under sustained overload a KEEP_LAST history overwrites samples, + /// so per-sample owed counts would accumulate unbounded debt for data that + /// no longer exists - millions of no-op pokes after the storm. One pending + /// drain per producer is lossless (each admitted run drains and re-arms + /// until the retained history is empty) with debt bounded at 1. + void submitGuaranteedDrain(const void *key, std::function job, + espp::QosBand band = espp::QosBand::Normal); + + /// Drop any PARKED guaranteed job for `key` so the retry timer cannot + /// resubmit it after the producer has been deleted. Call this when an + /// endpoint keyed here is being torn down individually (a single writer + /// delete, not a full stop()): a parked progress() job would otherwise be + /// resurrected against a reset/reused endpoint or an already-released port. + /// Only removes not-yet-accepted work; a job already handed to the pool is + /// made safe by the endpoint's own reset()/progress() init guard. + void cancelGuaranteed(const void *key); /// Stop receive dispatch and the worker pool. Must be called before the /// objects referenced by in-flight/queued jobs (writers, participants) are @@ -78,17 +144,42 @@ class EsppTransport : public espp::BaseComponent { Channel *findChannel(Ip4Port_t port); const Channel *findChannel(Ip4Port_t port) const; - Channel *createChannel(Ip4Port_t receivePort, bool allow_reuse); - bool startReceiver(Channel &channel, Ip4Port_t receivePort); + /// Destroy a retired socket once the reactor's removal has fully completed + /// (invoked by the removal-completion callback; may run on the releasing + /// caller, a pool worker, or the reactor loop). No-op if stop() already + /// cleared it - the pointer is only used to find the entry. + void destroyRetiredSocket(espp::UdpSocket *socket); + Channel *createChannel(Ip4Port_t receivePort, bool allow_reuse, const ChannelOptions &options); + bool startReceiver(Channel &channel, Ip4Port_t receivePort, const ChannelOptions &options); void onReceive(Ip4Port_t receivePort, std::vector &data, const espp::Socket::Info &sender) const; static std::string ip4ToString(const Ip4AddressBytes &addr); + /// Park a rejected guaranteed poke under \p key (coalescing into its owed + /// count) and (lazily) create the retry timer. Lossless and bounded by the + /// producer count. + void parkPendingJob(const void *key, std::function job, espp::QosBand band, + bool cap_to_one); + RxCallback m_rxCallback{nullptr}; void *m_callbackArgs{nullptr}; mutable std::recursive_mutex m_mutex; std::array m_channels{}; + /// Sockets released at runtime (releaseReceivePort) are RETIRED here, not + /// destroyed inline: SocketReactor::remove() intentionally defers + /// unregistration while a dispatch is in flight, and that dispatch's + /// handler references the UdpSocket - destroying it immediately is a + /// use-after-free (a handler was observed blocking forever on the freed + /// object's internal mutex, wedging SocketReactor::stop()'s in-flight + /// wait). Each retired socket is destroyed by the reactor's + /// removal-completion callback (destroyRetiredSocket) as soon as the + /// registration is fully gone and no handler can reference it - closing the + /// fd and unbinding the port promptly, so endpoint churn does not + /// accumulate fds (relevant to lwIP's small socket budget). stop() clears + /// any stragglers whose completion never fired. Declared before + /// m_pool/m_reactor so it is destroyed AFTER them (reverse member order). + std::vector> m_retiredSockets{}; /// Shared worker pool for received-datagram dispatch (via the reactor) and /// asynchronous writer work (submit()). Declared after m_channels and before /// m_reactor: destruction runs reactor -> pool -> channels. @@ -100,6 +191,32 @@ class EsppTransport : public espp::BaseComponent { /// (reverse member order): the reactor must stop before its sockets die. std::shared_ptr m_reactor{}; mutable std::vector m_multicastGroups; + + /// Guaranteed-submission retry state (see submitGuaranteed()). The timer is + /// created lazily on the first rejection and NEVER self-cancels (its callback + /// always returns false; it is a cheap no-op once the pending map drains); + /// only stop() cancels it - synchronously, BEFORE stopping the reactor/pool, + /// so no retry callback runs during or after teardown. (A self-cancelling + /// espp::Timer leaves running_ set while its task exits, so a later start() + /// would no-op against a dead task and strand parked work - hence keep-alive.) + struct PendingProgress { + std::function job; + espp::QosBand band{espp::QosBand::Normal}; + std::size_t count{0}; ///< owed progress() runs (one per rejected poke), never dropped + }; + std::mutex m_pendingMutex; + /// Coalesced per producer (writer pointer): repeated rejected pokes for the + /// same writer accumulate in `count` rather than growing the map, so memory + /// is bounded by the producer count regardless of publish volume. + std::map m_pendingByKey; + /// Round-robin cursor advanced each retry tick so a different pending key + /// leads the drain each tick. Admission is deliberately band-agnostic (pure + /// rotation): every owed key is admitted within N ticks of a free slot, so + /// no producer can be starved at this stage; band priority is applied by the + /// pool's banded queues once a job is admitted. + std::size_t m_drainRotor{0}; + std::unique_ptr m_retryTimer; + bool m_stopping{false}; }; } // namespace rtps diff --git a/components/rtps/include/rtps/config_desktop.hpp b/components/rtps/include/rtps/config_desktop.hpp index 36fca514d8..0411fc2b24 100644 --- a/components/rtps/include/rtps/config_desktop.hpp +++ b/components/rtps/include/rtps/config_desktop.hpp @@ -41,6 +41,19 @@ namespace rtps { // storage. See storages/StorageArray.hpp. Capacity only - never touches wire bytes. namespace Config { +// --------------------------------------------------------------------------- +// Per-limit overrides: every capacity cap below can be raised (or lowered) +// individually WITHOUT switching profiles by defining RTPS_CFG_ as a +// compile definition before this header is included - e.g. +// -DRTPS_CFG_NUM_STATELESS_WRITERS=16. On ESP-IDF the Kconfig options under +// "RTPS -> Custom capacity overrides" wire these up (0 = keep the profile +// default); on host builds pass RTPS_LIMIT_OVERRIDES to espp.cmake. The +// overrides MUST be applied when compiling the rtps sources themselves (the +// pools are sized in the library), which both mechanisms guarantee; defining +// them for only a consumer translation unit would silently disagree with the +// library. Capacity-only: no bytes on the wire change. +// --------------------------------------------------------------------------- + const VendorId_t VENDOR_ID = {13, 37}; const std::array IP_ADDRESS = {192, 168, 4, 1}; // Needs to be set in lwipcfg.h too. // GUID_RANDOM: derive each participant prefix from OS entropy (see @@ -71,29 +84,79 @@ const uint8_t DOMAIN_ID = 0; // 230 possible with UDP #endif const DataSize_t MAX_SAMPLE_SIZE = RTPS_MAX_SAMPLE_SIZE; -const uint8_t MAX_NUM_PARTICIPANTS = 8; -const uint8_t NUM_STATELESS_WRITERS = 16; -const uint8_t NUM_STATELESS_READERS = 16; -const uint8_t NUM_STATEFUL_READERS = 32; -const uint8_t NUM_STATEFUL_WRITERS = 32; -const uint8_t NUM_WRITERS_PER_PARTICIPANT = 16; -const uint8_t NUM_READERS_PER_PARTICIPANT = 16; -const uint8_t NUM_WRITER_PROXIES_PER_READER = 8; -const uint8_t NUM_READER_PROXIES_PER_WRITER = 8; +#ifndef RTPS_CFG_MAX_NUM_PARTICIPANTS +#define RTPS_CFG_MAX_NUM_PARTICIPANTS 8 +#endif +const uint8_t MAX_NUM_PARTICIPANTS = RTPS_CFG_MAX_NUM_PARTICIPANTS; +#ifndef RTPS_CFG_NUM_STATELESS_WRITERS +#define RTPS_CFG_NUM_STATELESS_WRITERS 16 +#endif +const uint8_t NUM_STATELESS_WRITERS = RTPS_CFG_NUM_STATELESS_WRITERS; +#ifndef RTPS_CFG_NUM_STATELESS_READERS +#define RTPS_CFG_NUM_STATELESS_READERS 16 +#endif +const uint8_t NUM_STATELESS_READERS = RTPS_CFG_NUM_STATELESS_READERS; +#ifndef RTPS_CFG_NUM_STATEFUL_READERS +#define RTPS_CFG_NUM_STATEFUL_READERS 32 +#endif +const uint8_t NUM_STATEFUL_READERS = RTPS_CFG_NUM_STATEFUL_READERS; +#ifndef RTPS_CFG_NUM_STATEFUL_WRITERS +#define RTPS_CFG_NUM_STATEFUL_WRITERS 32 +#endif +const uint8_t NUM_STATEFUL_WRITERS = RTPS_CFG_NUM_STATEFUL_WRITERS; +#ifndef RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT +#define RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT 16 +#endif +const uint8_t NUM_WRITERS_PER_PARTICIPANT = RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT; +#ifndef RTPS_CFG_NUM_READERS_PER_PARTICIPANT +#define RTPS_CFG_NUM_READERS_PER_PARTICIPANT 16 +#endif +const uint8_t NUM_READERS_PER_PARTICIPANT = RTPS_CFG_NUM_READERS_PER_PARTICIPANT; +#ifndef RTPS_CFG_NUM_WRITER_PROXIES_PER_READER +#define RTPS_CFG_NUM_WRITER_PROXIES_PER_READER 8 +#endif +const uint8_t NUM_WRITER_PROXIES_PER_READER = RTPS_CFG_NUM_WRITER_PROXIES_PER_READER; +#ifndef RTPS_CFG_NUM_READER_PROXIES_PER_WRITER +#define RTPS_CFG_NUM_READER_PROXIES_PER_WRITER 8 +#endif +const uint8_t NUM_READER_PROXIES_PER_WRITER = RTPS_CFG_NUM_READER_PROXIES_PER_WRITER; // uint16_t (not uint8_t): these bound SEDP MemoryPool<> sizes and the host // value (256) already exceeds the 255 uint8_t range; host_large goes higher // still. MemoryPool widens the value, so uint16_t is safe. -const uint16_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 256; -const uint16_t MAX_NUM_UNMATCHED_REMOTE_READERS = 128; +#ifndef RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS +#define RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS 256 +#endif +const uint16_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS; +#ifndef RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS +#define RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS 128 +#endif +const uint16_t MAX_NUM_UNMATCHED_REMOTE_READERS = RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS; -const uint8_t MAX_NUM_READER_CALLBACKS = 8; +#ifndef RTPS_CFG_MAX_NUM_READER_CALLBACKS +#define RTPS_CFG_MAX_NUM_READER_CALLBACKS 8 +#endif +const uint8_t MAX_NUM_READER_CALLBACKS = RTPS_CFG_MAX_NUM_READER_CALLBACKS; -const uint8_t HISTORY_SIZE_STATELESS = 2; -const uint8_t HISTORY_SIZE_STATEFUL = 16; +#ifndef RTPS_CFG_HISTORY_SIZE_STATELESS +// 8 (was 2): with only 2 slots a saturated BEST_EFFORT publisher immediately +// overwrites unsent samples; the relaxed host profile can afford real slack. +#define RTPS_CFG_HISTORY_SIZE_STATELESS 8 +#endif +const uint8_t HISTORY_SIZE_STATELESS = RTPS_CFG_HISTORY_SIZE_STATELESS; +#ifndef RTPS_CFG_HISTORY_SIZE_STATEFUL +#define RTPS_CFG_HISTORY_SIZE_STATEFUL 16 +#endif +const uint8_t HISTORY_SIZE_STATEFUL = RTPS_CFG_HISTORY_SIZE_STATEFUL; -const uint8_t MAX_TYPENAME_LENGTH = 64; -const uint8_t MAX_TOPICNAME_LENGTH = 64; +#ifndef RTPS_CFG_MAX_TYPENAME_LENGTH +#define RTPS_CFG_MAX_TYPENAME_LENGTH 64 +#endif +const uint8_t MAX_TYPENAME_LENGTH = RTPS_CFG_MAX_TYPENAME_LENGTH; +#ifndef RTPS_CFG_MAX_TOPICNAME_LENGTH +#define RTPS_CFG_MAX_TOPICNAME_LENGTH 64 +#endif +const uint8_t MAX_TOPICNAME_LENGTH = RTPS_CFG_MAX_TOPICNAME_LENGTH; const int HEARTBEAT_STACKSIZE = 1200; // byte const int THREAD_POOL_WRITER_STACKSIZE = 1100; // byte @@ -112,7 +175,15 @@ const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { 180, 0}; // Absolute maximum lease duration, ignoring remote participant info -const int MAX_NUM_UDP_CONNECTIONS = 16; +// Transport channel pool: 2 shared multicast channels (SPDP metatraffic + +// user multicast) + 2 unicast channels per participant (builtin + user), plus +// any dedicated endpoint ports (max_prioritized_endpoint_ports, default 4) +// drawn at runtime. Sized so the profile's MAX_NUM_PARTICIPANTS budget fits +// with dedicated-port headroom; cross-checked at build time. +#ifndef RTPS_CFG_MAX_NUM_UDP_CONNECTIONS +#define RTPS_CFG_MAX_NUM_UDP_CONNECTIONS 24 +#endif +const int MAX_NUM_UDP_CONNECTIONS = RTPS_CFG_MAX_NUM_UDP_CONNECTIONS; const int THREAD_POOL_NUM_WRITERS = 2; const int THREAD_POOL_NUM_READERS = 2; diff --git a/components/rtps/include/rtps/config_esp32.hpp b/components/rtps/include/rtps/config_esp32.hpp index d1575dcb4f..da01a1e116 100644 --- a/components/rtps/include/rtps/config_esp32.hpp +++ b/components/rtps/include/rtps/config_esp32.hpp @@ -34,6 +34,19 @@ namespace rtps { #define OS_IS_FREERTOS namespace Config { +// --------------------------------------------------------------------------- +// Per-limit overrides: every capacity cap below can be raised (or lowered) +// individually WITHOUT switching profiles by defining RTPS_CFG_ as a +// compile definition before this header is included - e.g. +// -DRTPS_CFG_NUM_STATELESS_WRITERS=16. On ESP-IDF the Kconfig options under +// "RTPS -> Custom capacity overrides" wire these up (0 = keep the profile +// default); on host builds pass RTPS_LIMIT_OVERRIDES to espp.cmake. The +// overrides MUST be applied when compiling the rtps sources themselves (the +// pools are sized in the library), which both mechanisms guarantee; defining +// them for only a consumer translation unit would silently disagree with the +// library. Capacity-only: no bytes on the wire change. +// --------------------------------------------------------------------------- + const VendorId_t VENDOR_ID = {13, 37}; const std::array IP_ADDRESS = {192, 168, 4, 1}; // Fallback: must match DHCPS server netif IP. @@ -49,26 +62,74 @@ const uint8_t DOMAIN_ID = 0; // 230 possible with UDP #endif const DataSize_t MAX_SAMPLE_SIZE = RTPS_MAX_SAMPLE_SIZE; -const uint8_t NUM_STATELESS_WRITERS = 5; -const uint8_t NUM_STATELESS_READERS = 5; -const uint8_t NUM_STATEFUL_READERS = 5; -const uint8_t NUM_STATEFUL_WRITERS = 5; -const uint8_t MAX_NUM_PARTICIPANTS = 1; -const uint8_t NUM_WRITERS_PER_PARTICIPANT = 10; -const uint8_t NUM_READERS_PER_PARTICIPANT = 10; -const uint8_t NUM_WRITER_PROXIES_PER_READER = 6; -const uint8_t NUM_READER_PROXIES_PER_WRITER = 6; +#ifndef RTPS_CFG_NUM_STATELESS_WRITERS +#define RTPS_CFG_NUM_STATELESS_WRITERS 5 +#endif +const uint8_t NUM_STATELESS_WRITERS = RTPS_CFG_NUM_STATELESS_WRITERS; +#ifndef RTPS_CFG_NUM_STATELESS_READERS +#define RTPS_CFG_NUM_STATELESS_READERS 5 +#endif +const uint8_t NUM_STATELESS_READERS = RTPS_CFG_NUM_STATELESS_READERS; +#ifndef RTPS_CFG_NUM_STATEFUL_READERS +#define RTPS_CFG_NUM_STATEFUL_READERS 5 +#endif +const uint8_t NUM_STATEFUL_READERS = RTPS_CFG_NUM_STATEFUL_READERS; +#ifndef RTPS_CFG_NUM_STATEFUL_WRITERS +#define RTPS_CFG_NUM_STATEFUL_WRITERS 5 +#endif +const uint8_t NUM_STATEFUL_WRITERS = RTPS_CFG_NUM_STATEFUL_WRITERS; +#ifndef RTPS_CFG_MAX_NUM_PARTICIPANTS +#define RTPS_CFG_MAX_NUM_PARTICIPANTS 1 +#endif +const uint8_t MAX_NUM_PARTICIPANTS = RTPS_CFG_MAX_NUM_PARTICIPANTS; +#ifndef RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT +#define RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT 10 +#endif +const uint8_t NUM_WRITERS_PER_PARTICIPANT = RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT; +#ifndef RTPS_CFG_NUM_READERS_PER_PARTICIPANT +#define RTPS_CFG_NUM_READERS_PER_PARTICIPANT 10 +#endif +const uint8_t NUM_READERS_PER_PARTICIPANT = RTPS_CFG_NUM_READERS_PER_PARTICIPANT; +#ifndef RTPS_CFG_NUM_WRITER_PROXIES_PER_READER +#define RTPS_CFG_NUM_WRITER_PROXIES_PER_READER 6 +#endif +const uint8_t NUM_WRITER_PROXIES_PER_READER = RTPS_CFG_NUM_WRITER_PROXIES_PER_READER; +#ifndef RTPS_CFG_NUM_READER_PROXIES_PER_WRITER +#define RTPS_CFG_NUM_READER_PROXIES_PER_WRITER 6 +#endif +const uint8_t NUM_READER_PROXIES_PER_WRITER = RTPS_CFG_NUM_READER_PROXIES_PER_WRITER; -const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 50; -const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = 50; +#ifndef RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS +#define RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS 50 +#endif +const uint8_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS; +#ifndef RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS +#define RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS 50 +#endif +const uint8_t MAX_NUM_UNMATCHED_REMOTE_READERS = RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS; -const uint8_t MAX_NUM_READER_CALLBACKS = 5; +#ifndef RTPS_CFG_MAX_NUM_READER_CALLBACKS +#define RTPS_CFG_MAX_NUM_READER_CALLBACKS 5 +#endif +const uint8_t MAX_NUM_READER_CALLBACKS = RTPS_CFG_MAX_NUM_READER_CALLBACKS; -const uint8_t HISTORY_SIZE_STATELESS = 2; -const uint8_t HISTORY_SIZE_STATEFUL = 10; +#ifndef RTPS_CFG_HISTORY_SIZE_STATELESS +#define RTPS_CFG_HISTORY_SIZE_STATELESS 2 +#endif +const uint8_t HISTORY_SIZE_STATELESS = RTPS_CFG_HISTORY_SIZE_STATELESS; +#ifndef RTPS_CFG_HISTORY_SIZE_STATEFUL +#define RTPS_CFG_HISTORY_SIZE_STATEFUL 10 +#endif +const uint8_t HISTORY_SIZE_STATEFUL = RTPS_CFG_HISTORY_SIZE_STATEFUL; -const uint8_t MAX_TYPENAME_LENGTH = 64; -const uint8_t MAX_TOPICNAME_LENGTH = 64; +#ifndef RTPS_CFG_MAX_TYPENAME_LENGTH +#define RTPS_CFG_MAX_TYPENAME_LENGTH 64 +#endif +const uint8_t MAX_TYPENAME_LENGTH = RTPS_CFG_MAX_TYPENAME_LENGTH; +#ifndef RTPS_CFG_MAX_TOPICNAME_LENGTH +#define RTPS_CFG_MAX_TOPICNAME_LENGTH 64 +#endif +const uint8_t MAX_TOPICNAME_LENGTH = RTPS_CFG_MAX_TOPICNAME_LENGTH; const int HEARTBEAT_STACKSIZE = 1024 * 6; // byte const int THREAD_POOL_WRITER_STACKSIZE = 4096; // byte @@ -89,7 +150,15 @@ const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { const Duration_t SPDP_LEASE_DURATION = {5, 0}; -const int MAX_NUM_UDP_CONNECTIONS = 10; +// Transport channel pool: 2 shared multicast channels (SPDP metatraffic + +// user multicast) + 2 unicast channels per participant (builtin + user), plus +// any dedicated endpoint ports (max_prioritized_endpoint_ports, default 4) +// drawn at runtime. Sized so the profile's MAX_NUM_PARTICIPANTS budget fits +// with dedicated-port headroom; cross-checked at build time. +#ifndef RTPS_CFG_MAX_NUM_UDP_CONNECTIONS +#define RTPS_CFG_MAX_NUM_UDP_CONNECTIONS 10 +#endif +const int MAX_NUM_UDP_CONNECTIONS = RTPS_CFG_MAX_NUM_UDP_CONNECTIONS; const int THREAD_POOL_NUM_WRITERS = 2; const int THREAD_POOL_NUM_READERS = 2; diff --git a/components/rtps/include/rtps/config_host_large.hpp b/components/rtps/include/rtps/config_host_large.hpp index 09099a64d4..4312dee815 100644 --- a/components/rtps/include/rtps/config_host_large.hpp +++ b/components/rtps/include/rtps/config_host_large.hpp @@ -41,6 +41,19 @@ namespace rtps { // storage. See storages/StorageArray.hpp. Capacity only - never touches wire bytes. namespace Config { +// --------------------------------------------------------------------------- +// Per-limit overrides: every capacity cap below can be raised (or lowered) +// individually WITHOUT switching profiles by defining RTPS_CFG_ as a +// compile definition before this header is included - e.g. +// -DRTPS_CFG_NUM_STATELESS_WRITERS=16. On ESP-IDF the Kconfig options under +// "RTPS -> Custom capacity overrides" wire these up (0 = keep the profile +// default); on host builds pass RTPS_LIMIT_OVERRIDES to espp.cmake. The +// overrides MUST be applied when compiling the rtps sources themselves (the +// pools are sized in the library), which both mechanisms guarantee; defining +// them for only a consumer translation unit would silently disagree with the +// library. Capacity-only: no bytes on the wire change. +// --------------------------------------------------------------------------- + const VendorId_t VENDOR_ID = {13, 37}; const std::array IP_ADDRESS = {192, 168, 4, 1}; // Needs to be set in lwipcfg.h too. // GUID_RANDOM: derive each participant prefix from OS entropy (see @@ -65,29 +78,79 @@ const uint8_t DOMAIN_ID = 0; // 230 possible with UDP #endif const DataSize_t MAX_SAMPLE_SIZE = RTPS_MAX_SAMPLE_SIZE; -const uint8_t MAX_NUM_PARTICIPANTS = 32; -const uint8_t NUM_STATELESS_WRITERS = 64; -const uint8_t NUM_STATELESS_READERS = 64; -const uint8_t NUM_STATEFUL_READERS = 128; -const uint8_t NUM_STATEFUL_WRITERS = 128; -const uint8_t NUM_WRITERS_PER_PARTICIPANT = 64; -const uint8_t NUM_READERS_PER_PARTICIPANT = 64; -const uint8_t NUM_WRITER_PROXIES_PER_READER = 16; -const uint8_t NUM_READER_PROXIES_PER_WRITER = 16; +#ifndef RTPS_CFG_MAX_NUM_PARTICIPANTS +#define RTPS_CFG_MAX_NUM_PARTICIPANTS 32 +#endif +const uint8_t MAX_NUM_PARTICIPANTS = RTPS_CFG_MAX_NUM_PARTICIPANTS; +#ifndef RTPS_CFG_NUM_STATELESS_WRITERS +#define RTPS_CFG_NUM_STATELESS_WRITERS 64 +#endif +const uint8_t NUM_STATELESS_WRITERS = RTPS_CFG_NUM_STATELESS_WRITERS; +#ifndef RTPS_CFG_NUM_STATELESS_READERS +#define RTPS_CFG_NUM_STATELESS_READERS 64 +#endif +const uint8_t NUM_STATELESS_READERS = RTPS_CFG_NUM_STATELESS_READERS; +#ifndef RTPS_CFG_NUM_STATEFUL_READERS +#define RTPS_CFG_NUM_STATEFUL_READERS 128 +#endif +const uint8_t NUM_STATEFUL_READERS = RTPS_CFG_NUM_STATEFUL_READERS; +#ifndef RTPS_CFG_NUM_STATEFUL_WRITERS +#define RTPS_CFG_NUM_STATEFUL_WRITERS 128 +#endif +const uint8_t NUM_STATEFUL_WRITERS = RTPS_CFG_NUM_STATEFUL_WRITERS; +#ifndef RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT +#define RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT 64 +#endif +const uint8_t NUM_WRITERS_PER_PARTICIPANT = RTPS_CFG_NUM_WRITERS_PER_PARTICIPANT; +#ifndef RTPS_CFG_NUM_READERS_PER_PARTICIPANT +#define RTPS_CFG_NUM_READERS_PER_PARTICIPANT 64 +#endif +const uint8_t NUM_READERS_PER_PARTICIPANT = RTPS_CFG_NUM_READERS_PER_PARTICIPANT; +#ifndef RTPS_CFG_NUM_WRITER_PROXIES_PER_READER +#define RTPS_CFG_NUM_WRITER_PROXIES_PER_READER 16 +#endif +const uint8_t NUM_WRITER_PROXIES_PER_READER = RTPS_CFG_NUM_WRITER_PROXIES_PER_READER; +#ifndef RTPS_CFG_NUM_READER_PROXIES_PER_WRITER +#define RTPS_CFG_NUM_READER_PROXIES_PER_WRITER 16 +#endif +const uint8_t NUM_READER_PROXIES_PER_WRITER = RTPS_CFG_NUM_READER_PROXIES_PER_WRITER; // uint16_t (not uint8_t): these bound SEDP MemoryPool<> sizes and the values // here (1024 / 512) far exceed the 255 uint8_t range. // MemoryPool widens the value, so uint16_t is safe. -const uint16_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = 1024; -const uint16_t MAX_NUM_UNMATCHED_REMOTE_READERS = 512; +#ifndef RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS +#define RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS 1024 +#endif +const uint16_t MAX_NUM_UNMATCHED_REMOTE_WRITERS = RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_WRITERS; +#ifndef RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS +#define RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS 512 +#endif +const uint16_t MAX_NUM_UNMATCHED_REMOTE_READERS = RTPS_CFG_MAX_NUM_UNMATCHED_REMOTE_READERS; -const uint8_t MAX_NUM_READER_CALLBACKS = 16; +#ifndef RTPS_CFG_MAX_NUM_READER_CALLBACKS +#define RTPS_CFG_MAX_NUM_READER_CALLBACKS 16 +#endif +const uint8_t MAX_NUM_READER_CALLBACKS = RTPS_CFG_MAX_NUM_READER_CALLBACKS; -const uint8_t HISTORY_SIZE_STATELESS = 2; -const uint8_t HISTORY_SIZE_STATEFUL = 32; +#ifndef RTPS_CFG_HISTORY_SIZE_STATELESS +// 32 (was 2): see config_desktop.hpp - the generous profile should not share +// the embedded profile's minimal best-effort history. +#define RTPS_CFG_HISTORY_SIZE_STATELESS 32 +#endif +const uint8_t HISTORY_SIZE_STATELESS = RTPS_CFG_HISTORY_SIZE_STATELESS; +#ifndef RTPS_CFG_HISTORY_SIZE_STATEFUL +#define RTPS_CFG_HISTORY_SIZE_STATEFUL 32 +#endif +const uint8_t HISTORY_SIZE_STATEFUL = RTPS_CFG_HISTORY_SIZE_STATEFUL; -const uint8_t MAX_TYPENAME_LENGTH = 64; -const uint8_t MAX_TOPICNAME_LENGTH = 64; +#ifndef RTPS_CFG_MAX_TYPENAME_LENGTH +#define RTPS_CFG_MAX_TYPENAME_LENGTH 64 +#endif +const uint8_t MAX_TYPENAME_LENGTH = RTPS_CFG_MAX_TYPENAME_LENGTH; +#ifndef RTPS_CFG_MAX_TOPICNAME_LENGTH +#define RTPS_CFG_MAX_TOPICNAME_LENGTH 64 +#endif +const uint8_t MAX_TOPICNAME_LENGTH = RTPS_CFG_MAX_TOPICNAME_LENGTH; const int HEARTBEAT_STACKSIZE = 1200; // byte const int THREAD_POOL_WRITER_STACKSIZE = 1100; // byte @@ -106,7 +169,15 @@ const Duration_t SPDP_DEFAULT_REMOTE_LEASE_DURATION = { const Duration_t SPDP_MAX_REMOTE_LEASE_DURATION = { 180, 0}; // Absolute maximum lease duration, ignoring remote participant info -const int MAX_NUM_UDP_CONNECTIONS = 32; +// Transport channel pool: 2 shared multicast channels (SPDP metatraffic + +// user multicast) + 2 unicast channels per participant (builtin + user), plus +// any dedicated endpoint ports (max_prioritized_endpoint_ports, default 4) +// drawn at runtime. Sized so the profile's MAX_NUM_PARTICIPANTS budget fits +// with dedicated-port headroom; cross-checked at build time. +#ifndef RTPS_CFG_MAX_NUM_UDP_CONNECTIONS +#define RTPS_CFG_MAX_NUM_UDP_CONNECTIONS 72 +#endif +const int MAX_NUM_UDP_CONNECTIONS = RTPS_CFG_MAX_NUM_UDP_CONNECTIONS; const int THREAD_POOL_NUM_WRITERS = 2; const int THREAD_POOL_NUM_READERS = 2; diff --git a/components/rtps/include/rtps/discovery/SEDPAgent.hpp b/components/rtps/include/rtps/discovery/SEDPAgent.hpp index ae60cec56b..0178b2717f 100644 --- a/components/rtps/include/rtps/discovery/SEDPAgent.hpp +++ b/components/rtps/include/rtps/discovery/SEDPAgent.hpp @@ -58,6 +58,14 @@ class SEDPAgent : public espp::BaseComponent { uint32_t getNumRemoteUnmatchedReaders(); uint32_t getNumRemoteUnmatchedWriters(); + /// The agent's discovery mutex (recursive). Exposed so an endpoint deletion + /// can be made ATOMIC with the participant's slot removal under the global + /// lock order (SEDPAgent::m_mutex -> Participant::m_mutex): holding it across + /// deleteReader/deleteWriter() AND the slot-clear prevents a concurrent SEDP + /// receive handler from matching a remote to the endpoint in the window + /// between its disposal and its removal from the participant's tables. + std::recursive_mutex &getMutex() { return m_mutex; } + protected: // For testing purposes void handlePublisherReaderMessage(const TopicData &writerData, const ReaderCacheChange &change); void handleSubscriptionReaderMessage(const TopicData &writerData, diff --git a/components/rtps/include/rtps/discovery/SPDPAgent.hpp b/components/rtps/include/rtps/discovery/SPDPAgent.hpp index 616bcc409f..a650fa5ad7 100644 --- a/components/rtps/include/rtps/discovery/SPDPAgent.hpp +++ b/components/rtps/include/rtps/discovery/SPDPAgent.hpp @@ -35,6 +35,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/utils/Log.hpp" #include "task.hpp" +#include #include #include @@ -72,7 +73,9 @@ class SPDPAgent : public espp::BaseComponent { private: Participant *mp_participant = nullptr; BuiltInEndpoints m_buildInEndpoints; - bool m_running = false; + // Atomic: start()/stop() flip it from the app thread while the Domain's + // protocol-scheduler thread polls isRunning() every announce cycle. + std::atomic m_running{false}; std::array m_outputBuffer{}; // TODO check required size std::array m_inputBuffer{}; ParticipantProxyData m_proxyDataBuffer{}; diff --git a/components/rtps/include/rtps/discovery/TopicData.hpp b/components/rtps/include/rtps/discovery/TopicData.hpp index 25fe57c5b6..1ec7afc141 100644 --- a/components/rtps/include/rtps/discovery/TopicData.hpp +++ b/components/rtps/include/rtps/discovery/TopicData.hpp @@ -28,10 +28,13 @@ Author: i11 - Embedded Software, RWTH Aachen University #define SUPPRESS_UNICAST 0 +#include "dscp.hpp" +#include "qos_band.hpp" #include "rtps/config.hpp" #include "rtps/utils/CdrBuffer.hpp" #include "rtps/utils/hash.hpp" #include +#include #include #include @@ -50,6 +53,23 @@ struct TopicData { FullLengthLocator unicastLocator; FullLengthLocator multicastLocator; + // --- Local-only endpoint scheduling attributes ---------------------------- + // These are NEVER serialized to (or parsed from) the wire: serializeInto() + // and readFromBuffer() ignore them, so the SEDP encoding is unchanged. They + // only steer how the LOCAL endpoint's traffic is scheduled (see + // Domain::createWriter/createReader and EsppTransport). + // + /// Priority band for this endpoint's received-traffic dispatch (and, when a + /// dedicated port is granted, for that port's reactor registration). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP code point applied to the endpoint's dedicated socket (marks + /// the traffic it SENDS; requires a dedicated port to take effect). + std::optional dscp{}; + /// True when Domain granted this (local) endpoint a dedicated unicast port: + /// unicastLocator then carries that port instead of the participant's shared + /// user-unicast port. Always false for remote endpoints. + bool hasDedicatedPort{false}; + uint8_t statusInfo = 0; bool statusInfoValid = false; // Use Case: Remotes communicates id of deleted endpoint through key_hash diff --git a/components/rtps/include/rtps/entities/Domain.hpp b/components/rtps/include/rtps/entities/Domain.hpp index 03e8d85605..e1ac7ecf1f 100644 --- a/components/rtps/include/rtps/entities/Domain.hpp +++ b/components/rtps/include/rtps/entities/Domain.hpp @@ -41,10 +41,43 @@ Author: i11 - Embedded Software, RWTH Aachen University #include namespace rtps { + +/// Runtime scheduling configuration for a Domain (all fields optional; the +/// defaults preserve pre-band behavior except that metatraffic - SPDP/SEDP +/// discovery - is dispatched at QosBand::High so discovery stays responsive +/// under user-traffic load). +struct DomainConfig { + /// Band for the metatraffic (SPDP multicast + SEDP unicast) channels. + espp::QosBand metatraffic_band{espp::QosBand::High}; + /// Band for the shared user-traffic (user unicast + user multicast) channels. + espp::QosBand user_traffic_band{espp::QosBand::Normal}; + /// Allow endpoints with a non-default band (or a dscp) to be granted their + /// own dedicated unicast port (announced via their SEDP per-endpoint unicast + /// locator). Disable to force banded endpoints onto the shared user port. + bool enable_dedicated_endpoint_ports{true}; + /// Ration for dedicated endpoint ports (each one consumes a UDP socket/fd; + /// lwIP on ESP32 defaults to ~10 sockets total). When exhausted, further + /// banded endpoints fall back to the shared user port (with a warning). + /// The cap is a TRUE fd bound: released sockets whose fd is still open + /// awaiting the reactor's removal completion (retired) count toward it. + uint8_t max_prioritized_endpoint_ports{4}; +}; + +/// Per-endpoint scheduling options for createWriter()/createReader(). +struct EndpointOptions { + /// Priority band for the endpoint's received-traffic dispatch. A non-Normal + /// band requests a dedicated unicast port (see DomainConfig). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP code point for traffic the endpoint sends; requires (and by + /// itself requests) a dedicated port, since DSCP is per-socket. + std::optional dscp{}; +}; + class Domain : public espp::BaseComponent { public: - explicit Domain(const Ip4AddressBytes &localIpAddress); - Domain(EsppTransport &transport, const Ip4AddressBytes &localIpAddress); + explicit Domain(const Ip4AddressBytes &localIpAddress, const DomainConfig &config = {}); + Domain(EsppTransport &transport, const Ip4AddressBytes &localIpAddress, + const DomainConfig &config = {}); ~Domain(); bool completeInit(); @@ -52,9 +85,11 @@ class Domain : public espp::BaseComponent { Participant *createParticipant(); Writer *createWriter(Participant &part, const char *topicName, const char *typeName, - bool reliable, bool enforceUnicast = false); + bool reliable, bool enforceUnicast = false, + const EndpointOptions &options = {}); Reader *createReader(Participant &part, const char *topicName, const char *typeName, - bool reliable, Ip4AddressBytes mcastaddress = {0, 0, 0, 0}); + bool reliable, Ip4AddressBytes mcastaddress = {0, 0, 0, 0}, + const EndpointOptions &options = {}); Writer *writerExists(Participant &part, const char *topicName, const char *typeName, bool reliable); @@ -64,6 +99,11 @@ class Domain : public espp::BaseComponent { bool deleteWriter(Participant &part, Writer *writer); bool deleteReader(Participant &part, Reader *reader); + /// The transport the domain receives/sends through. Exposed so higher layers + /// (e.g. the espp facade's banded deferred dispatch) can submit work onto the + /// transport's worker pool at a chosen priority band. + EsppTransport &getTransport() { return *m_transport; } + void printInfo(); private: @@ -89,6 +129,54 @@ class Domain : public espp::BaseComponent { static constexpr uint8_t PARTICIPANT_PORT_PROBE_LIMIT = 16; Participant *findParticipantById(ParticipantId_t id); + DomainConfig m_config{}; + + // --- Dedicated endpoint ports (per-endpoint priority) --------------------- + // Deterministic allocation strategy: dedicated ports live in this domain's + // RTPS port block at offset DEDICATED_PORT_OFFSET, i.e. + // port = 7400 + 250*DOMAIN_ID + DEDICATED_PORT_OFFSET + n + // with n probed linearly (reuse-disabled bind, so a port taken by another + // process on this host fails loudly and the next one is tried). The standard + // RTPS offsets (builtin/user, multicast/unicast) stay below 100 for + // participant ids 0..44 only, so createParticipant() ENFORCES that cap on + // the id probe while dedicated ports are enabled - an id past it would bind + // its shared user-unicast port inside this range, where a dedicated-port + // probe would mistake the existing channel for a fresh allocation and + // misroute that participant's traffic. The whole range stays inside this + // domain's 250-port block (offsets 100..249 -> up to 150 candidate ports; + // allocation is additionally rationed by + // DomainConfig::max_prioritized_endpoint_ports). + static constexpr uint16_t DEDICATED_PORT_OFFSET = 100; + static constexpr uint16_t DEDICATED_PORT_PROBE_LIMIT = 16; + struct DedicatedPort { + Ip4Port_t port{0}; + Participant *participant{nullptr}; + }; + /// Active dedicated ports, for receive routing (port -> owning participant) + /// and for release on endpoint deletion. Bounded by the ration. Guarded by + /// m_dedicatedPortsMutex - its OWN small mutex, NOT m_mutex: the lookup runs + /// on the receive path (receiveCallback on a pool worker), and taking + /// m_mutex there would let an API caller holding m_mutex across a blocking + /// operation stall every receive worker. + std::vector m_dedicatedPorts; + mutable std::mutex m_dedicatedPortsMutex; + /// Next port offset to try, so allocation walks forward deterministically. + uint16_t m_nextDedicatedPortOffset = 0; + /// Allocate (bind + register) a dedicated unicast port for an endpoint of + /// `part` at `band` (optionally DSCP-marked). Returns 0 when disabled, the + /// ration is exhausted, or no free port was found - callers then fall back + /// to the shared user-unicast port. + Ip4Port_t allocateDedicatedEndpointPort(Participant &part, espp::QosBand band, + const std::optional &dscp); + /// Release an endpoint's dedicated port (no-op for port 0 / unknown ports). + void releaseDedicatedEndpointPort(Ip4Port_t port); + Participant *findParticipantByDedicatedPort(Ip4Port_t port); + /// Apply EndpointOptions to freshly-built endpoint attributes: copies + /// band/dscp and, when the options request priority, tries to allocate a + /// dedicated port and rewrites attributes.unicastLocator to it. + void applyEndpointOptions(Participant &part, TopicData &attributes, + const EndpointOptions &options); + /// Single deadline-scheduled protocol task: drives SPDP announcements for /// every participant and heartbeat ticks for every stateful writer, /// replacing one SPDP thread per participant plus one heartbeat thread per @@ -97,6 +185,15 @@ class Domain : public espp::BaseComponent { bool protocolLoop(std::mutex &m, std::condition_variable &cv, bool ¬ified); void nudgeProtocol(); std::unique_ptr m_protocolTask; + /// Leaf mutex guarding the publication, use and teardown of the three task + /// synchronization pointers below. protocolLoop() publishes them (under this + /// mutex) before its first wait; nudgeProtocol() - called from arbitrary + /// publisher threads via the writers' protocol nudge - reads them under it; + /// stop() nulls them under it before the task (and with it the pointed-to + /// mutex/cv) is destroyed, so a late nudge is a safe no-op instead of a + /// use-after-free. Lock order: this mutex may be held while taking the + /// task's mutex, never the reverse. + std::mutex m_protocolNudgeMutex; std::mutex *m_protocolMutex = nullptr; std::condition_variable *m_protocolCv = nullptr; bool *m_protocolNotified = nullptr; @@ -124,7 +221,13 @@ class Domain : public espp::BaseComponent { void receiveCallback(const PacketInfo &packet); GuidPrefix_t generateGuidPrefix(ParticipantId_t id) const; - void createBuiltinWritersAndReaders(Participant &part); + //! Allocate + wire the participant's builtin discovery endpoints. Returns + //! false (after rolling back any endpoint this invocation initialized) when + //! a GLOBAL endpoint pool is exhausted - each participant consumes 1 + //! stateless writer/reader and 2 stateful writers/readers, so undersized + //! limits (or too many participants) must fail participant creation cleanly + //! instead of dereferencing a null builtin. + bool createBuiltinWritersAndReaders(Participant &part); bool initializeTransport(); void registerMulticastPort(FullLengthLocator mcastLocator); static void receiveJumppad(void *callee, const PacketInfo &packet); diff --git a/components/rtps/include/rtps/entities/Participant.hpp b/components/rtps/include/rtps/entities/Participant.hpp index bd5ddef3d0..898549bdc3 100644 --- a/components/rtps/include/rtps/entities/Participant.hpp +++ b/components/rtps/include/rtps/entities/Participant.hpp @@ -82,12 +82,22 @@ class Participant : public espp::BaseComponent { //! (Probably) Thread safe if writers cannot be removed Writer *getWriter(EntityId_t id); + //! Lookup variant for the receive path: also captures the endpoint's pooled + //! slot generation while m_mutex is held (i.e. atomically with the slot + //! still being registered). Dispatch through the endpoint's *IfCurrent + //! wrapper with this generation then rejects the delivery if the endpoint + //! was deleted - and its slot possibly reused - after the lookup. + Writer *getWriter(EntityId_t id, uint32_t &generation_out); Writer *getMatchingWriter(const TopicData &topicData); Writer *getMatchingWriter(const TopicDataCompressed &topicData); //! (Probably) Thread safe if readers cannot be removed Reader *getReader(EntityId_t id); + //! See getWriter(id, generation_out). + Reader *getReader(EntityId_t id, uint32_t &generation_out); Reader *getReaderByWriterId(const Guid_t &guid); + //! See getWriter(id, generation_out). + Reader *getReaderByWriterId(const Guid_t &guid, uint32_t &generation_out); Reader *getMatchingReader(const TopicData &topicData); Reader *getMatchingReader(const TopicDataCompressed &topicData); diff --git a/components/rtps/include/rtps/entities/Reader.hpp b/components/rtps/include/rtps/entities/Reader.hpp index 52a977eacc..0883566fd1 100644 --- a/components/rtps/include/rtps/entities/Reader.hpp +++ b/components/rtps/include/rtps/entities/Reader.hpp @@ -33,6 +33,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/WriterProxy.hpp" #include "rtps/rpc/sample_identity.hpp" #include "rtps/storages/MemoryPool.hpp" +#include #include #include #ifdef RTPS_ENABLE_FRAGMENTATION @@ -126,6 +127,29 @@ class Reader : public espp::BaseComponent { virtual bool sendPreemptiveAckNack(const WriterProxy &writer); + /// Pooled-slot reuse generation, captured by the receive path inside the + /// participant's locked endpoint lookup (see Participant::getReader(id, + /// generation_out)) and re-checked by the *IfCurrent dispatch wrappers below. + uint32_t generation() const { return m_generation_.load(); } + /// Guarded dispatch for the receive path: runs the corresponding virtual only + /// if `generation` still matches. Together with reset() - which bumps the + /// generation FIRST and then waits for in-flight guarded dispatches to drain + /// (before any state is torn down) - this closes the window where a receive + /// worker that already resolved this pooled Reader* dispatches into a + /// deleted endpoint or into the NEXT endpoint reusing the slot (which would + /// deliver an old topic's payload to the new callback). + void newChangeIfCurrent(uint32_t generation, const ReaderCacheChange &cacheChange); + bool onNewHeartbeatIfCurrent(uint32_t generation, const SubmessageHeartbeat &msg, + const GuidPrefix_t &remotePrefix); + bool onNewGapIfCurrent(uint32_t generation, const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix); +#ifdef RTPS_ENABLE_FRAGMENTATION + void newFragmentIfCurrent(uint32_t generation, const Guid_t &writerGuid, + const SequenceNumber_t &sn, uint32_t fragmentStartingNum, + uint16_t fragmentsInSubmessage, uint16_t fragmentSize, + uint32_t sampleSize, const uint8_t *fragData, DataSize_t fragDataLen); +#endif + #ifdef RTPS_ENABLE_FRAGMENTATION /// Accumulate one DATA_FRAG fragment (best-effort reassembly). When all /// fragments of the sample identified by (writerGuid, sn) have arrived, the @@ -144,14 +168,34 @@ class Reader : public espp::BaseComponent { SequenceNumber_t m_sedp_sequence_number; - bool m_is_initialized_ = false; + // Atomic: written by init()/reset() on the app thread and read as the + // unlocked fast-path guard in newChange()/onNewHeartbeat() on the receive + // workers. The seq_cst store in init() also publishes the preceding member + // writes to a worker that observes initialized == true. + std::atomic m_is_initialized_{false}; Reader(); virtual ~Reader() = default; MemoryPool m_proxies; callbackIdentifier_t m_callback_identifier = 1; - uint8_t m_callback_count = 0; + // Atomic: mutated by registerCallback()/removeCallback() (app threads) and + // read as newChange()'s unlocked fast-path guard on the receive workers. + std::atomic m_callback_count{0}; + + //! Wait for in-flight guarded dispatches on OTHER threads (a dispatch on the + //! calling thread is excluded so a callback can initiate its own reader's + //! removal without deadlocking). Used by reset() and removeCallback(). + void drainDispatchesForTeardown(); + + // Pooled-slot reuse guard for the receive path (see the *IfCurrent wrappers). + // reset() bumps m_generation_ FIRST, then spins (lock-free, before taking any + // reader mutex) until m_active_dispatches_ drains: a dispatch that passed its + // generation check before the bump completes against the still-intact + // endpoint; one that checks after no-ops. The spin must not hold the reader + // mutexes - an in-flight dispatch needs them to finish. + std::atomic m_generation_{0}; + std::atomic m_active_dispatches_{0}; using callbackElement_t = struct { callbackFunction_t function; void *arg; diff --git a/components/rtps/include/rtps/entities/StatefulReader.hpp b/components/rtps/include/rtps/entities/StatefulReader.hpp index 800715bb77..7fd9e6d16b 100644 --- a/components/rtps/include/rtps/entities/StatefulReader.hpp +++ b/components/rtps/include/rtps/entities/StatefulReader.hpp @@ -32,6 +32,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/Reader.hpp" #include "rtps/entities/WriterProxy.hpp" #include "rtps/storages/MemoryPool.hpp" +#include namespace rtps { class EsppTransport; @@ -54,6 +55,16 @@ class StatefulReader final : public Reader { private: Ip4Port_t m_srcPort; // TODO intended for reuse but buffer not used as such EsppTransport *m_transport; + /// Serializes sample DELIVERY (the expectedSN claim + the user callbacks) + /// without holding m_proxies_mutex across user code. A leaf in the lock + /// order: newChange() acquires it FIRST and only nests m_proxies_mutex + /// briefly inside for the claim; nothing acquires it while holding any other + /// engine/facade lock. This preserves the strict in-order, + /// one-callback-at-a-time semantics the proxies mutex used to provide while + /// keeping user callbacks (which may call back into the facade and from + /// there into SEDP/participant/proxies locks) off the proxies mutex - + /// breaking the callback->facade->SEDP->proxies lock-order cycle. + std::mutex m_delivery_mutex; }; } // namespace rtps diff --git a/components/rtps/include/rtps/entities/Writer.hpp b/components/rtps/include/rtps/entities/Writer.hpp index 25983a43e6..506c0e926c 100644 --- a/components/rtps/include/rtps/entities/Writer.hpp +++ b/components/rtps/include/rtps/entities/Writer.hpp @@ -32,6 +32,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/storages/CacheChange.hpp" #include "rtps/storages/MemoryPool.hpp" +#include #include #include @@ -80,6 +81,26 @@ class Writer : public espp::BaseComponent { virtual void setAllChangesToUnsent() = 0; virtual void onNewAckNack(const SubmessageAckNack &msg, const GuidPrefix_t &sourceGuidPrefix) = 0; + //! Dispatch an ACKNACK only if `generation` still matches (checked under + //! m_mutex together with initialization). The receive path captures the + //! generation inside the participant's locked endpoint lookup, so a handler + //! that obtained this pooled Writer* just before it was deleted (and possibly + //! reused for another endpoint) no-ops instead of mutating/retransmitting the + //! new endpoint's history. + void onNewAckNackIfCurrent(uint32_t generation, const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix); + + //! Pooled-slot reuse generation (see onNewAckNackIfCurrent); captured by the + //! receive path inside the participant's locked endpoint lookup. + uint32_t generation() const { return m_generation_.load(); } + + //! Number of UNSENT samples this writer's history has overwritten under + //! KEEP_LAST overflow (newChange() on a full static ring drops the oldest + //! unsent change and advances the send cursor past it). The facade uses the + //! delta across a publish() to surface the loss; Diagnostics::Writer keeps a + //! process-wide total. + uint32_t historyDrops() const { return m_history_drops_.load(); } + using dumpProxyCallback = void (*)(const Writer *writer, const ReaderProxy &, void *arg); int dumpAllProxies(dumpProxyCallback target, void *arg); @@ -123,9 +144,27 @@ class Writer : public espp::BaseComponent { friend class SizeInspector; bool m_is_initialized_ = false; + // Bumped by reset() (under m_mutex) each time this pooled writer slot is torn + // down. A guaranteed progress() job captures the generation at submit time + // and runs via progressIfCurrent(), which re-checks it under m_mutex - so a + // job accepted by the pool before deletion cannot run against a reset writer + // or the next endpoint that reuses this slot (a stale generation no-ops). + // Atomic so the receive path can capture it inside the participant's locked + // endpoint lookup (see Participant::getWriter(id, generation_out)) and + // onNewAckNackIfCurrent() can reject a dispatch that lost the race with + // deletion/reuse; all bumps still happen under m_mutex. + std::atomic m_generation_{0}; + //! Unsent samples overwritten by KEEP_LAST overflow (see historyDrops()). + std::atomic m_history_drops_{0}; virtual ~Writer() = default; MemoryPool m_proxies; + //! Snapshot the current initialization generation (taken at job submit time). + uint32_t currentGeneration(); + //! Run progress() only if `generation` still matches AND the writer is still + //! initialized, atomically under m_mutex. Used by the guaranteed-job lambdas. + void progressIfCurrent(uint32_t generation); + void resetSendOptions(); void manageSendOptions(); bool isIrrelevant(ChangeKind_t kind) const; diff --git a/components/rtps/include/rtps/utils/Diagnostics.hpp b/components/rtps/include/rtps/utils/Diagnostics.hpp index 1cc9e95504..7cdb2d0e48 100644 --- a/components/rtps/include/rtps/utils/Diagnostics.hpp +++ b/components/rtps/include/rtps/utils/Diagnostics.hpp @@ -26,58 +26,68 @@ Author: i11 - Embedded Software, RWTH Aachen University #ifndef RTPS_DIAGNOSTICS_H #define RTPS_DIAGNOSTICS_H +#include #include namespace rtps { namespace Diagnostics { +// All counters are atomic: they are bumped from transport/receive worker +// threads and read from application threads (plain uint32_t was a data race). namespace ThreadPool { -extern uint32_t dropped_incoming_packets_usertraffic; -extern uint32_t dropped_incoming_packets_metatraffic; +extern std::atomic dropped_incoming_packets_usertraffic; +extern std::atomic dropped_incoming_packets_metatraffic; -extern uint32_t dropped_outgoing_packets_usertraffic; -extern uint32_t dropped_outgoing_packets_metatraffic; +extern std::atomic dropped_outgoing_packets_usertraffic; +extern std::atomic dropped_outgoing_packets_metatraffic; -extern uint32_t processed_incoming_metatraffic; -extern uint32_t processed_outgoing_metatraffic; -extern uint32_t processed_incoming_usertraffic; -extern uint32_t processed_outgoing_usertraffic; +extern std::atomic processed_incoming_metatraffic; +extern std::atomic processed_outgoing_metatraffic; +extern std::atomic processed_incoming_usertraffic; +extern std::atomic processed_outgoing_usertraffic; -extern uint32_t max_ever_elements_outgoing_usertraffic_queue; -extern uint32_t max_ever_elements_incoming_usertraffic_queue; +extern std::atomic max_ever_elements_outgoing_usertraffic_queue; +extern std::atomic max_ever_elements_incoming_usertraffic_queue; -extern uint32_t max_ever_elements_outgoing_metatraffic_queue; -extern uint32_t max_ever_elements_incoming_metatraffic_queue; +extern std::atomic max_ever_elements_outgoing_metatraffic_queue; +extern std::atomic max_ever_elements_incoming_metatraffic_queue; } // namespace ThreadPool namespace StatefulReader { -extern uint32_t sfr_unexpected_sn; -extern uint32_t sfr_retransmit_requests; +extern std::atomic sfr_unexpected_sn; +extern std::atomic sfr_retransmit_requests; } // namespace StatefulReader +namespace Writer { +/// UNSENT samples overwritten by KEEP_LAST history overflow across all writers +/// (a saturated publisher outrunning the send path). See also the per-writer +/// rtps::Writer::historyDrops() and the facade's rate-limited publish warning. +extern std::atomic history_overwrite_drops; +} // namespace Writer + namespace Network { -extern uint32_t lwip_allocation_failures; +extern std::atomic lwip_allocation_failures; } namespace OS { -extern uint32_t current_free_heap_size; +extern std::atomic current_free_heap_size; } namespace SEDP { -extern uint32_t max_ever_remote_participants; -extern uint32_t current_remote_participants; +extern std::atomic max_ever_remote_participants; +extern std::atomic current_remote_participants; -extern uint32_t max_ever_matched_reader_proxies; -extern uint32_t current_max_matched_reader_proxies; +extern std::atomic max_ever_matched_reader_proxies; +extern std::atomic current_max_matched_reader_proxies; -extern uint32_t max_ever_matched_writer_proxies; -extern uint32_t current_max_matched_writer_proxies; +extern std::atomic max_ever_matched_writer_proxies; +extern std::atomic current_max_matched_writer_proxies; -extern uint32_t max_ever_unmatched_reader_proxies; -extern uint32_t current_max_unmatched_reader_proxies; +extern std::atomic max_ever_unmatched_reader_proxies; +extern std::atomic current_max_unmatched_reader_proxies; -extern uint32_t max_ever_unmatched_writer_proxies; -extern uint32_t current_max_unmatched_writer_proxies; +extern std::atomic max_ever_unmatched_writer_proxies; +extern std::atomic current_max_unmatched_writer_proxies; } // namespace SEDP } // namespace Diagnostics diff --git a/components/rtps/include/rtps_action.hpp b/components/rtps/include/rtps_action.hpp index c58e0c24e2..2d01a8a011 100644 --- a/components/rtps/include/rtps_action.hpp +++ b/components/rtps/include/rtps_action.hpp @@ -123,6 +123,12 @@ template class Acti goal_callback_t on_goal; ///< Accept/reject each incoming goal. execute_callback_t execute; ///< Run each accepted goal (own thread). RtpsProtocol protocol{RtpsProtocol::ROS2}; ///< Wire protocol. + /// Priority band inherited by all of the action's underlying endpoints + /// (see RtpsParticipant::ActionConfig::band, incl. the ration note). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP marking for the traffic the server sends (see + /// RtpsParticipant::ActionConfig::dscp). + std::optional dscp{}; }; /// Construct and register the action server on a started participant, which @@ -134,7 +140,7 @@ template class Acti auto execute = config.execute; if (config.protocol == RtpsProtocol::NATIVE) { valid_ = participant.add_native_action_server( - {config.action, config.type_name}, + {config.action, config.type_name, config.band, config.dscp}, [on_goal](std::span goal_bytes) -> bool { auto g = detail::rtps_deserialize(goal_bytes); return g && (!on_goal || on_goal(*g)); @@ -157,7 +163,7 @@ template class Acti }); } else { valid_ = participant.add_action_server( - {config.action, config.type_name}, + {config.action, config.type_name, config.band, config.dscp}, [on_goal](const RtpsParticipant::GoalId &, std::span goal_bytes) -> bool { auto g = detail::rtps_deserialize(goal_bytes); return g && (!on_goal || on_goal(*g)); @@ -218,6 +224,12 @@ template class Acti std::string action; ///< Action name, e.g. "/fibonacci". std::string type_name; ///< Base DDS type (ROS 2), or any matching name (native). RtpsProtocol protocol{RtpsProtocol::ROS2}; ///< Wire protocol. + /// Priority band inherited by all of the action's underlying endpoints + /// (see RtpsParticipant::ActionConfig::band, incl. the ration note). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP marking for the traffic the client sends (see + /// RtpsParticipant::ActionConfig::dscp). + std::optional dscp{}; }; /// Construct and register the action client on a started participant, which @@ -226,9 +238,11 @@ template class Acti /// \param config The client configuration. ActionClient(RtpsParticipant &participant, const Config &config) { if (config.protocol == RtpsProtocol::NATIVE) { - native_ = participant.add_native_action_client({config.action, config.type_name}); + native_ = participant.add_native_action_client( + {config.action, config.type_name, config.band, config.dscp}); } else { - ros_ = participant.add_action_client({config.action, config.type_name}); + ros_ = participant.add_action_client( + {config.action, config.type_name, config.band, config.dscp}); } } diff --git a/components/rtps/include/rtps_participant.hpp b/components/rtps/include/rtps_participant.hpp index 3eb0d1739a..eda539489a 100644 --- a/components/rtps/include/rtps_participant.hpp +++ b/components/rtps/include/rtps_participant.hpp @@ -3,7 +3,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -16,6 +18,9 @@ #include #include "base_component.hpp" +#include "dscp.hpp" // espp::Dscp (per-endpoint outbound marking) +#include "qos_band.hpp" // espp::QosBand (per-endpoint / per-channel priority) +#include "timer.hpp" // retry timer for rejected deferred-drain arms // Forward declarations of the embeddedRTPS engine types (see // components/rtps/include/rtps/). The engine headers are only needed @@ -26,6 +31,7 @@ class Participant; class Writer; class Reader; class ReaderCacheChange; +class EsppTransport; } // namespace rtps // The RPC layer (services + actions, ROS-interoperable and native) is compiled @@ -53,12 +59,21 @@ namespace espp { /// "std_msgs::msg::dds_::String_" matches a ROS 2 std_msgs/String subscriber /// on /chatter). /// -/// Phase 1 facade (see components/rtps/REFACTOR_PLAN.md): the engine -/// beneath is unchanged, so its current limitations apply - domain id is fixed -/// at compile time (Config::DOMAIN_ID, default 0), announcement/heartbeat -/// periods are compile-time constants, endpoint counts are bounded by the -/// engine's pools, and a second RtpsParticipant in the same process will -/// collide on unicast ports (scheduled fix in Phase 2). +/// Engine limitations that still apply (see components/rtps/REFACTOR_PLAN.md): +/// domain id is fixed at compile time (Config::DOMAIN_ID, default 0), +/// announcement/heartbeat periods are compile-time constants, and endpoint +/// counts are bounded by the engine's pools. Multiple RtpsParticipants per +/// process/host work - each probes forward to free unicast ports. +/// +/// Priority scheduling: transport channels dispatch at espp::QosBand bands +/// (metatraffic High by default - Config::metatraffic_band; user traffic +/// Normal). Endpoints get per-endpoint priority via WriterConfig::band / +/// ReaderConfig::band (and ServiceConfig / ActionConfig): a banded endpoint is +/// granted a dedicated, band-scheduled (optionally DSCP-marked) unicast port, +/// announced to peers via its SEDP unicast locator; when the dedicated-port +/// ration (Config::max_prioritized_endpoint_ports) is exhausted, banded +/// readers fall back to deferred banded dispatch. See the component README's +/// "Priority scheduling" section. /// /// \section rtps_participant_ex1 RtpsParticipant Example /// \snippet rtps_example.cpp rtps participant example @@ -96,6 +111,25 @@ class RtpsParticipant : public BaseComponent { /// compiled in (always on host; opt-in on ESP32). Ignored for samples that /// fit a single DATA submessage. uint16_t fragment_size{63000}; + /// Priority band for this writer's endpoint (see espp::QosBand). Two + /// distinct effects: + /// - Outbound pool scheduling (ALWAYS applies, no dedicated port + /// needed): the writer's progress()/send work is submitted to the + /// transport pool at this band, so a higher-band writer's outgoing + /// DATA is scheduled ahead of lower-band work under load. + /// - Inbound socket dispatch (requires a DEDICATED unicast port): a + /// non-Normal band (or a set dscp) requests a dedicated port so + /// inbound protocol traffic addressed to it (ACKNACKs from reliable + /// readers) is dispatched at this band and outgoing DATA leaves from + /// that socket. Rationed - see Config::max_prioritized_endpoint_ports; + /// when no dedicated port is available only the inbound-dispatch part + /// is lost (ACKNACKs share the participant's user-unicast port), while + /// the outbound pool scheduling above still applies. + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP code point (e.g. espp::Dscp::Ef) marking the traffic this + /// writer SENDS. Requires (and by itself requests) a dedicated port, since + /// DSCP is per-socket; ignored when none could be allocated. + std::optional dscp{}; }; /// Configuration for a reader (subscribing endpoint). @@ -104,6 +138,22 @@ class RtpsParticipant : public BaseComponent { std::string type_name; ///< DDS type name (e.g. "std_msgs::msg::dds_::String_"). Reliability reliability{Reliability::BEST_EFFORT}; ///< Reliability QoS. sample_callback_t on_sample{nullptr}; ///< Called for each received sample. + /// Priority band for this reader's endpoint (see espp::QosBand). A + /// non-Normal band (or a set dscp) requests a DEDICATED unicast port, + /// announced to peers via the endpoint's SEDP unicast locator (standard + /// DDS, honored by FastDDS/ROS 2), so this reader's samples arrive on + /// their own socket and are dispatched at this band ahead of Normal + /// traffic. Rationed - see Config::max_prioritized_endpoint_ports; when no + /// dedicated port is available (or dedicated ports are disabled) the + /// reader falls back to DEFERRED banded dispatch: its on_sample runs from + /// a bounded per-reader queue re-submitted to the transport pool at this + /// band (ordering preserved, one in-flight callback per reader) instead of + /// inline on the shared-port receive worker. + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP code point marking the traffic this reader SENDS (its + /// ACKNACKs, when reliable). Requires a dedicated port; ignored when none + /// could be allocated. + std::optional dscp{}; }; /// Configuration for the participant. @@ -115,6 +165,27 @@ class RtpsParticipant : public BaseComponent { matched_callback_t on_publisher_matched{nullptr}; ///< A writer gained a remote reader. matched_callback_t on_subscriber_matched{nullptr}; ///< A reader gained a remote writer. Logger::Verbosity log_level{Logger::Verbosity::WARN}; ///< Facade log verbosity. + /// Priority band for the metatraffic (SPDP/SEDP discovery) channels. High + /// by default so discovery dispatch stays responsive when user traffic + /// backs the worker pool up; set to QosBand::Normal for the exact pre-band + /// behavior. + espp::QosBand metatraffic_band{espp::QosBand::High}; + /// Priority band for the shared user-traffic channels (user unicast + + /// user multicast). Normal by default (pre-band behavior). + espp::QosBand user_traffic_band{espp::QosBand::Normal}; + /// Allow endpoints with a non-Normal band (or a dscp) to get a dedicated + /// unicast port (see WriterConfig::band / ReaderConfig::band). Disable to + /// force every banded endpoint onto the shared user port (readers then use + /// deferred banded dispatch). + bool enable_dedicated_endpoint_ports{true}; + /// Cap on dedicated endpoint ports. Each consumes one UDP socket/fd - on + /// ESP32, lwIP's CONFIG_LWIP_MAX_SOCKETS defaults to ~10 total and the + /// participant already uses 4 - so dedicated ports are deliberately + /// rationed. When exhausted, further banded endpoints log a warning and + /// fall back to the shared port (readers: deferred banded dispatch). The + /// cap is a TRUE fd bound: a released endpoint's socket counts against it + /// until its fd actually closes (normally immediate). + uint8_t max_prioritized_endpoint_ports{4}; }; /// Construct the participant (does not open sockets; see start()). @@ -207,6 +278,15 @@ class RtpsParticipant : public BaseComponent { /// Base DDS service type, e.g. "example_interfaces::srv::dds_::AddTwoInts". /// The _Request_/_Response_ suffixes are derived internally. std::string type_name; + /// Priority band applied to BOTH of the service's endpoints: for a server + /// its request reader + reply writer, for a client its request writer + + /// reply reader (see WriterConfig::band / ReaderConfig::band for the + /// dedicated-port / deferred-dispatch semantics; note each banded endpoint + /// counts against Config::max_prioritized_endpoint_ports). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP code point applied to both endpoints' dedicated sockets + /// (marks the requests/replies this side SENDS). + std::optional dscp{}; }; /// Handle to reply to a service request later (deferred reply). Copyable and @@ -270,8 +350,10 @@ class RtpsParticipant : public BaseComponent { private: friend class RtpsParticipant; struct Impl; - explicit ServiceClient(std::unique_ptr impl); - std::unique_ptr impl_; + explicit ServiceClient(std::shared_ptr impl); + /// shared_ptr: deferred drain jobs capture the Impl (see DeferredDispatch), + /// so it must be shareable and outlive queued work. + std::shared_ptr impl_; }; /// Add a service server. The handler is invoked for each request; its return @@ -306,6 +388,17 @@ class RtpsParticipant : public BaseComponent { std::string action; ///< ROS 2 action name, e.g. "/fibonacci". /// Base DDS action type, e.g. "example_interfaces::action::dds_::Fibonacci". std::string type_name; + /// Priority band inherited by ALL of the action's underlying endpoints: + /// the send_goal/cancel_goal/get_result service endpoints and the + /// feedback/status topic endpoints (a ROS action server is ~8 endpoints, + /// a client ~7 - far more than the default dedicated-port ration of + /// Config::max_prioritized_endpoint_ports, so most of a banded action's + /// endpoints will use the shared port; readers there get deferred banded + /// dispatch. Raise the cap if you want dedicated ports for a whole + /// action.) Native actions inherit it on their ~3 endpoints likewise. + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP code point for the endpoints' dedicated sockets. + std::optional dscp{}; }; /// Server-side handle to a running goal, passed to the execute callback (which @@ -495,14 +588,75 @@ class RtpsParticipant : public BaseComponent { #endif // RTPS_WITH_RPC protected: + /// Deferred banded dispatch for a banded endpoint that did NOT get a + /// dedicated port (ration exhausted or dedicated ports disabled): instead of + /// running the user callback inline on the shared-port receive worker, each + /// delivery is queued (bounded) and drained by a single in-flight job + /// re-submitted to the transport's worker pool at `band` - one delivery per + /// job, mirroring the reactor's one-shot pattern - preserving per-endpoint + /// ordering while letting the pool schedule it against other bands. When + /// disabled (the default path and dedicated-port endpoints), run() executes + /// the delivery inline, exactly as before. + struct DeferredDispatch { + bool enabled{false}; + espp::QosBand band{espp::QosBand::Normal}; + rtps::EsppTransport *transport{nullptr}; + std::mutex mutex; ///< guards queue / flags / dropped + std::deque> queue; ///< pending deliveries (bounded) + bool in_flight{false}; ///< a drain job is queued/running + bool needs_arm{false}; ///< an arm was rejected; retried by the timer + bool closed{false}; ///< close() ran: drop queue, refuse new work + bool delivering{false}; ///< a delivery callback is executing right now + std::condition_variable drain_done; ///< signalled when a delivery finishes (close() waits) + std::size_t dropped{0}; ///< deliveries dropped (queue full) + /// Lazy retry timer, created only when the transport pool rejects a drain + /// arm: guarantees a queued (possibly lone/last) delivery is re-armed even + /// if no further traffic arrives. Its callback captures the OWNING context + /// weakly (no cycle). It NEVER self-cancels (the callback always returns + /// false, no-op'ing once there is nothing to re-arm); only close() cancels + /// it, synchronously. A self-cancelling espp::Timer leaves running_ set + /// while its task exits, so a later start() would no-op against a dead task + /// and strand the parked delivery - keeping it alive avoids that race. + std::unique_ptr retry_timer; + /// Pending-delivery bound per endpoint: beyond it the NEWEST delivery is + /// dropped (with a warning), so a stalled callback cannot queue without + /// limit beyond the pool's own bounds. + static constexpr std::size_t max_queued = 32; + + /// Run `delivery` inline (when not enabled) or enqueue it and arm the + /// single drain job at `band`. `owner` is the shared context this + /// dispatcher is embedded in: every drain job (and the retry timer) + /// captures it, so queued work can never outlive the context (the + /// lifetime model for all deferred work - see close()). + void run_or_defer(std::function delivery, std::shared_ptr owner); + + /// Quiesce: drop all queued deliveries, refuse new ones, and cancel the + /// retry timer (synchronously - after close() returns no timer callback + /// is running). MUST be called before the owning context's references are + /// released and before its engine endpoints are deleted; the at-most-one + /// in-flight delivery finishes against memory kept alive by its shared + /// owner capture. + void close(); + + private: + /// Submit the single drain job (in_flight must already be true). On pool + /// rejection: flags needs_arm and starts the retry timer. + void arm(std::shared_ptr owner); + void drain(std::shared_ptr owner); ///< one delivery, then re-arm if queued + void ensure_retry_timer_locked(const std::shared_ptr &owner); ///< mutex held + }; + /// Per-reader context bridging the engine's C function-pointer callback to /// the std::function callback; heap-allocated so its address stays stable /// for the lifetime of the reader. - struct ReaderContext { + struct ReaderContext : std::enable_shared_from_this { RtpsParticipant *self{nullptr}; sample_callback_t on_sample{nullptr}; std::mutex buffer_mutex; std::vector buffer; + DeferredDispatch deferred; ///< banded shared-port readers only + std::string topic; ///< for rollback of partially-built composites + rtps::Reader *reader{nullptr}; ///< engine endpoint (for rollback deletion) }; static void reader_trampoline(void *arg, const rtps::ReaderCacheChange &change); @@ -516,17 +670,84 @@ class RtpsParticipant : public BaseComponent { struct ServiceServerContext; static void service_request_trampoline(void *arg, const rtps::ReaderCacheChange &change); static void service_reply_trampoline(void *arg, const rtps::ReaderCacheChange &change); + + /// Composite (action) transaction support: the internal add_* variants + /// return the exact handle they created, and the remove_* helpers remove + /// exactly that handle - so a failed composite rolls back only what THIS + /// invocation built (never a concurrently added endpoint), and calling a + /// composite twice cannot delete the first instance's endpoints. Each + /// removal closes the handle's deferred dispatcher before deleting its + /// engine endpoints (the deferred-work lifetime discipline). + struct NativeServiceServerContext; + std::shared_ptr + add_service_server_deferred_internal(const ServiceConfig &config, + service_deferred_handler_t handler); + std::shared_ptr + add_native_service_server_internal(const ServiceConfig &config, service_handler_t handler); + /// Removal invariant (all remove_* helpers): ENGINE deletion first, facade + /// state (registry entry, deferred close, handle fields) mutated only after + /// the deletion is CONFIRMED. A failed deletion leaves every remaining + /// handle - and the still-live callbacks/dispatchers they anchor - intact + /// for retry; partial progress is recorded by nulling/clearing the already + /// deleted endpoint's field so a retry resumes where it left off. + bool remove_service_server(const std::shared_ptr &server); + bool remove_service_client(const std::shared_ptr &client); + bool remove_native_service_server(const std::shared_ptr &server); + bool remove_native_service_client(const std::shared_ptr &client); #endif // RTPS_WITH_RPC bool resolve_interface_address(std::array &ip_bytes) const; + /// Rollback helpers for partially-built composite endpoints (services / + /// actions): remove a previously added writer/reader so a failed composite + /// leaves no announced endpoint and no consumed dedicated-port ration slot + /// behind. Both lock mutex_ internally - callers must NOT hold it. + bool remove_writer(const std::string &topic); + bool remove_reader(const std::string &topic); + Config config_; std::atomic started_{false}; mutable std::mutex mutex_; ///< guards domain_/participant_/writers_/reader_contexts_ std::unique_ptr domain_; rtps::Participant *participant_{nullptr}; std::unordered_map writers_; - std::vector> reader_contexts_; + /// shared_ptr (not unique_ptr): deferred deliveries and drain jobs capture + /// the context, so it stays alive until the last queued job releases it + /// even if it is removed/rolled back first (see DeferredDispatch). + std::vector> reader_contexts_; + + // Active engine operations: removal/quiesce sequences that must dereference + // domain_/participant_ OUTSIDE mutex_ (their deferred close() waits for an + // in-flight user callback that may take mutex_, so they cannot hold it). + // begin_engine_op() registers such an operation under mutex_ (failing once + // teardown has begun); stop() waits for the count to reach zero BEFORE + // stopping/destroying the engine, so those unlocked phases can never race + // domain teardown into a use-after-free. The cv waits on mutex_ (and + // releases it while waiting, so an operation's callback can still take it). + int active_engine_ops_{0}; + std::condition_variable engine_ops_cv_; + bool stopping_{false}; ///< set in stop() phase 1 (under mutex_): no new ops + bool begin_engine_op(); + void end_engine_op(); + /// RAII for begin/end_engine_op(); use after a successful begin. + struct EngineOpGuard { + explicit EngineOpGuard(RtpsParticipant &p) + : p_(p) {} + ~EngineOpGuard() { p_.end_engine_op(); } + RtpsParticipant &p_; + }; + + // Endpoints whose deletion failed during a creation-time rollback (e.g. the + // SEDP dispose could not be sent, so Domain::deleteWriter/deleteReader + // returned false and the endpoint stayed registered with its dedicated port). + // Retained (not dropped) so stop() can retry the deletion instead of leaking + // an untracked endpoint. Guarded by mutex_. + std::vector orphaned_writers_; + std::vector orphaned_readers_; + // Delete an endpoint during a rollback; on failure retain it in the orphan + // list above. Called with mutex_ held. + void rollback_delete_writer(rtps::Writer *writer); + void rollback_delete_reader(rtps::Reader *reader); // Shared liveness token for async RPC reply paths. A deferred service responder // (which user code may hold and fulfill arbitrarily long after the request) @@ -553,7 +774,6 @@ class RtpsParticipant : public BaseComponent { std::vector> action_servers_; std::vector> action_clients_; - struct NativeServiceServerContext; std::vector> native_service_servers_; std::vector> native_service_clients_; diff --git a/components/rtps/include/rtps_pubsub.hpp b/components/rtps/include/rtps_pubsub.hpp index ca407b661a..e7d0e1082f 100644 --- a/components/rtps/include/rtps_pubsub.hpp +++ b/components/rtps/include/rtps_pubsub.hpp @@ -46,6 +46,12 @@ template class Publisher { std::string type_name; ///< DDS type name (must match the peer for interop). RtpsParticipant::Reliability reliability{ RtpsParticipant::Reliability::BEST_EFFORT}; ///< Reliability QoS. + /// Priority band for the underlying writer endpoint (see + /// RtpsParticipant::WriterConfig::band). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP marking for the traffic this publisher sends (see + /// RtpsParticipant::WriterConfig::dscp). + std::optional dscp{}; }; /// Construct and register a writer on the participant. The participant must @@ -60,6 +66,8 @@ template class Publisher { .topic = config.topic, .type_name = config.type_name, .reliability = config.reliability, + .band = config.band, + .dscp = config.dscp, }); } @@ -133,6 +141,13 @@ template class Subscriber { RtpsParticipant::Reliability reliability{ RtpsParticipant::Reliability::BEST_EFFORT}; ///< Reliability QoS. message_callback_t on_message{nullptr}; ///< Typed sample callback. + /// Priority band for the underlying reader endpoint: dedicated receive + /// port when available, deferred banded dispatch otherwise (see + /// RtpsParticipant::ReaderConfig::band). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP marking for the traffic this subscriber sends (its + /// ACKNACKs; see RtpsParticipant::ReaderConfig::dscp). + std::optional dscp{}; }; /// Construct and register a reader on the participant. The participant must @@ -169,6 +184,8 @@ template class Subscriber { (*callback)(*sample); } }, + .band = config.band, + .dscp = config.dscp, }); } diff --git a/components/rtps/include/rtps_service.hpp b/components/rtps/include/rtps_service.hpp index 3ff9abbedc..664843c151 100644 --- a/components/rtps/include/rtps_service.hpp +++ b/components/rtps/include/rtps_service.hpp @@ -48,6 +48,12 @@ template class ServiceServer { std::string type_name; ///< Base DDS type (ROS 2), or any matching name (native). handler_t handler; ///< Request -> Response. RtpsProtocol protocol{RtpsProtocol::ROS2}; ///< Wire protocol. + /// Priority band for the server's endpoints (see + /// RtpsParticipant::ServiceConfig::band). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP marking for the replies this server sends (see + /// RtpsParticipant::ServiceConfig::dscp). + std::optional dscp{}; }; /// Construct and register the server on a started participant, which must @@ -64,11 +70,11 @@ template class ServiceServer { return detail::rtps_serialize(handler(*req)); }; if (config.protocol == RtpsProtocol::NATIVE) { - valid_ = participant.add_native_service_server({config.service, config.type_name}, - std::move(byte_handler)); + valid_ = participant.add_native_service_server( + {config.service, config.type_name, config.band, config.dscp}, std::move(byte_handler)); } else { - valid_ = participant.add_service_server({config.service, config.type_name}, - std::move(byte_handler)); + valid_ = participant.add_service_server( + {config.service, config.type_name, config.band, config.dscp}, std::move(byte_handler)); } } @@ -105,6 +111,12 @@ template class ServiceClient { std::string service; ///< Service name, e.g. "/add_two_ints". std::string type_name; ///< Base DDS type (ROS 2), or any matching name (native). RtpsProtocol protocol{RtpsProtocol::ROS2}; ///< Wire protocol. + /// Priority band for the client's endpoints (see + /// RtpsParticipant::ServiceConfig::band). + espp::QosBand band{espp::QosBand::Normal}; + /// Optional DSCP marking for the requests this client sends (see + /// RtpsParticipant::ServiceConfig::dscp). + std::optional dscp{}; }; /// Construct and register the client on a started participant, which must @@ -113,9 +125,11 @@ template class ServiceClient { /// \param config The client configuration (service name, type, protocol). ServiceClient(RtpsParticipant &participant, const Config &config) { if (config.protocol == RtpsProtocol::NATIVE) { - native_ = participant.add_native_service_client({config.service, config.type_name}); + native_ = participant.add_native_service_client( + {config.service, config.type_name, config.band, config.dscp}); } else { - ros_ = participant.add_service_client({config.service, config.type_name}); + ros_ = participant.add_service_client( + {config.service, config.type_name, config.band, config.dscp}); } } diff --git a/components/rtps/interop/run_interop.sh b/components/rtps/interop/run_interop.sh index d10baaa220..adb7486a64 100755 --- a/components/rtps/interop/run_interop.sh +++ b/components/rtps/interop/run_interop.sh @@ -34,6 +34,10 @@ cmake -S lib -B lib/build -DCMAKE_BUILD_TYPE=Release -DESPP_INSTALL=ON -DCMAKE_I rtps_native_service_loopback rtps_native_action_loopback rtps_typed_rpc_loopback \ rtps_service_interop_server rtps_service_interop_client \ rtps_action_interop_server rtps_action_interop_client \ + rtps_sedp_dedicated_locator rtps_banded_pubsub rtps_banded_deferred rtps_banded_ration \ + rtps_banded_churn rtps_service_rollback rtps_deferred_recovery rtps_guaranteed_submit \ + rtps_remove_reader_deadlock rtps_guaranteed_fairness rtps_writer_churn \ + rtps_stateless_saturation rtps_callback_reentrancy \ rtps_interop_pub rtps_interop_sub > /tmp/build.log 2>&1 build_rc=$? result "build" $build_rc @@ -66,6 +70,62 @@ note "facade <-> facade in-process (two participants, port probing)" note "typed pub/sub in-process" "$BIN"/rtps_typed_pubsub; result "typed_loopback" $? +note "per-endpoint priority: dedicated ports (SEDP locator + ration) + banded loopbacks" +"$BIN"/rtps_sedp_dedicated_locator; result "sedp_dedicated_locator" $? +"$BIN"/rtps_banded_pubsub; result "banded_pubsub" $? +"$BIN"/rtps_banded_deferred; result "banded_deferred" $? +"$BIN"/rtps_banded_ration; result "banded_ration" $? +# Teardown-under-load regression (the CI shutdown-hang class): dedicated-port +# churn + stop() with deferred deliveries in flight must complete promptly. +timeout 120 "$BIN"/rtps_banded_churn; result "banded_churn" $? +# Transactional composite creation: partial failures must leak nothing. +timeout 60 "$BIN"/rtps_service_rollback; result "service_rollback" $? +# Deferred-arm recovery: a rejected drain arm must never strand a delivery. +timeout 60 "$BIN"/rtps_deferred_recovery; result "deferred_recovery" $? +# Guaranteed writer-progress submission: a pool-rejected progress poke must be +# re-armed by the retry mechanism, never dropped. +timeout 60 "$BIN"/rtps_guaranteed_submit; result "guaranteed_submit" $? +# remove_reader must not deadlock when the removed reader's callback calls back +# into the participant (e.g. publish()). +timeout 60 "$BIN"/rtps_remove_reader_deadlock; result "remove_reader_deadlock" $? +# Guaranteed-retry admission fairness: under sustained higher-band overload a +# lower-band producer must still be admitted (eventual-run), and no owed run +# may be lost. +timeout 90 "$BIN"/rtps_guaranteed_fairness; result "guaranteed_fairness" $? +# Writer create/publish/delete churn under load: deletion racing parked/queued +# progress() jobs must neither crash nor wedge the SEDP announcement stream. +timeout 120 "$BIN"/rtps_writer_churn; result "writer_churn" $? +# Best-effort saturation must not collapse: a burst far faster than the send +# path is (near-)losslessly drained with growable history. +timeout 90 "$BIN"/rtps_stateless_saturation; result "stateless_saturation" $? +# Reentrant cross-callback removal: a callback removing a later-registered +# callback (and freeing its arg) must prevent the stale snapshot entry from +# being invoked. +timeout 30 "$BIN"/rtps_callback_reentrancy; result "callback_reentrancy" $? + +# The same saturation test against a STATIC-storage build of the engine with a +# 2-slot best-effort history: this exercises the KEEP_LAST overwrite path (the +# test requires overflow drops to be counted) and the progress() cursor clamp +# (the test requires overflow drops to be COUNTED, delivered+dropped to +# conserve the burst, and delivery above the total-collapse level - the +# pre-fix cursor bug delivered only the final ring contents). +# Built as a separate lib/test pair because the storage model and the history +# depth are baked into the library at compile time. +note "static-storage saturation gate (KEEP_LAST drop-oldest, cursor clamp)" +STATIC_FLAGS="-DRTPS_STORAGE_STATIC -DRTPS_CFG_HISTORY_SIZE_STATELESS=2" +cmake -S lib -B lib/build-static -DCMAKE_BUILD_TYPE=Release -DESPP_INSTALL=ON \ + -DESPP_BUILD_PYTHON=OFF -DCMAKE_INSTALL_PREFIX=/tmp/espp/install-static \ + -DCMAKE_CXX_FLAGS="$STATIC_FLAGS" > /tmp/cmake_static.log 2>&1 \ + && cmake --build lib/build-static -j"$(nproc)" --target install > /tmp/build_static.log 2>&1 \ + && cmake -S pc -B pc/build-static -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_PREFIX_PATH=/tmp/espp/install-static \ + -DCMAKE_CXX_FLAGS="$STATIC_FLAGS" > /tmp/cmake_static_pc.log 2>&1 \ + && cmake --build pc/build-static -j"$(nproc)" --target rtps_stateless_saturation > /tmp/build_static_pc.log 2>&1 +static_build_rc=$? +result "static_build" $static_build_rc +if [ $static_build_rc -ne 0 ]; then tail -20 /tmp/cmake_static.log /tmp/build_static.log /tmp/build_static_pc.log; fi +timeout 90 pc/build-static/rtps_stateless_saturation; result "stateless_saturation_static" $? + # Regression guard: a reliable writer under backlog must retain + send every # sample on the dynamic (host) storage path (no cursor-advance-as-drop skip). # Non-fragmented small samples, so robust in the shared-netns container. @@ -153,6 +213,23 @@ kill $ROS_PID 2>/dev/null; wait $ROS_PID 2>/dev/null cat /tmp/sub1.log result "ros2_pub->espp_sub" $sub_rc +note "ROS 2 publisher -> espp BANDED subscriber (dedicated unicast port)" +# The espp reader runs at QosBand::High (band=1), so it is granted a dedicated +# unicast port (7400+250*domain+100+n) announced via its SEDP per-endpoint +# unicast locator. FastDDS honors that locator and sends the topic's DATA to +# the dedicated port - delivery here proves a dedicated-port endpoint +# interoperates with FastDDS/ROS 2. +"$BIN"/rtps_interop_sub rt/chatter std_msgs::msg::dds_::String_ 1 3 30 "" 0 1 > /tmp/subband.log 2>&1 & +SUBBAND_PID=$! +sleep 3 +timeout 35 ros2 topic pub -r 5 /chatter std_msgs/msg/String "data: 'ros2 to banded espp'" > /tmp/rospubband.log 2>&1 & +ROSBAND_PID=$! +wait $SUBBAND_PID +subband_rc=$? +kill $ROSBAND_PID 2>/dev/null; wait $ROSBAND_PID 2>/dev/null +cat /tmp/subband.log +result "ros2_pub->espp_banded_sub" $subband_rc + note "espp best-effort publisher -> ROS 2 best-effort subscriber" "$BIN"/rtps_interop_pub rt/chatter std_msgs::msg::dds_::String_ 0 60 200 > /tmp/pub2.log 2>&1 & ESPP_PID=$! diff --git a/components/rtps/src/communication/EsppTransport.cpp b/components/rtps/src/communication/EsppTransport.cpp index 0690dbd388..11eaf8ea2e 100644 --- a/components/rtps/src/communication/EsppTransport.cpp +++ b/components/rtps/src/communication/EsppTransport.cpp @@ -80,6 +80,10 @@ EsppTransport::EsppTransport(RxCallback callback, void *args) , m_callbackArgs(args) { espp::ThreadPool::Config pool_config; pool_config.worker_count = 2; + // Bounded queue: rejected submissions are real backpressure signals (the + // reactor re-arms the socket and the deferred dispatchers retry via their + // timer), instead of an unbounded heap-backed backlog under overload. + pool_config.max_queue_size = 64; pool_config.worker_task_config = { .name = "rtps_worker", .stack_size_bytes = Config::THREAD_POOL_READER_STACKSIZE, @@ -117,7 +121,8 @@ std::string EsppTransport::ip4ToString(const Ip4AddressBytes &addr) { "." + std::to_string(addr[3]); } -bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { +bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort, + const ChannelOptions &options) { if (!channel.socket) { logger_.error("startReceiver called with null socket on port {}", receivePort); return false; @@ -130,6 +135,11 @@ bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { // preserving RTPS's per-locator ordering. espp::UdpSocket::ReceiveConfig receive_config; receive_config.port = receivePort; + // Priority band for this channel's receive dispatch (metatraffic channels + // run above user channels by default - see Domain) and optional DSCP marking + // for traffic sent from this socket (dedicated endpoint ports). + receive_config.band = options.band; + receive_config.dscp = options.dscp; #ifdef RTPS_ENABLE_FRAGMENTATION // With fragmentation enabled a peer (e.g. FastDDS/ROS 2) may send DATA_FRAG // fragments as large as a full UDP datagram (~64 KB), so the per-datagram read @@ -155,7 +165,8 @@ bool EsppTransport::startReceiver(Channel &channel, Ip4Port_t receivePort) { return true; } -EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort, bool allow_reuse) { +EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort, bool allow_reuse, + const ChannelOptions &options) { for (auto &channel : m_channels) { if (channel.in_use) { continue; @@ -175,7 +186,8 @@ EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort, bool // large (fragmented) sample is not dropped before the reactor drains it. // Best-effort: some stacks clamp SO_RCVBUF, so failure is ignored. Only // compiled when fragmentation is enabled (never on the ESP32 default build). - (void)channel.socket->set_receive_buffer_size(4 * 1024 * 1024); // request 4 MB (kernel may clamp) + (void)channel.socket->set_receive_buffer_size(4 * 1024 * + 1024); // request 4 MB (kernel may clamp) #endif if (!allow_reuse && !channel.socket->disable_reuse()) { @@ -187,7 +199,7 @@ EsppTransport::Channel *EsppTransport::createChannel(Ip4Port_t receivePort, bool channel.port = receivePort; channel.in_use = true; - if (!startReceiver(channel, receivePort)) { + if (!startReceiver(channel, receivePort, options)) { channel.socket.reset(); channel.port = 0; channel.in_use = false; @@ -218,24 +230,164 @@ void EsppTransport::onReceive(Ip4Port_t receivePort, std::vector &data, static_cast(sender.port), remoteAddress); } -bool EsppTransport::submit(std::function job) { - if (!m_pool || !m_pool->try_submit(std::move(job))) { +bool EsppTransport::submit(std::function job, espp::QosBand band) { + if (!m_pool || !m_pool->try_submit(std::move(job), band)) { logger_.warn("Transport worker pool rejected a job (stopped or queue full)"); return false; } return true; } +void EsppTransport::submitGuaranteed(const void *key, std::function job, + espp::QosBand band) { + // try_submit only moves the job on the accept path; a rejected submit leaves + // `job` intact, so it is safe to std::move here and still park it below. + if (m_pool && m_pool->try_submit(std::move(job), band)) { + return; + } + parkPendingJob(key, std::move(job), band, /*cap_to_one=*/false); +} +void EsppTransport::submitGuaranteedDrain(const void *key, std::function job, + espp::QosBand band) { + // Same accept path as submitGuaranteed(); on rejection park with the + // pending-count capped at one (drain semantics - see the header). + if (m_pool && m_pool->try_submit(std::move(job), band)) { + return; + } + parkPendingJob(key, std::move(job), band, /*cap_to_one=*/true); +} + +void EsppTransport::cancelGuaranteed(const void *key) { + std::lock_guard lock(m_pendingMutex); + m_pendingByKey.erase(key); +} + +void EsppTransport::parkPendingJob(const void *key, std::function job, espp::QosBand band, + bool cap_to_one) { + std::lock_guard lock(m_pendingMutex); + if (m_stopping) { + return; // teardown: nothing to guarantee anymore + } + // Coalesce by producer: a repeated poke for the same writer just bumps its + // owed count (the job/band are identical), so the map is bounded by the + // producer count and nothing is ever dropped. Each owed run re-invokes + // progress() once, preserving one-poke/one-sample. + auto &entry = m_pendingByKey[key]; + entry.job = std::move(job); + entry.band = band; + if (cap_to_one) { + // Drain semantics (submitGuaranteedDrain): the job re-arms itself while + // unsent data remains, so one pending run per producer suffices; counting + // per poke would accumulate debt for samples a KEEP_LAST history has + // already overwritten. + entry.count = 1; + } else { + ++entry.count; + } + if (m_retryTimer) { + return; // already running; it drains owed runs on its next tick + } + // The retry timer NEVER self-cancels (callback always returns false). A + // self-cancelling espp::Timer stops its task but leaves running_ set, so a + // later start() would no-op against a dead task and strand parked work; + // keeping it alive until stop() sidesteps that. Cost: a 20 ms mutex+empty + // check, paid only after an actual pool rejection and only until stop(). + m_retryTimer = std::make_unique(espp::Timer::Config{ + .name = "rtps_job_retry", + .period = std::chrono::milliseconds(20), + .delay = std::chrono::milliseconds(20), + .callback = [this]() -> bool { + std::lock_guard lock(m_pendingMutex); + if (m_stopping) { + return false; // stop() cancels this timer; nothing to do + } + // Fair drain: submit at most one owed run per key per pass and rotate + // the starting key across ticks, with NO band ordering at this + // admission stage. Draining one key to exhaustion starved later keys; + // sorting by band here was no better - a continuously-owed higher-band + // producer would take the only freed slot on every tick and starve + // lower bands forever, violating submitGuaranteed()'s eventual-run + // contract. Round-robin admission guarantees every key is admitted + // within N ticks of a free slot; PRIORITY is preserved because each job + // is submitted at its own band and the pool's banded queues (with + // aging) decide execution order among admitted jobs. + using MapIt = std::map::iterator; + std::vector ready; + ready.reserve(m_pendingByKey.size()); + for (auto it = m_pendingByKey.begin(); it != m_pendingByKey.end(); ++it) { + if (it->second.count > 0) { + ready.push_back(it); + } + } + if (!ready.empty()) { + // Rotate the start so a different key leads each tick. + std::rotate(ready.begin(), ready.begin() + (m_drainRotor++ % ready.size()), ready.end()); + bool saturated = false; + while (!saturated) { + bool submitted_this_pass = false; + for (MapIt it : ready) { + if (it->second.count == 0) { + continue; + } + // Pass a COPY of the job - it is reused `count` times, so it must + // not be moved out (try_submit takes it by rvalue). + if (!m_pool || + !m_pool->try_submit(std::function(it->second.job), it->second.band)) { + saturated = true; // pool full; remaining owed runs retry next tick + break; + } + --it->second.count; + submitted_this_pass = true; + } + if (!submitted_this_pass) { + break; // every key drained (or nothing left to submit) + } + } + // Drop fully-drained keys. + for (auto it = m_pendingByKey.begin(); it != m_pendingByKey.end();) { + if (it->second.count == 0) { + it = m_pendingByKey.erase(it); + } else { + ++it; + } + } + } + return false; // never self-cancel (see above) + }, + .auto_start = true, + .log_level = espp::Logger::Verbosity::WARN, + }); +} + void EsppTransport::stop() { + // Quiesce the guaranteed-job retry FIRST: cancel is synchronous, so no + // retry callback runs during or after the reactor/pool teardown below, and + // parked jobs (which may reference writers) are dropped before endpoint + // teardown. + { + std::lock_guard lock(m_pendingMutex); + m_stopping = true; + m_pendingByKey.clear(); + } + if (m_retryTimer) { + m_retryTimer->cancel(); + } if (m_reactor) { m_reactor->stop(); } if (m_pool) { m_pool->stop(); } + // Backstop: retired sockets are normally destroyed promptly by the + // reactor's removal-completion callback; anything still parked here (e.g. + // a completion that never fired because the pool died first) is safe to + // close now that the reactor and pool have quiesced. + std::lock_guard lock(m_mutex); + m_retiredSockets.clear(); } -bool EsppTransport::ensureReceivePort(Ip4Port_t receivePort, bool is_multicast) { +bool EsppTransport::ensureReceivePort(Ip4Port_t receivePort, bool is_multicast, + const ChannelOptions &options) { std::lock_guard lock(m_mutex); Channel *existing = findChannel(receivePort); @@ -243,7 +395,7 @@ bool EsppTransport::ensureReceivePort(Ip4Port_t receivePort, bool is_multicast) return true; } - Channel *created = createChannel(receivePort, /*allow_reuse=*/is_multicast); + Channel *created = createChannel(receivePort, /*allow_reuse=*/is_multicast, options); return created != nullptr; } @@ -253,16 +405,55 @@ bool EsppTransport::releaseReceivePort(Ip4Port_t receivePort) { if (channel == nullptr) { return false; } - if (channel->reactor_id != espp::SocketReactor::INVALID_ID) { - m_reactor->remove(channel->reactor_id); - channel->reactor_id = espp::SocketReactor::INVALID_ID; - } - channel->socket.reset(); + // RETIRE the socket instead of destroying it here: an in-flight (or + // just-submitted) reactor dispatch still references it, and destroying it + // now is a use-after-free - a handler was observed blocking forever on the + // freed object's internal logger mutex, which then wedged + // SocketReactor::stop()'s in-flight wait (the CI shutdown hang). The + // reactor's removal-completion callback destroys the retired socket - + // closing the fd and unbinding the port - as soon as the registration is + // fully gone and no handler can reference it (usually immediately; at the + // latest when the in-flight handler finishes). stop() remains the backstop + // for any socket whose completion never fired (e.g. the pool died first). + // The socket must be parked BEFORE remove(): the completion callback can + // run synchronously on this thread. + espp::UdpSocket *retired = channel->socket.get(); + m_retiredSockets.push_back(std::move(channel->socket)); + const espp::SocketReactor::Id reactor_id = channel->reactor_id; + channel->reactor_id = espp::SocketReactor::INVALID_ID; channel->port = 0; channel->in_use = false; + if (reactor_id != espp::SocketReactor::INVALID_ID) { + // Non-blocking: never wait here - the in-flight handler may need locks + // this caller's stack holds (Domain/transport mutexes). + m_reactor->remove(reactor_id, [this, retired]() { destroyRetiredSocket(retired); }); + } else { + // Never registered on the reactor: nothing can reference it. + destroyRetiredSocket(retired); + } return true; } +void EsppTransport::destroyRetiredSocket(espp::UdpSocket *socket) { + // Removal-completion path: may run on the releasing caller's own thread + // (recursive m_mutex makes that safe), a reactor pool worker, or the + // reactor loop. Erasing under m_mutex closes the fd and unbinds the port; + // racing stop() (which clears the whole retired list) simply makes this a + // no-op - the pointer is only used to FIND the entry, never dereferenced. + std::lock_guard lock(m_mutex); + const auto it = std::find_if( + m_retiredSockets.begin(), m_retiredSockets.end(), + [socket](const std::unique_ptr &s) { return s.get() == socket; }); + if (it != m_retiredSockets.end()) { + m_retiredSockets.erase(it); + } +} + +std::size_t EsppTransport::retiredSocketCount() const { + std::lock_guard lock(m_mutex); + return m_retiredSockets.size(); +} + bool EsppTransport::joinMultiCastGroup(const Ip4AddressBytes &addr) const { std::lock_guard lock(m_mutex); @@ -306,7 +497,7 @@ void EsppTransport::sendPacket(PacketInfo &info) { if (channel == nullptr) { // Sending from one of our own unicast ports: apply unicast semantics // (no port sharing) if the channel was not already registered. - channel = createChannel(info.srcPort, /*allow_reuse=*/false); + channel = createChannel(info.srcPort, /*allow_reuse=*/false, ChannelOptions{}); } if (channel == nullptr || !channel->socket) { diff --git a/components/rtps/src/entities/Domain.cpp b/components/rtps/src/entities/Domain.cpp index 45ac57cd8d..f56e8db2b9 100644 --- a/components/rtps/src/entities/Domain.cpp +++ b/components/rtps/src/entities/Domain.cpp @@ -26,6 +26,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/entities/Domain.hpp" #include "rtps/utils/Log.hpp" #include "rtps/utils/udpUtils.hpp" +#include #include #include #include @@ -44,29 +45,35 @@ Author: i11 - Embedded Software, RWTH Aachen University using rtps::Domain; -Domain::Domain(const rtps::Ip4AddressBytes &localIpAddress) +Domain::Domain(const rtps::Ip4AddressBytes &localIpAddress, const DomainConfig &config) : espp::BaseComponent("RtpsDomain", espp::Logger::Verbosity::WARN) , m_defaultTransport(&Domain::datagramJumppad, this) , m_transport(&m_defaultTransport) - , m_localIpAddress(localIpAddress) { + , m_localIpAddress(localIpAddress) + , m_config(config) { m_transportSetupOk = initializeTransport(); } -Domain::Domain(rtps::EsppTransport &transport, const rtps::Ip4AddressBytes &localIpAddress) +Domain::Domain(rtps::EsppTransport &transport, const rtps::Ip4AddressBytes &localIpAddress, + const DomainConfig &config) : espp::BaseComponent("RtpsDomain", espp::Logger::Verbosity::WARN) , m_defaultTransport(&Domain::datagramJumppad, this) , m_transport(&transport) - , m_localIpAddress(localIpAddress) { + , m_localIpAddress(localIpAddress) + , m_config(config) { m_transportSetupOk = initializeTransport(); } bool Domain::initializeTransport() { assert(m_transport != nullptr); - bool success = true; - success = - m_transport->ensureReceivePort(getUserMulticastPort(), /*is_multicast=*/true) && success; - success = - m_transport->ensureReceivePort(getBuiltInMulticastPort(), /*is_multicast=*/true) && success; + // Metatraffic (SPDP discovery multicast) is registered at the configured + // metatraffic band (High by default) so discovery dispatch overtakes queued + // user-traffic handling; user multicast runs at the user-traffic band. + bool success = m_transport->ensureReceivePort(getUserMulticastPort(), /*is_multicast=*/true, + {.band = m_config.user_traffic_band}); + success = m_transport->ensureReceivePort(getBuiltInMulticastPort(), /*is_multicast=*/true, + {.band = m_config.metatraffic_band}) && + success; success = m_transport->joinMultiCastGroup({239, 255, 0, 1}) && success; return success; } @@ -111,6 +118,15 @@ void Domain::stop() { m_protocolStopRequested = true; nudgeProtocol(); // wake the loop so it observes the stop flag m_protocolTask->stop(); // returns promptly + // Retract the published task synchronization pointers BEFORE the task (and + // the mutex/cv they point into) is destroyed: a late nudge from a racing + // publisher then no-ops instead of locking a destroyed mutex. + { + std::lock_guard nudge_lock(m_protocolNudgeMutex); + m_protocolMutex = nullptr; + m_protocolCv = nullptr; + m_protocolNotified = nullptr; + } m_protocolTask.reset(); m_protocolStopRequested = false; } @@ -150,12 +166,20 @@ bool Domain::protocolLoop(std::mutex &m, std::condition_variable &cv, bool ¬i next_deadline = std::min(next_deadline, writer_deadline); } + // Publish the task's synchronization objects for nudgeProtocol() under the + // nudge mutex: nudges arrive from arbitrary publisher threads, so unguarded + // pointer writes here would race their reads (and re-writing every + // iteration raced even after the first). The values are stable for the + // task's lifetime, so this is one uncontended lock per tick. + { + std::lock_guard nudge_lock(m_protocolNudgeMutex); + m_protocolMutex = &m; + m_protocolCv = &cv; + m_protocolNotified = ¬ified; + } // Sleep until the earliest deadline; a publish on a reliable writer (or // stop()) notifies the cv to re-evaluate immediately. std::unique_lock lock(m); - m_protocolMutex = &m; - m_protocolCv = &cv; - m_protocolNotified = ¬ified; cv.wait_until(lock, next_deadline, [¬ified] { return notified; }); if (notified) { notified = false; @@ -170,6 +194,11 @@ bool Domain::protocolLoop(std::mutex &m, std::condition_variable &cv, bool ¬i } void Domain::nudgeProtocol() { + // Read the published pointers under the nudge mutex (see the member doc): + // this synchronizes with protocolLoop()'s publication and with stop()'s + // nulling, so a nudge racing either is a safe no-op rather than a torn read + // or a use-after-free of the destroyed task's mutex/cv. + std::lock_guard nudge_lock(m_protocolNudgeMutex); if (m_protocolMutex != nullptr && m_protocolCv != nullptr && m_protocolNotified != nullptr) { std::lock_guard lock(*m_protocolMutex); *m_protocolNotified = true; @@ -220,6 +249,13 @@ void Domain::receiveCallback(const PacketInfo &packet) { m_participants[slot].newMessage(payload, payload_size); } } + } else if (Participant *dedicated = findParticipantByDedicatedPort(packet.destPort); + dedicated != nullptr) { + // A dedicated (per-endpoint) unicast port: route straight to the owning + // participant; the engine's MessageReceiver demuxes by entity id, so the + // local port the datagram arrived on is otherwise irrelevant. + DOMAIN_LOG("Domain: Got message on dedicated endpoint port {}", packet.destPort); + dedicated->newMessage(payload, payload_size); } else { // Pass to addressed one only (Unicast, by Port) ParticipantId_t id = @@ -254,13 +290,42 @@ rtps::Participant *Domain::createParticipant() { // same strategy FastDDS uses. Ids may therefore skip values; slots are // tracked separately (m_numParticipants). ParticipantId_t candidate = m_nextParticipantId; - const ParticipantId_t last_candidate = m_nextParticipantId + PARTICIPANT_PORT_PROBE_LIMIT; + ParticipantId_t last_candidate = m_nextParticipantId + PARTICIPANT_PORT_PROBE_LIMIT; + if (m_config.enable_dedicated_endpoint_ports) { + // ENFORCE the id/dedicated-range separation (see DEDICATED_PORT_OFFSET in + // the header): an id whose standard user-unicast offset (D3 + PG * id, + // the larger of the two unicast offsets) reaches DEDICATED_PORT_OFFSET + // would bind its SHARED user port inside the dedicated range - a later + // allocateDedicatedEndpointPort() probe would then hit that already-bound + // port, ensureReceivePort() would report the existing shared channel as a + // successful dedicated allocation, and the dedicated-port registry would + // route that participant's user traffic to the wrong participant. With + // the standard offsets this caps ids at 44 - far beyond the id budget of + // any supported participant count, so the cap only bites when port + // probing has skipped absurdly many occupied ids. + constexpr auto kMaxIdBelowDedicatedRange = + static_cast((DEDICATED_PORT_OFFSET - 1 - D3) / PG); + if (last_candidate > kMaxIdBelowDedicatedRange + 1) { + last_candidate = kMaxIdBelowDedicatedRange + 1; + } + if (candidate > kMaxIdBelowDedicatedRange) { + DOMAIN_LOG("Participant id probe reached {} but ids above {} are excluded while " + "dedicated endpoint ports are enabled (their standard unicast ports would " + "fall inside the dedicated range)", + candidate, kMaxIdBelowDedicatedRange); + m_transportSetupOk = false; + return nullptr; + } + } bool ports_ok = false; for (; candidate < last_candidate; ++candidate) { - if (!m_transport->ensureReceivePort(getUserUnicastPort(candidate), /*is_multicast=*/false)) { + if (!m_transport->ensureReceivePort(getUserUnicastPort(candidate), /*is_multicast=*/false, + {.band = m_config.user_traffic_band})) { continue; } - if (m_transport->ensureReceivePort(getBuiltInUnicastPort(candidate), /*is_multicast=*/false)) { + // SEDP unicast is metatraffic: keep discovery dispatch above user traffic. + if (m_transport->ensureReceivePort(getBuiltInUnicastPort(candidate), /*is_multicast=*/false, + {.band = m_config.metatraffic_band})) { ports_ok = true; break; } @@ -277,17 +342,70 @@ rtps::Participant *Domain::createParticipant() { auto &entry = m_participants[m_numParticipants]; ++m_numParticipants; entry.reuse(generateGuidPrefix(candidate), candidate, m_localIpAddress); - createBuiltinWritersAndReaders(entry); + if (!createBuiltinWritersAndReaders(entry)) { + // Pool exhaustion (see createBuiltinWritersAndReaders): unwind the slot + // and the probed ports so the failure is clean and the caller gets a + // truthful nullptr instead of a participant missing its discovery + // endpoints (or a crash). + --m_numParticipants; + m_transport->releaseReceivePort(getUserUnicastPort(candidate)); + m_transport->releaseReceivePort(getBuiltInUnicastPort(candidate)); + return nullptr; + } m_nextParticipantId = static_cast(candidate + 1); return &entry; } -void Domain::createBuiltinWritersAndReaders(Participant &part) { +bool Domain::createBuiltinWritersAndReaders(Participant &part) { + // Every allocation below is null-checked: the pools are GLOBAL, so a limits + // override (or profile) sized for fewer participants than + // MAX_NUM_PARTICIPANTS legitimately runs out here - that must fail the + // participant cleanly, not crash. Allocation and init() interleave (the + // allocator returns the first UNINITIALIZED slot, so a second allocation + // from the same pool must come after the first one's init()); on a failure + // partway, the endpoints this invocation already initialized are reset() so + // nothing leaks from the pools. + StatelessWriter *spdpWriter = nullptr; + StatelessReader *spdpReader = nullptr; + StatefulReader *sedpPubReader = nullptr; + StatefulReader *sedpSubReader = nullptr; + StatefulWriter *sedpPubWriter = nullptr; + const auto fail = [&](const char *what) { + DOMAIN_LOG("Builtin endpoint pool exhausted creating participant ({}): each participant " + "consumes 1 stateless writer/reader and 2 stateful writers/readers from the " + "GLOBAL pools - raise the NUM_STATELESS_*/NUM_STATEFUL_* limits or lower " + "MAX_NUM_PARTICIPANTS", + what); + (void)what; + if (spdpWriter != nullptr) { + spdpWriter->reset(); + } + if (spdpReader != nullptr) { + spdpReader->reset(); + } + if (sedpPubReader != nullptr) { + sedpPubReader->reset(); + } + if (sedpSubReader != nullptr) { + sedpSubReader->reset(); + } + if (sedpPubWriter != nullptr) { + sedpPubWriter->reset(); + } + return false; + }; + // SPDP - StatelessWriter *spdpWriter = + spdpWriter = getNextUnusedEndpoint(m_statelessWriters); - StatelessReader *spdpReader = + if (spdpWriter == nullptr) { + return fail("stateless writer for SPDP"); + } + spdpReader = getNextUnusedEndpoint(m_statelessReaders); + if (spdpReader == nullptr) { + return fail("stateless reader for SPDP"); + } TopicData spdpWriterAttributes; spdpWriterAttributes.topicName[0] = '\0'; @@ -320,24 +438,36 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { sedpAttributes.unicastLocator = getBuiltInUnicastLocator(part.m_participantId, m_localIpAddress); // READER - StatefulReader *sedpPubReader = + sedpPubReader = getNextUnusedEndpoint(m_statefulReaders); + if (sedpPubReader == nullptr) { + return fail("stateful reader for SEDP publications"); + } sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER; sedpPubReader->init(sedpAttributes, *m_transport); - StatefulReader *sedpSubReader = + sedpSubReader = getNextUnusedEndpoint(m_statefulReaders); + if (sedpSubReader == nullptr) { + return fail("stateful reader for SEDP subscriptions"); + } sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER; sedpSubReader->init(sedpAttributes, *m_transport); // WRITER - StatefulWriter *sedpPubWriter = + sedpPubWriter = getNextUnusedEndpoint(m_statefulWriters); + if (sedpPubWriter == nullptr) { + return fail("stateful writer for SEDP publications"); + } sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER; sedpPubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, *m_transport); StatefulWriter *sedpSubWriter = getNextUnusedEndpoint(m_statefulWriters); + if (sedpSubWriter == nullptr) { + return fail("stateful writer for SEDP subscriptions"); + } sedpAttributes.endpointGuid.entityId = ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER; sedpSubWriter->init(sedpAttributes, TopicKind_t::NO_KEY, *m_transport); @@ -351,6 +481,7 @@ void Domain::createBuiltinWritersAndReaders(Participant &part) { endpoints.sedpSubWriter = sedpSubWriter; part.addBuiltInEndpoints(endpoints); + return true; } rtps::Participant *Domain::findParticipantById(ParticipantId_t id) { @@ -362,6 +493,119 @@ rtps::Participant *Domain::findParticipantById(ParticipantId_t id) { return nullptr; } +rtps::Participant *Domain::findParticipantByDedicatedPort(Ip4Port_t port) { + // Receive-path lookup: guarded by the registry's own small mutex, NOT + // m_mutex - an API caller holding m_mutex (create/delete) must never be + // able to stall the receive workers (see m_dedicatedPorts). + std::lock_guard lock(m_dedicatedPortsMutex); + const auto it = std::find_if(m_dedicatedPorts.begin(), m_dedicatedPorts.end(), + [port](const DedicatedPort &entry) { return entry.port == port; }); + return (it != m_dedicatedPorts.end()) ? it->participant : nullptr; +} + +rtps::Ip4Port_t Domain::allocateDedicatedEndpointPort(Participant &part, espp::QosBand band, + const std::optional &dscp) { + // Caller holds m_mutex (createWriter/createReader). + if (!m_config.enable_dedicated_endpoint_ports) { + return 0; + } + // The ration must be a TRUE fd bound: count both the active dedicated + // ports AND released sockets whose fd is still open awaiting the reactor's + // removal completion (retired sockets close promptly, but a handler in + // flight defers the close - without counting them, churn could push the + // real fd usage past the cap). + std::size_t active = 0; + { + std::lock_guard registry_lock(m_dedicatedPortsMutex); + active = m_dedicatedPorts.size(); + } + const std::size_t retired = m_transport->retiredSocketCount(); + if (active + retired >= m_config.max_prioritized_endpoint_ports) { + logger_.warn("Dedicated endpoint port ration exhausted ({} active + {} retired, cap {}); " + "falling back to the shared user-unicast port", + active, retired, + static_cast(m_config.max_prioritized_endpoint_ports)); + return 0; + } + const Ip4Port_t base = 7400 + 250 * Config::DOMAIN_ID + DEDICATED_PORT_OFFSET; + const uint16_t first_offset = m_nextDedicatedPortOffset; + uint16_t probed = 0; + for (uint16_t probe = 0; probe < DEDICATED_PORT_PROBE_LIMIT; ++probe) { + const uint16_t offset = first_offset + probe; + if (DEDICATED_PORT_OFFSET + offset > 249) { + break; // stay inside this domain's 250-port block + } + ++probed; + const Ip4Port_t port = base + offset; + // Reuse-disabled unicast bind: a port taken by another process fails + // loudly here and the next candidate is probed (same strategy as the + // participant-id port probe above). + if (m_transport->ensureReceivePort(port, /*is_multicast=*/false, + {.band = band, .dscp = dscp})) { + m_nextDedicatedPortOffset = offset + 1; + std::lock_guard registry_lock(m_dedicatedPortsMutex); + m_dedicatedPorts.push_back(DedicatedPort{port, &part}); + return port; + } + } + // Advance PAST the failed window so the next allocation probes fresh ports; + // without this a fully-occupied window (e.g. ports taken by another + // process) would be retried forever and dedicated allocation would be + // permanently stuck even though later ports in the 100..249 block are free. + m_nextDedicatedPortOffset = first_offset + probed; + logger_.warn("No free dedicated endpoint port (probed {} from offset {}; next allocation " + "starts at offset {}); falling back to the shared user-unicast port", + probed, first_offset, m_nextDedicatedPortOffset); + return 0; +} + +void Domain::releaseDedicatedEndpointPort(Ip4Port_t port) { + // Caller holds m_mutex (deleteWriter/deleteReader); the registry has its + // own mutex, held only for the erase (never across the transport call). + if (port == 0) { + return; + } + bool releasing = false; + { + std::lock_guard registry_lock(m_dedicatedPortsMutex); + const auto it = std::find_if(m_dedicatedPorts.begin(), m_dedicatedPorts.end(), + [port](const DedicatedPort &entry) { return entry.port == port; }); + if (it != m_dedicatedPorts.end()) { + m_dedicatedPorts.erase(it); + releasing = true; + } + } + if (releasing) { + m_transport->releaseReceivePort(port); + } +} + +void Domain::applyEndpointOptions(Participant &part, TopicData &attributes, + const EndpointOptions &options) { + attributes.band = options.band; + attributes.dscp = options.dscp; + // A non-default band (or a DSCP marking, which is per-socket) requests a + // dedicated unicast port. On success the endpoint's SEDP announcement + // carries the dedicated port as its per-endpoint unicast locator (a standard + // DDS parameter - PID_UNICAST_LOCATOR - so FastDDS/ROS 2 peers send this + // endpoint's traffic there), and the endpoint also SENDS from that socket + // (m_srcPort follows the unicast locator), so the DSCP marking applies to + // its outgoing traffic. On failure (disabled / ration exhausted / no free + // port) the endpoint keeps the shared user-unicast locator; a higher layer + // may then apply deferred banded dispatch (see the espp facade). + if (options.band == espp::QosBand::Normal && !options.dscp.has_value()) { + return; + } + const Ip4Port_t port = allocateDedicatedEndpointPort(part, options.band, options.dscp); + if (port == 0) { + return; + } + attributes.unicastLocator = FullLengthLocator::createUDPv4Locator( + m_localIpAddress[0], m_localIpAddress[1], m_localIpAddress[2], m_localIpAddress[3], port); + attributes.hasDedicatedPort = true; + DOMAIN_LOG("Granted dedicated endpoint port {} (band {})", port, static_cast(options.band)); +} + void Domain::registerMulticastPort(FullLengthLocator mcastLocator) { if (mcastLocator.kind == LocatorKind_t::LOCATOR_KIND_UDPv4) { m_transportSetupOk = @@ -377,7 +621,7 @@ rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, con for (unsigned int i = 0; i < m_statefulReaders.size(); i++) { if (m_statefulReaders[i].isInitialized()) { if (strncmp(m_statefulReaders[i].m_attributes.topicName, topicName, - Config::MAX_TYPENAME_LENGTH) != 0) { + Config::MAX_TOPICNAME_LENGTH) != 0) { continue; } @@ -395,7 +639,7 @@ rtps::Reader *Domain::readerExists(Participant &part, const char *topicName, con for (unsigned int i = 0; i < m_statelessReaders.size(); i++) { if (m_statelessReaders[i].isInitialized()) { if (strncmp(m_statelessReaders[i].m_attributes.topicName, topicName, - Config::MAX_TYPENAME_LENGTH) != 0) { + Config::MAX_TOPICNAME_LENGTH) != 0) { continue; } @@ -421,7 +665,7 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, con for (unsigned int i = 0; i < m_statefulWriters.size(); i++) { if (m_statefulWriters[i].isInitialized()) { if (strncmp(m_statefulWriters[i].m_attributes.topicName, topicName, - Config::MAX_TYPENAME_LENGTH) != 0) { + Config::MAX_TOPICNAME_LENGTH) != 0) { continue; } @@ -439,7 +683,7 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, con for (unsigned int i = 0; i < m_statelessWriters.size(); i++) { if (m_statelessWriters[i].isInitialized()) { if (strncmp(m_statelessWriters[i].m_attributes.topicName, topicName, - Config::MAX_TYPENAME_LENGTH) != 0) { + Config::MAX_TOPICNAME_LENGTH) != 0) { continue; } @@ -459,7 +703,19 @@ rtps::Writer *Domain::writerExists(Participant &part, const char *topicName, con } rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, const char *typeName, - bool reliable, bool enforceUnicast) { + bool reliable, bool enforceUnicast, + const EndpointOptions &options) { + // Validate the DSCP up front: an out-of-range code point can never be + // applied (Socket::set_dscp rejects > 63), so probing would burn dedicated + // -port offsets on binds that fail their marking and then silently fall + // back without the requested behavior. Reject the writer instead so the + // misconfiguration is an explicit endpoint-creation error. + if (options.dscp.has_value() && static_cast(options.dscp.value()) > 63) { + logger_.error("Invalid DSCP {} for writer '{}' (valid code points are 0..63); rejecting", + static_cast(options.dscp.value()), topicName); + return nullptr; + } + std::lock_guard lock(m_mutex); StatelessWriter *statelessWriter = getNextUnusedEndpoint(m_statelessWriters); @@ -491,18 +747,26 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, con EntityKind_t::USER_DEFINED_WRITER_WITHOUT_KEY}; attributes.unicastLocator = getUserUnicastLocator(part.m_participantId, m_localIpAddress); attributes.durabilityKind = DurabilityKind_t::TRANSIENT_LOCAL; + applyEndpointOptions(part, attributes, options); DOMAIN_LOG("Creating writer[{}, {}]", topicName, typeName); + // On any failure below, return the endpoint's dedicated port (if one was + // granted) so it is not leaked. + const Ip4Port_t dedicated_port = + attributes.hasDedicatedPort ? static_cast(attributes.unicastLocator.port) : 0; + if (reliable) { attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; if (!statefulWriter->init(attributes, TopicKind_t::NO_KEY, *m_transport, enforceUnicast)) { DOMAIN_LOG("StatefulWriter init failed."); + releaseDedicatedEndpointPort(dedicated_port); return nullptr; } if (!part.addWriter(statefulWriter)) { + releaseDedicatedEndpointPort(dedicated_port); return nullptr; } return statefulWriter; @@ -511,10 +775,12 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, con if (!statelessWriter->init(attributes, TopicKind_t::NO_KEY, *m_transport, enforceUnicast)) { DOMAIN_LOG("StatelessWriter init failed."); + releaseDedicatedEndpointPort(dedicated_port); return nullptr; } if (!part.addWriter(statelessWriter)) { + releaseDedicatedEndpointPort(dedicated_port); return nullptr; } return statelessWriter; @@ -522,7 +788,19 @@ rtps::Writer *Domain::createWriter(Participant &part, const char *topicName, con } rtps::Reader *Domain::createReader(Participant &part, const char *topicName, const char *typeName, - bool reliable, rtps::Ip4AddressBytes mcastaddress) { + bool reliable, rtps::Ip4AddressBytes mcastaddress, + const EndpointOptions &options) { + // Validate the DSCP up front: an out-of-range code point can never be + // applied (Socket::set_dscp rejects > 63), so probing would burn dedicated + // -port offsets on binds that fail their marking and then silently fall + // back without the requested behavior. Reject the reader instead so the + // misconfiguration is an explicit endpoint-creation error. + if (options.dscp.has_value() && static_cast(options.dscp.value()) > 63) { + logger_.error("Invalid DSCP {} for reader '{}' (valid code points are 0..63); rejecting", + static_cast(options.dscp.value()), topicName); + return nullptr; + } + std::lock_guard lock(m_mutex); StatelessReader *statelessReader = getNextUnusedEndpoint(m_statelessReaders); @@ -572,9 +850,15 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, con } } attributes.durabilityKind = DurabilityKind_t::VOLATILE; + applyEndpointOptions(part, attributes, options); DOMAIN_LOG("Creating reader[{}, {}]", topicName, typeName); + // On any failure below, return the endpoint's dedicated port (if one was + // granted) so it is not leaked. + const Ip4Port_t dedicated_port = + attributes.hasDedicatedPort ? static_cast(attributes.unicastLocator.port) : 0; + if (reliable) { attributes.reliabilityKind = ReliabilityKind_t::RELIABLE; @@ -584,6 +868,7 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, con if (!part.addReader(statefulReader)) { DOMAIN_LOG("Failed to add reader to participant."); + releaseDedicatedEndpointPort(dedicated_port); return nullptr; } return statefulReader; @@ -594,6 +879,7 @@ rtps::Reader *Domain::createReader(Participant &part, const char *topicName, con statelessReader->init(attributes); if (!part.addReader(statelessReader)) { + releaseDedicatedEndpointPort(dedicated_port); return nullptr; } return statelessReader; @@ -609,7 +895,18 @@ bool rtps::Domain::deleteReader(Participant &part, Reader *reader) { return false; } + // Capture the dedicated port BEFORE reset() wipes the attributes, but + // release it AFTER reset(): reset() drains the in-flight guarded dispatches, + // and a still-running HEARTBEAT/GAP handler may sendPacket() from this + // source port - releasing first would let EsppTransport::sendPacket() + // recreate it as an ordinary untracked channel (default band/DSCP) and leak + // the fd. Mirrors the writer teardown below. + const bool had_dedicated_port = reader->m_attributes.hasDedicatedPort; + const auto dedicated_port = static_cast(reader->m_attributes.unicastLocator.port); reader->reset(); + if (had_dedicated_port) { + releaseDedicatedEndpointPort(dedicated_port); + } return true; } @@ -622,7 +919,23 @@ bool rtps::Domain::deleteWriter(Participant &part, Writer *writer) { return false; } + // Cancel any PARKED guaranteed progress() job for this writer so the retry + // timer cannot resurrect it after deletion. A job already handed to the pool + // is made safe by reset() below (progress() no-ops once !initialized). + if (m_transport != nullptr) { + m_transport->cancelGuaranteed(writer); + } + + // Capture the dedicated port BEFORE reset() wipes the attributes, but release + // it AFTER reset(): reset() quiesces any in-flight progress() (it takes the + // writer's m_mutex and clears m_is_initialized_), so once it returns no job + // can still send on this port. + const bool had_dedicated_port = writer->m_attributes.hasDedicatedPort; + const auto dedicated_port = static_cast(writer->m_attributes.unicastLocator.port); writer->reset(); + if (had_dedicated_port) { + releaseDedicatedEndpointPort(dedicated_port); + } return true; } diff --git a/components/rtps/src/entities/Participant.cpp b/components/rtps/src/entities/Participant.cpp index 2317df8239..6c5ce7ce08 100644 --- a/components/rtps/src/entities/Participant.cpp +++ b/components/rtps/src/entities/Participant.cpp @@ -101,17 +101,30 @@ bool Participant::registerOnNewSubscriberMatchedCallback(void (*callback)(void * } rtps::Writer *Participant::addWriter(Writer *pWriter) { - std::lock_guard lock(m_mutex); - for (unsigned int i = 0; i < m_writers.size(); i++) { - if (m_writers[i] == nullptr) { - m_writers[i] = pWriter; - if (m_hasBuilInEndpoints) { - m_sedpAgent.addWriter(*pWriter); + // Reserve the slot under m_mutex, but announce via the SEDP agent OUTSIDE + // it: the agent locks SEDPAgent::m_mutex and then this mutex (its receive + // handlers and tryMatchUnmatchedEndpoints() call back into this + // participant), so nesting the agent call under m_mutex is a lock-order + // inversion that deadlocks under concurrent discovery traffic. The global + // order is SEDPAgent::m_mutex -> Participant::m_mutex, never the reverse. + bool inserted = false; + { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == nullptr) { + m_writers[i] = pWriter; + inserted = true; + break; } - return pWriter; } } - return nullptr; + if (!inserted) { + return nullptr; + } + if (m_hasBuilInEndpoints) { + m_sedpAgent.addWriter(*pWriter); + } + return pWriter; } bool Participant::isWritersFull() { @@ -126,43 +139,96 @@ bool Participant::isWritersFull() { } rtps::Reader *Participant::addReader(Reader *pReader) { - std::lock_guard lock(m_mutex); - for (unsigned int i = 0; i < m_readers.size(); i++) { - if (m_readers[i] == nullptr) { - m_readers[i] = pReader; - if (m_hasBuilInEndpoints) { - m_sedpAgent.addReader(*pReader); + // Slot under m_mutex, SEDP announcement outside it - see addWriter() for + // the lock-order rationale (SEDPAgent::m_mutex must never be acquired while + // holding m_mutex). + bool inserted = false; + { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == nullptr) { + m_readers[i] = pReader; + inserted = true; + break; } - return pReader; } } - - return nullptr; + if (!inserted) { + return nullptr; + } + if (m_hasBuilInEndpoints) { + m_sedpAgent.addReader(*pReader); + } + return pReader; } bool Participant::deleteReader(Reader *reader) { + // Membership check under m_mutex first (pointer identity - endpoints are + // pooled objects owned by the Domain - which also guards the empty (nullptr) + // slots the previous sequence-number comparison dereferenced). + bool found = false; + { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_readers.size(); i++) { + if (m_readers[i] == reader) { + found = true; + break; + } + } + } + if (!found || reader == nullptr) { + return false; + } + // Make the SEDP disposal and the slot removal ATOMIC by holding the agent's + // mutex across both, in the global lock order (SEDPAgent::m_mutex -> + // Participant::m_mutex, see addWriter()). Without this, a concurrent SEDP + // receive handler could match a newly announced remote writer to this reader + // in the window between its disposal and the slot-clear - consuming the + // remote from the unmatched registry just before the reader vanishes, losing + // that match for any replacement reader. Both mutexes are recursive, so the + // agent's own lock in deleteReader() nests harmlessly. + std::lock_guard sedp_lock(m_sedpAgent.getMutex()); + if (!m_sedpAgent.deleteReader(reader)) { + PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + return false; + } std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_readers.size(); i++) { - if (m_readers[i]->getSEDPSequenceNumber() == reader->getSEDPSequenceNumber()) { - if (m_sedpAgent.deleteReader(reader)) { - m_readers[i] = nullptr; - return true; - } - PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + if (m_readers[i] == reader) { + m_readers[i] = nullptr; + return true; } } return false; } bool Participant::deleteWriter(Writer *writer) { + // Same structure and lock-order rationale as deleteReader(). + bool found = false; + { + std::lock_guard lock(m_mutex); + for (unsigned int i = 0; i < m_writers.size(); i++) { + if (m_writers[i] == writer) { + found = true; + break; + } + } + } + if (!found || writer == nullptr) { + return false; + } + // Atomic disposal + slot removal under the agent's mutex, in the global lock + // order (SEDPAgent::m_mutex -> Participant::m_mutex) - see deleteReader(). + std::lock_guard sedp_lock(m_sedpAgent.getMutex()); + if (!m_sedpAgent.deleteWriter(writer)) { + PARTICIPANT_LOG("Found writer but SEDP deletion failed"); + return false; + } std::lock_guard lock(m_mutex); for (unsigned int i = 0; i < m_writers.size(); i++) { - if (m_writers[i]->getSEDPSequenceNumber() == writer->getSEDPSequenceNumber()) { - if (m_sedpAgent.deleteWriter(writer)) { - m_writers[i] = nullptr; - return true; - } - PARTICIPANT_LOG("Found reader but SEDP deletion failed"); + if (m_writers[i] == writer) { + m_writers[i] = nullptr; + return true; } } return false; @@ -218,6 +284,39 @@ rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid) { return nullptr; } +// Generation-capturing lookup variants for the receive path. The generation is +// read while m_mutex is held, i.e. atomically with the slot still being +// registered: deleteReader()/deleteWriter() clear the slot under this mutex +// BEFORE reset() bumps the generation, so a pointer returned here always comes +// with the pre-deletion generation and a stale dispatch is rejected by the +// endpoint's *IfCurrent check. +rtps::Writer *Participant::getWriter(EntityId_t id, uint32_t &generation_out) { + std::lock_guard lock(m_mutex); + Writer *writer = getWriter(id); + if (writer != nullptr) { + generation_out = writer->generation(); + } + return writer; +} + +rtps::Reader *Participant::getReader(EntityId_t id, uint32_t &generation_out) { + std::lock_guard lock(m_mutex); + Reader *reader = getReader(id); + if (reader != nullptr) { + generation_out = reader->generation(); + } + return reader; +} + +rtps::Reader *Participant::getReaderByWriterId(const Guid_t &guid, uint32_t &generation_out) { + std::lock_guard lock(m_mutex); + Reader *reader = getReaderByWriterId(guid); + if (reader != nullptr) { + generation_out = reader->generation(); + } + return reader; +} + rtps::Writer *Participant::getMatchingWriter(const TopicData &readerTopicData) { std::lock_guard lock(m_mutex); for (size_t i = 0; i < m_writers.size(); ++i) { @@ -284,6 +383,14 @@ bool Participant::addNewRemoteParticipant(const ParticipantProxyData &remotePart } bool Participant::removeRemoteParticipant(const GuidPrefix_t &prefix) { + // Documented global lock order: SEDPAgent::m_mutex BEFORE + // Participant::m_mutex. The removal below calls into the SEDP agent + // (removeUnmatchedEntitiesOfParticipant takes its mutex), so acquire the + // agent mutex first - taking m_mutex alone here and letting the nested call + // grab the agent mutex would be an ABBA inversion against the SEDP receive + // handlers, which hold the agent mutex and call findRemoteParticipant() + // (m_mutex). Callers must not already hold m_mutex without the agent mutex. + std::lock_guard sedp_lock(m_sedpAgent.getMutex()); std::lock_guard lock(m_mutex); auto isElementToRemove = [&](const ParticipantProxyData &proxy) { return proxy.m_guid.prefix == prefix; @@ -385,30 +492,45 @@ uint32_t Participant::getRemoteParticipantCount() { rtps::MessageReceiver *Participant::getMessageReceiver() { return &m_receiver; } bool Participant::checkAndResetHeartbeats() { - std::lock_guard lock1(m_mutex); - std::lock_guard lock2(m_spdpAgent.m_mutex); - PARTICIPANT_LOG("Have {} remote participants", - (unsigned int)m_remoteParticipants.getNumElements()); - PARTICIPANT_LOG("Unmatched remote writers/readers, {} / {}", - static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters()), - static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); - for (auto &remote : m_remoteParticipants) { - PARTICIPANT_LOG("Remote GUID = {} {} {} {} | Age = {} [ms]", remote.m_guid.prefix.id[4], - remote.m_guid.prefix.id[5], remote.m_guid.prefix.id[6], - remote.m_guid.prefix.id[7], - (unsigned int)remote.getAliveSignalAgeInMilliseconds()); - if (remote.isAlive()) { - continue; - } - PARTICIPANT_LOG("removing remote participant"); - bool success = removeRemoteParticipant(remote.m_guid.prefix); - if (!success) { - return false; - } else { - return true; + // Phase 1 - SCAN ONLY. Lock order: SPDP-agent mutex BEFORE the participant + // mutex, matching the SPDP receive path (handleSPDPPackage holds the agent + // mutex and then calls findRemoteParticipant, which takes m_mutex). The + // expired participant is only SELECTED here; the removal itself must run + // with these locks RELEASED, because removeRemoteParticipant() acquires the + // SEDP-agent mutex before m_mutex (the documented global order) - removing + // while m_mutex is held would be an ABBA inversion against the SEDP receive + // handlers, which hold the SEDP mutex and call findRemoteParticipant(). + GuidPrefix_t expiredPrefix{}; + bool haveExpired = false; + { + std::lock_guard lock1(m_spdpAgent.m_mutex); + std::lock_guard lock2(m_mutex); + PARTICIPANT_LOG("Have {} remote participants", + (unsigned int)m_remoteParticipants.getNumElements()); + PARTICIPANT_LOG("Unmatched remote writers/readers, {} / {}", + static_cast(m_sedpAgent.getNumRemoteUnmatchedWriters()), + static_cast(m_sedpAgent.getNumRemoteUnmatchedReaders())); + for (auto &remote : m_remoteParticipants) { + PARTICIPANT_LOG("Remote GUID = {} {} {} {} | Age = {} [ms]", remote.m_guid.prefix.id[4], + remote.m_guid.prefix.id[5], remote.m_guid.prefix.id[6], + remote.m_guid.prefix.id[7], + (unsigned int)remote.getAliveSignalAgeInMilliseconds()); + if (remote.isAlive()) { + continue; + } + PARTICIPANT_LOG("removing remote participant"); + expiredPrefix = remote.m_guid.prefix; + haveExpired = true; + break; } } - return true; + if (!haveExpired) { + return true; + } + // Phase 2 - remove with no locks held (a liveness refresh racing this + // window loses by design: the lease already expired, and SPDP rediscovery + // re-adds the participant). + return removeRemoteParticipant(expiredPrefix); } void Participant::printInfo() { @@ -497,8 +619,17 @@ void Participant::printInfo() { rtps::SPDPAgent &Participant::getSPDPAgent() { return m_spdpAgent; } void Participant::addBuiltInEndpoints(BuiltInEndpoints &endpoints) { - std::lock_guard lock(m_mutex); - m_hasBuilInEndpoints = true; + // Only the flag needs m_mutex. The agent init()s register reader callbacks + // (Reader::m_callback_mutex) and the add*() calls take the SEDP/participant + // locks themselves; running them under m_mutex would establish a + // participant -> callback-mutex (and participant -> SEDP) order that + // inverts the discovery receive path (callback/SEDP mutex -> participant) + // and could deadlock init against a concurrently arriving SPDP/SEDP + // datagram. + { + std::lock_guard lock(m_mutex); + m_hasBuilInEndpoints = true; + } m_spdpAgent.init(*this, endpoints); m_sedpAgent.init(*this, endpoints); diff --git a/components/rtps/src/entities/Reader.cpp b/components/rtps/src/entities/Reader.cpp index 5f3858b383..99632b6f01 100644 --- a/components/rtps/src/entities/Reader.cpp +++ b/components/rtps/src/entities/Reader.cpp @@ -1,10 +1,12 @@ #include +#include #include #include #include #include #include #include +#include using namespace rtps; @@ -14,10 +16,48 @@ Reader::Reader() } void Reader::executeCallbacks(const ReaderCacheChange &cacheChange) { - std::lock_guard lock(m_callback_mutex); - for (unsigned int i = 0; i < m_callbacks.size(); i++) { - if (m_callbacks[i].function != nullptr) { - m_callbacks[i].function(m_callbacks[i].arg, cacheChange); + // Snapshot the registrations under m_callback_mutex, then invoke UNLOCKED: + // holding the mutex across the callbacks put it on every user/discovery + // callback stack (the SPDP/SEDP handlers take the participant/SEDP mutexes, + // and user handlers may create endpoints, i.e. register callbacks on other + // readers), creating lock-order inversions against registerCallback(). + // + // Each entry is REVALIDATED against the live table (by its unique, + // never-reused identifier) immediately before its invocation: an earlier + // callback in this very loop may have removed a later one and freed its arg + // (removeCallback()'s drain rightly excludes the caller's own dispatch, so + // it returns while this loop is still running) - invoking the stale snapshot + // entry would be a use-after-free. Cross-thread removals are excluded by the + // drain itself (they wait for this dispatch), so the unlocked window between + // revalidation and invocation cannot see a concurrent removal complete. + // Full teardown is prevented at the LIFECYCLE level: every engine invocation + // path runs inside a generation-guarded dispatch (see the *IfCurrent + // wrappers), and Reader::reset() retires the generation and drains in-flight + // dispatches before a slot's callbacks are cleared or its owner torn down. + decltype(m_callbacks) snapshot; + { + std::lock_guard lock(m_callback_mutex); + snapshot = m_callbacks; + } + for (unsigned int i = 0; i < snapshot.size(); i++) { + if (snapshot[i].function == nullptr) { + continue; + } + callbackFunction_t fn = nullptr; + void *arg = nullptr; + { + std::lock_guard lock(m_callback_mutex); + for (unsigned int j = 0; j < m_callbacks.size(); j++) { + if (m_callbacks[j].identifier == snapshot[i].identifier && + m_callbacks[j].function != nullptr) { + fn = m_callbacks[j].function; + arg = m_callbacks[j].arg; + break; + } + } + } + if (fn != nullptr) { + fn(arg, cacheChange); } } } @@ -118,6 +158,18 @@ void Reader::newFragment(const Guid_t &writerGuid, const SequenceNumber_t &sn, #endif void Reader::reset() { + // Retire this pooled slot's generation FIRST: any receive dispatch that + // captured the old generation at lookup but has not yet passed its guarded + // check will now no-op (see the *IfCurrent wrappers). Then wait - WITHOUT + // holding the reader mutexes, which an in-flight dispatch needs to finish - + // for dispatches that passed their check before the bump: they run against + // the still-intact endpoint and must complete before its state is torn down + // (and before the slot can be reused for another endpoint). The drain is + // self-aware: removal initiated from within this reader's own callback does + // not wait on itself (see drainDispatchesForTeardown()). + ++m_generation_; + drainDispatchesForTeardown(); + std::lock_guard lock1(m_proxies_mutex); std::lock_guard lock2(m_callback_mutex); @@ -131,7 +183,90 @@ void Reader::reset() { m_is_initialized_ = false; } +namespace { +// The reader whose guarded dispatch is running on THIS thread (nullptr when +// none). Lets the teardown drains below recognize their OWN dispatch: a +// callback may legally initiate removal of its own reader (previously +// reentrant via the recursive callback mutex), and unconditionally waiting for +// the dispatch count to reach zero would then deadlock on the caller's own +// count. Engine dispatches never nest across readers on one thread (delivery +// is serialized per reader and callbacks do not synchronously drive another +// reader's dispatch), so a single pointer suffices. +thread_local const void *t_dispatching_reader = nullptr; + +// Counts a guarded receive dispatch in/out of the reader (RAII so an early +// return cannot leak the count and wedge the teardown drains). +struct DispatchGuard { + explicit DispatchGuard(std::atomic &count, const void *reader) + : count_(count) + , prev_(t_dispatching_reader) { + count_.fetch_add(1); + t_dispatching_reader = reader; + } + ~DispatchGuard() { + t_dispatching_reader = prev_; + count_.fetch_sub(1); + } + std::atomic &count_; + const void *prev_; +}; +} // namespace + +void Reader::drainDispatchesForTeardown() { + // Wait for in-flight guarded dispatches on OTHER threads. If this thread is + // itself inside this reader's dispatch (removal initiated from the callback), + // its own count is excluded - the callback cannot return until the removal + // does, so waiting for it would deadlock; it runs against still-intact state + // and unwinds through the snapshot-invoking executeCallbacks() safely. + const int self = (t_dispatching_reader == this) ? 1 : 0; + while (m_active_dispatches_.load() > self) { + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } +} + +void Reader::newChangeIfCurrent(uint32_t generation, const ReaderCacheChange &cacheChange) { + DispatchGuard guard(m_active_dispatches_, this); + if (generation != m_generation_.load()) { + return; // slot deleted (and possibly reused) since the lookup + } + newChange(cacheChange); +} + +bool Reader::onNewHeartbeatIfCurrent(uint32_t generation, const SubmessageHeartbeat &msg, + const GuidPrefix_t &remotePrefix) { + DispatchGuard guard(m_active_dispatches_, this); + if (generation != m_generation_.load()) { + return false; + } + return onNewHeartbeat(msg, remotePrefix); +} + +bool Reader::onNewGapIfCurrent(uint32_t generation, const SubmessageGap &msg, + const GuidPrefix_t &remotePrefix) { + DispatchGuard guard(m_active_dispatches_, this); + if (generation != m_generation_.load()) { + return false; + } + return onNewGapMessage(msg, remotePrefix); +} + +#ifdef RTPS_ENABLE_FRAGMENTATION +void Reader::newFragmentIfCurrent(uint32_t generation, const Guid_t &writerGuid, + const SequenceNumber_t &sn, uint32_t fragmentStartingNum, + uint16_t fragmentsInSubmessage, uint16_t fragmentSize, + uint32_t sampleSize, const uint8_t *fragData, + DataSize_t fragDataLen) { + DispatchGuard guard(m_active_dispatches_, this); + if (generation != m_generation_.load()) { + return; + } + newFragment(writerGuid, sn, fragmentStartingNum, fragmentsInSubmessage, fragmentSize, sampleSize, + fragData, fragDataLen); +} +#endif + bool Reader::isProxy(const Guid_t &guid) { + std::lock_guard lock(m_proxies_mutex); for (const auto &proxy : m_proxies) { if (proxy.remoteWriterGuid.operator==(guid)) { return true; @@ -141,6 +276,7 @@ bool Reader::isProxy(const Guid_t &guid) { } WriterProxy *Reader::getProxy(Guid_t guid) { + std::lock_guard lock(m_proxies_mutex); auto isElementToFind = [&](const WriterProxy &proxy) { return proxy.remoteWriterGuid == guid; }; auto thunk = [](void *arg, const WriterProxy &value) { return (*static_cast(arg))(value); @@ -170,17 +306,34 @@ Reader::callbackIdentifier_t Reader::registerCallback(Reader::callbackFunction_t uint32_t Reader::getProxiesCount() { return m_proxies.getNumElements(); } bool Reader::removeCallback(Reader::callbackIdentifier_t identifier) { - std::lock_guard lock(m_callback_mutex); - for (unsigned int i = 0; i < m_callbacks.size(); i++) { - if (m_callbacks[i].identifier == identifier) { - m_callbacks[i].function = nullptr; - m_callbacks[i].arg = nullptr; - m_callback_count--; - return true; + bool removed = false; + { + std::lock_guard lock(m_callback_mutex); + for (unsigned int i = 0; i < m_callbacks.size(); i++) { + if (m_callbacks[i].identifier == identifier) { + m_callbacks[i].function = nullptr; + m_callbacks[i].arg = nullptr; + m_callback_count--; + removed = true; + break; + } } } - - return false; + if (removed) { + // Removal-completion guarantee: executeCallbacks() snapshots the + // registration array and invokes UNLOCKED, so a snapshot taken before the + // clear above can still hold this registration. Every such invocation runs + // inside a guarded dispatch (m_active_dispatches_), so draining it here + // guarantees that when removeCallback() returns, no OTHER thread is (or + // will be) running a callback taken from a pre-removal snapshot - the + // caller may then free the registration's arg. Dispatches that snapshot + // after the clear no longer contain it. The drain is self-aware: a + // callback removing ITSELF (or its reader) does not wait on its own + // dispatch - previously reentrant via the recursive callback mutex, and + // by definition that callback's invocation is already past. + drainDispatchesForTeardown(); + } + return removed; } uint8_t Reader::getNumCallbacks() { return m_callback_count; } diff --git a/components/rtps/src/entities/StatefulReader.cpp b/components/rtps/src/entities/StatefulReader.cpp index f88e18a73a..2f5824a025 100644 --- a/components/rtps/src/entities/StatefulReader.cpp +++ b/components/rtps/src/entities/StatefulReader.cpp @@ -24,11 +24,13 @@ Author: i11 - Embedded Software, RWTH Aachen University */ #include "rtps/entities/StatefulReader.hpp" + #include "rtps/communication/EsppTransport.hpp" #include "rtps/messages/MessageFactory.hpp" #include "rtps/storages/PayloadBuffer.hpp" #include "rtps/utils/Diagnostics.hpp" #include "rtps/utils/Log.hpp" +#include #include #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE @@ -71,18 +73,31 @@ void StatefulReader::newChange(const ReaderCacheChange &cacheChange) { if (m_callback_count == 0 || !m_is_initialized_) { return; } - std::lock_guard lock(m_proxies_mutex); - for (auto &proxy : m_proxies) { - if (proxy.remoteWriterGuid == cacheChange.writerGuid) { + // Serialize the whole delivery (claim + callbacks) on m_delivery_mutex, and + // hold m_proxies_mutex only for the expectedSN claim - NOT across the user + // callbacks. Invoking user code under the proxies mutex created a lock-order + // cycle: a callback calling back into the facade (e.g. add_writer/publish) + // reaches the facade/SEDP/participant locks, while the SEDP receive path + // takes those locks and then this reader's proxies mutex + // (addNewMatchedWriter). The delivery mutex is a leaf acquired first, so the + // strict in-order, one-callback-at-a-time semantics are preserved while the + // proxies mutex stays off every user-callback stack. + std::lock_guard delivery(m_delivery_mutex); + bool deliver = false; + { + std::lock_guard lock(m_proxies_mutex); + auto matches_writer = [&](const WriterProxy &proxy) { + return proxy.remoteWriterGuid == cacheChange.writerGuid; + }; + auto it = std::find_if(m_proxies.begin(), m_proxies.end(), matches_writer); + if (it != m_proxies.end()) { + WriterProxy &proxy = *it; if (proxy.expectedSN == cacheChange.sn) { - SFR_LOG("Delivering SN {}.{} | GUID {} {} {} {}", (int)cacheChange.sn.high, - (int)cacheChange.sn.low, cacheChange.writerGuid.prefix.id[0], - cacheChange.writerGuid.prefix.id[1], cacheChange.writerGuid.prefix.id[2], - cacheChange.writerGuid.prefix.id[3]); - executeCallbacks(cacheChange); + // Claim the SN under the proxies mutex; the callbacks run below, + // still serialized by m_delivery_mutex so delivery order matches + // claim order. ++proxy.expectedSN; - SFR_LOG("Done processing SN {}.{}", (int)cacheChange.sn.high, (int)cacheChange.sn.low); - return; + deliver = true; } else { Diagnostics::StatefulReader::sfr_unexpected_sn++; SFR_LOG("Unexpected SN {}.{} != {}.{}, dropping! GUID {} {} {} {}", @@ -93,12 +108,24 @@ void StatefulReader::newChange(const ReaderCacheChange &cacheChange) { } } } + if (deliver) { + SFR_LOG("Delivering SN {}.{} | GUID {} {} {} {}", (int)cacheChange.sn.high, + (int)cacheChange.sn.low, cacheChange.writerGuid.prefix.id[0], + cacheChange.writerGuid.prefix.id[1], cacheChange.writerGuid.prefix.id[2], + cacheChange.writerGuid.prefix.id[3]); + executeCallbacks(cacheChange); + SFR_LOG("Done processing SN {}.{}", (int)cacheChange.sn.high, (int)cacheChange.sn.low); + } } bool StatefulReader::addNewMatchedWriter(const WriterProxy &newProxy) { #if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE SFR_LOG("New writer added"); #endif + // Guard the pool mutation: newChange()/onNewHeartbeat() iterate m_proxies + // under m_proxies_mutex on the receive workers, and an unlocked add races + // them (endpoint (re)announcements arrive on a different worker). + std::lock_guard lock(m_proxies_mutex); return m_proxies.add(newProxy); } diff --git a/components/rtps/src/entities/StatefulWriter.cpp b/components/rtps/src/entities/StatefulWriter.cpp index a899960ad6..3bff1a6931 100644 --- a/components/rtps/src/entities/StatefulWriter.cpp +++ b/components/rtps/src/entities/StatefulWriter.cpp @@ -28,6 +28,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/messages/MessageFactory.hpp" #include "rtps/messages/MessageTypes.hpp" #include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/Diagnostics.hpp" #include "rtps/utils/Log.hpp" #include #include @@ -56,6 +57,13 @@ StatefulWriter::~StatefulWriter() = default; bool StatefulWriter::init(TopicData attributes, TopicKind_t topicKind, EsppTransport &driver, bool enfUnicast) { + // Take m_mutex across the FULL (re)initialization: the protocol scheduler's + // heartbeatTick() and a stale generation-guarded progress() job read + // m_is_initialized_ / m_nextHeartbeat / m_proxies / m_history under this + // lock, and a pooled writer slot can be re-init()ed while either is running + // on another thread - unlocked writes here would be a data race exposing + // partially initialized state. + std::lock_guard lock(m_mutex); m_attributes = attributes; @@ -70,6 +78,11 @@ bool StatefulWriter::init(TopicData attributes, TopicKind_t topicKind, EsppTrans m_history.clear(); m_hbCount = {1}; + // Reused pooled slot: this is a NEW logical writer, so its public drop + // counter must not inherit the previous endpoint's total (the facade + // attributes publish()-time overflow warnings to it). + m_history_drops_ = 0; + // Thread already exists, do not create new one (reusing slot case) m_is_initialized_ = true; @@ -81,7 +94,14 @@ bool StatefulWriter::init(TopicData attributes, TopicKind_t topicKind, EsppTrans } void StatefulWriter::reset() { + // Clear the init flag under m_mutex so it synchronizes with progress() / + // newChange() (which read it under the same lock): an in-flight progress() + // completes before reset() proceeds, and any later job sees !initialized. + // Bump the generation so an already-accepted guaranteed job cannot run + // against this slot once it is reused for another endpoint. + std::lock_guard lock(m_mutex); m_is_initialized_ = false; + ++m_generation_; // TODO } @@ -119,13 +139,21 @@ StatefulWriter::newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t siz const SequenceNumber_t minAfter = m_history.getCurrentSeqNumMin(); if (minBefore < minAfter && m_nextSequenceNumberToSend < minAfter) { m_nextSequenceNumberToSend = minAfter; // Skip past the dropped change + // Count the loss (an UNSENT change was overwritten): per-writer for the + // facade's publish()-time warning, process-wide for Diagnostics. + ++m_history_drops_; + ++Diagnostics::Writer::history_overwrite_drops; SFW_LOG("History full, dropped oldest {}.", this->m_attributes.topicName); } } if (m_transport != nullptr) { // Run the send asynchronously on the transport's worker pool (never inline // under the caller's locks), matching the previous ThreadPool semantics. - m_transport->submit([this]() { progress(); }); + // Guaranteed + banded: a bounded-queue rejection must not strand unsent + // samples (a lone best-effort DATA has no recovery path), and a + // prioritized endpoint's outbound work runs at ITS band end-to-end. + m_transport->submitGuaranteedDrain( + this, [this, gen = currentGeneration()]() { progressIfCurrent(gen); }, m_attributes.band); } // Piggyback: pull the next heartbeat evaluation forward so a reliable // publish is followed promptly by a HEARTBEAT instead of waiting out the @@ -143,7 +171,38 @@ StatefulWriter::newChange(ChangeKind_t kind, const uint8_t *data, DataSize_t siz void StatefulWriter::progress() { INIT_GUARD() std::lock_guard lock(m_mutex); + // A guaranteed progress() job may still be parked/queued when this writer is + // deleted. reset() clears m_is_initialized_ under m_mutex, so a job that runs + // after deletion no-ops here instead of sending the (now reset) history or + // touching an already-released dedicated port. Mirrors newChange()'s guard. + if (!m_is_initialized_) { + return; + } + // Skip any hole the cursor points at: a change can be dropped WITHOUT the + // cursor advancing past it - e.g. an unsent dispose-after-write removed by + // dropDisposeAfterWriteChanges() when its endpoint was deleted before the + // send cursor reached it (rapid endpoint churn). The SN is gone from + // history, so it can never be sent; without advancing, the cursor would be + // stuck and every LATER change (for the SEDP writer: every subsequent + // endpoint announcement of this participant) never transmitted. Jump to the + // history minimum when the cursor fell behind it, and step past mid-history + // holes until a live change (or the end) is reached - in THIS poke, so one + // poke cannot be swallowed by a run of consecutive holes. Readers are told + // of the skip by the normal GAP/heartbeat machinery (same recovery as the + // history-full drop in newChange()). CacheChange *next = m_history.getChangeBySN(m_nextSequenceNumberToSend); + if (next == nullptr && !m_history.isEmpty()) { + const SequenceNumber_t minSN = m_history.getCurrentSeqNumMin(); + if (m_nextSequenceNumberToSend < minSN) { + SFW_LOG("Cursor fell behind history; jumping to SN ({},{})", minSN.high, minSN.low); + m_nextSequenceNumberToSend = minSN; + next = m_history.getChangeBySN(m_nextSequenceNumberToSend); + } + while (next == nullptr && m_nextSequenceNumberToSend < m_history.getCurrentSeqNumMax()) { + ++m_nextSequenceNumberToSend; // mid-history hole: step past it + next = m_history.getChangeBySN(m_nextSequenceNumberToSend); + } + } if (next != nullptr) { uint32_t i = 0; for (const auto &proxy : m_proxies) { @@ -182,6 +241,17 @@ void StatefulWriter::progress() { SFW_LOG("HB from progress"); sendHeartBeat(); + // Drain re-arm (see EsppTransport::submitGuaranteedDrain): pokes park + // with a pending-count of at most ONE, so this run must resubmit itself + // while unsent samples remain - each admitted run sends one sample and + // re-arms until the cursor catches up with the history, keeping the + // parked debt per writer bounded at one (m_mutex is held, so the + // cursor/history read is stable). + if (!m_history.isEmpty() && m_nextSequenceNumberToSend <= m_history.getCurrentSeqNumMax() && + m_transport != nullptr) { + m_transport->submitGuaranteedDrain( + this, [this, gen = currentGeneration()]() { progressIfCurrent(gen); }, m_attributes.band); + } } else { SFW_LOG("Couldn't get a CacheChange with SN ({},{})", m_nextSequenceNumberToSend.high, m_nextSequenceNumberToSend.low); @@ -197,7 +267,11 @@ void StatefulWriter::setAllChangesToUnsent() { if (m_transport != nullptr) { // Run the send asynchronously on the transport's worker pool (never inline // under the caller's locks), matching the previous ThreadPool semantics. - m_transport->submit([this]() { progress(); }); + // Guaranteed + banded: a bounded-queue rejection must not strand unsent + // samples (a lone best-effort DATA has no recovery path), and a + // prioritized endpoint's outbound work runs at ITS band end-to-end. + m_transport->submitGuaranteedDrain( + this, [this, gen = currentGeneration()]() { progressIfCurrent(gen); }, m_attributes.band); } // Piggyback: pull the next heartbeat evaluation forward so a reliable // publish is followed promptly by a HEARTBEAT instead of waiting out the @@ -486,6 +560,11 @@ bool StatefulWriter::sendDataWRMulticast(const ReaderProxy &reader, const CacheC std::chrono::steady_clock::time_point StatefulWriter::heartbeatTick(std::chrono::steady_clock::time_point now) { + // Hold m_mutex across the WHOLE tick: sendHeartBeat() locks it, but the + // unconfirmed-changes scan below also iterates m_proxies, which the SEDP + // receive workers mutate under m_mutex - the scan must not run unlocked + // (m_mutex is recursive, so the nested guards stay harmless). + std::lock_guard lock(m_mutex); if (!m_is_initialized_) { // Not ticking: report a far-future deadline so the scheduler ignores us. return now + std::chrono::hours(24); @@ -548,6 +627,13 @@ void StatefulWriter::dropDisposeAfterWriteChanges() { void StatefulWriter::sendHeartBeat() { INIT_GUARD() + // Hold m_mutex across the WHOLE proxy iteration: matched-reader proxies are + // added/removed under m_mutex from the SEDP receive workers (endpoint + // (un)announcements), and iterating the pool unlocked from the protocol + // task races those mutations - observed as a SIGSEGV in this loop during + // endpoint churn. m_mutex is recursive, so the pre-existing inner history + // guard below stays harmless. + std::lock_guard proxies_lock(m_mutex); if (m_proxies.isEmpty() || !m_is_initialized_) { SFW_LOG("Skipping heartbeat. No proxies."); diff --git a/components/rtps/src/entities/StatelessReader.cpp b/components/rtps/src/entities/StatelessReader.cpp index c63799a9da..9d61977595 100644 --- a/components/rtps/src/entities/StatelessReader.cpp +++ b/components/rtps/src/entities/StatelessReader.cpp @@ -63,6 +63,9 @@ bool StatelessReader::addNewMatchedWriter(const WriterProxy &newProxy) { SLR_LOG("Adding WriterProxy"); printGuid(newProxy.remoteWriterGuid); #endif + // Guard the pool mutation against concurrent m_proxies iteration (see + // StatefulReader::addNewMatchedWriter). + std::lock_guard lock(m_proxies_mutex); return m_proxies.add(newProxy); } diff --git a/components/rtps/src/entities/StatelessWriter.cpp b/components/rtps/src/entities/StatelessWriter.cpp index 546434f0d3..11e9b85204 100644 --- a/components/rtps/src/entities/StatelessWriter.cpp +++ b/components/rtps/src/entities/StatelessWriter.cpp @@ -32,6 +32,7 @@ Author: i11 - Embedded Software, RWTH Aachen University #include "rtps/communication/PacketInfo.hpp" #include "rtps/messages/MessageFactory.hpp" #include "rtps/storages/PayloadBuffer.hpp" +#include "rtps/utils/Diagnostics.hpp" #include "rtps/utils/Log.hpp" #include "rtps/utils/udpUtils.hpp" #include @@ -59,6 +60,12 @@ StatelessWriter::~StatelessWriter() { bool StatelessWriter::init(TopicData attributes, TopicKind_t topicKind, EsppTransport &driver, bool enfUnicast) { + // Take m_mutex across the FULL (re)initialization: progress() (possibly a + // stale generation-guarded job for the slot's previous owner) reads + // m_is_initialized_ / m_proxies / m_history under this lock, and a pooled + // writer slot can be re-init()ed while such a job runs on another thread - + // unlocked writes here would be a data race exposing partial state. + std::lock_guard lock(m_mutex); m_attributes = attributes; @@ -67,6 +74,9 @@ bool StatelessWriter::init(TopicData attributes, TopicKind_t topicKind, EsppTran m_topicKind = topicKind; m_nextSequenceNumberToSend = {0, 1}; + // Reused pooled slot: fresh logical writer, fresh drop counter (the facade + // attributes publish()-time overflow warnings to it). + m_history_drops_ = 0; m_is_initialized_ = true; m_proxies.clear(); @@ -77,7 +87,15 @@ bool StatelessWriter::init(TopicData attributes, TopicKind_t topicKind, EsppTran return true; } -void StatelessWriter::reset() { m_is_initialized_ = false; } +void StatelessWriter::reset() { + // Clear the init flag under m_mutex so it synchronizes with progress(): an + // in-flight progress() completes before reset() proceeds, and any later job + // sees !initialized and no-ops. Bump the generation so an already-accepted + // guaranteed job cannot run against this slot once it is reused. + std::lock_guard lock(m_mutex); + m_is_initialized_ = false; + ++m_generation_; +} const CacheChange *StatelessWriter::newChange(rtps::ChangeKind_t kind, const uint8_t *data, DataSize_t size, bool inLineQoS, @@ -113,13 +131,21 @@ const CacheChange *StatelessWriter::newChange(rtps::ChangeKind_t kind, const uin const SequenceNumber_t minAfter = m_history.getSeqNumMin(); if (minBefore < minAfter && m_nextSequenceNumberToSend < minAfter) { m_nextSequenceNumberToSend = minAfter; // Skip past the dropped sample + // Count the loss (an UNSENT sample was overwritten): per-writer for the + // facade's publish()-time warning, process-wide for Diagnostics. + ++m_history_drops_; + ++Diagnostics::Writer::history_overwrite_drops; SLW_LOG("History full, dropped oldest {}", this->m_attributes.topicName); } } if (m_transport != nullptr) { // Run the send asynchronously on the transport's worker pool (never inline // under the caller's locks), matching the previous ThreadPool semantics. - m_transport->submit([this]() { progress(); }); + // Guaranteed + banded: a bounded-queue rejection must not strand unsent + // samples (a lone best-effort DATA has no recovery path), and a + // prioritized endpoint's outbound work runs at ITS band end-to-end. + m_transport->submitGuaranteedDrain( + this, [this, gen = currentGeneration()]() { progressIfCurrent(gen); }, m_attributes.band); } SLW_LOG("Adding new data."); @@ -140,7 +166,11 @@ void StatelessWriter::setAllChangesToUnsent() { if (m_transport != nullptr) { // Run the send asynchronously on the transport's worker pool (never inline // under the caller's locks), matching the previous ThreadPool semantics. - m_transport->submit([this]() { progress(); }); + // Guaranteed + banded: a bounded-queue rejection must not strand unsent + // samples (a lone best-effort DATA has no recovery path), and a + // prioritized endpoint's outbound work runs at ITS band end-to-end. + m_transport->submitGuaranteedDrain( + this, [this, gen = currentGeneration()]() { progressIfCurrent(gen); }, m_attributes.band); } } @@ -155,10 +185,38 @@ void StatelessWriter::progress() { // TODO smarter packaging e.g. by creating MessageStruct and serializing // after adjusting values. + // Hold m_mutex across the proxy iteration: proxies are added/removed under + // m_mutex from the SEDP receive workers, and iterating unlocked races those + // mutations (see StatefulWriter::sendHeartBeat). m_mutex is recursive, so + // the pre-existing inner history guard stays harmless. + std::lock_guard proxies_lock(m_mutex); + // A guaranteed progress() job may still be parked/queued when this writer is + // deleted; reset() clears m_is_initialized_ under m_mutex, so a late job + // no-ops here instead of sending on a reset/reused endpoint or a released + // dedicated port. + if (!m_is_initialized_) { + return; + } if (m_proxies.getNumElements() == 0) { SLW_LOG("No proxy!"); } + // Clamp a send cursor that fell behind the history: under KEEP_LAST + // overflow, newChange() overwrites the oldest UNSENT change and advances the + // cursor - but by the time this (queued) progress() runs, further publishes + // may have overwritten the cursor's change again. Without the clamp every + // such invocation returned early having sent NOTHING, so a saturated + // best-effort writer collapsed to ~zero delivery instead of degrading to + // drop-oldest (the counterpart of StatefulWriter::progress()'s hole-skip; + // the ring is contiguous, so resuming at the minimum is sufficient. m_mutex + // is held, so the cursor/history are stable across the clamp and sends). + { + const SequenceNumber_t minSN = m_history.getSeqNumMin(); + if (!(minSN == SEQUENCENUMBER_UNKNOWN) && m_nextSequenceNumberToSend < minSN) { + m_nextSequenceNumberToSend = minSN; // resume at the oldest live sample + } + } + for (const auto &proxy : m_proxies) { SLW_LOG("Progress."); @@ -244,6 +302,19 @@ void StatelessWriter::progress() { m_history.removeUntilIncl(m_nextSequenceNumberToSend); ++m_nextSequenceNumberToSend; + + // Drain re-arm (see EsppTransport::submitGuaranteedDrain): pokes park with a + // pending-count of at most ONE, so this run must resubmit itself while + // unsent samples remain - each admitted run sends one sample and re-arms + // until the retained history is empty, keeping the parked debt per writer + // bounded at one regardless of how many samples a KEEP_LAST overflow storm + // published (m_mutex is held, so the cursor/history read is stable). + const SequenceNumber_t maxSN = m_history.getSeqNumMax(); + if (!(maxSN == SEQUENCENUMBER_UNKNOWN) && m_nextSequenceNumberToSend <= maxSN && + m_transport != nullptr) { + m_transport->submitGuaranteedDrain( + this, [this, gen = currentGeneration()]() { progressIfCurrent(gen); }, m_attributes.band); + } } #ifdef RTPS_ENABLE_FRAGMENTATION diff --git a/components/rtps/src/entities/Writer.cpp b/components/rtps/src/entities/Writer.cpp index 6d1e2ff379..3425f053e0 100644 --- a/components/rtps/src/entities/Writer.cpp +++ b/components/rtps/src/entities/Writer.cpp @@ -142,3 +142,33 @@ int rtps::Writer::dumpAllProxies(dumpProxyCallback target, void *arg) { } return dump_count; } + +uint32_t rtps::Writer::currentGeneration() { return m_generation_.load(); } + +void rtps::Writer::onNewAckNackIfCurrent(uint32_t generation, const SubmessageAckNack &msg, + const GuidPrefix_t &sourceGuidPrefix) { + // Check generation AND initialization atomically with the dispatch: reset() + // bumps m_generation_ under m_mutex, so a receive handler that captured the + // generation (inside the participant's locked lookup) just before this + // pooled slot was deleted either completes here against the still-intact + // endpoint (reset() waits on m_mutex) or no-ops after the bump - it can + // never mutate/retransmit the history of the NEXT endpoint in this slot. + std::lock_guard lock(m_mutex); + if (generation != m_generation_.load() || !m_is_initialized_) { + return; + } + onNewAckNack(msg, sourceGuidPrefix); +} + +void rtps::Writer::progressIfCurrent(uint32_t generation) { + // Check the generation AND initialization atomically with the send: reset() + // bumps m_generation_ / clears m_is_initialized_ under m_mutex, so a job that + // was accepted by the pool before this writer was deleted (and possibly reused + // for another endpoint) no-ops here instead of sending on the wrong endpoint. + // m_mutex is recursive, so the progress() override re-locking is harmless. + std::lock_guard lock(m_mutex); + if (generation != m_generation_ || !m_is_initialized_) { + return; + } + progress(); +} diff --git a/components/rtps/src/messages/MessageReceiver.cpp b/components/rtps/src/messages/MessageReceiver.cpp index f9619f0d18..d51da9ace6 100644 --- a/components/rtps/src/messages/MessageReceiver.cpp +++ b/components/rtps/src/messages/MessageReceiver.cpp @@ -220,18 +220,23 @@ bool MessageReceiver::processDataSubmessage(MessageProcessingInfo &msgInfo, RECV_LOG("Received data message size {}", static_cast(size)); + // Capture the reader's pooled-slot generation atomically with the lookup and + // dispatch through the guarded wrapper: the endpoint can be deleted (and its + // slot reused) between resolving the pointer and delivering, and a stale + // delivery must not reach the slot's NEXT endpoint (wrong topic/callback). Reader *reader; + uint32_t readerGen = 0; if (dataSubmsg.readerId == ENTITYID_UNKNOWN) { #if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE RECV_LOG("Received ENTITYID_UNKNOWN readerID, searching for writer ID = "); printGuid(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); #endif - reader = - mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); + reader = mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}, + readerGen); if (reader != nullptr) RECV_LOG("Found reader!"); } else { - reader = mp_part->getReader(dataSubmsg.readerId); + reader = mp_part->getReader(dataSubmsg.readerId, readerGen); #if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE auto reader_by_writer = mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, dataSubmsg.writerId}); @@ -247,7 +252,7 @@ bool MessageReceiver::processDataSubmessage(MessageProcessingInfo &msgInfo, ReaderCacheChange change{ChangeKind_t::ALIVE, writerGuid, dataSubmsg.writerSN, serializedData, size, hasRelatedSampleIdentity, relatedSampleIdentity}; - reader->newChange(change); + reader->newChangeIfCurrent(readerGen, change); } else { #if RECV_VERBOSE && RTPS_GLOBAL_VERBOSE RECV_LOG("Couldn't find a reader with id: "); @@ -318,16 +323,18 @@ bool MessageReceiver::processDataFragSubmessage(MessageProcessingInfo &msgInfo, const DataSize_t fragDataLen = static_cast(submessageEnd - serializedData); Reader *reader; + uint32_t readerGen = 0; if (frag.readerId == ENTITYID_UNKNOWN) { - reader = mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, frag.writerId}); + reader = mp_part->getReaderByWriterId(Guid_t{sourceState.sourceGuidPrefix, frag.writerId}, + readerGen); } else { - reader = mp_part->getReader(frag.readerId); + reader = mp_part->getReader(frag.readerId, readerGen); } if (reader != nullptr) { Guid_t writerGuid{sourceState.sourceGuidPrefix, frag.writerId}; - reader->newFragment(writerGuid, frag.writerSN, frag.fragmentStartingNum, - frag.fragmentsInSubmessage, frag.fragmentSize, frag.sampleSize, - serializedData, fragDataLen); + reader->newFragmentIfCurrent(readerGen, writerGuid, frag.writerSN, frag.fragmentStartingNum, + frag.fragmentsInSubmessage, frag.fragmentSize, frag.sampleSize, + serializedData, fragDataLen); } return true; } @@ -340,9 +347,10 @@ bool MessageReceiver::processHeartbeatSubmessage(MessageProcessingInfo &msgInfo, return false; } - Reader *reader = mp_part->getReader(submsgHB.readerId); + uint32_t readerGen = 0; + Reader *reader = mp_part->getReader(submsgHB.readerId, readerGen); if (reader != nullptr) { - reader->onNewHeartbeat(submsgHB, sourceState.sourceGuidPrefix); + reader->onNewHeartbeatIfCurrent(readerGen, submsgHB, sourceState.sourceGuidPrefix); mp_part->refreshRemoteParticipantLiveliness(sourceState.sourceGuidPrefix); return true; } else { @@ -357,9 +365,10 @@ bool MessageReceiver::processAckNackSubmessage(MessageProcessingInfo &msgInfo, return false; } - Writer *writer = mp_part->getWriter(submsgAckNack.writerId); + uint32_t writerGen = 0; + Writer *writer = mp_part->getWriter(submsgAckNack.writerId, writerGen); if (writer != nullptr) { - writer->onNewAckNack(submsgAckNack, sourceState.sourceGuidPrefix); + writer->onNewAckNackIfCurrent(writerGen, submsgAckNack, sourceState.sourceGuidPrefix); return true; } else { return false; @@ -373,9 +382,10 @@ bool MessageReceiver::processGapSubmessage(MessageProcessingInfo &msgInfo, return false; } - Reader *reader = mp_part->getReader(submsgGap.readerId); + uint32_t readerGen = 0; + Reader *reader = mp_part->getReader(submsgGap.readerId, readerGen); if (reader != nullptr) { - reader->onNewGapMessage(submsgGap, sourceState.sourceGuidPrefix); + reader->onNewGapIfCurrent(readerGen, submsgGap, sourceState.sourceGuidPrefix); return true; } else { return false; diff --git a/components/rtps/src/rtps_participant.cpp b/components/rtps/src/rtps_participant.cpp index 1439da857b..1ca4ea1148 100644 --- a/components/rtps/src/rtps_participant.cpp +++ b/components/rtps/src/rtps_participant.cpp @@ -151,7 +151,16 @@ bool RtpsParticipant::start() { return false; } - domain_ = std::make_unique(ip_bytes); + // Channel/endpoint scheduling: metatraffic (SPDP/SEDP) dispatches at + // metatraffic_band (High by default), user traffic at user_traffic_band; + // banded endpoints may get dedicated ports, rationed by the configured cap. + const rtps::DomainConfig domain_config{ + .metatraffic_band = config_.metatraffic_band, + .user_traffic_band = config_.user_traffic_band, + .enable_dedicated_endpoint_ports = config_.enable_dedicated_endpoint_ports, + .max_prioritized_endpoint_ports = config_.max_prioritized_endpoint_ports, + }; + domain_ = std::make_unique(ip_bytes, domain_config); // Fresh liveness token for this run (a prior stop() left the old one flipped). live_ = std::make_shared(); @@ -201,12 +210,54 @@ bool RtpsParticipant::add_writer(const WriterConfig &config) { logger_.error("Writer '{}': fragment_size must be non-zero", config.topic); return false; } + if (config.dscp.has_value() && static_cast(config.dscp.value()) > 63) { + logger_.error("Writer '{}': invalid DSCP {} (valid code points are 0..63)", config.topic, + static_cast(config.dscp.value())); + return false; + } rtps::Writer *writer = domain_->createWriter(*participant_, config.topic.c_str(), config.type_name.c_str(), - config.reliability == Reliability::RELIABLE); + config.reliability == Reliability::RELIABLE, /*enforceUnicast=*/false, + rtps::EndpointOptions{.band = config.band, .dscp = config.dscp}); if (writer == nullptr) { - logger_.error("Engine could not create writer '{}' (pool exhausted or name too long)", - config.topic); + // Name the exact failure: which limit bound, its configured size, and the + // knob that raises it - so hitting a pool ceiling is a one-line config fix + // instead of a debugging session (the builtin discovery endpoints consume + // slots from these same pools, which makes the usable count non-obvious). + if (config.topic.size() >= rtps::Config::MAX_TOPICNAME_LENGTH || + config.type_name.size() >= rtps::Config::MAX_TYPENAME_LENGTH) { + // >= : the engine stores names in fixed arrays with a terminating NUL, + // so a name of exactly MAX_*_LENGTH is rejected there too. + logger_.error("Engine could not create writer '{}': topic/type name too long " + "(MAX_TOPICNAME_LENGTH={}, MAX_TYPENAME_LENGTH={})", + config.topic, static_cast(rtps::Config::MAX_TOPICNAME_LENGTH), + static_cast(rtps::Config::MAX_TYPENAME_LENGTH)); + } else if (config.reliability == Reliability::RELIABLE) { + // Two limits can bind (whichever is hit first): the stateful pool and the + // per-participant writer cap; the builtin discovery writers (1 SPDP + 2 + // SEDP) consume slots from both, hence the "usable" numbers. + logger_.error( + "Engine could not create writer '{}': RELIABLE writer capacity reached - stateful pool " + "NUM_STATEFUL_WRITERS={} (2 reserved for SEDP -> {} usable) and/or per-participant cap " + "NUM_WRITERS_PER_PARTICIPANT={} (3 builtin writers -> {} usable). Select a larger limits " + "profile (CONFIG_RTPS_LIMITS_PROFILE_HOST or _HOST_LARGE; capacity-only, no wire " + "change).", + config.topic, static_cast(rtps::Config::NUM_STATEFUL_WRITERS), + static_cast(rtps::Config::NUM_STATEFUL_WRITERS) - 2, + static_cast(rtps::Config::NUM_WRITERS_PER_PARTICIPANT), + static_cast(rtps::Config::NUM_WRITERS_PER_PARTICIPANT) - 3); + } else { + logger_.error( + "Engine could not create writer '{}': BEST_EFFORT writer capacity reached - stateless " + "pool NUM_STATELESS_WRITERS={} (1 reserved for SPDP -> {} usable) and/or per-participant " + "cap NUM_WRITERS_PER_PARTICIPANT={} (3 builtin writers -> {} usable). Select a larger " + "limits profile (CONFIG_RTPS_LIMITS_PROFILE_HOST or _HOST_LARGE; capacity-only, no wire " + "change).", + config.topic, static_cast(rtps::Config::NUM_STATELESS_WRITERS), + static_cast(rtps::Config::NUM_STATELESS_WRITERS) - 1, + static_cast(rtps::Config::NUM_WRITERS_PER_PARTICIPANT), + static_cast(rtps::Config::NUM_WRITERS_PER_PARTICIPANT) - 3); + } return false; } // Per-writer fragment size (only used when a sample exceeds a single DATA @@ -225,17 +276,67 @@ bool RtpsParticipant::add_reader(const ReaderConfig &config) { logger_.error("Cannot add reader '{}': not started", config.topic); return false; } - rtps::Reader *reader = - domain_->createReader(*participant_, config.topic.c_str(), config.type_name.c_str(), - config.reliability == Reliability::RELIABLE); + if (config.dscp.has_value() && static_cast(config.dscp.value()) > 63) { + logger_.error("Reader '{}': invalid DSCP {} (valid code points are 0..63)", config.topic, + static_cast(config.dscp.value())); + return false; + } + rtps::Reader *reader = domain_->createReader( + *participant_, config.topic.c_str(), config.type_name.c_str(), + config.reliability == Reliability::RELIABLE, /*mcastaddress=*/{0, 0, 0, 0}, + rtps::EndpointOptions{.band = config.band, .dscp = config.dscp}); if (reader == nullptr) { - logger_.error("Engine could not create reader '{}' (pool exhausted or name too long)", - config.topic); + // Same actionable diagnostics as add_writer(): name the bound limit, its + // size, and the Kconfig knob (builtin discovery readers consume slots from + // these pools: 1 stateless for SPDP, 2 stateful for SEDP). + if (config.topic.size() >= rtps::Config::MAX_TOPICNAME_LENGTH || + config.type_name.size() >= rtps::Config::MAX_TYPENAME_LENGTH) { + // >= : matches the engine's fixed-array + NUL bound (see add_writer()). + logger_.error("Engine could not create reader '{}': topic/type name too long " + "(MAX_TOPICNAME_LENGTH={}, MAX_TYPENAME_LENGTH={})", + config.topic, static_cast(rtps::Config::MAX_TOPICNAME_LENGTH), + static_cast(rtps::Config::MAX_TYPENAME_LENGTH)); + } else if (config.reliability == Reliability::RELIABLE) { + logger_.error( + "Engine could not create reader '{}': RELIABLE reader capacity reached - stateful pool " + "NUM_STATEFUL_READERS={} (2 reserved for SEDP -> {} usable) and/or per-participant cap " + "NUM_READERS_PER_PARTICIPANT={} (3 builtin readers -> {} usable). Select a larger limits " + "profile (CONFIG_RTPS_LIMITS_PROFILE_HOST or _HOST_LARGE; capacity-only, no wire " + "change).", + config.topic, static_cast(rtps::Config::NUM_STATEFUL_READERS), + static_cast(rtps::Config::NUM_STATEFUL_READERS) - 2, + static_cast(rtps::Config::NUM_READERS_PER_PARTICIPANT), + static_cast(rtps::Config::NUM_READERS_PER_PARTICIPANT) - 3); + } else { + logger_.error( + "Engine could not create reader '{}': BEST_EFFORT reader capacity reached - stateless " + "pool NUM_STATELESS_READERS={} (1 reserved for SPDP -> {} usable) and/or per-participant " + "cap NUM_READERS_PER_PARTICIPANT={} (3 builtin readers -> {} usable). Select a larger " + "limits profile (CONFIG_RTPS_LIMITS_PROFILE_HOST or _HOST_LARGE; capacity-only, no wire " + "change).", + config.topic, static_cast(rtps::Config::NUM_STATELESS_READERS), + static_cast(rtps::Config::NUM_STATELESS_READERS) - 1, + static_cast(rtps::Config::NUM_READERS_PER_PARTICIPANT), + static_cast(rtps::Config::NUM_READERS_PER_PARTICIPANT) - 3); + } return false; } - auto ctx = std::make_unique(); + auto ctx = std::make_shared(); ctx->self = this; ctx->on_sample = config.on_sample; + ctx->topic = config.topic; + ctx->reader = reader; + // Banded reader without a dedicated port (ration exhausted or dedicated + // ports disabled): fall back to deferred banded dispatch of on_sample (see + // DeferredDispatch). Dedicated-port readers are already dispatched at their + // band by the reactor, so they keep the inline path. + if (config.band != espp::QosBand::Normal && !reader->m_attributes.hasDedicatedPort) { + ctx->deferred.enabled = true; + ctx->deferred.band = config.band; + ctx->deferred.transport = &domain_->getTransport(); + logger_.info("Reader '{}' uses deferred banded dispatch (band {}, no dedicated port)", + config.topic, static_cast(config.band)); + } if (config.on_sample) { if (reader->registerCallback(&reader_trampoline, ctx.get()) == 0) { logger_.error("Engine could not register the sample callback for '{}'", config.topic); @@ -249,6 +350,83 @@ bool RtpsParticipant::add_reader(const ReaderConfig &config) { return true; } +bool RtpsParticipant::remove_writer(const std::string &topic) { + std::lock_guard lock(mutex_); + // Reject once teardown has begun: after stop()'s phase-1.5 wait releases + // mutex_, phase 4 runs domain_->stop() WITHOUT it, so holding mutex_ here no + // longer serializes against the engine's own teardown - a deleteWriter() + // racing domain_->stop() must not start. (domain_ is only reset in phase 5 + // under mutex_, so this check is what makes the dereference safe.) The + // teardown reclaims every endpoint anyway, so a rejected removal during + // stop leaks nothing. + if (stopping_) { + return false; + } + auto it = writers_.find(topic); + if (it == writers_.end() || domain_ == nullptr || participant_ == nullptr) { + return false; + } + // Delete the engine endpoint FIRST (announces the SEDP disposal and + // releases any dedicated port); only then drop our handle. If deletion + // fails the writer is still active in the engine - erasing the map entry + // then would strand it with no handle to retry/publish through. + if (!domain_->deleteWriter(*participant_, it->second)) { + return false; + } + writers_.erase(it); + return true; +} + +bool RtpsParticipant::remove_reader(const std::string &topic) { + // Supported from within the reader's OWN on_sample for INLINE (Normal-band / + // non-deferred) readers: the engine's teardown drain excludes the caller's + // own dispatch and the delivery trampoline holds the context alive across + // the callback. NOT supported from within a DEFERRED reader's own callback: + // the dispatcher's close() below waits for the in-flight delivery - i.e. + // the caller - and would deadlock. + // + // Select the most-recent matching context and detach it from the list under + // the lock (composites roll back most recent first). Detaching up front both + // keeps a concurrent remover / stop() from processing it and lets us do the + // engine deletion + quiesce WITHOUT holding mutex_: close() waits for the + // in-flight delivery, and that user callback may itself call back into the + // participant (e.g. publish()) and take mutex_ - holding it here would + // deadlock. `target` keeps the context alive throughout. + // Pin the engine for the unlocked phase below: stop() waits for active + // engine ops before stopping/destroying the domain, so domain_/participant_ + // stay valid across the deletion + quiesce even if a stop() races in. + if (!begin_engine_op()) { + return false; + } + EngineOpGuard op_guard(*this); + std::shared_ptr target; + { + std::lock_guard lock(mutex_); + for (auto it = reader_contexts_.rbegin(); it != reader_contexts_.rend(); ++it) { + if ((*it)->topic == topic && (*it)->reader != nullptr) { + target = *it; + reader_contexts_.erase(std::next(it).base()); + break; + } + } + } + if (!target) { + return false; + } + // ENGINE deletion FIRST (clears the callback registration under the engine's + // locks). On failure the reader is still live, so re-attach the (intact) + // context for a retry rather than leaking it. + if (!domain_->deleteReader(*participant_, target->reader)) { + std::lock_guard lock(mutex_); + reader_contexts_.push_back(std::move(target)); + return false; + } + // Quiesce the dispatcher (waits for the in-flight delivery). The context is + // freed when `target` goes out of scope here - after close() has drained. + target->deferred.close(); + return true; +} + bool RtpsParticipant::publish(std::string_view topic, std::span cdr_payload) { std::lock_guard lock(mutex_); if (!started_) { @@ -283,29 +461,254 @@ bool RtpsParticipant::publish(std::string_view topic, std::span c return false; } #endif + // KEEP_LAST overflow visibility: on a full static history ring the engine + // OVERWRITES the oldest unsent sample and still accepts the new one, so a + // saturated publisher would otherwise report nothing but success while + // silently losing data. Detect the overwrite via the per-writer drop counter + // delta across this call and surface it (rate-limited; the process-wide + // total lives in rtps::Diagnostics::Writer::history_overwrite_drops). + const uint32_t drops_before = it->second->historyDrops(); const auto *change = it->second->newChange(rtps::ChangeKind_t::ALIVE, cdr_payload.data(), static_cast(cdr_payload.size())); if (change == nullptr) { logger_.warn("Writer history full for topic '{}'; sample dropped", topic); return false; } + const uint32_t drops_now = it->second->historyDrops(); + if (drops_now != drops_before) { + logger_.warn_rate_limited( + "History overflow on topic '{}': publish outran the send path, oldest UNSENT sample " + "overwritten (writer total: {}). This sample WAS queued (KEEP_LAST). Raise the history " + "depth (RTPS_CFG_HISTORY_SIZE_STATELESS / _STATEFUL or menuconfig 'Custom capacity " + "overrides'), enable RTPS_STORAGE_DYNAMIC, or pace the publisher.", + topic, drops_now); + } return true; } +namespace { +// DeferredDispatch has no logger of its own (it is a small POD-ish helper +// embedded in several contexts), so drops are reported through this one. +espp::Logger s_deferred_logger({.tag = "RtpsDeferred", .level = espp::Logger::Verbosity::WARN}); +} // namespace + +void RtpsParticipant::DeferredDispatch::run_or_defer(std::function delivery, + std::shared_ptr owner) { + if (!enabled || transport == nullptr) { + delivery(); + return; + } + bool do_arm = false; + { + std::lock_guard lock(mutex); + if (closed) { + return; // quiesced: the endpoint is being removed + } + if (queue.size() >= max_queued) { + ++dropped; + s_deferred_logger.warn( + "Deferred delivery queue full ({}); dropping sample (total dropped {})", max_queued, + dropped); + return; + } + queue.push_back(std::move(delivery)); + if (!in_flight) { + in_flight = true; + needs_arm = false; + do_arm = true; + } + } + if (do_arm) { + arm(std::move(owner)); + } +} + +void RtpsParticipant::DeferredDispatch::arm(std::shared_ptr owner) { + // in_flight is already true (set by the caller under the mutex). The drain + // job captures `owner` (the shared context embedding this dispatcher), so + // queued work can never outlive the context. + if (transport->submit([this, owner]() { drain(owner); }, band)) { + return; + } + // Pool full/stopped: a queued (possibly lone/last) delivery must never be + // stranded waiting for traffic that may not come - flag the failed arm and + // let the retry timer recover it. + std::lock_guard lock(mutex); + in_flight = false; + if (closed) { + return; + } + needs_arm = true; + ensure_retry_timer_locked(owner); +} + +void RtpsParticipant::DeferredDispatch::ensure_retry_timer_locked( + const std::shared_ptr &owner) { + if (retry_timer) { + return; // already running; it re-arms from needs_arm on its next tick + } + // The timer runs until close() cancels it synchronously - it NEVER + // self-cancels. A self-cancelling espp::Timer (callback returning true) + // stops its underlying task but leaves Timer::running_ set, so a later + // start() would CAS-fail ("already running") and no-op against a dead task, + // stranding the parked work. Keeping it alive (callback always returns + // false) sidesteps that entirely; the per-tick cost is a mutex + empty + // check, paid only after an actual pool rejection and only until close(). + // + // Weak owner capture: the context owns this dispatcher (and thus the timer), + // so a strong capture would be a cycle; the callback promotes it per tick. + std::weak_ptr weak_owner = owner; + retry_timer = std::make_unique(espp::Timer::Config{ + .name = "rtps_defer_arm", + .period = std::chrono::milliseconds(20), + .delay = std::chrono::milliseconds(20), + .callback = [this, weak_owner]() -> bool { + auto strong = weak_owner.lock(); + if (!strong) { + return false; // owner gone (close() cancels first); nothing to do + } + { + std::lock_guard lock(mutex); + if (closed || !needs_arm || in_flight) { + return false; // nothing to recover this tick; keep the timer alive + } + if (queue.empty()) { + needs_arm = false; + return false; + } + in_flight = true; + needs_arm = false; + } + // Reached only when a drain is owed and now in_flight: try to arm it. + if (!transport->submit([this, strong]() { drain(strong); }, band)) { + std::lock_guard lock(mutex); + in_flight = false; + if (!closed) { + needs_arm = true; // still saturated; the next tick retries + } + } + return false; // never self-cancel (see above) + }, + .auto_start = true, + .log_level = espp::Logger::Verbosity::WARN, + }); +} + +void RtpsParticipant::DeferredDispatch::close() { + { + std::lock_guard lock(mutex); + closed = true; + needs_arm = false; + queue.clear(); + } + // Cancel the retry timer synchronously and OUTSIDE the lock: cancel() joins + // the timer task, whose callback takes `mutex` - holding it here would + // deadlock. After cancel() returns no timer callback is running or will run. + if (retry_timer) { + retry_timer->cancel(); + } + // Wait for any in-flight delivery to finish. A delivery runs a user handler + // that may still be using endpoints the caller is about to delete (e.g. a + // service reply writer via ServiceResponder::reply()); close() must not + // return until it completes. The caller must NOT hold Participant::mutex_ + // here (the delivery may need it) - every remove_* path calls close() + // outside that lock. + std::unique_lock lock(mutex); + drain_done.wait(lock, [this]() { return !delivering; }); +} + +void RtpsParticipant::DeferredDispatch::drain(std::shared_ptr owner) { + // One delivery per job (mirrors the reactor's one-shot arming): pop the + // oldest, run it OUTSIDE the lock, then re-arm while more are pending. + std::function delivery; + { + std::lock_guard lock(mutex); + if (closed || queue.empty()) { + in_flight = false; + return; + } + delivery = std::move(queue.front()); + queue.pop_front(); + // Mark a delivery as executing so close() can wait for it before the + // caller deletes endpoints the delivery may still use (e.g. a service + // reply writer). Runs OUTSIDE the lock below; close() waits on drain_done. + delivering = true; + } + // Exception boundary, mirroring SocketReactor::dispatch(): this runs as a + // plain pool job, and a throwing user callback would otherwise kill the + // worker AND leave in_flight set, permanently wedging this dispatcher. +#if defined(__cpp_exceptions) && __cpp_exceptions + try { + delivery(); + } catch (const std::exception &e) { + s_deferred_logger.error("Exception in deferred delivery: {}", e.what()); + } catch (...) { + s_deferred_logger.error("Unknown exception in deferred delivery"); + } +#else + // C++ exceptions are disabled (e.g. the ESP-IDF default), so a throwing + // delivery would abort regardless; call it directly. + delivery(); +#endif + bool rearm = false; + { + std::lock_guard lock(mutex); + delivering = false; + drain_done.notify_all(); // wake close() if it is waiting for us + if (closed || queue.empty()) { + in_flight = false; + } else { + rearm = true; + } + } + if (rearm) { + // Same guaranteed-recovery path as the initial arm: a rejected re-arm + // flags needs_arm and the retry timer picks it up. + arm(std::move(owner)); + } +} + void RtpsParticipant::reader_trampoline(void *arg, const rtps::ReaderCacheChange &change) { auto *ctx = static_cast(arg); if (ctx == nullptr || !ctx->on_sample) { return; } + if (ctx->deferred.enabled) { + // Banded shared-port reader: copy the payload now (`change` is only valid + // during this callback) and deliver it from the pool at the reader's band. + // shared_ptr payload (rather than a moved vector) keeps the closure + // cheaply copyable inside std::function and avoids a GCC 15 + // -Wfree-nonheap-object false positive on moved-vector captures. + auto sample = std::make_shared>(change.getDataSize()); + if (sample->empty() || !change.copyInto(sample->data(), change.getDataSize())) { + return; + } + // Shared capture: the delivery (and the drain job) own the context, so a + // concurrent remove/rollback cannot free it under queued work; close() + // (run by every removal path) stops further deliveries. + auto self = ctx->shared_from_this(); + ctx->deferred.run_or_defer( + [self, sample]() { + self->on_sample(std::span(sample->data(), sample->size())); + }, + self); + return; + } // Serialize deliveries per reader: the engine may invoke this from a worker - // thread while a previous delivery is still running. - std::lock_guard lock(ctx->buffer_mutex); + // thread while a previous delivery is still running. Hold a shared reference + // for the delivery's duration: the callback may legally remove its OWN + // reader (remove_reader() from on_sample), which detaches and releases the + // registry's reference while this invocation is still on the stack - the + // engine's teardown drain deliberately excludes the caller's own dispatch, + // so without this hold the context would be destroyed under the callback. + auto self = ctx->shared_from_this(); + std::lock_guard lock(self->buffer_mutex); const auto size = change.getDataSize(); - ctx->buffer.resize(size); - if (size == 0 || !change.copyInto(ctx->buffer.data(), size)) { + self->buffer.resize(size); + if (size == 0 || !change.copyInto(self->buffer.data(), size)) { return; } - ctx->on_sample(std::span(ctx->buffer.data(), ctx->buffer.size())); + self->on_sample(std::span(self->buffer.data(), self->buffer.size())); } void RtpsParticipant::publisher_matched_trampoline(void *arg) { @@ -335,11 +738,20 @@ uint64_t seq_key(const rtps::SequenceNumber_t &sn) { } // namespace // Per-server bridge: engine request-reader callback -> user handler -> reply. -struct RtpsParticipant::ServiceServerContext { +struct RtpsParticipant::ServiceServerContext + : std::enable_shared_from_this { RtpsParticipant *self{nullptr}; service_deferred_handler_t handler{nullptr}; // sync handlers are wrapped as deferred rtps::Writer *reply_writer{nullptr}; rtps::Reader *request_reader{nullptr}; + DeferredDispatch deferred; // banded request reader without a dedicated port + // Endpoint-scoped liveness for RETAINED ServiceResponders: a responder may + // legally outlive the handler invocation, and the participant-wide token + // only covers stop() - an individual removal (e.g. an action rollback) + // deletes/reuses this reply writer while the participant stays alive. + // remove_service_server() flips this false (under its lock, which waits for + // an in-flight reply()) BEFORE the writer is deleted. + std::shared_ptr writer_live{std::make_shared()}; }; // Deferred-reply state: the reply writer + the identity to echo, so a response @@ -351,6 +763,11 @@ struct RtpsParticipant::ServiceResponder::State { // Held so a deferred reply that races participant shutdown no-ops instead of // writing through a freed engine writer (see RtpsParticipant::Liveness). std::shared_ptr live; + // Endpoint-scoped: invalidated by remove_service_server() BEFORE the reply + // writer is deleted, so a responder retained past an individual removal + // (e.g. an action rollback) no-ops instead of writing through a + // reset/reused writer slot while the participant is still alive. + std::shared_ptr endpoint_live; }; void RtpsParticipant::ServiceResponder::reply(std::span response) const { @@ -367,13 +784,23 @@ void RtpsParticipant::ServiceResponder::reply(std::span response) if (!state_->replied.compare_exchange_strong(expected, true)) { return; // reply exactly once } - // Hold the liveness lock across the write: stop() flips `alive` false under - // the same lock before destroying the domain, so we either complete the write - // against a still-valid writer or observe !alive and drop. + // Hold BOTH liveness locks across the write: stop() flips the participant + // token false and remove_service_server() flips the endpoint token false - + // each under its own lock, before the writer is destroyed/deleted - so we + // either complete the write against a still-valid writer or observe !alive + // and drop. Lock order participant -> endpoint (removal only ever takes the + // endpoint lock alone, so there is no reverse nesting). std::lock_guard live_lock(state_->live->m); if (!state_->live->alive) { return; } + std::unique_lock endpoint_lock; + if (state_->endpoint_live) { + endpoint_lock = std::unique_lock(state_->endpoint_live->m); + if (!state_->endpoint_live->alive) { + return; + } + } state_->reply_writer->newChangeWithRelatedSampleIdentity( rtps::ChangeKind_t::ALIVE, response.data(), static_cast(response.size()), state_->related); @@ -382,7 +809,8 @@ void RtpsParticipant::ServiceResponder::reply(std::span response) // Client state: request writer + pending-request table keyed by the request's // RTPS writerSeqNumber (which the server echoes in the reply's // related_sample_identity), matched on our own reply-reader GUID. -struct RtpsParticipant::ServiceClient::Impl { +struct RtpsParticipant::ServiceClient::Impl + : std::enable_shared_from_this { struct SyncSlot { std::mutex m; std::condition_variable cv; @@ -396,9 +824,11 @@ struct RtpsParticipant::ServiceClient::Impl { RtpsParticipant *self{nullptr}; rtps::Writer *request_writer{nullptr}; + rtps::Reader *reply_reader{nullptr}; ///< retained for composite (action) rollback rtps::Guid_t reply_reader_guid{}; std::mutex mutex; std::unordered_map pending; + DeferredDispatch deferred; // banded reply reader without a dedicated port // Send a request carrying our reply-reader GUID as related_sample_identity // (with an UNKNOWN sequence number, per rmw), register the pending entry keyed @@ -431,9 +861,11 @@ void RtpsParticipant::service_request_trampoline(void *arg, const rtps::ReaderCa if (ctx == nullptr || !ctx->handler || ctx->reply_writer == nullptr) { return; } - // Copy the request payload (valid only during this callback). - std::vector request(change.getDataSize()); - if (!request.empty() && !change.copyInto(request.data(), change.getDataSize())) { + // Copy the request payload (valid only during this callback). shared_ptr so + // the deferred closure stays cheaply copyable inside std::function (and to + // avoid a GCC 15 -Wfree-nonheap-object false positive on moved vectors). + auto request = std::make_shared>(change.getDataSize()); + if (!request->empty() && !change.copyInto(request->data(), change.getDataSize())) { return; } @@ -443,11 +875,21 @@ void RtpsParticipant::service_request_trampoline(void *arg, const rtps::ReaderCa auto state = std::make_shared(); state->reply_writer = ctx->reply_writer; state->live = ctx->self->live_; + state->endpoint_live = ctx->writer_live; state->related.writer_guid = change.hasRelatedSampleIdentity ? change.relatedSampleIdentity.writer_guid : change.writerGuid; state->related.sequence_number = change.sn; - ctx->handler(request, ServiceResponder(state)); + // Inline for the default path; banded shared-port servers run the handler + // from the pool at their band instead (see DeferredDispatch). Shared + // capture: queued work owns the context, so removal/rollback cannot free it + // underneath (close() stops further deliveries). + auto owner = ctx->shared_from_this(); + ctx->deferred.run_or_defer( + [owner, request, responder = ServiceResponder(state)]() { + owner->handler(std::span(request->data(), request->size()), responder); + }, + owner); } void RtpsParticipant::service_reply_trampoline(void *arg, const rtps::ReaderCacheChange &change) { @@ -461,8 +903,9 @@ void RtpsParticipant::service_reply_trampoline(void *arg, const rtps::ReaderCach } const uint64_t key = seq_key(change.relatedSampleIdentity.sequence_number); - std::vector reply(change.getDataSize()); - if (!reply.empty() && !change.copyInto(reply.data(), change.getDataSize())) { + // shared_ptr payload: see service_request_trampoline. + auto reply = std::make_shared>(change.getDataSize()); + if (!reply->empty() && !change.copyInto(reply->data(), change.getDataSize())) { return; } @@ -476,17 +919,26 @@ void RtpsParticipant::service_reply_trampoline(void *arg, const rtps::ReaderCach pending = std::move(it->second); impl->pending.erase(it); } - if (pending.sync) { - std::lock_guard lock(pending.sync->m); - pending.sync->reply = std::move(reply); - pending.sync->done = true; - pending.sync->cv.notify_one(); - } else if (pending.on_reply) { - pending.on_reply(reply); - } + // Correlation (map lookup/erase) ran inline above; only the user-facing + // delivery is deferred for banded shared-port clients (inline by default). + // The delivery body is self-contained (pending + reply), but the drain job + // still owns the Impl via the shared owner capture. + auto owner = impl->shared_from_this(); + impl->deferred.run_or_defer( + [pending = std::move(pending), reply]() { + if (pending.sync) { + std::lock_guard lock(pending.sync->m); + pending.sync->reply = std::move(*reply); + pending.sync->done = true; + pending.sync->cv.notify_one(); + } else if (pending.on_reply) { + pending.on_reply(std::span(reply->data(), reply->size())); + } + }, + owner); } -RtpsParticipant::ServiceClient::ServiceClient(std::unique_ptr impl) +RtpsParticipant::ServiceClient::ServiceClient(std::shared_ptr impl) : impl_(std::move(impl)) {} RtpsParticipant::ServiceClient::~ServiceClient() = default; @@ -538,36 +990,111 @@ bool RtpsParticipant::add_service_server(const ServiceConfig &config, service_ha bool RtpsParticipant::add_service_server_deferred(const ServiceConfig &config, service_deferred_handler_t handler) { + return add_service_server_deferred_internal(config, std::move(handler)) != nullptr; +} + +// Internal variant returning the exact context created, so composite builders +// (actions) can roll back precisely what THIS invocation added. +bool RtpsParticipant::begin_engine_op() { + std::lock_guard lock(mutex_); + if (stopping_ || domain_ == nullptr || participant_ == nullptr) { + // Teardown has begun (or never started): the engine will be (or already + // is) destroyed; the caller must not touch it. During stop() the domain + // teardown itself reclaims every endpoint, so skipping the individual + // removal leaks nothing. + return false; + } + ++active_engine_ops_; + return true; +} + +void RtpsParticipant::end_engine_op() { + std::lock_guard lock(mutex_); + if (--active_engine_ops_ == 0) { + engine_ops_cv_.notify_all(); + } +} + +void RtpsParticipant::rollback_delete_writer(rtps::Writer *writer) { + if (writer == nullptr) { + return; + } + if (domain_ == nullptr || participant_ == nullptr || + !domain_->deleteWriter(*participant_, writer)) { + // Deletion failed (e.g. the SEDP dispose could not be sent): the endpoint + // stays registered and keeps its dedicated port. Retain it so stop() can + // retry rather than dropping the only handle and leaking an untracked + // endpoint. + logger_.warn("Rollback: writer deletion failed; retaining for cleanup at stop()"); + orphaned_writers_.push_back(writer); + } +} + +void RtpsParticipant::rollback_delete_reader(rtps::Reader *reader) { + if (reader == nullptr) { + return; + } + if (domain_ == nullptr || participant_ == nullptr || + !domain_->deleteReader(*participant_, reader)) { + logger_.warn("Rollback: reader deletion failed; retaining for cleanup at stop()"); + orphaned_readers_.push_back(reader); + } +} + +std::shared_ptr +RtpsParticipant::add_service_server_deferred_internal(const ServiceConfig &config, + service_deferred_handler_t handler) { std::lock_guard lock(mutex_); if (!started_) { logger_.error("Cannot add service server '{}': not started", config.service); - return false; + return nullptr; } const std::string req_topic = rtps::rpc::service_request_topic(config.service); const std::string rep_topic = rtps::rpc::service_reply_topic(config.service); const std::string req_type = rtps::rpc::service_request_type(config.type_name); const std::string rep_type = rtps::rpc::service_response_type(config.type_name); + // The service's band/dscp apply to BOTH endpoints (request reader + reply + // writer) - each banded endpoint may get a dedicated port (rationed). + const rtps::EndpointOptions endpoint_options{.band = config.band, .dscp = config.dscp}; rtps::Writer *reply_writer = - domain_->createWriter(*participant_, rep_topic.c_str(), rep_type.c_str(), /*reliable=*/true); + domain_->createWriter(*participant_, rep_topic.c_str(), rep_type.c_str(), /*reliable=*/true, + /*enforceUnicast=*/false, endpoint_options); rtps::Reader *request_reader = - domain_->createReader(*participant_, req_topic.c_str(), req_type.c_str(), /*reliable=*/true); + domain_->createReader(*participant_, req_topic.c_str(), req_type.c_str(), /*reliable=*/true, + /*mcastaddress=*/{0, 0, 0, 0}, endpoint_options); if (reply_writer == nullptr || request_reader == nullptr) { + // Transactional: a partial failure must not leave the successful endpoint + // announced (and its dedicated-port ration slot consumed). A deletion that + // itself fails is retained for retry rather than leaked. + rollback_delete_writer(reply_writer); + rollback_delete_reader(request_reader); logger_.error("Service server '{}': endpoint creation failed", config.service); - return false; + return nullptr; } auto ctx = std::make_shared(); ctx->self = this; ctx->handler = std::move(handler); ctx->reply_writer = reply_writer; ctx->request_reader = request_reader; + if (config.band != espp::QosBand::Normal && !request_reader->m_attributes.hasDedicatedPort) { + // Banded request reader on the shared port: run the handler deferred at + // the service's band instead of inline on the receive worker. + ctx->deferred.enabled = true; + ctx->deferred.band = config.band; + ctx->deferred.transport = &domain_->getTransport(); + logger_.info("Service server '{}' uses deferred banded dispatch (band {}, no dedicated port)", + config.service, static_cast(config.band)); + } if (request_reader->registerCallback(&service_request_trampoline, ctx.get()) == 0) { + rollback_delete_reader(request_reader); + rollback_delete_writer(reply_writer); logger_.error("Service server '{}': could not register request callback", config.service); - return false; + return nullptr; } - service_servers_.push_back(std::move(ctx)); + service_servers_.push_back(ctx); logger_.info("Added service server: '{}' ({})", config.service, config.type_name); - return true; + return ctx; } // =========================================================================== @@ -606,6 +1133,23 @@ void reap_and_store(std::mutex &m, std::vector &threads, std:: threads.push_back(ActionExecThread{std::move(th), std::move(finished)}); } +// Join every execute worker and clear the list. A joinable std::thread's +// destructor calls std::terminate, so any worker an accepted goal spawned MUST +// be joined before its owning context is destroyed - during shutdown AND when a +// partially-created action server is rolled back (its goal service was already +// announced, so a peer could have submitted a goal). Callers should first +// remove the endpoints / signal cooperative cancellation so this cannot block +// on a long-running execute callback. +void join_exec_threads(std::mutex &m, std::vector &threads) { + std::lock_guard lock(m); + for (auto &t : threads) { + if (t.thread.joinable()) { + t.thread.join(); + } + } + threads.clear(); +} + // Generate a unique 16-byte goal id: random_device bytes mixed with a process // counter so uniqueness holds even if random_device is weak (e.g. on an MCU). ract::GoalUuid generate_goal_id() { @@ -726,6 +1270,93 @@ void RtpsParticipant::ActionGoalHandle::canceled(std::span result terminate(static_cast(ract::GoalStatus::CANCELED), result); } +bool RtpsParticipant::remove_service_server(const std::shared_ptr &server) { + if (server == nullptr) { + return false; + } + // Pin the engine: the deletions + deferred close() below run OUTSIDE mutex_ + // (close() waits for an in-flight handler that may take mutex_), and stop() + // must not stop/destroy the domain while they are using it. + if (!begin_engine_op()) { + return false; + } + EngineOpGuard op_guard(*this); + // Ordering (all confirmed before facade state is mutated; on failure the + // context stays registered and live for retry, already-deleted endpoints + // nulled): + // 1. delete the request READER first, so no NEW request is dispatched; + // 2. close() the deferred dispatcher, which WAITS for the in-flight + // request handler to finish - that handler may still call + // ServiceResponder::reply() through the reply writer, so it must be + // quiesced BEFORE the writer is deleted; + // 3. delete the reply WRITER, now that no handler can reference it; + // 4. drop the registry entry. + if (server->request_reader != nullptr) { + if (!domain_->deleteReader(*participant_, server->request_reader)) { + return false; + } + server->request_reader = nullptr; + } + server->deferred.close(); + // Invalidate RETAINED responders before the reply writer dies: holding the + // endpoint-liveness lock waits for an in-flight reply() to finish against + // the still-live writer, and any later reply() observes !alive and no-ops + // instead of writing through the deleted (possibly reused) writer slot. + if (server->writer_live) { + std::lock_guard endpoint_lock(server->writer_live->m); + server->writer_live->alive = false; + } + if (server->reply_writer != nullptr) { + if (!domain_->deleteWriter(*participant_, server->reply_writer)) { + return false; + } + server->reply_writer = nullptr; + } + { + // Remove exactly THIS handle (pointer identity) - a concurrently added + // server is untouched. + std::lock_guard lock(mutex_); + std::erase(service_servers_, server); + } + return true; +} + +bool RtpsParticipant::remove_service_client(const std::shared_ptr &client) { + if (client == nullptr) { + return false; + } + // Pin the engine for the unlocked deletion + quiesce (see + // remove_service_server()). + if (!begin_engine_op()) { + return false; + } + EngineOpGuard op_guard(*this); + // Same ordering as remove_service_server(): delete the reply READER first + // (stops new reply deliveries), then close() the deferred dispatcher + // (WAITS for the in-flight reply callback, which references this Impl), + // then delete the request WRITER, then drop the registry entry. Facade + // state is mutated only on confirmed engine deletion; partial progress is + // nulled for retry. + if (client->impl_->reply_reader != nullptr) { + if (!domain_->deleteReader(*participant_, client->impl_->reply_reader)) { + return false; + } + client->impl_->reply_reader = nullptr; + } + client->impl_->deferred.close(); + if (client->impl_->request_writer != nullptr) { + if (!domain_->deleteWriter(*participant_, client->impl_->request_writer)) { + return false; + } + client->impl_->request_writer = nullptr; + } + { + std::lock_guard lock(mutex_); + std::erase(service_clients_, client); + } + return true; +} + bool RtpsParticipant::add_action_server(const ActionConfig &config, action_goal_callback_t on_goal, action_execute_callback_t execute, action_cancel_callback_t on_cancel) { @@ -733,26 +1364,80 @@ bool RtpsParticipant::add_action_server(const ActionConfig &config, action_goal_ logger_.error("Cannot add action server '{}': not started", config.action); return false; } + // Pin the ENTIRE composite transaction as one engine operation: the nested + // adds release mutex_ between endpoints, so without the pin a concurrent + // stop() could tear the engine down mid-build (or race the registry commit + // below against teardown's container clearing). With the operation + // registered, stop() waits at its phase 1.5 until this function returns. + if (!begin_engine_op()) { + logger_.error("Cannot add action server '{}': shutting down", config.action); + return false; + } + EngineOpGuard op_guard(*this); auto ctx = std::make_shared(); ctx->self = this; ctx->feedback_topic = rtps::rpc::action_feedback_topic(config.action); ctx->status_topic = rtps::rpc::action_status_topic(config.action); ctx->execute = std::move(execute); - // Feedback + status publishers (plain reliable topics). - if (!add_writer({ctx->feedback_topic, rtps::rpc::action_feedback_type(config.type_name), - Reliability::RELIABLE}) || - !add_writer({ctx->status_topic, rtps::rpc::action_status_type(), Reliability::RELIABLE})) { + // Feedback + status publishers (plain reliable topics). The action's + // band/dscp are inherited by every underlying endpoint (see ActionConfig). + // Track exactly what THIS invocation created: a failed add_writer() (e.g. + // the topic already exists because the action was added twice) must NOT + // cause the rollback to delete another instance's endpoints, and a + // concurrent add by another thread must never be rolled back as collateral. + const bool created_feedback = + add_writer({.topic = ctx->feedback_topic, + .type_name = rtps::rpc::action_feedback_type(config.type_name), + .reliability = Reliability::RELIABLE, + .band = config.band, + .dscp = config.dscp}); + const bool created_status = + created_feedback && add_writer({.topic = ctx->status_topic, + .type_name = rtps::rpc::action_status_type(), + .reliability = Reliability::RELIABLE, + .band = config.band, + .dscp = config.dscp}); + if (!created_feedback || !created_status) { + if (created_feedback) { + remove_writer(ctx->feedback_topic); + } logger_.error("Action server '{}': feedback/status writer creation failed", config.action); return false; } + // The exact service-server handles created by this invocation (for precise + // rollback - see the internal add variant). + std::vector> created_servers; + const auto add_sync_tracked = [this, &created_servers](const ServiceConfig &cfg, + service_handler_t h) -> bool { + auto server = add_service_server_deferred_internal( + cfg, [h = std::move(h)](std::span request, ServiceResponder resp) { + resp.reply(h(request)); + }); + if (server != nullptr) { + created_servers.push_back(std::move(server)); + return true; + } + return false; + }; + const auto add_deferred_tracked = [this, &created_servers](const ServiceConfig &cfg, + service_deferred_handler_t h) -> bool { + auto server = add_service_server_deferred_internal(cfg, std::move(h)); + if (server != nullptr) { + created_servers.push_back(std::move(server)); + return true; + } + return false; + }; + auto weak = std::weak_ptr(ctx); // send_goal service: accept/reject, then spawn the execute thread. const ServiceConfig send_goal_cfg{rtps::rpc::action_send_goal_service(config.action), - rtps::rpc::action_send_goal_type(config.type_name)}; - bool ok = add_service_server( + rtps::rpc::action_send_goal_type(config.type_name), config.band, + config.dscp}; + bool ok = add_sync_tracked( send_goal_cfg, [this, weak, on_goal](std::span req) -> std::vector { auto server = weak.lock(); ract::GoalUuid id{}; @@ -795,8 +1480,9 @@ bool RtpsParticipant::add_action_server(const ActionConfig &config, action_goal_ // get_result service (DEFERRED): reply now if done, else hold the responder. const ServiceConfig get_result_cfg{rtps::rpc::action_get_result_service(config.action), - rtps::rpc::action_get_result_type(config.type_name)}; - ok = ok && add_service_server_deferred( + rtps::rpc::action_get_result_type(config.type_name), + config.band, config.dscp}; + ok = ok && add_deferred_tracked( get_result_cfg, [weak](std::span req, ServiceResponder responder) { auto server = weak.lock(); ract::GoalUuid id{}; @@ -837,37 +1523,75 @@ bool RtpsParticipant::add_action_server(const ActionConfig &config, action_goal_ // cancel_goal service: mark the goal canceling; the execute callback observes // is_canceling(). Minimal CancelGoal_Response (return_code=0, empty list). const ServiceConfig cancel_cfg{rtps::rpc::action_cancel_goal_service(config.action), - rtps::rpc::action_cancel_goal_type()}; + rtps::rpc::action_cancel_goal_type(), config.band, config.dscp}; ok = ok && - add_service_server( - cancel_cfg, [weak, on_cancel](std::span req) -> std::vector { - auto server = weak.lock(); - // CancelGoal_Request: goal_info{ goal_id: UUID(16), stamp }. - if (server != nullptr && req.size() >= 4 + 16) { - ract::GoalUuid id{}; - std::memcpy(id.data(), req.data() + 4, 16); - std::shared_ptr gstate; - { - std::lock_guard lock(server->goals_mutex); - auto it = server->goals.find(id); - if (it != server->goals.end()) { - gstate = it->second; - } - } - if (gstate && (!on_cancel || on_cancel(id))) { - gstate->cancel_requested.store(true); - } - } - // CancelGoal_Response: return_code:int8 + pad(3) + goals[]=0. - std::vector resp{0x00, 0x01, 0x00, 0x00, 0, 0, 0, 0, 0, 0, 0, 0}; - return resp; - }); + add_sync_tracked(cancel_cfg, + [weak, on_cancel](std::span req) -> std::vector { + auto server = weak.lock(); + // CancelGoal_Request: goal_info{ goal_id: UUID(16), stamp }. + if (server != nullptr && req.size() >= 4 + 16) { + ract::GoalUuid id{}; + std::memcpy(id.data(), req.data() + 4, 16); + std::shared_ptr gstate; + { + std::lock_guard lock(server->goals_mutex); + auto it = server->goals.find(id); + if (it != server->goals.end()) { + gstate = it->second; + } + } + if (gstate && (!on_cancel || on_cancel(id))) { + gstate->cancel_requested.store(true); + } + } + // CancelGoal_Response: return_code:int8 + pad(3) + goals[]=0. + std::vector resp{0x00, 0x01, 0x00, 0x00, 0, 0, 0, 0, 0, 0, 0, 0}; + return resp; + }); if (!ok) { + // Transactional: unwind EXACTLY the endpoints this invocation created - + // the tracked service-server handles and the two topic writers (created + // above by this call) - so nothing stays announced (or holds a ration + // slot) for the action that failed to build, and nothing else is touched. + // The goal service was announced before this failure, so a peer may already + // have submitted an accepted goal and spawned a joinable execute worker in + // ctx->exec_threads. Teardown order matters: + // 1. remove the send_goal service FIRST (created_servers[0]) so no NEW + // goal can spawn another worker after the join below; + // 2. signal cooperative cancellation and JOIN the workers - BEFORE the + // get_result service (and its reply writer) is deleted, because a + // deferred get_result handler may have handed a ServiceResponder to a + // worker, whose goal-terminating reply() must go through that writer + // while it is still alive (deleting it first would leave the + // responder replying through a reset/reused writer slot); + // 3. only then remove the remaining services and the topic writers. + // A joinable std::thread destructor would std::terminate, so the join must + // also precede ctx destruction. Not under mutex_, so workers can take it. + if (!created_servers.empty()) { + remove_service_server(created_servers.front()); // send_goal: stop new spawns + } + { + std::lock_guard lock(ctx->goals_mutex); + for (auto &kv : ctx->goals) { + kv.second->cancel_requested.store(true); + } + } + join_exec_threads(ctx->threads_mutex, ctx->exec_threads); + for (size_t i = 1; i < created_servers.size(); ++i) { + remove_service_server(created_servers[i]); + } + remove_writer(ctx->status_topic); + remove_writer(ctx->feedback_topic); logger_.error("Action server '{}': service endpoint creation failed", config.action); return false; } - action_servers_.push_back(std::move(ctx)); + { + // Commit under mutex_: the registry vectors are iterated/cleared by + // stop()'s teardown and mutated by concurrent adds. + std::lock_guard lock(mutex_); + action_servers_.push_back(std::move(ctx)); + } logger_.info("Added action server: '{}' ({})", config.action, config.type_name); return true; } @@ -964,17 +1688,36 @@ RtpsParticipant::add_action_client(const ActionConfig &config) { logger_.error("Cannot add action client '{}': not started", config.action); return nullptr; } + // Pin the ENTIRE composite transaction as one engine operation: the nested + // adds release mutex_ between endpoints, so without the pin a concurrent + // stop() could tear the engine down mid-build (or race the registry commit + // below against teardown's container clearing). With the operation + // registered, stop() waits at its phase 1.5 until this function returns. + if (!begin_engine_op()) { + logger_.error("Cannot add action client '{}': shutting down", config.action); + return nullptr; + } + EngineOpGuard op_guard(*this); auto impl = std::make_unique(); impl->self = this; impl->action = config.action; + // The action's band/dscp are inherited by every underlying endpoint. On a + // later failure exactly the handles created HERE are removed (precise + // rollback - never a concurrently added endpoint). impl->send_goal_client = add_service_client({rtps::rpc::action_send_goal_service(config.action), - rtps::rpc::action_send_goal_type(config.type_name)}); - impl->get_result_client = - add_service_client({rtps::rpc::action_get_result_service(config.action), - rtps::rpc::action_get_result_type(config.type_name)}); - impl->cancel_client = add_service_client( - {rtps::rpc::action_cancel_goal_service(config.action), rtps::rpc::action_cancel_goal_type()}); + rtps::rpc::action_send_goal_type(config.type_name), + config.band, config.dscp}); + impl->get_result_client = add_service_client({rtps::rpc::action_get_result_service(config.action), + rtps::rpc::action_get_result_type(config.type_name), + config.band, config.dscp}); + impl->cancel_client = + add_service_client({rtps::rpc::action_cancel_goal_service(config.action), + rtps::rpc::action_cancel_goal_type(), config.band, config.dscp}); if (!impl->send_goal_client || !impl->get_result_client || !impl->cancel_client) { + // Transactional: unwind exactly the service clients that DID build. + remove_service_client(impl->send_goal_client); + remove_service_client(impl->get_result_client); + remove_service_client(impl->cancel_client); logger_.error("Action client '{}': service client creation failed", config.action); return nullptr; } @@ -1000,13 +1743,20 @@ RtpsParticipant::add_action_client(const ActionConfig &config) { if (cb) { cb({fb.data(), fb.size()}); } - }})) { + }, + config.band, config.dscp})) { + remove_service_client(impl->send_goal_client); + remove_service_client(impl->get_result_client); + remove_service_client(impl->cancel_client); logger_.error("Action client '{}': feedback reader creation failed", config.action); return nullptr; } auto client = std::shared_ptr(new ActionClient(std::move(impl))); - action_clients_.push_back(client); + { + std::lock_guard lock(mutex_); // commit vs stop()/concurrent adds + action_clients_.push_back(client); + } logger_.info("Added action client: '{}' ({})", config.action, config.type_name); return client; } @@ -1023,19 +1773,40 @@ RtpsParticipant::add_service_client(const ServiceConfig &config) { const std::string req_type = rtps::rpc::service_request_type(config.type_name); const std::string rep_type = rtps::rpc::service_response_type(config.type_name); + // The service's band/dscp apply to BOTH endpoints (reply reader + request + // writer) - each banded endpoint may get a dedicated port (rationed). + const rtps::EndpointOptions endpoint_options{.band = config.band, .dscp = config.dscp}; rtps::Reader *reply_reader = - domain_->createReader(*participant_, rep_topic.c_str(), rep_type.c_str(), /*reliable=*/true); + domain_->createReader(*participant_, rep_topic.c_str(), rep_type.c_str(), /*reliable=*/true, + /*mcastaddress=*/{0, 0, 0, 0}, endpoint_options); rtps::Writer *request_writer = - domain_->createWriter(*participant_, req_topic.c_str(), req_type.c_str(), /*reliable=*/true); + domain_->createWriter(*participant_, req_topic.c_str(), req_type.c_str(), /*reliable=*/true, + /*enforceUnicast=*/false, endpoint_options); if (reply_reader == nullptr || request_writer == nullptr) { + // Transactional: see add_service_server_deferred(). A deletion that itself + // fails is retained for retry rather than leaked. + rollback_delete_reader(reply_reader); + rollback_delete_writer(request_writer); logger_.error("Service client '{}': endpoint creation failed", config.service); return nullptr; } - auto impl = std::make_unique(); + auto impl = std::make_shared(); impl->self = this; impl->request_writer = request_writer; + impl->reply_reader = reply_reader; impl->reply_reader_guid = reply_reader->m_attributes.endpointGuid; + if (config.band != espp::QosBand::Normal && !reply_reader->m_attributes.hasDedicatedPort) { + // Banded reply reader on the shared port: deliver replies deferred at the + // service's band instead of inline on the receive worker. + impl->deferred.enabled = true; + impl->deferred.band = config.band; + impl->deferred.transport = &domain_->getTransport(); + logger_.info("Service client '{}' uses deferred banded dispatch (band {}, no dedicated port)", + config.service, static_cast(config.band)); + } if (reply_reader->registerCallback(&service_reply_trampoline, impl.get()) == 0) { + rollback_delete_reader(reply_reader); + rollback_delete_writer(request_writer); logger_.error("Service client '{}': could not register reply callback", config.service); return nullptr; } @@ -1053,7 +1824,12 @@ RtpsParticipant::add_service_client(const ServiceConfig &config) { struct RtpsParticipant::NativeServiceServerContext { RtpsParticipant *self{nullptr}; std::string reply_topic; + std::string request_topic; ///< retained for composite (native action) rollback service_handler_t handler{nullptr}; + // Partial-removal markers (see remove_native_service_server): flags, not + // cleared strings - in-flight handlers still read the topic strings. + bool request_removed{false}; + bool reply_removed{false}; }; struct RtpsParticipant::NativeServiceClient::Impl { @@ -1069,6 +1845,10 @@ struct RtpsParticipant::NativeServiceClient::Impl { }; RtpsParticipant *self{nullptr}; std::string request_topic; + std::string reply_topic; ///< retained for composite (native action) rollback + // Partial-removal markers (see remove_native_service_client). + bool reply_removed{false}; + bool request_removed{false}; std::array my_prefix{}; std::atomic next_id{1}; std::mutex mutex; @@ -1134,21 +1914,106 @@ RtpsParticipant::NativeServiceClient::call_future(std::span reque return future; } +bool RtpsParticipant::remove_native_service_server( + const std::shared_ptr &server) { + if (server == nullptr) { + return false; + } + // Same invariant as remove_service_server(): the endpoints (a facade reader + // whose callback captures this context, and a writer) are deleted FIRST via + // remove_reader/remove_writer - which themselves only mutate facade state on + // confirmed engine deletion - and the registry entry is dropped only after + // both succeed. Removal flags record partial progress for retry. + if (!server->request_removed) { + if (!remove_reader(server->request_topic)) { + return false; + } + server->request_removed = true; + } + if (!server->reply_removed) { + if (!remove_writer(server->reply_topic)) { + return false; + } + server->reply_removed = true; + } + { + // Remove exactly THIS handle - a concurrently added server is untouched. + std::lock_guard lock(mutex_); + std::erase(native_service_servers_, server); + } + return true; +} + +bool RtpsParticipant::remove_native_service_client( + const std::shared_ptr &client) { + if (client == nullptr) { + return false; + } + // Same invariant as remove_native_service_server(): the reply reader's + // callback captures the Impl, so its engine deletion must be confirmed + // before this handle is unregistered. + if (!client->impl_->reply_removed) { + if (!remove_reader(client->impl_->reply_topic)) { + return false; + } + client->impl_->reply_removed = true; + } + if (!client->impl_->request_removed) { + if (!remove_writer(client->impl_->request_topic)) { + return false; + } + client->impl_->request_removed = true; + } + { + std::lock_guard lock(mutex_); + std::erase(native_service_clients_, client); + } + return true; +} + bool RtpsParticipant::add_native_service_server(const ServiceConfig &config, service_handler_t handler) { + return add_native_service_server_internal(config, std::move(handler)) != nullptr; +} + +// Internal variant returning the exact context created (precise composite +// rollback - see add_service_server_deferred_internal). +std::shared_ptr +RtpsParticipant::add_native_service_server_internal(const ServiceConfig &config, + service_handler_t handler) { if (!started_) { logger_.error("Cannot add native service server '{}': not started", config.service); - return false; + return nullptr; + } + // Pin the ENTIRE composite transaction as one engine operation (same idiom + // as the composite action builders): the nested add_writer/add_reader + // release mutex_ between endpoints, so without the pin a concurrent stop() + // could tear the engine down between the adds - and the registry commit + // below could then append a stale server context after teardown cleared the + // containers. With the operation registered, stop() waits at its phase 1.5 + // until this function returns; once stopping, the add is rejected here. + if (!begin_engine_op()) { + logger_.error("Cannot add native service server '{}': shutting down", config.service); + return nullptr; } + EngineOpGuard op_guard(*this); auto ctx = std::make_shared(); ctx->self = this; ctx->reply_topic = rtps::rpc::native_reply_topic(config.service); + ctx->request_topic = rtps::rpc::native_request_topic(config.service); ctx->handler = std::move(handler); - const std::string req_topic = rtps::rpc::native_request_topic(config.service); - - if (!add_writer({ctx->reply_topic, config.type_name, Reliability::RELIABLE})) { + const std::string &req_topic = ctx->request_topic; + + // The service's band/dscp apply to both native endpoints (request reader + + // reply writer); the request reader inherits deferred banded dispatch from + // add_reader() when it gets no dedicated port. + if (!add_writer({.topic = ctx->reply_topic, + .type_name = config.type_name, + .reliability = Reliability::RELIABLE, + .band = config.band, + .dscp = config.dscp})) { logger_.error("Native service server '{}': reply writer failed", config.service); - return false; + return nullptr; } NativeServiceServerContext *raw = ctx.get(); if (!add_reader({req_topic, config.type_name, Reliability::RELIABLE, @@ -1168,28 +2033,53 @@ bool RtpsParticipant::add_native_service_server(const ServiceConfig &config, rh.op = rtps::rpc::NativeOp::REPLY; auto out = rtps::rpc::native_encode(rh, reply); raw->self->publish(raw->reply_topic, {out.data(), out.size()}); - }})) { + }, + config.band, config.dscp})) { + // Transactional: don't leave the reply writer announced (nor its ration + // slot consumed) when the pair could not be completed. + remove_writer(ctx->reply_topic); logger_.error("Native service server '{}': request reader failed", config.service); - return false; + return nullptr; + } + { + std::lock_guard lock(mutex_); + native_service_servers_.push_back(ctx); } - native_service_servers_.push_back(std::move(ctx)); logger_.info("Added native service server: '{}'", config.service); - return true; + return ctx; } std::shared_ptr RtpsParticipant::add_native_service_client(const ServiceConfig &config) { - if (!started_ || participant_ == nullptr) { + if (!started_) { logger_.error("Cannot add native service client '{}': not started", config.service); return nullptr; } + // Pin the ENTIRE composite transaction as one engine operation (same idiom + // as the composite action builders): the nested add_writer/add_reader + // release mutex_ between endpoints, so without the pin a concurrent stop() + // could tear the engine down between the adds and the registry commit below + // could append a stale client into a cleared container. The pin also makes + // the participant_ dereference below safe: begin_engine_op() validates the + // pointer under mutex_, and stop() cannot reset it while the operation is + // registered (it waits at phase 1.5). + if (!begin_engine_op()) { + logger_.error("Cannot add native service client '{}': shutting down", config.service); + return nullptr; + } + EngineOpGuard op_guard(*this); auto impl = std::make_unique(); impl->self = this; impl->request_topic = rtps::rpc::native_request_topic(config.service); + impl->reply_topic = rtps::rpc::native_reply_topic(config.service); impl->my_prefix = participant_->m_guidPrefix.id; - const std::string rep_topic = rtps::rpc::native_reply_topic(config.service); + const std::string &rep_topic = impl->reply_topic; - if (!add_writer({impl->request_topic, config.type_name, Reliability::RELIABLE})) { + if (!add_writer({.topic = impl->request_topic, + .type_name = config.type_name, + .reliability = Reliability::RELIABLE, + .band = config.band, + .dscp = config.dscp})) { logger_.error("Native service client '{}': request writer failed", config.service); return nullptr; } @@ -1220,12 +2110,18 @@ RtpsParticipant::add_native_service_client(const ServiceConfig &config) { } else if (p.on_reply) { p.on_reply(payload); } - }})) { + }, + config.band, config.dscp})) { + // Transactional: see add_native_service_server(). + remove_writer(impl->request_topic); logger_.error("Native service client '{}': reply reader failed", config.service); return nullptr; } auto client = std::shared_ptr(new NativeServiceClient(std::move(impl))); - native_service_clients_.push_back(client); + { + std::lock_guard lock(mutex_); + native_service_clients_.push_back(client); + } logger_.info("Added native service client: '{}'", config.service); return client; } @@ -1299,19 +2195,35 @@ bool RtpsParticipant::add_native_action_server(const ActionConfig &config, logger_.error("Cannot add native action server '{}': not started", config.action); return false; } + // Pin the ENTIRE composite transaction as one engine operation: the nested + // adds release mutex_ between endpoints, so without the pin a concurrent + // stop() could tear the engine down mid-build (or race the registry commit + // below against teardown's container clearing). With the operation + // registered, stop() waits at its phase 1.5 until this function returns. + if (!begin_engine_op()) { + logger_.error("Cannot add native action server '{}': shutting down", config.action); + return false; + } + EngineOpGuard op_guard(*this); auto ctx = std::make_shared(); ctx->self = this; ctx->feedback_topic = rtps::rpc::native_feedback_topic(config.action); ctx->execute = std::move(execute); - if (!add_writer({ctx->feedback_topic, config.type_name, Reliability::RELIABLE})) { + // The action's band/dscp are inherited by all ~3 native endpoints. + if (!add_writer({.topic = ctx->feedback_topic, + .type_name = config.type_name, + .reliability = Reliability::RELIABLE, + .band = config.band, + .dscp = config.dscp})) { logger_.error("Native action server '{}': feedback writer failed", config.action); return false; } auto weak = std::weak_ptr(ctx); // The send_goal native service: accept -> spawn execute -> reply goal_handle. - const bool ok = add_native_service_server( - {rtps::rpc::native_goal_service(config.action), config.type_name}, + // Handles created by THIS invocation, for precise rollback. + const auto goal_server = add_native_service_server_internal( + {rtps::rpc::native_goal_service(config.action), config.type_name, config.band, config.dscp}, [this, weak, on_goal](std::span goal) -> std::vector { auto server = weak.lock(); if (server == nullptr || (on_goal && !on_goal(goal))) { @@ -1344,14 +2256,15 @@ bool RtpsParticipant::add_native_action_server(const ActionConfig &config, } return rtps::rpc::native_make_goal_reply(true, handle); }); - if (!ok) { + if (goal_server == nullptr) { + remove_writer(ctx->feedback_topic); logger_.error("Native action server '{}': goal service failed", config.action); return false; } // The cancel native service: mark a running goal canceling (the execute // callback observes is_canceling()); on_cancel, if set, gates acceptance. - const bool cancel_ok = add_native_service_server( - {rtps::rpc::native_cancel_service(config.action), config.type_name}, + const auto cancel_server = add_native_service_server_internal( + {rtps::rpc::native_cancel_service(config.action), config.type_name, config.band, config.dscp}, [weak, on_cancel](std::span req) -> std::vector { auto server = weak.lock(); uint32_t handle = 0; @@ -1375,11 +2288,24 @@ bool RtpsParticipant::add_native_action_server(const ActionConfig &config, } return rtps::rpc::native_make_cancel_reply(accept); }); - if (!cancel_ok) { + if (cancel_server == nullptr) { + // Transactional: unwind EXACTLY what this invocation created - the goal + // service handle and the feedback writer. The goal service was announced + // before this failure, so a peer may already have submitted an accepted + // goal and spawned a joinable execute worker: remove the endpoints, then + // JOIN before ctx is destroyed (a joinable std::thread destructor would + // call std::terminate). Matches the native-server teardown in stop(), which + // likewise joins without a cancel signal (native goals are weak refs). + remove_native_service_server(goal_server); + remove_writer(ctx->feedback_topic); + join_exec_threads(ctx->threads_mutex, ctx->exec_threads); logger_.error("Native action server '{}': cancel service failed", config.action); return false; } - native_action_servers_.push_back(std::move(ctx)); + { + std::lock_guard lock(mutex_); // commit vs stop()/concurrent adds + native_action_servers_.push_back(std::move(ctx)); + } logger_.info("Added native action server: '{}'", config.action); return true; } @@ -1491,48 +2417,69 @@ RtpsParticipant::add_native_action_client(const ActionConfig &config) { logger_.error("Cannot add native action client '{}': not started", config.action); return nullptr; } + // Pin the ENTIRE composite transaction as one engine operation: the nested + // adds release mutex_ between endpoints, so without the pin a concurrent + // stop() could tear the engine down mid-build (or race the registry commit + // below against teardown's container clearing). With the operation + // registered, stop() waits at its phase 1.5 until this function returns. + if (!begin_engine_op()) { + logger_.error("Cannot add native action client '{}': shutting down", config.action); + return nullptr; + } + EngineOpGuard op_guard(*this); auto impl = std::make_unique(); impl->self = this; - impl->goal_client = - add_native_service_client({rtps::rpc::native_goal_service(config.action), config.type_name}); - impl->cancel_client = add_native_service_client( - {rtps::rpc::native_cancel_service(config.action), config.type_name}); + // The action's band/dscp are inherited by all native client endpoints. On a + // later failure exactly the handles created HERE are removed. + impl->goal_client = add_native_service_client( + {rtps::rpc::native_goal_service(config.action), config.type_name, config.band, config.dscp}); + impl->cancel_client = add_native_service_client({rtps::rpc::native_cancel_service(config.action), + config.type_name, config.band, config.dscp}); if (!impl->goal_client || !impl->cancel_client) { + // Transactional: unwind exactly the native service client that DID build. + remove_native_service_client(impl->goal_client); + remove_native_service_client(impl->cancel_client); logger_.error("Native action client '{}': goal/cancel client failed", config.action); return nullptr; } NativeActionClient::Impl *raw = impl.get(); // Feedback subscriber: route feedback/result by goal_handle; terminal status // (>= SUCCEEDED) delivers the result and retires the goal. - if (!add_reader({rtps::rpc::native_feedback_topic(config.action), config.type_name, - Reliability::RELIABLE, [raw](std::span msg) { - uint32_t handle = 0; - rtps::rpc::NativeGoalStatus status{}; - std::vector payload; - if (!rtps::rpc::native_parse_feedback(msg, handle, status, payload)) { - return; - } - { - std::lock_guard lock(raw->mutex); - if (raw->goals.find(handle) == raw->goals.end()) { - // Goal not registered yet (its send_goal reply is still in - // flight): buffer this early message, bounded, for replay when - // send_goal installs the goal. See Impl::pending_early. - if (raw->pending_early_count < - NativeActionClient::Impl::kMaxPendingEarly) { - raw->pending_early[handle].push_back({status, std::move(payload)}); - ++raw->pending_early_count; - } - return; - } - } - NativeActionClient::Impl::deliver(raw, handle, status, payload); - }})) { + if (!add_reader( + {rtps::rpc::native_feedback_topic(config.action), config.type_name, Reliability::RELIABLE, + [raw](std::span msg) { + uint32_t handle = 0; + rtps::rpc::NativeGoalStatus status{}; + std::vector payload; + if (!rtps::rpc::native_parse_feedback(msg, handle, status, payload)) { + return; + } + { + std::lock_guard lock(raw->mutex); + if (raw->goals.find(handle) == raw->goals.end()) { + // Goal not registered yet (its send_goal reply is still in + // flight): buffer this early message, bounded, for replay when + // send_goal installs the goal. See Impl::pending_early. + if (raw->pending_early_count < NativeActionClient::Impl::kMaxPendingEarly) { + raw->pending_early[handle].push_back({status, std::move(payload)}); + ++raw->pending_early_count; + } + return; + } + } + NativeActionClient::Impl::deliver(raw, handle, status, payload); + }, + config.band, config.dscp})) { + remove_native_service_client(impl->goal_client); + remove_native_service_client(impl->cancel_client); logger_.error("Native action client '{}': feedback reader failed", config.action); return nullptr; } auto client = std::shared_ptr(new NativeActionClient(std::move(impl))); - native_action_clients_.push_back(client); + { + std::lock_guard lock(mutex_); // commit vs stop()/concurrent adds + native_action_clients_.push_back(client); + } logger_.info("Added native action client: '{}'", config.action); return client; } @@ -1543,13 +2490,25 @@ RtpsParticipant::add_native_action_client(const ActionConfig &config) { // unique_ptr/shared_ptr elements are destroyed - see the note by the constructor. void RtpsParticipant::stop() { // Phase 1: flip started_ under mutex_ so no further publish()/add_*()/reply - // proceeds past its started_ check. + // proceeds past its started_ check, and stopping_ so no NEW engine operation + // (an unlocked removal/quiesce sequence) can begin. { std::lock_guard lock(mutex_); if (!started_) { return; } started_ = false; + stopping_ = true; + } + // Phase 1.5: wait for in-flight engine operations to finish. Removals + // deliberately dereference domain_/participant_ OUTSIDE mutex_ (their + // deferred close() waits for user callbacks that may take mutex_), so the + // engine must stay alive until they complete. cv.wait releases mutex_ while + // waiting, so those callbacks can still acquire it and the operations can + // finish; begin_engine_op() rejects new operations now that stopping_ is set. + { + std::unique_lock lock(mutex_); + engine_ops_cv_.wait(lock, [this] { return active_engine_ops_ == 0; }); } // Phase 2: invalidate deferred RPC replies. A service responder held by user // code checks live_->alive under this lock before writing through its engine @@ -1560,7 +2519,25 @@ void RtpsParticipant::stop() { std::lock_guard lock(live_->m); live_->alive = false; } - // Phase 3: stop the engine (no more reader/service callbacks fire), then join + // Phase 3: retry any endpoint deletions that a creation-time rollback could + // not complete (the SEDP dispose failed then, so the endpoint stayed + // registered with its dedicated port). Do it while the domain is still live so + // a successful retry releases the port and disposes cleanly; whatever still + // fails is torn down by domain_->stop() below regardless. + { + std::lock_guard lock(mutex_); + if (domain_ != nullptr && participant_ != nullptr) { + for (auto *writer : orphaned_writers_) { + domain_->deleteWriter(*participant_, writer); + } + for (auto *reader : orphaned_readers_) { + domain_->deleteReader(*participant_, reader); + } + } + orphaned_writers_.clear(); + orphaned_readers_.clear(); + } + // Phase 4: stop the engine (no more reader/service callbacks fire), then join // every owned action-execute worker so none touches this participant after // the domain and its writers are gone. Done WITHOUT mutex_ held: a worker's // final publish()/reply must be able to take mutex_/live_ and run to @@ -1570,15 +2547,6 @@ void RtpsParticipant::stop() { domain_->stop(); } #ifdef RTPS_WITH_RPC - const auto join_workers = [](std::mutex &m, std::vector &threads) { - std::lock_guard lock(m); - for (auto &t : threads) { - if (t.thread.joinable()) { - t.thread.join(); - } - } - threads.clear(); - }; for (auto &ctx : action_servers_) { if (!ctx) { continue; @@ -1592,19 +2560,38 @@ void RtpsParticipant::stop() { kv.second->cancel_requested.store(true); } } - join_workers(ctx->threads_mutex, ctx->exec_threads); + join_exec_threads(ctx->threads_mutex, ctx->exec_threads); } for (auto &ctx : native_action_servers_) { if (ctx) { - join_workers(ctx->threads_mutex, ctx->exec_threads); + join_exec_threads(ctx->threads_mutex, ctx->exec_threads); } } #endif // RTPS_WITH_RPC - // Phase 4: tear the domain down and drop bookkeeping under mutex_. The engine + // Phase 5: tear the domain down and drop bookkeeping under mutex_. The engine // owns the endpoint objects, so release our references before the domain (and // with it every writer/reader and their callback registrations) goes away. { std::lock_guard lock(mutex_); + // Quiesce every deferred dispatcher BEFORE releasing the context + // references: close() cancels each retry timer synchronously, so no timer + // callback can hold (and later drop, on its own thread) the last context + // reference - context destruction always happens here. + for (const auto &ctx : reader_contexts_) { + ctx->deferred.close(); + } +#ifdef RTPS_WITH_RPC + for (const auto &srv : service_servers_) { + if (srv) { + srv->deferred.close(); + } + } + for (const auto &cli : service_clients_) { + if (cli && cli->impl_) { + cli->impl_->deferred.close(); + } + } +#endif // RTPS_WITH_RPC writers_.clear(); participant_ = nullptr; domain_.reset(); @@ -1620,7 +2607,8 @@ void RtpsParticipant::stop() { native_action_servers_.clear(); native_service_clients_.clear(); native_service_servers_.clear(); -#endif // RTPS_WITH_RPC +#endif // RTPS_WITH_RPC + stopping_ = false; // teardown complete; a future start() may proceed } logger_.info("Stopped"); } diff --git a/components/rtps/src/utils/Diagnostics.cpp b/components/rtps/src/utils/Diagnostics.cpp index 84eb2c48b7..bd2c8ea0aa 100644 --- a/components/rtps/src/utils/Diagnostics.cpp +++ b/components/rtps/src/utils/Diagnostics.cpp @@ -4,49 +4,59 @@ namespace rtps { namespace Diagnostics { namespace ThreadPool { -uint32_t dropped_incoming_packets_usertraffic = 0; -uint32_t dropped_incoming_packets_metatraffic = 0; +std::atomic dropped_incoming_packets_usertraffic{0}; +std::atomic dropped_incoming_packets_metatraffic{0}; -uint32_t dropped_outgoing_packets_usertraffic = 0; -uint32_t dropped_outgoing_packets_metatraffic = 0; +std::atomic dropped_outgoing_packets_usertraffic{0}; +std::atomic dropped_outgoing_packets_metatraffic{0}; -uint32_t processed_incoming_metatraffic = 0; -uint32_t processed_outgoing_metatraffic = 0; -uint32_t processed_incoming_usertraffic = 0; -uint32_t processed_outgoing_usertraffic = 0; +std::atomic processed_incoming_metatraffic{0}; +std::atomic processed_outgoing_metatraffic{0}; +std::atomic processed_incoming_usertraffic{0}; +std::atomic processed_outgoing_usertraffic{0}; -uint32_t max_ever_elements_outgoing_usertraffic_queue; -uint32_t max_ever_elements_incoming_usertraffic_queue; +std::atomic max_ever_elements_outgoing_usertraffic_queue{0}; +std::atomic max_ever_elements_incoming_usertraffic_queue{0}; -uint32_t max_ever_elements_outgoing_metatraffic_queue; -uint32_t max_ever_elements_incoming_metatraffic_queue; +std::atomic max_ever_elements_outgoing_metatraffic_queue{0}; +std::atomic max_ever_elements_incoming_metatraffic_queue{0}; } // namespace ThreadPool namespace StatefulReader { -uint32_t sfr_unexpected_sn; -uint32_t sfr_retransmit_requests; +std::atomic sfr_unexpected_sn{0}; +std::atomic sfr_retransmit_requests{0}; } // namespace StatefulReader +namespace Writer { +std::atomic history_overwrite_drops{0}; +} // namespace Writer + namespace Network { -uint32_t lwip_allocation_failures; +std::atomic lwip_allocation_failures{0}; } +namespace OS { +// Declared in the header since the component consolidation (#712) but never +// defined - any ODR-use was an undefined symbol at link time. +std::atomic current_free_heap_size{0}; +} // namespace OS + namespace SEDP { -uint32_t max_ever_remote_participants; -uint32_t current_remote_participants; +std::atomic max_ever_remote_participants{0}; +std::atomic current_remote_participants{0}; -uint32_t max_ever_matched_reader_proxies; -uint32_t current_max_matched_reader_proxies; +std::atomic max_ever_matched_reader_proxies{0}; +std::atomic current_max_matched_reader_proxies{0}; -uint32_t max_ever_matched_writer_proxies; -uint32_t current_max_matched_writer_proxies; +std::atomic max_ever_matched_writer_proxies{0}; +std::atomic current_max_matched_writer_proxies{0}; -uint32_t max_ever_unmatched_reader_proxies; -uint32_t current_max_unmatched_reader_proxies; +std::atomic max_ever_unmatched_reader_proxies{0}; +std::atomic current_max_unmatched_reader_proxies{0}; -uint32_t max_ever_unmatched_writer_proxies; -uint32_t current_max_unmatched_writer_proxies; +std::atomic max_ever_unmatched_writer_proxies{0}; +std::atomic current_max_unmatched_writer_proxies{0}; } // namespace SEDP } // namespace Diagnostics diff --git a/components/socket/include/socket_reactor.hpp b/components/socket/include/socket_reactor.hpp index 19e36c175a..1a44bbd509 100644 --- a/components/socket/include/socket_reactor.hpp +++ b/components/socket/include/socket_reactor.hpp @@ -226,6 +226,10 @@ class SocketReactor : public BaseComponent { */ Id add_fd(sock_type_t fd, ReadHandler handler, QosBand band = QosBand::Normal); + /// Removal-completion notification for remove(): see the two-argument + /// remove() overload for the exact invocation guarantees. + using RemovedCallback = std::function; + /** * @brief Unregister a socket. Safe to call from any thread, including from * within a running handler. If a handler for this id is currently @@ -235,10 +239,51 @@ class SocketReactor : public BaseComponent { */ bool remove(Id id); + /** + * @brief Unregister a socket and be notified when the removal has fully + * completed - i.e. the registration is erased AND any handler that + * was running or pending for it has finished, so no reactor code can + * reference the socket/fd anymore. Use this to know when it is safe + * to destroy the socket (the reactor's handlers hold references to + * it, and remove() itself never blocks). + * + * Invocation guarantees for @p on_removed: + * - Invoked EXACTLY ONCE, and only when this call returns true (an unknown + * id returns false and never invokes it). + * - Thread: the CALLER's thread (synchronously, before remove() returns) + * when no handler is in flight at remove() time; otherwise the pool + * worker that finishes the in-flight handler, or the reactor loop thread + * when a pending dispatch is reverted (pool saturated). Callers must be + * prepared for any of the three. + * - Runs with NO reactor lock held: it may re-enter the reactor (add_*, + * remove), but must return promptly (it can run on a worker or the loop) + * and must not call stop() when it runs from a worker/loop context. + * - Residual fd caveat: an already-blocked select() may still have the fd + * in its interest set for one iteration (the reactor wakes itself on + * removal, so the window is tiny). Closing the fd from the callback is + * safe; if the OS immediately reuses the fd number, the worst case is one + * bounded spurious wake of the new registration (handlers must already + * tolerate spurious readiness - see add_udp_receiver()'s receive bound). + * - Calling remove() again for an id whose removal is still pending chains + * the callbacks (both fire on completion). + * + * @param id The registration Id returned by an add_* method. + * @param on_removed Invoked once the removal has fully completed (may be + * empty, making this identical to remove(id)). + * @return true if the id was found. + */ + bool remove(Id id, RemovedCallback on_removed); + /// @return the number of currently registered sockets. size_t num_registered() const; protected: + /// Invoke a removal-completion callback without letting an exception escape. + /// These run on a pool worker OUTSIDE the handler try/catch (and chained + /// callbacks must each run to honor the exactly-once guarantee), so a throw + /// here would otherwise terminate the worker or skip a chained callback. + void invoke_removed(const RemovedCallback &cb) noexcept; + struct Entry { sock_type_t fd{static_cast(-1)}; ///< Watched file descriptor. ReadHandler handler; ///< Handler run on the pool. @@ -246,6 +291,7 @@ class SocketReactor : public BaseComponent { bool armed{true}; ///< In the select set (not currently dispatched). bool in_flight{false}; ///< A pool job is currently running the handler. bool remove_requested{false}; ///< remove() was called while in-flight. + RemovedCallback on_removed{}; ///< Fired (unlocked) when the entry is erased. }; /// Validate an fd for registration: must be valid and (for the select() diff --git a/components/socket/src/socket.cpp b/components/socket/src/socket.cpp index 8bfc96ca5c..b178d6bc68 100644 --- a/components/socket/src/socket.cpp +++ b/components/socket/src/socket.cpp @@ -143,21 +143,29 @@ bool Socket::set_receive_timeout(const std::chrono::duration &timeout) { if (seconds <= 0) { return true; } +#if defined(_WIN32) + // Winsock's SO_RCVTIMEO takes a DWORD count of milliseconds, not a POSIX + // timeval - passing a timeval sets a garbage timeout. Round to the nearest ms + // and clamp a positive duration to at least 1 ms: a zero SO_RCVTIMEO means + // "no timeout" (unbounded) on Winsock, so a sub-millisecond request must not + // truncate to 0 (which would silently defeat the reactor's bounded read). + DWORD timeout_ms = static_cast(seconds * 1000.0f + 0.5f); + if (timeout_ms == 0) { + timeout_ms = 1; + } + int err = setsockopt(socket_, SOL_SOCKET, SO_RCVTIMEO, + reinterpret_cast(&timeout_ms), sizeof(timeout_ms)); +#else float intpart; float fractpart = modff(seconds, &intpart); const auto response_timeout_s = static_cast(intpart); const auto response_timeout_us = static_cast(fractpart * 1E6f); - //// Alternatively we could do this: - // int microseconds = - // (int)(std::chrono::duration_cast(timeout).count()) % (int)1E6; - // const time_t response_timeout_s = floor(seconds); - // const time_t response_timeout_us = microseconds; - struct timeval tv; tv.tv_sec = response_timeout_s; tv.tv_usec = response_timeout_us; int err = setsockopt(socket_, SOL_SOCKET, SO_RCVTIMEO, reinterpret_cast(&tv), sizeof(tv)); +#endif if (err < 0) { return false; } diff --git a/components/socket/src/socket_reactor.cpp b/components/socket/src/socket_reactor.cpp index c60f7513f4..24a5140ed2 100644 --- a/components/socket/src/socket_reactor.cpp +++ b/components/socket/src/socket_reactor.cpp @@ -202,6 +202,28 @@ SocketReactor::add_udp_receiver(espp::UdpSocket &socket, const auto callback = receive_config.on_receive_callback; const auto buffer_size = receive_config.buffer_size; sock_type_t fd = socket.native_handle(); + // Bound the handler's read: the reactor only reads AFTER select() reported + // the socket readable, but that readiness can be stale or spurious (Linux + // documents select() may report a UDP socket readable and a subsequent read + // still block, e.g. a checksum-failed datagram discarded in between). With + // an unbounded blocking recvfrom such a dispatch would never finish and + // stop()'s in-flight wait could hang forever. A 1 s cap is invisible on the + // data path (data is normally already queued) and guarantees every dispatch + // - and therefore stop() - makes progress. + // + // A bounded read is a REQUIREMENT of registering here, not best-effort: if + // the bound cannot be installed the hang guard is void, so registration + // fails. SO_RCVTIMEO is chosen over O_NONBLOCK deliberately - it bounds + // ONLY receives, while non-blocking mode would also make sends through this + // same fd (the owner and the echo path send on it) fail with EWOULDBLOCK + // under buffer pressure, silently changing send semantics. SO_RCVTIMEO is + // supported on POSIX, lwIP (LWIP_SO_RCVTIMEO), and Windows. + if (!socket.set_receive_timeout(std::chrono::duration(1.0f))) { + logger_.error("add_udp_receiver: could not set a receive timeout on port {}; refusing the " + "registration (an unbounded blocking read could hang stop() forever)", + receive_config.port); + return INVALID_ID; + } if (receive_config.dscp.has_value()) { // Mark this socket's transmitted packets (e.g. echo responses) with the // requested DSCP code point. Best-effort: network / driver treatment @@ -287,27 +309,68 @@ SocketReactor::Id SocketReactor::add_tcp_stream(espp::TcpSocket &connection, return id; } -bool SocketReactor::remove(SocketReactor::Id id) { +bool SocketReactor::remove(SocketReactor::Id id) { return remove(id, RemovedCallback{}); } + +bool SocketReactor::remove(SocketReactor::Id id, RemovedCallback on_removed) { bool found = false; + RemovedCallback completed; // invoked (unlocked) when the removal is already complete here { std::lock_guard lock(mutex_); auto it = entries_.find(id); if (it != entries_.end()) { found = true; if (it->second.in_flight) { - // A handler is running; defer erasure until dispatch() completes. + // A handler is running (or a dispatch is pending); defer erasure until + // dispatch() - or the pool-saturated revert in loop_iteration() - + // completes. The completion callback rides along on the entry; a + // repeated remove() for the same id chains the callbacks. it->second.remove_requested = true; + if (on_removed) { + if (it->second.on_removed) { + it->second.on_removed = [this, first = std::move(it->second.on_removed), + second = std::move(on_removed)]() { + // Run BOTH even if one throws, to honor the exactly-once contract. + invoke_removed(first); + invoke_removed(second); + }; + } else { + it->second.on_removed = std::move(on_removed); + } + } } else { entries_.erase(it); + completed = std::move(on_removed); } } } if (found) { wake(); } + if (completed) { + // Idle at remove() time: the removal is already complete - notify from + // the caller's thread, without the reactor lock. + invoke_removed(completed); + } return found; } +void SocketReactor::invoke_removed(const RemovedCallback &cb) noexcept { + if (!cb) { + return; + } +#if defined(__cpp_exceptions) && __cpp_exceptions + try { + cb(); + } catch (const std::exception &e) { + logger_.error("Exception in reactor removal callback: {}", e.what()); + } catch (...) { + logger_.error("Unknown exception in reactor removal callback"); + } +#else + cb(); +#endif +} + size_t SocketReactor::num_registered() const { std::lock_guard lock(mutex_); return entries_.size(); @@ -354,13 +417,18 @@ void SocketReactor::dispatch(SocketReactor::Id id) { #endif } bool wake_needed = false; + RemovedCallback removed; // deferred-removal completion, invoked unlocked below { std::lock_guard lock(mutex_); auto it = entries_.find(id); if (it != entries_.end()) { it->second.in_flight = false; if (it->second.remove_requested) { + removed = std::move(it->second.on_removed); entries_.erase(it); + // Wake so the loop drops the fd from its interest set promptly (the + // completion callback may close the fd). + wake_needed = true; } else { it->second.armed = true; // re-arm so the loop watches it again wake_needed = true; @@ -370,6 +438,12 @@ void SocketReactor::dispatch(SocketReactor::Id id) { if (wake_needed) { wake(); } + if (removed) { + // The handler has finished and the entry is gone: removal complete. + // Invoked on this pool worker, without the reactor lock (guarded so a + // throwing completion callback cannot escape the worker). + invoke_removed(removed); + } } bool SocketReactor::loop_iteration(std::mutex &, std::condition_variable &, bool &) { @@ -444,18 +518,28 @@ bool SocketReactor::loop_iteration(std::mutex &, std::condition_variable &, bool // Pool is saturated; revert and let the next select() re-report this fd // (the data stays buffered in the socket - natural backpressure). --in_flight_count_; - std::lock_guard lock(mutex_); - auto it = entries_.find(id); - if (it != entries_.end()) { - it->second.in_flight = false; - // Honor a remove() that arrived while this entry was marked in_flight, - // rather than blindly re-arming a logically-removed registration. - if (it->second.remove_requested) { - entries_.erase(it); - } else { - it->second.armed = true; + RemovedCallback removed; + { + std::lock_guard lock(mutex_); + auto it = entries_.find(id); + if (it != entries_.end()) { + it->second.in_flight = false; + // Honor a remove() that arrived while this entry was marked in_flight, + // rather than blindly re-arming a logically-removed registration. + if (it->second.remove_requested) { + removed = std::move(it->second.on_removed); + entries_.erase(it); + } else { + it->second.armed = true; + } } } + if (removed) { + // No handler ever ran for the reverted dispatch: removal complete. + // Invoked on the reactor loop thread, without the reactor lock (guarded + // so a throwing completion callback cannot escape the loop thread). + invoke_removed(removed); + } } } diff --git a/doc/en/protocols/rtps.rst b/doc/en/protocols/rtps.rst index 04d8f31e06..b9803c4d93 100644 --- a/doc/en/protocols/rtps.rst +++ b/doc/en/protocols/rtps.rst @@ -63,7 +63,7 @@ engine. subgraph plat["platform adapter (the ONLY porting layer)"] TR["rtps::EsppTransport"] SOCK["espp::UdpSocket × N ports"] - REACT["espp::SocketReactor → espp::ThreadPool"] + REACT["espp::SocketReactor → espp::ThreadPool (QosBand priority)"] CDR["espp::cdr (reflection CDR/XCDR)"] TR --> SOCK --> REACT end @@ -203,6 +203,53 @@ The component follows the standard UDPv4 RTPS port mapping formula: * - User unicast - ``7400 + 250 * domain + 11 + 2 * participant`` - ``7411`` + * - Dedicated endpoint (prioritized) + - ``7400 + 250 * domain + 100 + n`` + - ``7500``, ``7501``, … + +Every channel is one ``espp::UdpSocket`` registered on the transport's +``espp::SocketReactor`` at a **priority band** (:cpp:enum:`espp::QosBand`). +By default the *metatraffic* channels (SPDP multicast + SEDP unicast) run at +``QosBand::High`` so discovery dispatch overtakes queued user-traffic handling +under load, and the shared user channels run at ``Normal``; both are +configurable (``Config::metatraffic_band`` / ``Config::user_traffic_band``). + +Per-endpoint priority (dedicated ports) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +All user traffic for a participant normally shares ONE user-unicast port, so +per-socket priority alone cannot distinguish endpoints. An endpoint (writer or +reader) configured with a non-default ``band`` — or a ``dscp`` marking, which is +per-socket — is therefore granted its own **dedicated unicast port**: + +- the port is allocated deterministically from the domain's RTPS port block at + offset 100 (``7400 + 250*domain + 100 + n``). Each allocation probes at most + 16 consecutive candidates (a reuse-disabled bind, so ports taken by other + processes fail loudly) starting at an advancing cursor — if the whole window + is occupied, **that endpoint falls back to the shared user port** (with a + warning) and the cursor advances past the window, so the next allocation + probes fresh ports rather than one request scanning the entire 100..249 + range. The standard RTPS offsets stay below 100 only for participant ids + 0–44, so participant creation enforces that cap while dedicated ports are + enabled; +- its socket is registered on the reactor **at the endpoint's band** and + optionally DSCP-marked (:cpp:enum:`espp::Dscp`, e.g. ``Dscp::Ef``) — the + endpoint also *sends* from this socket, so the marking applies to its + outgoing traffic; +- the endpoint's SEDP announcement carries the dedicated port as its standard + per-endpoint unicast locator (``PID_UNICAST_LOCATOR``), so FastDDS / ROS 2 + peers send that endpoint's traffic straight to the prioritized socket. The + wire format is unchanged — only the announced port value differs. + +Dedicated ports are **rationed** (``Config::max_prioritized_endpoint_ports``, +default 4): each one consumes a UDP socket/fd, and lwIP on ESP32 defaults to +~10 sockets total of which the participant already uses 4. When the ration is +exhausted (or ``Config::enable_dedicated_endpoint_ports`` is false), a banded +endpoint logs a warning and falls back to the shared port; banded *readers* +then use **deferred banded dispatch** — samples are queued (bounded) and the +``on_sample`` callback is re-submitted to the transport's worker pool at the +reader's band, one in-flight delivery per reader, preserving order. Endpoints +left at ``QosBand::Normal`` keep the exact pre-band inline delivery path. Configuration ------------- diff --git a/lib/espp.cmake b/lib/espp.cmake index 10a965849a..563a2b3ca9 100644 --- a/lib/espp.cmake +++ b/lib/espp.cmake @@ -40,6 +40,158 @@ endif() add_compile_definitions(RTPS_CONFIG_HEADER="${RTPS_CONFIG_HEADER_FILE}") message(STATUS "RTPS limits profile: ${RTPS_LIMITS_PROFILE}") +# --------------------------------------------------------------------------- +# RTPS per-limit capacity overrides (fine-grained alternative to switching +# profiles): a semicolon list of NAME=VALUE entries, each overriding ONE +# capacity cap of the selected profile via its RTPS_CFG_ macro - e.g. +# -DRTPS_LIMIT_OVERRIDES="NUM_STATELESS_WRITERS=16;HISTORY_SIZE_STATEFUL=20" +# See the RTPS_CFG_* blocks in include/rtps/config_*.hpp for the knob names. +# Applied as GLOBAL compile definitions so the engine sources (which size the +# pools) and every consumer translation unit agree on the values - defining +# them for only a consumer TU would silently disagree with the library. +# Capacity-only: no bytes on the wire change. +# --------------------------------------------------------------------------- +set(RTPS_LIMIT_OVERRIDES "" CACHE STRING + "Semicolon list of RTPS capacity overrides, e.g. NUM_STATELESS_WRITERS=16;HISTORY_SIZE_STATEFUL=20") +# The supported knobs (must match the RTPS_CFG_* blocks in the profile headers). +# Validated: an unknown name would silently define an unused macro (no override +# at all), and an out-of-range value would silently truncate - e.g. 256 into a +# uint8_t knob becomes a ZERO-capacity pool. Most knobs back uint8_t constants +# (range 1..255); the unmatched-remote registries are uint16_t in the host +# profiles (host_large defaults them to 1024/512), so those two accept up to +# 65535 - EXCEPT under the embedded profile, where they are uint8_t too. +set(RTPS_LIMIT_KNOB_NAMES + NUM_STATELESS_WRITERS NUM_STATELESS_READERS NUM_STATEFUL_WRITERS NUM_STATEFUL_READERS + MAX_NUM_PARTICIPANTS NUM_WRITERS_PER_PARTICIPANT NUM_READERS_PER_PARTICIPANT + NUM_WRITER_PROXIES_PER_READER NUM_READER_PROXIES_PER_WRITER + MAX_NUM_UNMATCHED_REMOTE_WRITERS MAX_NUM_UNMATCHED_REMOTE_READERS + MAX_NUM_READER_CALLBACKS HISTORY_SIZE_STATELESS HISTORY_SIZE_STATEFUL + MAX_TYPENAME_LENGTH MAX_TOPICNAME_LENGTH MAX_NUM_UDP_CONNECTIONS) +set(RTPS_LIMIT_UINT16_KNOBS MAX_NUM_UNMATCHED_REMOTE_WRITERS MAX_NUM_UNMATCHED_REMOTE_READERS) +# Collect the validated definitions; they are applied directory-wide below AND +# exported via ESPP_RTPS_COMPILE_DEFINITIONS (they change public Config +# constants and array-backed layouts, so a find_package(espp) consumer MUST +# compile the headers with the same values the archive was built with). +set(RTPS_LIMIT_OVERRIDE_DEFS "") +foreach(override ${RTPS_LIMIT_OVERRIDES}) + if(NOT override MATCHES "^([A-Z_]+)=([0-9]+)$") + message(FATAL_ERROR "Invalid RTPS_LIMIT_OVERRIDES entry '${override}' (expected NAME=VALUE)") + endif() + string(REGEX REPLACE "^([A-Z_]+)=[0-9]+$" "\\1" _rtps_knob "${override}") + string(REGEX REPLACE "^[A-Z_]+=([0-9]+)$" "\\1" _rtps_value "${override}") + if(NOT _rtps_knob IN_LIST RTPS_LIMIT_KNOB_NAMES) + message(FATAL_ERROR + "Unknown RTPS limit knob '${_rtps_knob}' in RTPS_LIMIT_OVERRIDES. Supported knobs: " + "${RTPS_LIMIT_KNOB_NAMES}") + endif() + if(_rtps_knob IN_LIST RTPS_LIMIT_UINT16_KNOBS AND NOT RTPS_LIMITS_PROFILE STREQUAL "embedded") + set(_rtps_max 65535) + set(_rtps_type "uint16_t") + else() + set(_rtps_max 255) + set(_rtps_type "uint8_t") + endif() + # Builtin-aware minima: the discovery endpoints draw from these pools, so a + # technically-representable value below the reservation would crash startup + # (the second SEDP allocation returns null and createBuiltinWritersAndReaders + # dereferences it) or silently omit the builtins (per-participant caps < 3). + if(_rtps_knob STREQUAL "NUM_STATEFUL_WRITERS" OR _rtps_knob STREQUAL "NUM_STATEFUL_READERS") + set(_rtps_min 2) # 2 SEDP builtins + elseif(_rtps_knob STREQUAL "NUM_WRITERS_PER_PARTICIPANT" + OR _rtps_knob STREQUAL "NUM_READERS_PER_PARTICIPANT") + set(_rtps_min 3) # SPDP + 2 SEDP builtins + elseif(_rtps_knob STREQUAL "MAX_NUM_UDP_CONNECTIONS") + set(_rtps_min 4) # 2 shared multicast + 2 unicast channels for 1 participant + else() + set(_rtps_min 1) + endif() + if(_rtps_value LESS ${_rtps_min} OR _rtps_value GREATER ${_rtps_max}) + message(FATAL_ERROR + "RTPS limit override '${override}' out of range: ${_rtps_knob} is ${_rtps_type} under the " + "'${RTPS_LIMITS_PROFILE}' profile, valid range ${_rtps_min}..${_rtps_max} (minima account " + "for the builtin discovery endpoints)") + endif() + add_compile_definitions("RTPS_CFG_${override}") + list(APPEND RTPS_LIMIT_OVERRIDE_DEFS "RTPS_CFG_${override}") + set(RTPS_EFFECTIVE_${_rtps_knob} ${_rtps_value}) + message(STATUS "RTPS limit override: ${override}") +endforeach() + +# Cross-limit capacity check: the endpoint pools are GLOBAL, and EVERY +# participant consumes 1 stateless writer/reader + 2 stateful writers/readers +# for its builtin discovery endpoints - per-knob minima alone cannot see a +# combination like NUM_STATEFUL_WRITERS=2 with MAX_NUM_PARTICIPANTS=8, whose +# second participant would exhaust the pool (the engine now fails that +# participant cleanly, but a documented override should not configure a +# participant budget it cannot deliver). Effective value = override if given, +# else the selected profile's default. +if(RTPS_LIMITS_PROFILE STREQUAL "embedded") + set(_rtps_d_participants 1) + set(_rtps_d_stateless_w 5) + set(_rtps_d_stateless_r 5) + set(_rtps_d_stateful_w 5) + set(_rtps_d_stateful_r 5) + set(_rtps_d_channels 10) +elseif(RTPS_LIMITS_PROFILE STREQUAL "host") + set(_rtps_d_participants 8) + set(_rtps_d_stateless_w 16) + set(_rtps_d_stateless_r 16) + set(_rtps_d_stateful_w 32) + set(_rtps_d_stateful_r 32) + set(_rtps_d_channels 24) +else() # host_large + set(_rtps_d_participants 32) + set(_rtps_d_stateless_w 64) + set(_rtps_d_stateless_r 64) + set(_rtps_d_stateful_w 128) + set(_rtps_d_stateful_r 128) + set(_rtps_d_channels 72) +endif() +foreach(pair + "MAX_NUM_PARTICIPANTS;_rtps_d_participants" + "NUM_STATELESS_WRITERS;_rtps_d_stateless_w" + "NUM_STATELESS_READERS;_rtps_d_stateless_r" + "NUM_STATEFUL_WRITERS;_rtps_d_stateful_w" + "NUM_STATEFUL_READERS;_rtps_d_stateful_r" + "MAX_NUM_UDP_CONNECTIONS;_rtps_d_channels") + list(GET pair 0 _rtps_k) + list(GET pair 1 _rtps_dvar) + if(NOT DEFINED RTPS_EFFECTIVE_${_rtps_k}) + set(RTPS_EFFECTIVE_${_rtps_k} ${${_rtps_dvar}}) + endif() +endforeach() +math(EXPR _rtps_need_stateful "2 * ${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS}") +if(RTPS_EFFECTIVE_NUM_STATELESS_WRITERS LESS RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS + OR RTPS_EFFECTIVE_NUM_STATELESS_READERS LESS RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS + OR RTPS_EFFECTIVE_NUM_STATEFUL_WRITERS LESS _rtps_need_stateful + OR RTPS_EFFECTIVE_NUM_STATEFUL_READERS LESS _rtps_need_stateful) + message(FATAL_ERROR + "RTPS limits cannot host the participant budget: MAX_NUM_PARTICIPANTS=" + "${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS} needs >= ${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS} " + "stateless writers/readers (have ${RTPS_EFFECTIVE_NUM_STATELESS_WRITERS}/" + "${RTPS_EFFECTIVE_NUM_STATELESS_READERS}) and >= ${_rtps_need_stateful} stateful " + "writers/readers (have ${RTPS_EFFECTIVE_NUM_STATEFUL_WRITERS}/" + "${RTPS_EFFECTIVE_NUM_STATEFUL_READERS}) for the builtin discovery endpoints. Raise the " + "pool overrides or lower MAX_NUM_PARTICIPANTS.") +endif() +# Channel-pool constraint: a Domain permanently binds 2 shared multicast +# channels (SPDP metatraffic + user multicast) and 2 unicast channels per +# participant (builtin + user), all drawn from the same +# MAX_NUM_UDP_CONNECTIONS transport pool as runtime dedicated endpoint ports. +# Without this check an override combination can pass the endpoint-pool math +# above yet createParticipant() still fails on channels before reaching the +# advertised participant capacity. +math(EXPR _rtps_need_channels "2 + 2 * ${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS}") +if(RTPS_EFFECTIVE_MAX_NUM_UDP_CONNECTIONS LESS _rtps_need_channels) + message(FATAL_ERROR + "RTPS limits cannot host the participant budget: MAX_NUM_PARTICIPANTS=" + "${RTPS_EFFECTIVE_MAX_NUM_PARTICIPANTS} needs >= ${_rtps_need_channels} transport channels " + "(2 shared multicast + 2 unicast per participant) but MAX_NUM_UDP_CONNECTIONS is " + "${RTPS_EFFECTIVE_MAX_NUM_UDP_CONNECTIONS}. Raise MAX_NUM_UDP_CONNECTIONS (leave headroom " + "for dedicated endpoint ports - max_prioritized_endpoint_ports, default 4 - which draw " + "from the same pool at runtime) or lower MAX_NUM_PARTICIPANTS.") +endif() + # --------------------------------------------------------------------------- # RTPS best-effort DATA_FRAG fragmentation (Slice C). # @@ -64,7 +216,11 @@ message(STATUS "RTPS fragmentation: ON (max sample size ${RTPS_MAX_SAMPLE_SIZE} set(ESPP_RTPS_COMPILE_DEFINITIONS RTPS_CONFIG_HEADER="${RTPS_CONFIG_HEADER_FILE}" RTPS_ENABLE_FRAGMENTATION - RTPS_MAX_SAMPLE_SIZE=${RTPS_MAX_SAMPLE_SIZE}) + RTPS_MAX_SAMPLE_SIZE=${RTPS_MAX_SAMPLE_SIZE} + # Per-limit overrides are ABI-critical for the same reason as the profile + # header: they resize public Config constants and the array-backed pools, so + # exported consumers must see identical values (empty when no overrides). + ${RTPS_LIMIT_OVERRIDE_DEFS}) set(ESPP_EXTERNAL_INCLUDES ${ESPP_COMPONENTS}/serialization/detail/alpaca/include diff --git a/lib/python_bindings/rtps_bindings.cpp b/lib/python_bindings/rtps_bindings.cpp index fd64d2b0bb..644e33cd4e 100644 --- a/lib/python_bindings/rtps_bindings.cpp +++ b/lib/python_bindings/rtps_bindings.cpp @@ -129,6 +129,10 @@ struct PyRtpsConfig { py::function on_publisher_matched{}; py::function on_subscriber_matched{}; espp::Logger::Verbosity log_level{espp::Logger::Verbosity::WARN}; + espp::QosBand metatraffic_band{espp::QosBand::High}; + espp::QosBand user_traffic_band{espp::QosBand::Normal}; + bool enable_dedicated_endpoint_ports{true}; + uint8_t max_prioritized_endpoint_ports{4}; }; Rtps::Config to_config(const PyRtpsConfig &pc) { @@ -137,6 +141,10 @@ Rtps::Config to_config(const PyRtpsConfig &pc) { .on_publisher_matched = wrap_matched_callback(pc.on_publisher_matched), .on_subscriber_matched = wrap_matched_callback(pc.on_subscriber_matched), .log_level = pc.log_level, + .metatraffic_band = pc.metatraffic_band, + .user_traffic_band = pc.user_traffic_band, + .enable_dedicated_endpoint_ports = pc.enable_dedicated_endpoint_ports, + .max_prioritized_endpoint_ports = pc.max_prioritized_endpoint_ports, }; } @@ -162,22 +170,39 @@ void py_init_rtps(py::module &m) { py::class_(rtps, "Config") .def(py::init([](std::string interface_address, const py::object &on_publisher_matched, - const py::object &on_subscriber_matched, espp::Logger::Verbosity log_level) { + const py::object &on_subscriber_matched, espp::Logger::Verbosity log_level, + espp::QosBand metatraffic_band, espp::QosBand user_traffic_band, + bool enable_dedicated_endpoint_ports, + uint8_t max_prioritized_endpoint_ports) { PyRtpsConfig c; c.interface_address = std::move(interface_address); c.on_publisher_matched = as_function(on_publisher_matched); c.on_subscriber_matched = as_function(on_subscriber_matched); c.log_level = log_level; + c.metatraffic_band = metatraffic_band; + c.user_traffic_band = user_traffic_band; + c.enable_dedicated_endpoint_ports = enable_dedicated_endpoint_ports; + c.max_prioritized_endpoint_ports = max_prioritized_endpoint_ports; return c; }), py::arg("interface_address") = std::string{}, py::arg("on_publisher_matched") = py::none(), py::arg("on_subscriber_matched") = py::none(), - py::arg("log_level") = espp::Logger::Verbosity::WARN) + py::arg("log_level") = espp::Logger::Verbosity::WARN, + py::arg("metatraffic_band") = espp::QosBand::High, + py::arg("user_traffic_band") = espp::QosBand::Normal, + py::arg("enable_dedicated_endpoint_ports") = true, + py::arg("max_prioritized_endpoint_ports") = 4) .def_readwrite("interface_address", &PyRtpsConfig::interface_address) .def_readwrite("on_publisher_matched", &PyRtpsConfig::on_publisher_matched) .def_readwrite("on_subscriber_matched", &PyRtpsConfig::on_subscriber_matched) - .def_readwrite("log_level", &PyRtpsConfig::log_level); + .def_readwrite("log_level", &PyRtpsConfig::log_level) + .def_readwrite("metatraffic_band", &PyRtpsConfig::metatraffic_band) + .def_readwrite("user_traffic_band", &PyRtpsConfig::user_traffic_band) + .def_readwrite("enable_dedicated_endpoint_ports", + &PyRtpsConfig::enable_dedicated_endpoint_ports) + .def_readwrite("max_prioritized_endpoint_ports", + &PyRtpsConfig::max_prioritized_endpoint_ports); rtps.def(py::init([](const PyRtpsConfig &config) { return new Rtps(to_config(config)); }), py::arg("config") = PyRtpsConfig{}) @@ -188,18 +213,24 @@ void py_init_rtps(py::module &m) { .def("is_started", &Rtps::is_started) .def( "add_writer", - [](Rtps &self, const std::string &topic, const std::string &type_name, bool reliable) { + [](Rtps &self, const std::string &topic, const std::string &type_name, bool reliable, + espp::QosBand band, std::optional dscp) { return self.add_writer({.topic = topic, .type_name = type_name, .reliability = reliable ? Rtps::Reliability::RELIABLE - : Rtps::Reliability::BEST_EFFORT}); + : Rtps::Reliability::BEST_EFFORT, + .band = band, + .dscp = dscp}); }, py::arg("topic"), py::arg("type_name"), py::arg("reliable") = false, - py::call_guard(), "Add a publishing endpoint.") + py::arg("band") = espp::QosBand::Normal, py::arg("dscp") = py::none(), + py::call_guard(), + "Add a publishing endpoint. A non-Normal band (or a dscp) requests a dedicated,\n" + "band-scheduled (and DSCP-marked) unicast port for the writer (rationed).") .def( "add_reader", [](Rtps &self, const std::string &topic, const std::string &type_name, bool reliable, - const py::object &on_sample) { + const py::object &on_sample, espp::QosBand band, std::optional dscp) { // wrap under the GIL (we hold it here), then release for the engine call auto cb = wrap_sample_callback(as_function(on_sample)); py::gil_scoped_release release; @@ -207,11 +238,16 @@ void py_init_rtps(py::module &m) { .type_name = type_name, .reliability = reliable ? Rtps::Reliability::RELIABLE : Rtps::Reliability::BEST_EFFORT, - .on_sample = std::move(cb)}); + .on_sample = std::move(cb), + .band = band, + .dscp = dscp}); }, py::arg("topic"), py::arg("type_name"), py::arg("reliable") = false, - py::arg("on_sample") = py::none(), - "Add a subscribing endpoint; on_sample receives each sample as bytes.") + py::arg("on_sample") = py::none(), py::arg("band") = espp::QosBand::Normal, + py::arg("dscp") = py::none(), + "Add a subscribing endpoint; on_sample receives each sample as bytes. A non-Normal\n" + "band (or a dscp) requests a dedicated receive port (banded readers fall back to\n" + "deferred banded dispatch when no dedicated port is available).") .def( "publish", [](Rtps &self, const std::string &topic, const py::bytes &data) { @@ -279,20 +315,23 @@ void py_init_rtps(py::module &m) { rtps.def( "add_service_server", [](Rtps &self, const std::string &service, const std::string &type_name, - const py::function &handler) { + const py::function &handler, espp::QosBand band, std::optional dscp) { auto h = wrap_service_handler(handler); py::gil_scoped_release rel; - return self.add_service_server({service, type_name}, std::move(h)); + return self.add_service_server({service, type_name, band, dscp}, std::move(h)); }, py::arg("service"), py::arg("type_name"), py::arg("handler"), + py::arg("band") = espp::QosBand::Normal, py::arg("dscp") = py::none(), "Add a ROS 2 service server; handler(request_bytes) -> reply_bytes.") .def( "add_service_client", - [](Rtps &self, const std::string &service, const std::string &type_name) { + [](Rtps &self, const std::string &service, const std::string &type_name, + espp::QosBand band, std::optional dscp) { py::gil_scoped_release rel; - return self.add_service_client({service, type_name}); + return self.add_service_client({service, type_name, band, dscp}); }, - py::arg("service"), py::arg("type_name"), "Add a ROS 2 service client."); + py::arg("service"), py::arg("type_name"), py::arg("band") = espp::QosBand::Normal, + py::arg("dscp") = py::none(), "Add a ROS 2 service client."); // ---- Actions (AMI, ROS 2-interoperable) --------------------------------- py::class_( @@ -387,12 +426,13 @@ void py_init_rtps(py::module &m) { rtps.def( "add_action_server", [](Rtps &self, const std::string &action, const std::string &type_name, - const py::function &on_goal, const py::function &execute) { + const py::function &on_goal, const py::function &execute, espp::QosBand band, + std::optional dscp) { auto og = make_gil_safe_holder(on_goal); auto ex = make_gil_safe_holder(execute); py::gil_scoped_release rel; return self.add_action_server( - {action, type_name}, + {action, type_name, band, dscp}, [og](const Rtps::GoalId &, std::span goal) -> bool { py::gil_scoped_acquire gil; try { @@ -412,14 +452,17 @@ void py_init_rtps(py::module &m) { }); }, py::arg("action"), py::arg("type_name"), py::arg("on_goal"), py::arg("execute"), + py::arg("band") = espp::QosBand::Normal, py::arg("dscp") = py::none(), "Add a ROS 2 action server. on_goal(goal_bytes)->bool; execute(ActionGoalHandle)."); rtps.def( "add_action_client", - [](Rtps &self, const std::string &action, const std::string &type_name) { + [](Rtps &self, const std::string &action, const std::string &type_name, espp::QosBand band, + std::optional dscp) { py::gil_scoped_release rel; - return self.add_action_client({action, type_name}); + return self.add_action_client({action, type_name, band, dscp}); }, - py::arg("action"), py::arg("type_name"), "Add a ROS 2 action client."); + py::arg("action"), py::arg("type_name"), py::arg("band") = espp::QosBand::Normal, + py::arg("dscp") = py::none(), "Add a ROS 2 action client."); // ---- Native (espp<->espp) services + actions ---------------------------- py::class_>( @@ -535,24 +578,27 @@ void py_init_rtps(py::module &m) { rtps.def( "add_native_service_server", [](Rtps &self, const std::string &service, const std::string &type_name, - const py::function &handler) { + const py::function &handler, espp::QosBand band, std::optional dscp) { auto h = wrap_service_handler(handler); py::gil_scoped_release rel; - return self.add_native_service_server({service, type_name}, std::move(h)); + return self.add_native_service_server({service, type_name, band, dscp}, std::move(h)); }, - py::arg("service"), py::arg("type_name"), py::arg("handler")) + py::arg("service"), py::arg("type_name"), py::arg("handler"), + py::arg("band") = espp::QosBand::Normal, py::arg("dscp") = py::none()) .def( "add_native_service_client", - [](Rtps &self, const std::string &service, const std::string &type_name) { + [](Rtps &self, const std::string &service, const std::string &type_name, + espp::QosBand band, std::optional dscp) { py::gil_scoped_release rel; - return self.add_native_service_client({service, type_name}); + return self.add_native_service_client({service, type_name, band, dscp}); }, - py::arg("service"), py::arg("type_name")) + py::arg("service"), py::arg("type_name"), py::arg("band") = espp::QosBand::Normal, + py::arg("dscp") = py::none()) .def( "add_native_action_server", [](Rtps &self, const std::string &action, const std::string &type_name, - const py::function &on_goal, const py::function &execute, - const py::object &on_cancel) { + const py::function &on_goal, const py::function &execute, const py::object &on_cancel, + espp::QosBand band, std::optional dscp) { auto og = make_gil_safe_holder(on_goal); auto ex = make_gil_safe_holder(execute); Rtps::native_cancel_callback_t oc = nullptr; @@ -570,7 +616,7 @@ void py_init_rtps(py::module &m) { } py::gil_scoped_release rel; return self.add_native_action_server( - {action, type_name}, + {action, type_name, band, dscp}, [og](std::span goal) -> bool { py::gil_scoped_acquire gil; try { @@ -591,14 +637,17 @@ void py_init_rtps(py::module &m) { std::move(oc)); }, py::arg("action"), py::arg("type_name"), py::arg("on_goal"), py::arg("execute"), - py::arg("on_cancel") = py::none()) + py::arg("on_cancel") = py::none(), py::arg("band") = espp::QosBand::Normal, + py::arg("dscp") = py::none()) .def( "add_native_action_client", - [](Rtps &self, const std::string &action, const std::string &type_name) { + [](Rtps &self, const std::string &action, const std::string &type_name, + espp::QosBand band, std::optional dscp) { py::gil_scoped_release rel; - return self.add_native_action_client({action, type_name}); + return self.add_native_action_client({action, type_name, band, dscp}); }, - py::arg("action"), py::arg("type_name")); + py::arg("action"), py::arg("type_name"), py::arg("band") = espp::QosBand::Normal, + py::arg("dscp") = py::none()); py::class_(rtps, "NativeGoalHandle", "Server-side handle to a running native goal.") diff --git a/lib/python_bindings/socket_reactor_bindings.cpp b/lib/python_bindings/socket_reactor_bindings.cpp index 5fed766b4a..59e6f5d378 100644 --- a/lib/python_bindings/socket_reactor_bindings.cpp +++ b/lib/python_bindings/socket_reactor_bindings.cpp @@ -97,8 +97,9 @@ void py_init_socket_reactor(py::module &m) { "Stop the loop and wait for in-flight handlers to finish.") .def("is_running", &SocketReactor::is_running) .def("num_registered", &SocketReactor::num_registered) - .def("remove", &SocketReactor::remove, py::arg("id"), - "Unregister a socket by the id returned from add_udp_receiver().") + .def("remove", + static_cast(&SocketReactor::remove), + py::arg("id"), "Unregister a socket by the id returned from add_udp_receiver().") .def( "add_udp_receiver", [](SocketReactor &self, espp::UdpSocket &socket, std::size_t port, diff --git a/pc/tests/rtps_banded_churn.cpp b/pc/tests/rtps_banded_churn.cpp new file mode 100644 index 0000000000..03ee8a111f --- /dev/null +++ b/pc/tests/rtps_banded_churn.cpp @@ -0,0 +1,265 @@ +// Shutdown/teardown-under-load stress for per-endpoint priority: +// +// Phase 1 (churn): while a publisher floods a reliable topic, the subscriber +// domain repeatedly creates a banded (dedicated-port) reader, receives live +// traffic on it, and deletes it - exercising releaseReceivePort() with +// datagrams in flight on the released socket and immediate fd-number reuse by +// the next dedicated port. This is the reproducer for the CI shutdown hang: +// a stale select() readiness bit aliased onto a reused fd dispatched a +// handler with no data, whose unbounded blocking recvfrom wedged +// SocketReactor::stop() forever. +// +// Phase 2 (stop under load): a banded SHARED-port reader (dedicated ports +// disabled at the engine level is facade behavior; here ration cap 0) with +// deferred banded dispatch receives a flood, and the domains are stopped +// WHILE deliveries are in flight. +// +// The test must complete well under the external timeout the harness applies; +// any shutdown hang shows up as a timeout kill. +// +// Exits 0 on success. + +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps/entities/Domain.hpp" +#include "rtps_participant.hpp" + +#include "rtps_common.hpp" + +struct StringMsg { + std::string data; +}; + +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +using namespace std::chrono_literals; + +int main() { + constexpr int kChurnIterations = 25; + const char *topic = "churn_topic"; + const char *type = "std_msgs::msg::dds_::String_"; + + std::string ip; + rtps::Ip4AddressBytes ip_bytes{}; + // Portable interface discovery (rtps_common.hpp builds on POSIX and MSVC); + // the loopback fallback means no usable interface was found. + ip = rtps_test::guess_local_ipv4(); + unsigned ip_a = 0, ip_b = 0, ip_c = 0, ip_d = 0; + if (ip.rfind("127.", 0) == 0 || + std::sscanf(ip.c_str(), "%u.%u.%u.%u", &ip_a, &ip_b, &ip_c, &ip_d) != 4) { + std::printf("FAIL: no usable IPv4 interface\n"); + return 1; + } + ip_bytes = {static_cast(ip_a), static_cast(ip_b), static_cast(ip_c), + static_cast(ip_d)}; + + // ---- Phase 1: dedicated-port churn under flood -------------------------- + { + espp::RtpsParticipant pub( + {.interface_address = ip, .log_level = espp::Logger::Verbosity::WARN}); + if (!pub.start() || + !pub.add_writer({.topic = topic, + .type_name = type, + .reliability = espp::RtpsParticipant::Reliability::RELIABLE})) { + std::printf("FAIL: pub setup\n"); + return 1; + } + // Flood: publish continuously from a thread until told to stop. + std::atomic flood{true}; + std::thread flooder([&]() { + int i = 0; + while (flood.load()) { + auto bytes = cdr::serialize(StringMsg{"churn " + std::to_string(i++)}); + if (bytes) { + (void)pub.publish(topic, u8_span(*bytes)); + } + std::this_thread::sleep_for(2ms); + } + }); + + rtps::Domain sub_domain(ip_bytes); + rtps::Participant *part = sub_domain.createParticipant(); + if (part == nullptr) { + std::printf("FAIL: sub participant\n"); + flood = false; + flooder.join(); + return 1; + } + if (!sub_domain.completeInit()) { + std::printf("FAIL: sub completeInit\n"); + flood = false; + flooder.join(); + return 1; + } + + static std::atomic received{0}; + int churned = 0; + uint32_t first_dedicated_port = 0; // asserted bindable again after the churn + for (int iter = 0; iter < kChurnIterations; ++iter) { + rtps::Reader *reader = sub_domain.createReader(*part, topic, type, /*reliable=*/true, + {0, 0, 0, 0}, {.band = espp::QosBand::High}); + if (reader == nullptr) { + std::printf("FAIL: createReader iter %d\n", iter); + flood = false; + flooder.join(); + return 1; + } + if (!reader->m_attributes.hasDedicatedPort) { + std::printf("FAIL: no dedicated port at iter %d\n", iter); + flood = false; + flooder.join(); + return 1; + } + if (first_dedicated_port == 0) { + first_dedicated_port = reader->m_attributes.unicastLocator.port; + } + const int before = received.load(); + reader->registerCallback( + [](void *, const rtps::ReaderCacheChange &) { received.fetch_add(1); }, nullptr); + // Wait until live traffic flows over THIS dedicated port. This is a + // REQUIREMENT: an iteration that never sees a sample has not exercised + // the delete-with-traffic-in-flight race the test exists for, so it + // fails rather than silently churning idle sockets. The first iteration + // gets a longer deadline for SPDP/SEDP discovery and matching. + const auto deadline = std::chrono::steady_clock::now() + (iter == 0 ? 15s : 5s); + while (received.load() < before + 2 && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(5ms); + } + if (received.load() < before + 2) { + std::printf("FAIL: iter %d saw no live samples on its dedicated port (received %d)\n", iter, + received.load() - before); + flood = false; + flooder.join(); + return 1; + } + // Delete the reader (closing/releasing its dedicated port) WHILE the + // publisher is still sending to it - the next iteration's dedicated + // port immediately reuses the freed slot (and likely the fd number). + if (!sub_domain.deleteReader(*part, reader)) { + std::printf("FAIL: deleteReader iter %d\n", iter); + flood = false; + flooder.join(); + return 1; + } + ++churned; + } + + flood = false; + flooder.join(); + std::printf("phase1: churned %d dedicated-port readers, received %d samples\n", churned, + received.load()); + // Prompt fd/port release: the retired sockets must drain via the + // reactor's removal-completion callbacks (NOT accumulate until stop) - + // and the very first iteration's dedicated port must be bindable again + // by a fresh reuse-disabled socket, all BEFORE the domains stop. + { + const auto deadline = std::chrono::steady_clock::now() + 5s; + while (sub_domain.getTransport().retiredSocketCount() > 0 && + std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(20ms); + } + const std::size_t retired = sub_domain.getTransport().retiredSocketCount(); + if (retired != 0) { + std::printf("FAIL: %zu retired sockets still parked after churn\n", retired); + return 1; + } + bool rebindable = false; + const auto bind_deadline = std::chrono::steady_clock::now() + 2s; + while (!rebindable && std::chrono::steady_clock::now() < bind_deadline) { + espp::UdpSocket probe({.log_level = espp::Logger::Verbosity::NONE}); + espp::UdpSocket::ReceiveConfig rc; + rc.port = static_cast(first_dedicated_port); + rebindable = probe.is_valid() && probe.disable_reuse() && probe.bind(rc); + if (!rebindable) { + std::this_thread::sleep_for(20ms); + } + } + if (!rebindable) { + std::printf("FAIL: first dedicated port %u not released before stop\n", + static_cast(first_dedicated_port)); + return 1; + } + std::printf("phase1: retired sockets drained, port %u released before stop\n", + static_cast(first_dedicated_port)); + } + // Teardown with the peer still matched: sub_domain and pub stop here. A + // shutdown hang (the CI failure mode) trips the harness timeout. + sub_domain.stop(); + pub.stop(); + if (churned != kChurnIterations) { + std::printf("FAIL: churn incomplete\n"); + return 1; + } + } + + // ---- Phase 2: stop() while deferred deliveries are in flight ------------ + { + espp::RtpsParticipant pub( + {.interface_address = ip, .log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant sub({.interface_address = ip, + .log_level = espp::Logger::Verbosity::WARN, + .enable_dedicated_endpoint_ports = false}); + if (!pub.start() || !sub.start()) { + std::printf("FAIL: phase2 start\n"); + return 1; + } + if (!pub.add_writer({.topic = topic, + .type_name = type, + .reliability = espp::RtpsParticipant::Reliability::RELIABLE})) { + std::printf("FAIL: phase2 writer\n"); + return 1; + } + std::atomic received{0}; + // Banded shared-port reader -> deferred banded dispatch; the callback + // dawdles so deliveries are IN FLIGHT (and queued) when stop() runs. + if (!sub.add_reader({.topic = topic, + .type_name = type, + .reliability = espp::RtpsParticipant::Reliability::RELIABLE, + .on_sample = + [&received](std::span) { + received.fetch_add(1); + std::this_thread::sleep_for(20ms); + }, + .band = espp::QosBand::High})) { + std::printf("FAIL: phase2 reader\n"); + return 1; + } + std::atomic flood{true}; + std::thread flooder([&]() { + int i = 0; + while (flood.load()) { + auto bytes = cdr::serialize(StringMsg{"stop-load " + std::to_string(i++)}); + if (bytes) { + (void)pub.publish(topic, u8_span(*bytes)); + } + std::this_thread::sleep_for(2ms); + } + }); + // Wait for the pipeline to be visibly active, then stop UNDER load. + const auto deadline = std::chrono::steady_clock::now() + 10s; + while (received.load() < 3 && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(5ms); + } + const int seen = received.load(); + sub.stop(); // deliveries in flight + queued in the deferred dispatcher + flood = false; + flooder.join(); + pub.stop(); + std::printf("phase2: received %d before stop-under-load\n", seen); + if (seen < 3) { + std::printf("FAIL: phase2 no traffic before stop\n"); + return 1; + } + } + + std::printf("PASS\n"); + return 0; +} diff --git a/pc/tests/rtps_banded_deferred.cpp b/pc/tests/rtps_banded_deferred.cpp new file mode 100644 index 0000000000..82d6832473 --- /dev/null +++ b/pc/tests/rtps_banded_deferred.cpp @@ -0,0 +1,258 @@ +// Shared-port deferred banded dispatch: a banded reader that gets NO dedicated +// port (dedicated ports disabled on the subscriber) must still receive every +// sample, in order, with its callback re-submitted to the transport pool at the +// reader's band instead of running inline on the receive worker. +// +// Phase 1 (deterministic, unit-level): proves the CORE guarantee the loopback +// below cannot - that a shared-port banded delivery is dispatched through the +// pool AT ITS BAND. Two DeferredDispatch instances (Low + High) enqueue a +// delivery each while both transport workers are blocked, so both drain jobs +// sit in the pool queue together; a single freed worker must then service the +// High drain BEFORE the Low drain (band-priority pop). If the drain were +// resubmitted at Normal (the regression this guards), the two jobs would be +// FIFO-ordered and Low (enqueued first) would run first - failing the test. +// +// Phase 2 (loopback): the publisher sends kTotal sequence-numbered samples +// (reliable); the test requires all of them, strictly in order, at the +// subscriber (delivery + per-reader ordering through the real wiring). +// +// Exits 0 on success. + +#include +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps/communication/EsppTransport.hpp" +#include "rtps_participant.hpp" + +struct SeqMsg { + uint32_t seq; +}; + +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +using namespace std::chrono_literals; + +namespace { +// Expose the protected DeferredDispatch type for unit testing. +struct TestParticipant : espp::RtpsParticipant { + using espp::RtpsParticipant::DeferredDispatch; +}; +using DeferredDispatch = TestParticipant::DeferredDispatch; + +void noop_rx(void *, const uint8_t *, std::size_t, rtps::Ip4Port_t, rtps::Ip4Port_t, + const rtps::Ip4AddressBytes &) {} + +// Returns 0 on success, 1 on failure. +int run_band_queue_jump_test() { + rtps::EsppTransport transport(&noop_rx, nullptr); + auto owner = std::make_shared(0); + + auto low = std::make_shared(); + low->enabled = true; + low->band = espp::QosBand::Low; + low->transport = &transport; + auto high = std::make_shared(); + high->enabled = true; + high->band = espp::QosBand::High; + high->transport = &transport; + + // Block BOTH workers with independent release flags so we can later free + // exactly ONE and have it service both queued drains in band order. + std::atomic latched{0}; + std::atomic release_a{false}; + std::atomic release_b{false}; + const auto block = [&latched](std::atomic &rel) { + latched.fetch_add(1); + while (!rel.load()) { + std::this_thread::sleep_for(1ms); + } + }; + if (!transport.submit([&] { block(release_a); }) || + !transport.submit([&] { block(release_b); })) { + std::printf("FAIL: could not block the transport workers\n"); + return 1; + } + const auto latch_deadline = std::chrono::steady_clock::now() + 5s; + while (latched.load() < 2 && std::chrono::steady_clock::now() < latch_deadline) { + std::this_thread::sleep_for(1ms); + } + if (latched.load() < 2) { + std::printf("FAIL: workers never picked up the blockers\n"); + release_a = release_b = true; + return 1; + } + + // Enqueue Low FIRST, then High: both drain jobs are now queued at their + // bands while the workers are busy. + std::mutex order_mutex; + std::vector order; + low->run_or_defer( + [&] { + std::lock_guard lock(order_mutex); + order.push_back('L'); + }, + owner); + high->run_or_defer( + [&] { + std::lock_guard lock(order_mutex); + order.push_back('H'); + }, + owner); + + auto wait_for_count = [&](std::size_t n) { + const auto deadline = std::chrono::steady_clock::now() + 10s; + while (std::chrono::steady_clock::now() < deadline) { + { + std::lock_guard lock(order_mutex); + if (order.size() >= n) { + return true; + } + } + std::this_thread::sleep_for(2ms); + } + return false; + }; + + // Free ONE worker while both drains are queued: the queue-jump property is + // that it services the higher-priority High drain FIRST, so the very first + // delivery must be 'H' even though Low was enqueued first. (Two-phase, one + // delivery per freed worker, so the assertion never depends on a single + // worker draining both jobs within a timing window.) + release_b = true; + if (!wait_for_count(1)) { + std::printf("FAIL: no banded drain ran after freeing the first worker\n"); + release_a = true; + low->close(); + high->close(); + transport.stop(); + return 1; + } + { + std::lock_guard lock(order_mutex); + if (order[0] != 'H') { + std::printf("FAIL: High-band delivery did not overtake queued Low (first=%c)\n", order[0]); + release_a = true; + low->close(); + high->close(); + transport.stop(); + return 1; + } + } + + // Free the second worker: the remaining Low drain now runs. + release_a = true; + const bool both = wait_for_count(2); + low->close(); + high->close(); + transport.stop(); + + if (!both) { + std::printf("FAIL: the queued Low drain never ran\n"); + return 1; + } + // order[0] == 'H' is already established above; the queued Low must follow. + std::lock_guard lock(order_mutex); + if (order[1] != 'L') { + std::printf("FAIL: unexpected second delivery (%c)\n", order[1]); + return 1; + } + std::printf("queue-jump: High banded delivery overtook queued Low - PASS\n"); + return 0; +} +} // namespace + +int main() { + if (run_band_queue_jump_test() != 0) { + return 1; + } + + constexpr uint32_t kTotal = 30; // < the 32-entry deferred queue bound + constexpr auto kDeadline = 30s; + const char *topic = "deferred_loopback"; + const char *type = "espp::test::dds_::Seq_"; + using Reliability = espp::RtpsParticipant::Reliability; + + espp::RtpsParticipant pub({.log_level = espp::Logger::Verbosity::INFO}); + // Subscriber: dedicated ports DISABLED, so the banded reader must fall back + // to deferred banded dispatch on the shared user-unicast port. + espp::RtpsParticipant sub( + {.log_level = espp::Logger::Verbosity::INFO, .enable_dedicated_endpoint_ports = false}); + if (!pub.start() || !sub.start()) { + std::printf("FAIL: start\n"); + return 1; + } + if (!pub.add_writer({.topic = topic, .type_name = type, .reliability = Reliability::RELIABLE})) { + std::printf("FAIL: add_writer\n"); + return 1; + } + + std::mutex order_mutex; + std::vector order; + std::atomic received{0}; + if (!sub.add_reader({.topic = topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = + [&](std::span payload) { + auto msg = cdr::deserialize(std::as_bytes(payload)); + if (!msg) { + return; + } + std::lock_guard lock(order_mutex); + order.push_back(msg->seq); + received.fetch_add(1); + }, + .band = espp::QosBand::High})) { + std::printf("FAIL: add_reader\n"); + return 1; + } + + // Wait for discovery/matching, then send the numbered sequence. Reliable + // writers retransmit on the wire, but a sample must reach the reader at least + // once for the deferred queue to see it - send each one until acknowledged by + // observation (simple paced resend of the not-yet-seen head). + uint32_t next_to_send = 0; + const auto start = std::chrono::steady_clock::now(); + while (received.load() < kTotal && std::chrono::steady_clock::now() - start < kDeadline) { + if (next_to_send < kTotal) { + auto bytes = cdr::serialize(SeqMsg{next_to_send}); + if (bytes && pub.publish(topic, u8_span(*bytes))) { + next_to_send++; + std::this_thread::sleep_for(20ms); + continue; + } + } + std::this_thread::sleep_for(50ms); + } + + const uint32_t n = received.load(); + std::printf("sent=%u received=%u\n", next_to_send, n); + pub.stop(); + sub.stop(); + + if (n < kTotal) { + std::printf("FAIL: incomplete delivery\n"); + return 1; + } + // Ordering: per-reader order must be preserved by the single in-flight + // deferred drain - the recorded sequence must be exactly 0..kTotal-1. + { + std::lock_guard lock(order_mutex); + for (uint32_t i = 0; i < kTotal; ++i) { + if (order[i] != i) { + std::printf("FAIL: out-of-order delivery at index %u: got %u\n", i, order[i]); + return 1; + } + } + } + std::printf("PASS\n"); + return 0; +} diff --git a/pc/tests/rtps_banded_pubsub.cpp b/pc/tests/rtps_banded_pubsub.cpp new file mode 100644 index 0000000000..2e43426f4a --- /dev/null +++ b/pc/tests/rtps_banded_pubsub.cpp @@ -0,0 +1,149 @@ +// In-process loopback proving a banded subscriber on a DEDICATED unicast port +// interoperates end-to-end: +// +// publisher: espp::RtpsParticipant facade, plain reliable writer (Normal). +// subscriber: engine-level rtps::Domain so the test can see the reader's +// attributes - the reader is created at QosBand::High and must be granted a +// dedicated port (asserted, incl. the documented port range). +// +// The subscriber's SEDP announcement carries ONLY its per-endpoint unicast +// locator (the dedicated port) - the publisher's writer sends DATA exclusively +// to that announced locator (ReaderProxy::remoteLocator) - so end-to-end sample +// delivery proves the traffic flowed through the dedicated socket, not the +// shared user-unicast port. +// +// Exits 0 when at least kRequired samples arrive within the deadline. + +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps/entities/Domain.hpp" +#include "rtps/utils/udpUtils.hpp" +#include "rtps_participant.hpp" + +#include "rtps_common.hpp" + +struct StringMsg { + std::string data; +}; + +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +// First non-loopback, non-link-local IPv4 interface (same rule the facade's +// auto-detection uses); both sides must share it so their locators match. + +using namespace std::chrono_literals; + +int main() { + constexpr int kRequired = 5; + constexpr auto kDeadline = 20s; + const char *topic = "banded_loopback"; + const char *type = "std_msgs::msg::dds_::String_"; + + std::string ip; + rtps::Ip4AddressBytes ip_bytes{}; + // Portable interface discovery (rtps_common.hpp builds on POSIX and MSVC); + // the loopback fallback means no usable interface was found. + ip = rtps_test::guess_local_ipv4(); + unsigned ip_a = 0, ip_b = 0, ip_c = 0, ip_d = 0; + if (ip.rfind("127.", 0) == 0 || + std::sscanf(ip.c_str(), "%u.%u.%u.%u", &ip_a, &ip_b, &ip_c, &ip_d) != 4) { + std::printf("FAIL: no usable IPv4 interface\n"); + return 1; + } + ip_bytes = {static_cast(ip_a), static_cast(ip_b), static_cast(ip_c), + static_cast(ip_d)}; + + // Publisher: facade, default config, plain reliable writer. + espp::RtpsParticipant pub({.interface_address = ip, .log_level = espp::Logger::Verbosity::INFO}); + if (!pub.start()) { + std::printf("FAIL: pub start\n"); + return 1; + } + if (!pub.add_writer({.topic = topic, + .type_name = type, + .reliability = espp::RtpsParticipant::Reliability::RELIABLE})) { + std::printf("FAIL: add_writer\n"); + return 1; + } + + // Subscriber: engine-level domain so the dedicated port is observable. + rtps::Domain sub_domain(ip_bytes); + rtps::Participant *part = sub_domain.createParticipant(); + if (part == nullptr) { + std::printf("FAIL: sub createParticipant\n"); + return 1; + } + rtps::Reader *reader = sub_domain.createReader(*part, topic, type, /*reliable=*/true, + {0, 0, 0, 0}, {.band = espp::QosBand::High}); + if (reader == nullptr) { + std::printf("FAIL: sub createReader\n"); + return 1; + } + if (!reader->m_attributes.hasDedicatedPort) { + std::printf("FAIL: banded reader was not granted a dedicated port\n"); + return 1; + } + const auto dedicated_port = reader->m_attributes.unicastLocator.port; + const rtps::Ip4Port_t dedicated_base = 7400 + 250 * rtps::Config::DOMAIN_ID + 100; + if (dedicated_port < dedicated_base || + dedicated_port > static_cast(7400 + 250 * rtps::Config::DOMAIN_ID + 249)) { + std::printf("FAIL: dedicated port %u outside the documented range\n", + static_cast(dedicated_port)); + return 1; + } + if (dedicated_port == rtps::getUserUnicastPort(part->m_participantId)) { + std::printf("FAIL: dedicated port equals the shared user port\n"); + return 1; + } + std::printf("subscriber dedicated port: %u (shared would be %u)\n", + static_cast(dedicated_port), + static_cast(rtps::getUserUnicastPort(part->m_participantId))); + + static std::atomic received{0}; + reader->registerCallback( + [](void *, const rtps::ReaderCacheChange &change) { + std::vector payload(change.getDataSize()); + if (payload.empty() || !change.copyInto(payload.data(), change.getDataSize())) { + return; + } + if (cdr::deserialize( + std::as_bytes(std::span(payload.data(), payload.size())))) { + received.fetch_add(1); + } + }, + nullptr); + + if (!sub_domain.completeInit()) { + std::printf("FAIL: sub completeInit\n"); + return 1; + } + + int sent = 0; + const auto start = std::chrono::steady_clock::now(); + while (received.load() < kRequired && std::chrono::steady_clock::now() - start < kDeadline) { + auto bytes = cdr::serialize(StringMsg{"banded sample " + std::to_string(sent)}); + if (bytes && pub.publish(topic, u8_span(*bytes))) { + sent++; + } + std::this_thread::sleep_for(100ms); + } + + const int n = received.load(); + std::printf("sent=%d received=%d (via dedicated port %u)\n", sent, n, + static_cast(dedicated_port)); + pub.stop(); + sub_domain.stop(); + if (n >= kRequired) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +} diff --git a/pc/tests/rtps_banded_ration.cpp b/pc/tests/rtps_banded_ration.cpp new file mode 100644 index 0000000000..0dedfb61da --- /dev/null +++ b/pc/tests/rtps_banded_ration.cpp @@ -0,0 +1,105 @@ +// Ration exhaustion end-to-end: the subscriber caps dedicated endpoint ports at +// 1 (max_prioritized_endpoint_ports=1) but registers TWO banded (High) readers. +// The first gets the dedicated port; the second exceeds the ration, logs a +// warning, and falls back to the shared port with deferred banded dispatch. +// Both must still receive every published sample. +// +// Exits 0 when both readers receive at least kRequired samples in time. + +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps_participant.hpp" + +struct StringMsg { + std::string data; +}; + +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +using namespace std::chrono_literals; + +int main() { + constexpr int kRequired = 5; + constexpr auto kDeadline = 30s; + const char *topic_a = "ration_topic_a"; + const char *topic_b = "ration_topic_b"; + const char *type = "std_msgs::msg::dds_::String_"; + using Reliability = espp::RtpsParticipant::Reliability; + + espp::RtpsParticipant pub({.log_level = espp::Logger::Verbosity::INFO}); + espp::RtpsParticipant sub( + {.log_level = espp::Logger::Verbosity::INFO, .max_prioritized_endpoint_ports = 1}); + if (!pub.start() || !sub.start()) { + std::printf("FAIL: start\n"); + return 1; + } + if (!pub.add_writer( + {.topic = topic_a, .type_name = type, .reliability = Reliability::RELIABLE}) || + !pub.add_writer( + {.topic = topic_b, .type_name = type, .reliability = Reliability::RELIABLE})) { + std::printf("FAIL: add_writer\n"); + return 1; + } + + std::atomic received_a{0}; + std::atomic received_b{0}; + const auto count_into = [](std::atomic &counter) { + return [&counter](std::span payload) { + if (cdr::deserialize(std::as_bytes(payload))) { + counter.fetch_add(1); + } + }; + }; + // Reader A takes the single dedicated port; reader B exhausts the ration and + // must fall back (warning logged) to shared-port deferred dispatch. + if (!sub.add_reader({.topic = topic_a, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = count_into(received_a), + .band = espp::QosBand::High})) { + std::printf("FAIL: add_reader a\n"); + return 1; + } + if (!sub.add_reader({.topic = topic_b, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = count_into(received_b), + .band = espp::QosBand::High})) { + std::printf("FAIL: add_reader b\n"); + return 1; + } + + int sent = 0; + const auto start = std::chrono::steady_clock::now(); + while ((received_a.load() < kRequired || received_b.load() < kRequired) && + std::chrono::steady_clock::now() - start < kDeadline) { + auto bytes = cdr::serialize(StringMsg{"ration sample " + std::to_string(sent)}); + if (bytes) { + const bool a = pub.publish(topic_a, u8_span(*bytes)); + const bool b = pub.publish(topic_b, u8_span(*bytes)); + if (a && b) { + sent++; + } + } + std::this_thread::sleep_for(100ms); + } + + const int a = received_a.load(); + const int b = received_b.load(); + std::printf("sent=%d received_a=%d received_b=%d\n", sent, a, b); + pub.stop(); + sub.stop(); + if (a >= kRequired && b >= kRequired) { + std::printf("PASS\n"); + return 0; + } + std::printf("FAIL\n"); + return 1; +} diff --git a/pc/tests/rtps_callback_reentrancy.cpp b/pc/tests/rtps_callback_reentrancy.cpp new file mode 100644 index 0000000000..d130ec51e4 --- /dev/null +++ b/pc/tests/rtps_callback_reentrancy.cpp @@ -0,0 +1,88 @@ +// Reentrant cross-callback removal: while a reader dispatch is invoking its +// callback snapshot, callback A may legally call removeCallback(B) and then +// free B's argument - the removal-completion guarantee must hold even though +// the drain excludes A's own dispatch (the caller). The dispatch loop must +// therefore revalidate each registration against the LIVE table immediately +// before invoking it, or it calls the stale snapshot entry with a freed arg. +// +// Engine-level test (the facade registers a single callback per reader, so +// A-removes-B is only reachable through the engine API). Exits 0 on success. + +#include +#include + +#include "rtps/entities/StatelessReader.hpp" + +namespace { +struct TestState { + rtps::StatelessReader *reader{nullptr}; + rtps::Reader::callbackIdentifier_t b_id{0}; + bool b_arg_freed{false}; // set by A after removing B (simulates freeing) + bool b_ran_after_free{false}; + bool a_ran{false}; + bool b_ran{false}; +}; + +void callback_a(void *arg, const rtps::ReaderCacheChange &) { + auto *st = static_cast(arg); + st->a_ran = true; + // A removes B and "frees" B's argument - the documented removal-completion + // guarantee says B must not run after this returns. + st->reader->removeCallback(st->b_id); + st->b_arg_freed = true; +} + +void callback_b(void *arg, const rtps::ReaderCacheChange &) { + auto *st = static_cast(arg); + st->b_ran = true; + if (st->b_arg_freed) { + st->b_ran_after_free = true; // use-after-free in a real application + } +} +} // namespace + +int main() { + rtps::StatelessReader reader; + rtps::TopicData attributes{}; + std::strncpy(attributes.topicName, "reentrancy", sizeof(attributes.topicName) - 1); + std::strncpy(attributes.typeName, "test", sizeof(attributes.typeName) - 1); + if (!reader.init(attributes)) { + std::printf("FAIL: reader init\n"); + return 1; + } + + TestState st; + st.reader = &reader; + // Registration order matters: A must be invoked BEFORE B in the dispatch + // loop so its removal targets a not-yet-invoked snapshot entry. + const auto a_id = reader.registerCallback(&callback_a, &st); + st.b_id = reader.registerCallback(&callback_b, &st); + if (a_id == 0 || st.b_id == 0) { + std::printf("FAIL: registerCallback\n"); + return 1; + } + + const uint8_t payload[4] = {1, 2, 3, 4}; + rtps::Guid_t writer_guid{}; + rtps::ReaderCacheChange change{rtps::ChangeKind_t::ALIVE, writer_guid, + rtps::SequenceNumber_t{0, 1}, payload, sizeof(payload)}; + reader.newChangeIfCurrent(reader.generation(), change); + + if (!st.a_ran) { + std::printf("FAIL: callback A never ran\n"); + return 1; + } + if (st.b_ran_after_free) { + std::printf("FAIL: callback B invoked AFTER removeCallback(B) returned and its arg was " + "freed (stale snapshot entry)\n"); + return 1; + } + if (st.b_ran) { + // B ran before its removal completed - impossible here (A precedes B and + // removes it synchronously), so treat as a harness error. + std::printf("FAIL: unexpected ordering (B ran before A's removal)\n"); + return 1; + } + std::printf("PASS\n"); + return 0; +} diff --git a/pc/tests/rtps_deferred_recovery.cpp b/pc/tests/rtps_deferred_recovery.cpp new file mode 100644 index 0000000000..c6d1e34f38 --- /dev/null +++ b/pc/tests/rtps_deferred_recovery.cpp @@ -0,0 +1,116 @@ +// Deferred-dispatch arm recovery: when the transport pool REJECTS the drain +// arm (workers busy + bounded queue full), a queued - possibly lone/last - +// delivery must still be delivered without any further traffic: the dispatcher +// flags the failed arm and its retry timer re-arms the drain once the pool has +// capacity again. Before the fix, the delivery stayed queued forever (a +// reliable reader had already acked the sample, so nothing would ever +// retransmit it). +// +// Unit-level and fully deterministic: both transport workers are blocked on a +// latch, the bounded queue is filled until submit() rejects, ONE delivery is +// enqueued (arm rejected), the workers are released, and the delivery must +// arrive with NO further run_or_defer() calls. +// +// Exits 0 on success. + +#include +#include +#include +#include +#include + +#include "rtps/communication/EsppTransport.hpp" +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +// Expose the protected DeferredDispatch type for unit testing. +struct TestParticipant : espp::RtpsParticipant { + using espp::RtpsParticipant::DeferredDispatch; +}; +using DeferredDispatch = TestParticipant::DeferredDispatch; + +void noop_rx(void *, const uint8_t *, std::size_t, rtps::Ip4Port_t, rtps::Ip4Port_t, + const rtps::Ip4AddressBytes &) {} +} // namespace + +int main() { + rtps::EsppTransport transport(&noop_rx, nullptr); + + // The owning context stand-in: drain jobs and the retry timer capture it. + auto owner = std::make_shared(0); + auto dispatch = std::make_shared(); + dispatch->enabled = true; + dispatch->band = espp::QosBand::High; + dispatch->transport = &transport; + + // Saturate the pool: block both workers, then fill the bounded queue until + // submissions are rejected. CRITICAL for determinism: wait until BOTH + // blockers are actually RUNNING before filling. A blocker still sitting in + // the queue when the fill completes means a late-waking worker will pop the + // HIGH-band drain job ahead of the Normal-band fillers (band-priority pop), + // running the delivery while the test believes the pool is saturated - the + // observed flake this guard eliminates. + std::atomic release{false}; + std::atomic latched{0}; + const auto blocker = [&release, &latched]() { + latched.fetch_add(1); + while (!release.load()) { + std::this_thread::sleep_for(1ms); + } + }; + if (!transport.submit(blocker) || !transport.submit(blocker)) { + std::printf("FAIL: could not block the transport workers\n"); + return 1; + } + const auto latch_deadline = std::chrono::steady_clock::now() + 5s; + while (latched.load() < 2 && std::chrono::steady_clock::now() < latch_deadline) { + std::this_thread::sleep_for(1ms); + } + if (latched.load() < 2) { + std::printf("FAIL: workers never picked up the blockers\n"); + release = true; + return 1; + } + int fillers = 0; + while (transport.submit([]() {}) && fillers < 100000) { + ++fillers; + } + if (fillers >= 100000) { + std::printf("FAIL: transport queue never rejected (unbounded?)\n"); + release = true; + return 1; + } + std::printf("queue saturated after %d filler jobs\n", fillers); + + // Enqueue ONE delivery: the drain arm must be rejected right now. + std::atomic delivered{0}; + dispatch->run_or_defer([&delivered]() { delivered.fetch_add(1); }, owner); + std::this_thread::sleep_for(100ms); + if (delivered.load() != 0) { + std::printf("FAIL: delivery ran while the pool was saturated?\n"); + release = true; + return 1; + } + + // Release the workers. NO further traffic: only the retry timer can re-arm + // the drain - the queued lone delivery must arrive. + release = true; + const auto deadline = std::chrono::steady_clock::now() + 5s; + while (delivered.load() == 0 && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(10ms); + } + const int n = delivered.load(); + + // Lifetime discipline: quiesce before dropping references / stopping. + dispatch->close(); + transport.stop(); + + if (n != 1) { + std::printf("FAIL: lone queued delivery never recovered (delivered=%d)\n", n); + return 1; + } + std::printf("PASS\n"); + return 0; +} diff --git a/pc/tests/rtps_guaranteed_fairness.cpp b/pc/tests/rtps_guaranteed_fairness.cpp new file mode 100644 index 0000000000..2172b9ab85 --- /dev/null +++ b/pc/tests/rtps_guaranteed_fairness.cpp @@ -0,0 +1,172 @@ +// Fairness / eventual-run for the guaranteed-submission retry path +// (EsppTransport::submitGuaranteed): under SUSTAINED overload, every parked +// producer must eventually be admitted to the pool - regardless of its band and +// of how many higher-band producers are continuously owed. This is the +// regression pair that broke twice during review: +// - drain-one-key-to-exhaustion starved every later key (pointer order), and +// - band-sorted admission starved lower bands forever when the higher bands +// could consume every freed slot each retry tick. +// The admission stage must be band-agnostic round-robin; band priority belongs +// to the pool's banded queues AFTER admission. +// +// Deterministic shape (mirrors rtps_guaranteed_submit): both transport workers +// are latched, the bounded queue is filled with SLOW jobs, then owed runs are +// parked for 32 High keys (100 each) + 2 Normal keys (10 each) + 1 Low key (1). +// Releasing the latch creates sustained overload: workers free only ~8 slots +// per 20 ms retry tick while the High backlog (3200 slow jobs, ~6+ s of work) +// refills them. Band-sorted admission would keep the Low key saturated for the +// whole backlog (deterministically past the deadline); round-robin admission +// must admit it within one rotor revolution (< 1 s). +// +// Once the Low job has run, the remaining jobs switch to no-ops (fast drain) +// and the test verifies the lossless contract: every owed run for every key +// executed exactly once. +// +// Exits 0 on success. + +#include +#include +#include +#include +#include + +#include "rtps/communication/EsppTransport.hpp" + +using namespace std::chrono_literals; + +namespace { +void noop_rx(void *, const uint8_t *, std::size_t, rtps::Ip4Port_t, rtps::Ip4Port_t, + const rtps::Ip4AddressBytes &) {} +} // namespace + +int main() { + rtps::EsppTransport transport(&noop_rx, nullptr); + + // Latch both workers so the queue can be saturated deterministically. + std::atomic release{false}; + std::atomic latched{0}; + const auto blocker = [&release, &latched]() { + latched.fetch_add(1); + while (!release.load()) { + std::this_thread::sleep_for(1ms); + } + }; + if (!transport.submit(blocker) || !transport.submit(blocker)) { + std::printf("FAIL: could not block the transport workers\n"); + return 1; + } + const auto latch_deadline = std::chrono::steady_clock::now() + 5s; + while (latched.load() < 2 && std::chrono::steady_clock::now() < latch_deadline) { + std::this_thread::sleep_for(1ms); + } + if (latched.load() < 2) { + std::printf("FAIL: workers never picked up the blockers\n"); + release = true; + return 1; + } + + // While the Low producer is still starved, every job is SLOW (sleep) so the + // workers can only free a handful of queue slots per retry tick - sustained + // overload. After the Low job has run the fairness property is proven and + // the remaining backlog switches to no-ops so the test finishes quickly. + std::atomic fast_drain{false}; + const auto slow_work = [&fast_drain]() { + if (!fast_drain.load()) { + std::this_thread::sleep_for(4ms); + } + }; + + // Fill the bounded queue with SLOW jobs until submit() rejects. + int fillers = 0; + while (transport.submit(slow_work) && fillers < 100000) { + ++fillers; + } + if (fillers >= 100000) { + std::printf("FAIL: transport queue never rejected (unbounded?)\n"); + release = true; + return 1; + } + std::printf("queue saturated after %d slow filler jobs\n", fillers); + + // Park the competing producers (all rejected -> coalesced owed counts). + // Distinct key identities come from distinct array elements. + constexpr int kHighKeys = 32; + constexpr int kHighOwedEach = 100; // 3200 slow jobs ~= 6.4 s of backlog at 2 workers + constexpr int kNormalKeys = 2; + constexpr int kNormalOwedEach = 10; + static std::array keys{}; + + std::atomic high_ran{0}; + std::atomic normal_ran{0}; + std::atomic low_ran{false}; + + for (int k = 0; k < kHighKeys; ++k) { + for (int i = 0; i < kHighOwedEach; ++i) { + transport.submitGuaranteed( + &keys[k], + [&high_ran, &slow_work]() { + slow_work(); + high_ran.fetch_add(1); + }, + espp::QosBand::High); + } + } + for (int k = 0; k < kNormalKeys; ++k) { + for (int i = 0; i < kNormalOwedEach; ++i) { + transport.submitGuaranteed( + &keys[kHighKeys + k], + [&normal_ran, &slow_work]() { + slow_work(); + normal_ran.fetch_add(1); + }, + espp::QosBand::Normal); + } + } + transport.submitGuaranteed( + &keys[kHighKeys + kNormalKeys], [&low_ran]() { low_ran.store(true); }, espp::QosBand::Low); + + // Release the workers: sustained overload begins. The single Low owed run + // must be admitted while the High backlog is still deep. + const auto start = std::chrono::steady_clock::now(); + release = true; + const auto fairness_deadline = start + 5s; + while (!low_ran.load() && std::chrono::steady_clock::now() < fairness_deadline) { + std::this_thread::sleep_for(5ms); + } + if (!low_ran.load()) { + std::printf("FAIL: Low-band producer starved (high_ran=%d of %d while Low never admitted)\n", + high_ran.load(), kHighKeys * kHighOwedEach); + fast_drain = true; + transport.stop(); + return 1; + } + const auto low_ms = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start) + .count(); + std::printf("Low admitted after %lld ms (high backlog remaining: %d)\n", + static_cast(low_ms), kHighKeys * kHighOwedEach - high_ran.load()); + + // Lossless: with the fairness property proven, drain the remaining backlog + // fast and require every owed run to have executed exactly once. + fast_drain = true; + const int high_expected = kHighKeys * kHighOwedEach; + const int normal_expected = kNormalKeys * kNormalOwedEach; + const auto drain_deadline = std::chrono::steady_clock::now() + 20s; + while ((high_ran.load() < high_expected || normal_ran.load() < normal_expected) && + std::chrono::steady_clock::now() < drain_deadline) { + std::this_thread::sleep_for(10ms); + } + const int h = high_ran.load(); + const int n = normal_ran.load(); + + transport.stop(); + + if (h != high_expected || n != normal_expected) { + std::printf("FAIL: owed runs lost (high=%d/%d, normal=%d/%d)\n", h, high_expected, n, + normal_expected); + return 1; + } + std::printf("PASS (Low admitted under sustained High overload; all %d owed runs executed)\n", + high_expected + normal_expected + 1); + return 0; +} diff --git a/pc/tests/rtps_guaranteed_submit.cpp b/pc/tests/rtps_guaranteed_submit.cpp new file mode 100644 index 0000000000..e54bbcb15b --- /dev/null +++ b/pc/tests/rtps_guaranteed_submit.cpp @@ -0,0 +1,106 @@ +// Guaranteed transport submission (EsppTransport::submitGuaranteed): a writer's +// progress() poke MUST eventually run even when the bounded pool queue is full. +// A best-effort DATA has no heartbeat/acknack recovery path, so a silently +// rejected progress submission would strand the sample unsent forever (the +// regression this guards). On rejection the job is parked and a retry timer +// re-submits it once the pool has capacity - with NO further submissions. +// +// Unit-level and deterministic (mirrors rtps_deferred_recovery): both transport +// workers are blocked on a latch, the bounded queue is filled until submit() +// rejects, ONE guaranteed job is submitted (parked), the workers are released, +// and the job must run via the retry timer alone. +// +// Exits 0 on success. + +#include +#include +#include +#include + +#include "rtps/communication/EsppTransport.hpp" + +using namespace std::chrono_literals; + +namespace { +void noop_rx(void *, const uint8_t *, std::size_t, rtps::Ip4Port_t, rtps::Ip4Port_t, + const rtps::Ip4AddressBytes &) {} +} // namespace + +int main() { + rtps::EsppTransport transport(&noop_rx, nullptr); + + // Block both workers; wait until BOTH are actually running before filling the + // queue (a blocker still queued when the fill completes would let a + // late-waking worker service the guaranteed job early - see the same guard in + // rtps_deferred_recovery). + std::atomic release{false}; + std::atomic latched{0}; + const auto blocker = [&release, &latched]() { + latched.fetch_add(1); + while (!release.load()) { + std::this_thread::sleep_for(1ms); + } + }; + if (!transport.submit(blocker) || !transport.submit(blocker)) { + std::printf("FAIL: could not block the transport workers\n"); + return 1; + } + const auto latch_deadline = std::chrono::steady_clock::now() + 5s; + while (latched.load() < 2 && std::chrono::steady_clock::now() < latch_deadline) { + std::this_thread::sleep_for(1ms); + } + if (latched.load() < 2) { + std::printf("FAIL: workers never picked up the blockers\n"); + release = true; + return 1; + } + + // Fill the bounded queue until a plain submit() is rejected. + int fillers = 0; + while (transport.submit([]() {}) && fillers < 100000) { + ++fillers; + } + if (fillers >= 100000) { + std::printf("FAIL: transport queue never rejected (unbounded?)\n"); + release = true; + return 1; + } + std::printf("queue saturated after %d filler jobs\n", fillers); + + // Submit the SAME producer's guaranteed poke kExpected times while the pool + // is saturated: each is rejected and parked. This is the lossless-coalescing + // path - the pokes coalesce into one map entry with an owed count of + // kExpected, and NONE may be dropped (each parked poke maps one-to-one to a + // progress() run / one sample). + constexpr int kExpected = 300; // > any old fixed cap; proves nothing is dropped + std::atomic ran{0}; + const int producer = 0; // arbitrary producer identity (a real caller passes its writer) + for (int i = 0; i < kExpected; ++i) { + transport.submitGuaranteed( + &producer, [&ran]() { ran.fetch_add(1); }, espp::QosBand::High); + } + std::this_thread::sleep_for(100ms); + if (ran.load() != 0) { + std::printf("FAIL: guaranteed job ran while the pool was saturated?\n"); + release = true; + return 1; + } + + // Release the workers. NO further submissions: only the retry timer can get + // the parked pokes into the pool once the fillers drain. + release = true; + const auto deadline = std::chrono::steady_clock::now() + 10s; + while (ran.load() < kExpected && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(10ms); + } + const int n = ran.load(); + + transport.stop(); // cancels the retry timer, quiesces + + if (n != kExpected) { + std::printf("FAIL: parked guaranteed pokes lost (ran=%d, expected=%d)\n", n, kExpected); + return 1; + } + std::printf("PASS (all %d parked pokes recovered, none dropped)\n", kExpected); + return 0; +} diff --git a/pc/tests/rtps_interop_sub.cpp b/pc/tests/rtps_interop_sub.cpp index e5723e25d9..aee4af1eda 100644 --- a/pc/tests/rtps_interop_sub.cpp +++ b/pc/tests/rtps_interop_sub.cpp @@ -6,10 +6,15 @@ // std_msgs/String on /chatter. // // Usage: rtps_interop_sub [topic] [type] [reliable(0|1)] [required] [timeout_s] -// [interface_ip] [payload_bytes] +// [interface_ip] [payload_bytes] [band] // When payload_bytes > 0, each received String is verified byte-exact against the // deterministic payload_bytes-long pattern (proving fragmented >64 KB samples are // reassembled correctly); only byte-exact receptions count toward `required`. +// band (0=Critical 1=High 2=Normal 3=Low; default 2=Normal): the reader's +// espp::QosBand. A non-Normal band gives the reader a DEDICATED unicast port +// announced via its SEDP per-endpoint unicast locator - the interop matrix uses +// this to prove a FastDDS/ROS 2 peer honors that locator and delivers the +// topic's traffic to the dedicated port. // Exits 0 once `required` samples arrive within `timeout_s`. #include @@ -55,6 +60,15 @@ int main(int argc, char **argv) { const int timeout_s = (argc > 5) ? std::atoi(argv[5]) : 30; const char *interface_ip = (argc > 6) ? argv[6] : ""; // "" -> auto-detect const std::size_t payload_bytes = (argc > 7) ? std::strtoul(argv[7], nullptr, 10) : 0; + const int band_arg = (argc > 8) ? std::atoi(argv[8]) : static_cast(espp::QosBand::Normal); + // Validate before casting: an out-of-range value would index band arrays. + if (band_arg < static_cast(espp::QosBand::Critical) || + band_arg > static_cast(espp::QosBand::Low)) { + std::printf("FAIL: band must be 0..%d (0=Critical 1=High 2=Normal 3=Low), got %d\n", + static_cast(espp::QosBand::Low), band_arg); + return 1; + } + const auto band = static_cast(band_arg); const std::string expected = payload_bytes > 0 ? make_pattern(payload_bytes) : std::string{}; std::atomic received{0}; @@ -96,12 +110,13 @@ int main(int argc, char **argv) { std::fflush(stdout); } }, + .band = band, })) { std::printf("FAIL: add_reader\n"); return 1; } - std::printf("interop_sub: topic=%s type=%s reliable=%d required=%d timeout=%ds\n", topic, type, - reliable ? 1 : 0, required, timeout_s); + std::printf("interop_sub: topic=%s type=%s reliable=%d required=%d timeout=%ds band=%d\n", topic, + type, reliable ? 1 : 0, required, timeout_s, band_arg); const auto start = std::chrono::steady_clock::now(); while (received.load() < required && diff --git a/pc/tests/rtps_native_service_loopback.cpp b/pc/tests/rtps_native_service_loopback.cpp index 2b6e7849c6..07cfec5e9b 100644 --- a/pc/tests/rtps_native_service_loopback.cpp +++ b/pc/tests/rtps_native_service_loopback.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -80,22 +81,31 @@ int main() { // Async. bool async_ok = false; { - std::mutex m; - std::condition_variable cv; - bool done = false; - int64_t got = 0; - call->call_async(request(1000, 337), [&](std::span r) { + // The callback state is SHARED and captured by value: call_async retains + // the callback, so if the wait below times out a late reply may still + // invoke it - stack-captured references would then be use-after-scope. + // The shared_ptr keeps the state alive as long as the callback exists, and + // notifying under the lock orders any cv destruction after the notify. + struct AsyncState { + std::mutex m; + std::condition_variable cv; + bool done = false; + int64_t got = 0; + }; + auto st = std::make_shared(); + call->call_async(request(1000, 337), [st](std::span r) { + std::lock_guard lk(st->m); if (r.size() >= 12) { - std::lock_guard lk(m); - got = get_i64(r, 4); - done = true; + st->got = get_i64(r, 4); + st->done = true; } - cv.notify_one(); + st->cv.notify_one(); }); - std::unique_lock lk(m); - if (cv.wait_for(lk, 10s, [&] { return done; })) - async_ok = (got == 1337); - std::printf("native async: 1000+337 => %lld %s\n", (long long)got, async_ok ? "ok" : "FAIL"); + std::unique_lock lk(st->m); + if (st->cv.wait_for(lk, 10s, [&] { return st->done; })) + async_ok = (st->got == 1337); + std::printf("native async: 1000+337 => %lld %s\n", (long long)st->got, + async_ok ? "ok" : "FAIL"); } // Future. bool future_ok = false; diff --git a/pc/tests/rtps_remove_reader_deadlock.cpp b/pc/tests/rtps_remove_reader_deadlock.cpp new file mode 100644 index 0000000000..98485839b5 --- /dev/null +++ b/pc/tests/rtps_remove_reader_deadlock.cpp @@ -0,0 +1,208 @@ +// Regression: remove_reader() must not hold the facade mutex_ while quiescing +// the deferred dispatcher. close() waits for the in-flight delivery, and that +// user callback may legally call back into the participant (e.g. publish(), +// which takes mutex_). If remove_reader held mutex_ across close(), the +// callback's publish() would block on mutex_ while close() waits for the +// callback - a deadlock. +// +// This reproduces it deterministically: a banded shared-port reader (deferred +// dispatch) whose on_sample publishes to another topic is removed WHILE its +// callback is executing. remove_reader() must return promptly. +// +// Exits 0 on success, 1 on failure/deadlock. + +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps_participant.hpp" + +using namespace std::chrono_literals; + +namespace { +// Expose the protected remove_reader() for this unit test. +struct TestParticipant : espp::RtpsParticipant { + using espp::RtpsParticipant::remove_reader; + using espp::RtpsParticipant::RtpsParticipant; +}; + +struct SeqMsg { + uint32_t seq; +}; + +std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} +} // namespace + +int main() { + using Reliability = espp::RtpsParticipant::Reliability; + const char *type = "espp::test::dds_::Seq_"; + const char *topic_a = "deadlock_in"; // banded reader here + const char *topic_b = "deadlock_out"; // the callback publishes here + + // Separate publisher (samples on topic_a) and subscriber. The subscriber is + // the participant under test: it owns the banded reader on topic_a and a + // writer on topic_b that the reader's callback publishes to. Dedicated ports + // disabled so the banded reader falls back to DEFERRED dispatch (whose + // close() waits for the in-flight delivery). + espp::RtpsParticipant pub({.log_level = espp::Logger::Verbosity::WARN}); + TestParticipant part( + {.log_level = espp::Logger::Verbosity::WARN, .enable_dedicated_endpoint_ports = false}); + if (!pub.start() || !part.start()) { + std::printf("FAIL: start\n"); + return 1; + } + if (!pub.add_writer( + {.topic = topic_a, .type_name = type, .reliability = Reliability::RELIABLE}) || + !part.add_writer( + {.topic = topic_b, .type_name = type, .reliability = Reliability::RELIABLE})) { + std::printf("FAIL: add_writer\n"); + return 1; + } + + std::atomic in_callback{false}; + std::atomic gate_open{false}; + std::atomic callback_published{false}; + if (!part.add_reader({.topic = topic_a, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = + [&](std::span) { + in_callback = true; + // stay in-flight until the remover has started + while (!gate_open.load()) { + std::this_thread::sleep_for(1ms); + } + // a supported callback action that takes mutex_ + auto bytes = cdr::serialize(SeqMsg{0}); + if (bytes) { + part.publish(topic_b, u8_span(*bytes)); + } + callback_published = true; + }, + .band = espp::QosBand::High})) { + std::printf("FAIL: add_reader\n"); + return 1; + } + + // Drive samples until the callback is executing (deferred delivery in-flight). + std::atomic stop_pub{false}; + std::thread pub_thread([&]() { + while (!stop_pub.load() && !in_callback.load()) { + auto bytes = cdr::serialize(SeqMsg{1}); + if (bytes) { + pub.publish(topic_a, u8_span(*bytes)); + } + std::this_thread::sleep_for(10ms); + } + }); + + const auto entered_deadline = std::chrono::steady_clock::now() + 10s; + while (!in_callback.load() && std::chrono::steady_clock::now() < entered_deadline) { + std::this_thread::sleep_for(2ms); + } + stop_pub = true; + pub_thread.join(); + if (!in_callback.load()) { + std::printf("FAIL: callback never entered (no delivery)\n"); + return 1; + } + + // Remove the reader while its callback is in-flight. On a separate thread so + // a deadlock is observable via the watchdog rather than hanging the test. + std::atomic removed{false}; + std::atomic removed_ok{false}; + std::thread remover([&]() { + // Capture the RESULT too: an implementation that bailed out early (never + // exercising the deletion/quiesce under test) would otherwise still pass + // the prompt-return assertion below. + removed_ok = part.remove_reader(topic_a); + removed = true; + }); + // Give remove_reader() time to reach close()'s in-flight wait, then let the + // callback proceed to its publish() (which needs mutex_). + std::this_thread::sleep_for(100ms); + gate_open = true; + + const auto deadline = std::chrono::steady_clock::now() + 5s; + while (!removed.load() && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(5ms); + } + const bool ok = removed.load(); + if (ok) { + remover.join(); + } + // (If deadlocked, the remover thread is stuck; detach so the process can + // report the failure rather than hang on join.) + else { + remover.detach(); + } + + if (!ok) { + std::printf("FAIL: remove_reader() deadlocked (held mutex_ across close())\n"); + return 1; + } + if (!removed_ok.load()) { + std::printf("FAIL: remove_reader() returned false (removal not exercised)\n"); + return 1; + } + if (!callback_published.load()) { + std::printf("FAIL: callback's publish() never completed\n"); + return 1; + } + // Scenario 2: a callback removing ITS OWN reader. The engine's teardown + // drain must recognize the caller's own in-flight dispatch (previously + // reentrant via the recursive callback mutex; an unconditional + // wait-for-zero would deadlock on the callback's own dispatch count). + const char *topic_c = "deadlock_self"; + std::atomic self_removed{false}; + std::atomic self_removed_ok{false}; + // Completion flag, set AFTER the result is stored: the main thread must not + // judge self_removed_ok while remove_reader() is still running (waiting on + // the entry flag alone raced the result store and could fail a good run). + std::atomic self_remove_done{false}; + if (!pub.add_writer( + {.topic = topic_c, .type_name = type, .reliability = Reliability::RELIABLE})) { + std::printf("FAIL: scenario-2 add_writer\n"); + return 1; + } + if (!part.add_reader({.topic = topic_c, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = [&](std::span) { + if (!self_removed.exchange(true)) { + self_removed_ok = part.remove_reader(topic_c); + self_remove_done = true; + } + }})) { + std::printf("FAIL: scenario-2 add_reader\n"); + return 1; + } + const auto self_deadline = std::chrono::steady_clock::now() + 10s; + while (!self_remove_done.load() && std::chrono::steady_clock::now() < self_deadline) { + auto bytes = cdr::serialize(SeqMsg{2}); + if (bytes) { + pub.publish(topic_c, u8_span(*bytes)); + } + std::this_thread::sleep_for(10ms); + } + // The watchdog is the harness timeout: a self-wait deadlock would hang the + // callback (and this loop's publisher would keep running) until the kill. + if (!self_remove_done.load()) { + std::printf("FAIL: scenario-2 callback never ran (or removal never completed)\n"); + return 1; + } + if (!self_removed_ok.load()) { + std::printf("FAIL: scenario-2 remove_reader() from own callback failed\n"); + return 1; + } + + part.stop(); + pub.stop(); + std::printf("PASS\n"); + return 0; +} diff --git a/pc/tests/rtps_sedp_dedicated_locator.cpp b/pc/tests/rtps_sedp_dedicated_locator.cpp new file mode 100644 index 0000000000..60e832584a --- /dev/null +++ b/pc/tests/rtps_sedp_dedicated_locator.cpp @@ -0,0 +1,344 @@ +// Engine-level checks for per-endpoint priority (dedicated unicast ports): +// +// 1. A banded endpoint (reader or writer) is granted a dedicated unicast port +// from the documented range (7400 + 250*domain + 100 + n) and its SEDP +// announcement carries that port in the standard PID_UNICAST_LOCATOR +// parameter - verified byte-for-byte against the parameter encoding, plus a +// round-trip parse. +// 2. A default (Normal, no dscp) endpoint keeps the shared user-unicast port +// and no dedicated flag - i.e. the pre-band behavior is unchanged. +// 3. The ration (DomainConfig::max_prioritized_endpoint_ports) is enforced: +// endpoints beyond the cap fall back to the shared port. +// 4. Deleting a dedicated-port endpoint returns its port to the ration. +// 5. With enable_dedicated_endpoint_ports=false no dedicated port is granted. +// 6. The ration is a TRUE fd bound: a released socket whose fd is still open +// (removal completion pending behind a busy worker pool) counts against +// the cap; once it closes, the slot is usable again. +// 7. A fully-occupied probe window advances the allocator: after a failed +// allocation the next attempt probes fresh ports and succeeds. +// +// Exits 0 on success, 1 on the first failed check. + +#include +#include +#include +#include +#include +#include +#include + +#include "rtps/entities/Domain.hpp" +#include "rtps/messages/MessageTypes.hpp" +#include "rtps/utils/udpUtils.hpp" + +namespace { + +constexpr rtps::Ip4AddressBytes kIp{127, 0, 0, 1}; + +#define CHECK(cond, msg) \ + do { \ + if (!(cond)) { \ + std::printf("FAIL: %s (line %d)\n", msg, __LINE__); \ + return false; \ + } \ + } while (0) + +std::vector serialize_attributes(const rtps::TopicData &attributes) { + std::vector buf(1024, 0); + rtps::CdrSink sink{rtps::asWritableBytes(buf.data(), buf.size())}; + rtps::CdrWriter writer(sink); + if (!attributes.serializeInto(writer)) { + return {}; + } + buf.resize(sink.size()); + return buf; +} + +// The expected PID_UNICAST_LOCATOR parameter for a UDPv4 locator on kIp:port - +// the exact bytes a peer parses: pid(2) len(2) kind(4) port(4) address(16), +// all little-endian, address IPv4-mapped in the last 4 bytes. +std::vector expected_unicast_locator_param(uint32_t port) { + std::vector p; + const auto push_u16 = [&p](uint16_t v) { + p.push_back(static_cast(v & 0xFF)); + p.push_back(static_cast(v >> 8)); + }; + const auto push_u32 = [&p](uint32_t v) { + for (int i = 0; i < 4; ++i) { + p.push_back(static_cast((v >> (8 * i)) & 0xFF)); + } + }; + push_u16(static_cast(rtps::SMElement::ParameterId::PID_UNICAST_LOCATOR)); + push_u16(sizeof(rtps::FullLengthLocator)); // 24 + push_u32(static_cast(rtps::LocatorKind_t::LOCATOR_KIND_UDPv4)); + push_u32(port); + for (int i = 0; i < 12; ++i) { + p.push_back(0); + } + p.insert(p.end(), kIp.begin(), kIp.end()); + return p; +} + +// Poll (up to ~2 s) until a fresh reuse-disabled socket can bind the port - +// i.e. the previously bound fd was actually closed, not parked until stop(). +bool wait_for_port_released(uint16_t port) { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2); + while (std::chrono::steady_clock::now() < deadline) { + espp::UdpSocket probe({.log_level = espp::Logger::Verbosity::NONE}); + espp::UdpSocket::ReceiveConfig rc; + rc.port = port; + if (probe.is_valid() && probe.disable_reuse() && probe.bind(rc)) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + return false; +} + +// Poll (up to ~2 s) until the transport reports no retired sockets pending. +bool wait_for_no_retired_sockets(rtps::EsppTransport &transport) { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2); + while (std::chrono::steady_clock::now() < deadline) { + if (transport.retiredSocketCount() == 0) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + return false; +} + +bool contains(std::span haystack, std::span needle) { + if (needle.empty() || haystack.size() < needle.size()) { + return false; + } + for (size_t i = 0; i + needle.size() <= haystack.size(); ++i) { + if (std::memcmp(haystack.data() + i, needle.data(), needle.size()) == 0) { + return true; + } + } + return false; +} + +bool run_checks() { + const rtps::Ip4Port_t dedicated_base = 7400 + 250 * rtps::Config::DOMAIN_ID + 100; + const rtps::Ip4Port_t dedicated_end = 7400 + 250 * rtps::Config::DOMAIN_ID + 249; + + { + rtps::DomainConfig cfg; + cfg.max_prioritized_endpoint_ports = 2; // small ration for check 3 + rtps::Domain domain(kIp, cfg); + rtps::Participant *part = domain.createParticipant(); + CHECK(part != nullptr, "createParticipant"); + const rtps::Ip4Port_t shared_port = rtps::getUserUnicastPort(part->m_participantId); + + // 1a. Banded reader -> dedicated port, announced via PID_UNICAST_LOCATOR. + rtps::Reader *banded_reader = + domain.createReader(*part, "prio_topic", "PrioType", /*reliable=*/true, {0, 0, 0, 0}, + {.band = espp::QosBand::High, .dscp = espp::Dscp::Ef}); + CHECK(banded_reader != nullptr, "banded reader created"); + CHECK(banded_reader->m_attributes.hasDedicatedPort, "banded reader has dedicated port"); + const auto reader_port = banded_reader->m_attributes.unicastLocator.port; + CHECK(reader_port >= dedicated_base && reader_port <= dedicated_end, + "reader port in the documented dedicated range"); + CHECK(reader_port != shared_port, "reader port differs from the shared user port"); + CHECK(banded_reader->m_attributes.band == espp::QosBand::High, "band recorded"); + + const auto sedp = serialize_attributes(banded_reader->m_attributes); + CHECK(!sedp.empty(), "SEDP serialization"); + const auto expected = expected_unicast_locator_param(reader_port); + CHECK(contains(sedp, expected), + "SEDP bytes contain the dedicated-port PID_UNICAST_LOCATOR parameter"); + // Round-trip: a peer parsing the announcement sees the dedicated port. + rtps::TopicData parsed; + CHECK(parsed.readFromBuffer(std::span(sedp.data(), sedp.size())), + "SEDP round-trip parse"); + CHECK(parsed.unicastLocator.port == reader_port, "parsed locator carries the dedicated port"); + + // 2. Default endpoint: shared port, no dedicated flag (pre-band behavior). + rtps::Reader *normal_reader = + domain.createReader(*part, "normal_topic", "NormalType", /*reliable=*/true); + CHECK(normal_reader != nullptr, "normal reader created"); + CHECK(!normal_reader->m_attributes.hasDedicatedPort, "normal reader has no dedicated port"); + CHECK(normal_reader->m_attributes.unicastLocator.port == shared_port, + "normal reader keeps the shared user port"); + const auto normal_sedp = serialize_attributes(normal_reader->m_attributes); + CHECK(contains(normal_sedp, expected_unicast_locator_param(shared_port)), + "normal reader announces the shared port"); + + // 1b. Banded writer -> its own dedicated port too. + rtps::Writer *banded_writer = + domain.createWriter(*part, "prio_out", "PrioType", /*reliable=*/true, + /*enforceUnicast=*/false, {.band = espp::QosBand::Critical}); + CHECK(banded_writer != nullptr, "banded writer created"); + CHECK(banded_writer->m_attributes.hasDedicatedPort, "banded writer has dedicated port"); + const auto writer_port = banded_writer->m_attributes.unicastLocator.port; + CHECK(writer_port >= dedicated_base && writer_port <= dedicated_end, + "writer port in the dedicated range"); + CHECK(writer_port != reader_port, "writer and reader ports are distinct"); + + // 3. Ration exhausted (cap 2, both used): fall back to the shared port. + const rtps::Reader *over_cap = + domain.createReader(*part, "over_cap", "PrioType", /*reliable=*/true, {0, 0, 0, 0}, + {.band = espp::QosBand::High}); + CHECK(over_cap != nullptr, "over-cap reader still created"); + CHECK(!over_cap->m_attributes.hasDedicatedPort, "over-cap reader fell back to shared port"); + CHECK(over_cap->m_attributes.unicastLocator.port == shared_port, + "over-cap reader announces the shared port"); + CHECK(over_cap->m_attributes.band == espp::QosBand::High, + "over-cap reader keeps its band (for deferred dispatch)"); + + // 4. Deleting a dedicated-port endpoint returns its port to the ration + // AND promptly releases the underlying fd/port (the retired socket is + // destroyed by the reactor's removal-completion callback, not held + // until stop()): binding a fresh reuse-disabled socket to the released + // port must succeed well before the domain stops. + CHECK(domain.deleteReader(*part, banded_reader), "delete banded reader"); + CHECK(wait_for_port_released(static_cast(reader_port)), + "released dedicated port is bindable again before stop()"); + CHECK(wait_for_no_retired_sockets(domain.getTransport()), + "no retired sockets accumulate after the release"); + const rtps::Reader *after_delete = + domain.createReader(*part, "after_delete", "PrioType", /*reliable=*/true, {0, 0, 0, 0}, + {.band = espp::QosBand::High}); + CHECK(after_delete != nullptr, "post-delete reader created"); + CHECK(after_delete->m_attributes.hasDedicatedPort, + "released port made room for a new dedicated port"); + } + + // 6. TRUE fd bound: a released-but-not-yet-closed socket (its reactor + // dispatch is queued behind a saturated worker pool, deferring the + // removal completion) counts against the ration. + { + rtps::DomainConfig cfg; + cfg.max_prioritized_endpoint_ports = 1; + rtps::Domain domain(kIp, cfg); + rtps::Participant *part = domain.createParticipant(); + CHECK(part != nullptr, "createParticipant (fd bound)"); + + // Block BOTH transport pool workers so a dispatch for the dedicated + // socket stays queued (in flight from the reactor's perspective) across + // the deleteReader() below - deferring the removal completion. + std::atomic release_workers{false}; + const auto blocker = [&release_workers]() { + while (!release_workers.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + }; + CHECK(domain.getTransport().submit(blocker), "block worker 1"); + CHECK(domain.getTransport().submit(blocker), "block worker 2"); + + // Create a banded reader, make its dedicated socket readable so the loop + // queues a dispatch, then delete it: the retired socket must stay parked + // (count 1) until the workers drain. Collection timing is the only + // nondeterminism, so retry with a fresh reader if the dispatch was not + // yet queued when the delete happened. + bool pending_retirement = false; + for (int attempt = 0; attempt < 5 && !pending_retirement; ++attempt) { + rtps::Reader *banded = + domain.createReader(*part, ("fdbound" + std::to_string(attempt)).c_str(), "PrioType", + /*reliable=*/true, {0, 0, 0, 0}, {.band = espp::QosBand::High}); + CHECK(banded != nullptr, "fd-bound reader created"); + CHECK(banded->m_attributes.hasDedicatedPort, "fd-bound reader has dedicated port"); + const auto port = banded->m_attributes.unicastLocator.port; + espp::UdpSocket sender({.log_level = espp::Logger::Verbosity::NONE}); + const std::vector junk{0x00, 0x01, 0x02, 0x03}; + CHECK(sender.send(junk, {.ip_address = "127.0.0.1", .port = port}), + "send datagram to dedicated port"); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + CHECK(domain.deleteReader(*part, banded), "delete fd-bound reader"); + pending_retirement = domain.getTransport().retiredSocketCount() > 0; + } + CHECK(pending_retirement, "a retirement stayed pending behind the blocked pool"); + + // Cap 1 and one retired-but-open fd: a new banded reader must NOT get a + // dedicated port - the cap is a bound on real fds, not registry entries. + const rtps::Reader *while_pending = + domain.createReader(*part, "while_pending", "PrioType", /*reliable=*/true, {0, 0, 0, 0}, + {.band = espp::QosBand::High}); + CHECK(while_pending != nullptr, "reader created while retirement pending"); + CHECK(!while_pending->m_attributes.hasDedicatedPort, + "retired-but-open fd counts against the ration"); + + // Drain the pool: the queued dispatch runs, the removal completes, the + // retired fd closes - and the ration slot becomes usable again. + release_workers = true; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3); + while (domain.getTransport().retiredSocketCount() > 0 && + std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + CHECK(domain.getTransport().retiredSocketCount() == 0, "retired socket closed after drain"); + const rtps::Reader *after_drain = + domain.createReader(*part, "after_drain", "PrioType", /*reliable=*/true, {0, 0, 0, 0}, + {.band = espp::QosBand::High}); + CHECK(after_drain != nullptr, "reader created after drain"); + CHECK(after_drain->m_attributes.hasDedicatedPort, "ration slot usable after the fd closed"); + } + + // 7. Probe-window advance: when the current window is fully occupied (here + // by external reuse-disabled sockets), the allocator must move PAST the + // failed window so a later allocation succeeds on fresh ports. + { + rtps::DomainConfig cfg; + rtps::Domain domain(kIp, cfg); + rtps::Participant *part = domain.createParticipant(); + CHECK(part != nullptr, "createParticipant (probe window)"); + + // Occupy the entire first probe window externally. + std::vector> squatters; + for (uint16_t i = 0; i < 16; ++i) { + auto sock = std::make_unique( + espp::UdpSocket::Config{.log_level = espp::Logger::Verbosity::NONE}); + espp::UdpSocket::ReceiveConfig rc; + rc.port = static_cast(dedicated_base + i); + CHECK(sock->is_valid() && sock->disable_reuse() && sock->bind(rc), "squatter bind"); + squatters.push_back(std::move(sock)); + } + + // First allocation: whole window occupied -> falls back to shared port... + const rtps::Reader *blocked = + domain.createReader(*part, "blocked_window", "PrioType", /*reliable=*/true, {0, 0, 0, 0}, + {.band = espp::QosBand::High}); + CHECK(blocked != nullptr, "reader created against occupied window"); + CHECK(!blocked->m_attributes.hasDedicatedPort, "occupied window falls back"); + + // ...but the allocator advanced past the window: the next allocation + // probes fresh ports and succeeds (squatters still bound). + const rtps::Reader *advanced = + domain.createReader(*part, "advanced_window", "PrioType", /*reliable=*/true, {0, 0, 0, 0}, + {.band = espp::QosBand::High}); + CHECK(advanced != nullptr, "reader created after window advance"); + CHECK(advanced->m_attributes.hasDedicatedPort, "allocator advanced past the failed window"); + CHECK(advanced->m_attributes.unicastLocator.port >= dedicated_base + 16, + "new port comes from beyond the occupied window"); + } + + // 5. Dedicated ports disabled: banded endpoints stay on the shared port. + { + rtps::DomainConfig cfg; + cfg.enable_dedicated_endpoint_ports = false; + rtps::Domain domain(kIp, cfg); + rtps::Participant *part = domain.createParticipant(); + CHECK(part != nullptr, "createParticipant (disabled)"); + const rtps::Reader *reader = + domain.createReader(*part, "prio_topic", "PrioType", /*reliable=*/true, {0, 0, 0, 0}, + {.band = espp::QosBand::Critical}); + CHECK(reader != nullptr, "reader created (disabled)"); + CHECK(!reader->m_attributes.hasDedicatedPort, "no dedicated port when disabled"); + CHECK(reader->m_attributes.unicastLocator.port == + rtps::getUserUnicastPort(part->m_participantId), + "shared port when disabled"); + } + + return true; +} + +} // namespace + +int main() { + if (!run_checks()) { + return 1; + } + std::printf("PASS\n"); + return 0; +} diff --git a/pc/tests/rtps_service_loopback.cpp b/pc/tests/rtps_service_loopback.cpp index 3402d8152d..0f55c902d0 100644 --- a/pc/tests/rtps_service_loopback.cpp +++ b/pc/tests/rtps_service_loopback.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -104,41 +105,57 @@ int main() { // Asynchronous call: a second request must correlate independently. bool async_ok = false; { - std::mutex m; - std::condition_variable cv; - bool done = false; - int64_t got = 0; + // The callback state is SHARED and captured by value: call_async retains + // the callback, so if the wait below times out a late reply may still + // invoke it - stack-captured references would then be use-after-scope. + // The shared_ptr keeps the state alive as long as the callback exists, and + // notifying under the lock orders any cv destruction after the notify. + struct AsyncState { + std::mutex m; + std::condition_variable cv; + bool done = false; + int64_t got = 0; + }; + auto st = std::make_shared(); const int64_t a2 = 1000, b2 = 337; - if (call->call_async(encode_request(a2, b2), [&](std::span rep) { + if (call->call_async(encode_request(a2, b2), [st](std::span rep) { + std::lock_guard lk(st->m); if (rep.size() >= 4 + 8) { - std::lock_guard lk(m); - got = get_i64(rep, 4); - done = true; + st->got = get_i64(rep, 4); + st->done = true; } - cv.notify_one(); + st->cv.notify_one(); })) { - std::unique_lock lk(m); - if (cv.wait_for(lk, 10s, [&] { return done; })) { - async_ok = (got == a2 + b2); + std::unique_lock lk(st->m); + if (st->cv.wait_for(lk, 10s, [&] { return st->done; })) { + async_ok = (st->got == a2 + b2); } - std::printf("async call: got %lld (expected %lld) => %s\n", (long long)got, + std::printf("async call: got %lld (expected %lld) => %s\n", (long long)st->got, (long long)(a2 + b2), async_ok ? "ok" : "MISMATCH/timeout"); } } // Deferred server: a separate service whose handler replies from another // thread after a delay (exercises add_service_server_deferred + ServiceResponder). + // The worker registry is SHARED and captured by value: the handler can fire + // arbitrarily late (e.g. a dispatch landing after the call below timed out), + // so stack-captured references would be use-after-scope; the shared_ptr keeps + // the registry alive as long as the handler exists, and the workers are + // joined after server.stop() below, once no handler can run anymore. + struct WorkerState { + std::mutex wm; + std::vector workers; + }; + auto wstate = std::make_shared(); bool deferred_ok = false; { const char *dsvc = "/add_two_ints_deferred"; - std::vector workers; - std::mutex wm; server.add_service_server_deferred( {dsvc, "example_interfaces::srv::dds_::AddTwoInts"}, - [&](std::span req, espp::RtpsParticipant::ServiceResponder responder) { + [wstate](std::span req, espp::RtpsParticipant::ServiceResponder responder) { std::vector r(req.begin(), req.end()); - std::lock_guard lk(wm); - workers.emplace_back([r, responder]() { + std::lock_guard lk(wstate->wm); + wstate->workers.emplace_back([r, responder]() { std::this_thread::sleep_for(300ms); // reply later, off the worker thread if (r.size() >= 4 + 16) { responder.reply(encode_response(get_i64(r, 4) + get_i64(r, 12))); @@ -153,13 +170,10 @@ int main() { deferred_ok = (get_i64(*dreply, 4) == 42); } std::printf("deferred call: 11 + 31 = %lld => %s\n", - reply.has_value() ? (long long)get_i64(*reply, 4) : -1, + (dreply.has_value() && dreply->size() >= 4 + 8) ? (long long)get_i64(*dreply, 4) + : -1, deferred_ok ? "ok" : "MISMATCH/timeout"); } - for (auto &t : workers) { - if (t.joinable()) - t.join(); - } } // Future-based call: a third request must correlate independently. @@ -183,6 +197,24 @@ int main() { server.stop(); client.stop(); + // After stop() the facade has quiesced the deferred dispatchers, so no + // handler can spawn another reply worker: one locked drain now joins every + // worker - including any spawned by a late dispatch after a call timeout - + // with no pass limit needed. (A worker's own late reply() is a safe no-op + // once the participant is stopped.) + { + std::vector to_join; + { + std::lock_guard lk(wstate->wm); + to_join.swap(wstate->workers); + } + for (auto &t : to_join) { + if (t.joinable()) { + t.join(); + } + } + } + if (ok && async_ok && future_ok && deferred_ok) { std::printf("PASS\n"); return 0; diff --git a/pc/tests/rtps_service_rollback.cpp b/pc/tests/rtps_service_rollback.cpp new file mode 100644 index 0000000000..626ee215d4 --- /dev/null +++ b/pc/tests/rtps_service_rollback.cpp @@ -0,0 +1,228 @@ +// Transactional service/action creation: a partial endpoint-creation failure +// must roll back the endpoints that DID build - nothing stays announced, no +// engine pool slot stays consumed, and no dedicated-port ration slot (or fd) +// leaks - and the rollback is PRECISE: only the endpoints created by the +// failing invocation are removed (a duplicate add must not delete the first +// instance's endpoints; a concurrent add must never be rolled back as +// collateral). +// +// Failure seam: a service name chosen so the request topic ("rq/Request") +// exceeds the engine's MAX_TOPICNAME_LENGTH while the reply topic +// ("rr/Reply", two characters shorter) fits. The ROS service server +// creates writer-then-reader, the client reader-then-writer, so the same name +// fails the SECOND endpoint of each pair, leaving one successful (banded, +// dedicated-port) endpoint to roll back. +// +// 1. Server partial failure: add_service_server() returns false AND the +// dedicated port its reply writer had claimed becomes externally bindable +// (fd + ration slot released; no leaked SEDP announcement). +// 2. Client partial failure: same, for the reply reader's dedicated port. +// 3. Action-server rollback: with the participant's writer budget reduced to +// exactly 3 free slots, add_action_server() (which needs 5 writers) fails +// partway; the rollback must return all 3 slots - proven by 3 subsequent +// add_writer() calls succeeding. +// 4. The participant stays fully usable: a valid banded service then builds. +// 5. Duplicate add_action_server(): the second call fails (duplicate topics) +// but the FIRST action's feedback/status writers keep working - precise +// rollback removes only what the failing call created. +// 6. Rollback under concurrent adds: writers added by another thread while +// failing composites run are never rolled back as collateral. +// +// Deletion-failure ordering (facade keeps its writer-map entry / reader +// context when Domain::deleteWriter/deleteReader fails) is not covered here: +// inducing an engine deletion failure requires corrupting engine-internal +// state (the pooled endpoint must vanish from the participant while the +// facade still holds it), which no public API can do cheaply - the ordering +// is enforced by construction in remove_writer()/remove_reader(). +// +// Exits 0 on success. + +#include +#include +#include +#include +#include +#include + +#include "rtps/config.hpp" +#include "rtps_participant.hpp" +#include "udp_socket.hpp" + +using namespace std::chrono_literals; + +namespace { + +constexpr uint16_t kDedicatedBase = 7500; // 7400 + 250*domain(0) + 100 + +bool port_becomes_bindable(uint16_t port) { + const auto deadline = std::chrono::steady_clock::now() + 2s; + while (std::chrono::steady_clock::now() < deadline) { + espp::UdpSocket probe({.log_level = espp::Logger::Verbosity::NONE}); + espp::UdpSocket::ReceiveConfig rc; + rc.port = port; + if (probe.is_valid() && probe.disable_reuse() && probe.bind(rc)) { + return true; + } + std::this_thread::sleep_for(20ms); + } + return false; +} + +#define CHECK(cond, msg) \ + do { \ + if (!(cond)) { \ + std::printf("FAIL: %s (line %d)\n", msg, __LINE__); \ + return 1; \ + } \ + } while (0) + +} // namespace + +int main() { + using Reliability = espp::RtpsParticipant::Reliability; + + espp::RtpsParticipant participant({.log_level = espp::Logger::Verbosity::WARN}); + CHECK(participant.start(), "participant start"); + + // Service name sized so "rq/Request" == MAX_TOPICNAME_LENGTH (rejected: + // the engine requires strlen < MAX) while "rr/Reply" fits. + const std::size_t name_len = rtps::Config::MAX_TOPICNAME_LENGTH - 10; // 3 + len + 7 == MAX + const std::string bad_service = "/" + std::string(name_len - 0, 'x'); + static_assert(sizeof("rq/" + "Request") - + 1 == + 10); + + // 1. Server: reply writer (banded -> dedicated port 7500) succeeds, request + // reader fails -> the writer must be rolled back and its port released. + CHECK(!participant.add_service_server( + {.service = bad_service, + .type_name = "test::srv::dds_::Bad", + .band = espp::QosBand::High}, + [](std::span) { return std::vector{}; }), + "server creation reports failure"); + CHECK(port_becomes_bindable(kDedicatedBase), + "server rollback released the reply writer's dedicated port"); + + // 2. Client: reply reader (banded -> next dedicated port 7501) succeeds, + // request writer fails -> the reader must be rolled back. + CHECK(participant.add_service_client({.service = bad_service, + .type_name = "test::srv::dds_::Bad", + .band = espp::QosBand::High}) == nullptr, + "client creation reports failure"); + CHECK(port_becomes_bindable(kDedicatedBase + 1), + "client rollback released the reply reader's dedicated port"); + + // 3. Action-server rollback: reduce the participant's writer budget to + // exactly 3 free slots, then build an action (needs 5 writers: feedback, + // status, and 3 service reply writers). It fails partway; the rollback + // must return every slot it consumed. The usable capacity is measured + // dynamically (builtin discovery writers occupy participant slots too). + int capacity = 0; + { + espp::RtpsParticipant probe({.log_level = espp::Logger::Verbosity::ERROR}); + CHECK(probe.start(), "capacity probe start"); + while (probe.add_writer({.topic = "cap_" + std::to_string(capacity), + .type_name = "test::msg::dds_::Fill_", + .reliability = Reliability::RELIABLE})) { + ++capacity; + } + probe.stop(); + } + CHECK(capacity >= 5, "enough writer capacity for the scenario"); + const int fill = capacity - 3; + for (int i = 0; i < fill; ++i) { + CHECK(participant.add_writer({.topic = "fill_" + std::to_string(i), + .type_name = "test::msg::dds_::Fill_", + .reliability = Reliability::RELIABLE}), + "budget fill writer"); + } + CHECK(!participant.add_action_server( + {.action = "/rollback_probe", .type_name = "test::action::dds_::Probe"}, nullptr, + [](espp::RtpsParticipant::ActionGoalHandle) {}), + "action server creation reports failure"); + // All 3 slots the failed action consumed must be free again. + for (int i = 0; i < 3; ++i) { + CHECK(participant.add_writer({.topic = "post_rollback_" + std::to_string(i), + .type_name = "test::msg::dds_::Fill_", + .reliability = Reliability::RELIABLE}), + "post-rollback writer slot available"); + } + // Budget now exhausted for real - a further writer must fail (sanity check + // that the 3 successes above actually re-used the rolled-back slots). + CHECK(!participant.add_writer({.topic = "over_budget", + .type_name = "test::msg::dds_::Fill_", + .reliability = Reliability::RELIABLE}), + "writer budget exhausted after refill"); + + participant.stop(); + + // 4. Fresh participant: a valid banded service builds normally (the failure + // handling leaves the machinery intact). + espp::RtpsParticipant participant2({.log_level = espp::Logger::Verbosity::WARN}); + CHECK(participant2.start(), "participant2 start"); + CHECK(participant2.add_service_server( + {.service = "/good", .type_name = "test::srv::dds_::Good", .band = espp::QosBand::High}, + [](std::span) { return std::vector{}; }), + "valid banded service builds"); + // 5. Duplicate action: the second add_action_server() must fail without + // harming the first (its feedback/status writers keep accepting samples). + const char *dup_action = "/dup_action"; + CHECK( + participant2.add_action_server({.action = dup_action, .type_name = "test::action::dds_::Dup"}, + nullptr, [](espp::RtpsParticipant::ActionGoalHandle) {}), + "first action builds"); + CHECK(!participant2.add_action_server( + {.action = dup_action, .type_name = "test::action::dds_::Dup"}, nullptr, + [](espp::RtpsParticipant::ActionGoalHandle) {}), + "duplicate action fails"); + const std::vector payload{0x00, 0x01, 0x00, 0x00, 0x2a, 0x00, 0x00, 0x00}; + const std::string dup_feedback = "rt/dup_action/_action/feedback"; + const std::string dup_status = "rt/dup_action/_action/status"; + CHECK(participant2.publish(dup_feedback, payload), + "first action's feedback writer survives the duplicate's rollback"); + CHECK(participant2.publish(dup_status, payload), + "first action's status writer survives the duplicate's rollback"); + + // 6. Precise rollback under concurrent adds: writers added by another + // thread while failing composites run must never be rolled back. + // Writer-budget note: participant2 already carries 6 writers (the valid + // banded service + the first dup_action), so stay well inside the ~13 + // usable slots with 5 concurrent adds. + std::atomic adder_ok{true}; + std::thread adder([&participant2, &adder_ok]() { + for (int i = 0; i < 5; ++i) { + if (!participant2.add_writer({.topic = "conc_" + std::to_string(i), + .type_name = "test::msg::dds_::Conc_", + .reliability = Reliability::RELIABLE})) { + adder_ok = false; + return; + } + std::this_thread::sleep_for(1ms); + } + }); + for (int i = 0; i < 5; ++i) { + // Two flavors of failing composite run their (precise) rollbacks + // concurrently with the adder thread: a duplicate action (fails at the + // first step, rolls back nothing) and a partially-built service (its + // reply writer IS created and must be rolled back - real deletions racing + // the adds). + (void)participant2.add_action_server( + {.action = dup_action, .type_name = "test::action::dds_::Dup"}, nullptr, + [](espp::RtpsParticipant::ActionGoalHandle) {}); + (void)participant2.add_service_server( + {.service = bad_service, .type_name = "test::srv::dds_::Bad", .band = espp::QosBand::High}, + [](std::span) { return std::vector{}; }); + } + adder.join(); + CHECK(adder_ok.load(), "concurrent adds all succeeded"); + for (int i = 0; i < 5; ++i) { + CHECK(participant2.publish("conc_" + std::to_string(i), payload), + "concurrently added writer survives the failing composites"); + } + + participant2.stop(); + + std::printf("PASS\n"); + return 0; +} diff --git a/pc/tests/rtps_stateless_saturation.cpp b/pc/tests/rtps_stateless_saturation.cpp new file mode 100644 index 0000000000..87e61544b9 --- /dev/null +++ b/pc/tests/rtps_stateless_saturation.cpp @@ -0,0 +1,177 @@ +// Best-effort saturation: a BEST_EFFORT (StatelessWriter) publisher bursting +// far faster than the send path must NOT collapse. This is the regression pair +// for rammp-org/pace-racer-fw#14: +// - with growable (dynamic, the host/CI default) history, delivery must be +// (near-)lossless - the guaranteed progress() machinery drains everything; +// - with a full static ring the engine now degrades to KEEP_LAST drop-oldest +// (progress() clamps a cursor that fell behind the ring instead of sending +// nothing) and surfaces every overwritten sample via the facade's +// rate-limited warning and Diagnostics::Writer::history_overwrite_drops. +// The interop harness ALSO builds and runs this test in a static-storage +// variant (RTPS_STORAGE_STATIC + HISTORY_SIZE_STATELESS=2), where it +// requires overflow drops to have occurred AND delivery to stay well above +// the collapse level (~10% pre-fix vs ~60-70% post-fix measured). +// +// Exits 0 on success. +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps/utils/Diagnostics.hpp" +#include "rtps_participant.hpp" + +#include "rtps_common.hpp" + +struct StringMsg { + std::string data; +}; + +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +using namespace std::chrono_literals; + +int main() { + using Reliability = espp::RtpsParticipant::Reliability; + const char *type = "std_msgs::msg::dds_::String_"; + const char *topic = "saturation_topic"; + + std::string ip; + // Portable interface discovery (rtps_common.hpp builds on POSIX and MSVC); + // the loopback fallback means no usable interface was found. + ip = rtps_test::guess_local_ipv4(); + if (ip.rfind("127.", 0) == 0) { + std::printf("no iface\n"); + return 1; + } + + espp::RtpsParticipant pub({.interface_address = ip, .log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant sub({.interface_address = ip, .log_level = espp::Logger::Verbosity::WARN}); + std::atomic received{0}; + if (!pub.start() || !sub.start() || + !pub.add_writer( + {.topic = topic, .type_name = type, .reliability = Reliability::BEST_EFFORT}) || + !sub.add_reader({.topic = topic, + .type_name = type, + .reliability = Reliability::BEST_EFFORT, + .on_sample = [&](std::span) { received.fetch_add(1); }})) { + std::printf("setup failed\n"); + return 1; + } + + // Wait for the match (paced pre-publishes until one lands). + const auto match_deadline = std::chrono::steady_clock::now() + 15s; + while (received.load() == 0 && std::chrono::steady_clock::now() < match_deadline) { + auto bytes = cdr::serialize(StringMsg{"probe"}); + if (bytes) + (void)pub.publish(topic, u8_span(*bytes)); + std::this_thread::sleep_for(20ms); + } + if (received.load() == 0) { + std::printf("never matched\n"); + return 1; + } + received.store(0); + + // Saturation burst: publish back-to-back, no pacing. Sized so the burst's + // total datagram volume fits a default kernel UDP receive buffer: the loss + // guarded here is the WRITER silently failing to send (history collapse) - + // receiver-side kernel drops from a multi-hundred-KB burst are genuine + // best-effort wire loss and would flake the lossless assertion (observed in + // the slower interop container, where 5000 samples publish in <20 ms). + // (~500 datagrams: a default linux rmem of ~208 KB holds ~800 small + // datagrams after per-skb accounting overhead, so 500 leaves real margin. + // Margin, not immunity: the guaranteed-drain chain sends the burst + // back-to-back at pool speed, so a receiver draining concurrently in a slow + // container can still shed a few percent - hence the dynamic floor's slack + // below.) + constexpr int kBurst = 500; + int published_ok = 0; + const auto t0 = std::chrono::steady_clock::now(); + for (int i = 0; i < kBurst; ++i) { + auto bytes = cdr::serialize(StringMsg{"s" + std::to_string(i)}); + if (bytes && pub.publish(topic, u8_span(*bytes))) + ++published_ok; + } + const auto burst_ms = + std::chrono::duration_cast(std::chrono::steady_clock::now() - t0) + .count(); + // Drain: with dynamic history nothing may be dropped, so wait until the + // count stops growing (bounded), then judge. + int got = received.load(); + const auto drain_deadline = std::chrono::steady_clock::now() + 20s; + while (std::chrono::steady_clock::now() < drain_deadline) { + std::this_thread::sleep_for(250ms); + const int now = received.load(); + if (now == got && now > 0) { + break; // settled + } + got = now; + } + got = received.load(); + std::printf("published_ok=%d/%d in %lld ms; received=%d (%.1f%%)\n", published_ok, kBurst, + (long long)burst_ms, got, 100.0 * got / kBurst); + pub.stop(); + sub.stop(); + if (published_ok != kBurst) { + std::printf("FAIL: publish() rejected samples under saturation\n"); + return 1; + } + const uint32_t drops = rtps::Diagnostics::Writer::history_overwrite_drops.load(); + std::printf("history_overwrite_drops=%u\n", drops); +#if defined(RTPS_STORAGE_STATIC) + // Static KEEP_LAST ring (the interop harness builds this variant with + // HISTORY_SIZE_STATELESS=2). The DETERMINISTIC properties of the fix: + // 1. the burst overflows and every overwrite is COUNTED (drops > 0 - the + // overwrite accounting and the publish()-side warning path); + // 2. accounting conserves: every sample is either delivered or counted as + // dropped (received + drops == burst on the loss-free loopback; a small + // slack tolerates scheduling stragglers); + // 3. delivery does not TOTALLY collapse: with the pre-fix cursor bug an + // executed progress() almost always found its change overwritten and + // sent NOTHING - total delivery was just the final ring contents (a few + // samples). With the fix every executed poke delivers a live sample. + // The delivered FRACTION is deliberately not asserted tightly: it equals the + // number of pokes the pool manages to execute during/after the storm, which + // is scheduler-timing dependent (observed 6%-36% across host/container + // runs); the collapse floor below is a few times the bug's ceiling while + // staying under every observed fixed run. + if (drops == 0) { + std::printf("FAIL: static ring never overflowed - the KEEP_LAST path was not exercised\n"); + return 1; + } + if (got + static_cast(drops) < (kBurst * 95) / 100) { + std::printf("FAIL: accounting leak (received %d + drops %u < burst %d)\n", got, drops, kBurst); + return 1; + } + if (got < kBurst / 50) { + std::printf("FAIL: saturation collapse (%d/%d delivered)\n", got, kBurst); + return 1; + } +#else + // Growable (dynamic) history - the host/CI default. The DETERMINISTIC + // property: a dynamic history never overwrites, so every sample must reach + // the wire (drops == 0, and publish accepted all - asserted above). The + // delivered fraction is best-effort UDP: the guaranteed-drain chain sends + // the burst back-to-back at pool speed (no retry-timer metering), so a + // slower receiver's kernel buffer can shed a few percent as genuine wire + // loss (observed: 100% on host, ~95% in the interop container). A + // chain/parking regression instead delivers at most the pool-queue prefix + // of the burst (~64/500 = 13%), far below this floor. + if (drops != 0) { + std::printf("FAIL: dynamic history reported overwrite drops (%u)\n", drops); + return 1; + } + if (got < (kBurst * 80) / 100) { + std::printf("FAIL: saturation collapse (%d/%d delivered)\n", got, kBurst); + return 1; + } +#endif + std::printf("PASS\n"); + return 0; +} diff --git a/pc/tests/rtps_typed_rpc_loopback.cpp b/pc/tests/rtps_typed_rpc_loopback.cpp index 4c79f47f0a..990e804950 100644 --- a/pc/tests/rtps_typed_rpc_loopback.cpp +++ b/pc/tests/rtps_typed_rpc_loopback.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -99,26 +100,32 @@ int main() { } std::printf("typed service ros=%d native=%d\n", svc_ros, svc_native); - // Actions: typed feedback + result. + // Actions: typed feedback + result. The callback state is SHARED and + // captured by value: the client retains the callbacks until the result + // arrives, so if the wait below times out a late result/feedback would + // otherwise dereference destroyed lambda-stack state (use-after-scope). auto run_action = [](auto &cli, int order, const std::vector &expected) { - std::mutex m; - std::condition_variable cv; - bool done = false; - std::atomic fb{0}; - std::vector got; - espp::GoalStatus status{}; + struct GoalState { + std::mutex m; + std::condition_variable cv; + bool done = false; + std::atomic fb{0}; + std::vector got; + espp::GoalStatus status{}; + }; + auto st = std::make_shared(); cli.send_goal( - FibGoal{order}, [&](const FibSeq &) { fb.fetch_add(1); }, - [&](espp::GoalStatus st, const FibSeq &res) { - std::lock_guard lk(m); - status = st; - got = res.sequence; - done = true; - cv.notify_one(); + FibGoal{order}, [st](const FibSeq &) { st->fb.fetch_add(1); }, + [st](espp::GoalStatus gs, const FibSeq &res) { + std::lock_guard lk(st->m); + st->status = gs; + st->got = res.sequence; + st->done = true; + st->cv.notify_one(); }); - std::unique_lock lk(m); - cv.wait_for(lk, 15s, [&] { return done; }); - return status == espp::GoalStatus::SUCCEEDED && got == expected && fb.load() > 0; + std::unique_lock lk(st->m); + st->cv.wait_for(lk, 15s, [&] { return st->done; }); + return st->status == espp::GoalStatus::SUCCEEDED && st->got == expected && st->fb.load() > 0; }; act_ros = run_action(ros_act_cli, 5, {0, 1, 1, 2, 3, 5}); act_native = run_action(nat_act_cli, 5, {0, 1, 1, 2, 3, 5}); diff --git a/pc/tests/rtps_writer_churn.cpp b/pc/tests/rtps_writer_churn.cpp new file mode 100644 index 0000000000..acb83b77c8 --- /dev/null +++ b/pc/tests/rtps_writer_churn.cpp @@ -0,0 +1,231 @@ +// Writer create/publish/delete churn under load - the writer-side analog of +// rtps_banded_churn. Guards the writer-teardown lifetime class found in review: +// a reliable writer's publish() submits guaranteed progress() jobs keyed by the +// writer, so deleting it races any parked/queued job (formerly: use-after-free +// of the reset history, sends on a released dedicated port, stale jobs against +// a reused pool slot). The engine now cancels parked jobs, generation-guards +// accepted ones, and quiesces in-flight progress() under the writer mutex - +// this test churns exactly that window, repeatedly. +// +// Phase 1 (hostile churn): while a persistent flood writer keeps the +// transport pool busy, a banded (dedicated-port capable) writer on a second +// topic is repeatedly created, flood-published (no pacing - guaranteed +// progress() jobs park under saturation), and IMMEDIATELY deleted with those +// jobs still parked/in flight. Any lifetime bug is a crash/terminate (and a +// sanitizer report under the ASan/TSan CI leg); a leak of the dedicated-port +// ration eventually exhausts the ration and surfaces as add_writer failures. +// +// Phase 2 (verified churn): the same create/publish/delete cycle, but each +// iteration waits for at least one sample to arrive end-to-end before the +// delete - proving the churned writer slot is fully functional after every +// reuse (a stale-generation bug that ate samples would fail here). +// +// The test must complete well under the external timeout the harness applies; +// a deadlocked delete shows up as a timeout kill. +// +// Exits 0 on success. + +#include +#include +#include +#include +#include +#include +#include + +#include "cdr.hpp" +#include "rtps_participant.hpp" + +#include "rtps_common.hpp" + +struct StringMsg { + std::string data; +}; + +inline std::span u8_span(const std::vector &bytes) { + return {reinterpret_cast(bytes.data()), bytes.size()}; +} + +namespace { +// Expose the protected remove_writer() for this test (same pattern as +// rtps_remove_reader_deadlock exposing remove_reader()). +struct TestParticipant : espp::RtpsParticipant { + using espp::RtpsParticipant::remove_writer; + using espp::RtpsParticipant::RtpsParticipant; +}; +} // namespace + +using namespace std::chrono_literals; + +int main() { + using Reliability = espp::RtpsParticipant::Reliability; + const char *type = "std_msgs::msg::dds_::String_"; + const char *flood_topic = "writer_churn_flood"; + const char *churn_topic = "writer_churn_topic"; + + std::string ip; + // Portable interface discovery (rtps_common.hpp builds on POSIX and MSVC); + // the loopback fallback means no usable interface was found. + ip = rtps_test::guess_local_ipv4(); + if (ip.rfind("127.", 0) == 0) { + std::printf("FAIL: no usable IPv4 interface\n"); + return 1; + } + + TestParticipant pub({.interface_address = ip, .log_level = espp::Logger::Verbosity::WARN}); + espp::RtpsParticipant sub({.interface_address = ip, .log_level = espp::Logger::Verbosity::WARN}); + if (!pub.start() || !sub.start()) { + std::printf("FAIL: start\n"); + return 1; + } + + std::atomic flood_received{0}; + std::atomic churn_received{0}; + if (!pub.add_writer( + {.topic = flood_topic, .type_name = type, .reliability = Reliability::RELIABLE}) || + !sub.add_reader( + {.topic = flood_topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = [&](std::span) { flood_received.fetch_add(1); }}) || + !sub.add_reader( + {.topic = churn_topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .on_sample = [&](std::span) { churn_received.fetch_add(1); }})) { + std::printf("FAIL: persistent endpoint setup\n"); + return 1; + } + + // Persistent flood keeps the transport pool + reactor busy for the whole + // test so writer deletion always races live traffic. + std::atomic flood{true}; + std::thread flooder([&]() { + int i = 0; + while (flood.load()) { + auto bytes = cdr::serialize(StringMsg{"flood " + std::to_string(i++)}); + if (bytes) { + (void)pub.publish(flood_topic, u8_span(*bytes)); + } + std::this_thread::sleep_for(1ms); + } + }); + + // Wait for the persistent pair to match (discovery settled) before churning. + const auto match_deadline = std::chrono::steady_clock::now() + 15s; + while (flood_received.load() == 0 && std::chrono::steady_clock::now() < match_deadline) { + std::this_thread::sleep_for(10ms); + } + if (flood_received.load() == 0) { + std::printf("FAIL: flood topic never delivered (discovery)\n"); + flood = false; + flooder.join(); + return 1; + } + + // Baseline for the flood-progress assertion below: flood_received was + // already required to be nonzero above, so the final check must require an + // INCREASE across the churn (a plain nonzero check would be vacuous). + const int flood_before_churn = flood_received.load(); + + // ---- Phase 1: hostile churn - delete with progress() jobs in flight ------ + constexpr int kHostileIterations = 15; + constexpr int kBurst = 25; + for (int iter = 0; iter < kHostileIterations; ++iter) { + // Alternate bands so both the dedicated-port path (banded) and the shared + // path get churned. + const auto band = (iter % 2 == 0) ? espp::QosBand::High : espp::QosBand::Normal; + if (!pub.add_writer({.topic = churn_topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .band = band})) { + std::printf("FAIL: add_writer iteration %d (leaked ration slot?)\n", iter); + flood = false; + flooder.join(); + return 1; + } + for (int i = 0; i < kBurst; ++i) { + auto bytes = cdr::serialize(StringMsg{"churn " + std::to_string(i)}); + if (bytes) { + (void)pub.publish(churn_topic, u8_span(*bytes)); + } + } + // Delete immediately: the burst's guaranteed progress() jobs are still + // queued/parked. This is the raced window. + if (!pub.remove_writer(churn_topic)) { + std::printf("FAIL: remove_writer iteration %d\n", iter); + flood = false; + flooder.join(); + return 1; + } + } + std::printf("phase 1 OK: %d hostile churn iterations (churn samples so far: %d)\n", + kHostileIterations, churn_received.load()); + + // ---- Phase 2: verified churn - every reused slot must still deliver ------ + // Between verified iterations, let the SEDP disposal of the previous writer + // propagate before announcing its replacement: endpoints are pooled, so a + // rapid re-add can reuse the previous GUID, and if the reader processes the + // new announce BEFORE the old dispose it keeps stale reliable-stream state + // for that GUID and drops the new writer's samples as duplicates. That + // ordering hazard is inherent to same-topic churn on a shared pool (peers + // see two announcements racing), not the writer-teardown lifetime class this + // test guards - phase 1 covers the hostile no-settle path. + constexpr auto kSettle = 250ms; + constexpr int kVerifiedIterations = 5; + std::this_thread::sleep_for(kSettle); + for (int iter = 0; iter < kVerifiedIterations; ++iter) { + const int before = churn_received.load(); + if (!pub.add_writer({.topic = churn_topic, + .type_name = type, + .reliability = Reliability::RELIABLE, + .band = espp::QosBand::High})) { + std::printf("FAIL: verified add_writer iteration %d\n", iter); + flood = false; + flooder.join(); + return 1; + } + // Publish until at least one sample lands (reliable recovery covers the + // pre-match window), then delete. + const auto deliver_deadline = std::chrono::steady_clock::now() + 10s; + while (churn_received.load() == before && std::chrono::steady_clock::now() < deliver_deadline) { + auto bytes = cdr::serialize(StringMsg{"verified " + std::to_string(iter)}); + if (bytes) { + (void)pub.publish(churn_topic, u8_span(*bytes)); + } + std::this_thread::sleep_for(20ms); + } + if (churn_received.load() == before) { + std::printf("FAIL: churned writer slot never delivered (iteration %d)\n", iter); + flood = false; + flooder.join(); + return 1; + } + if (!pub.remove_writer(churn_topic)) { + std::printf("FAIL: verified remove_writer iteration %d\n", iter); + flood = false; + flooder.join(); + return 1; + } + std::this_thread::sleep_for(kSettle); // let the dispose land before the next announce + } + std::printf("phase 2 OK: %d verified churn iterations\n", kVerifiedIterations); + + // Flood must have kept MAKING PROGRESS across all the churn (>= ~40 ms + // worth of 1 ms-paced samples is a lenient floor that still catches a + // wedged flood, which would show zero new deliveries). + const int flood_final = flood_received.load(); + flood = false; + flooder.join(); + constexpr int kMinFloodProgress = 40; + if (flood_final - flood_before_churn < kMinFloodProgress) { + std::printf("FAIL: flood stalled during churn (before=%d, after=%d)\n", flood_before_churn, + flood_final); + return 1; + } + + pub.stop(); + sub.stop(); + std::printf("PASS (flood=%d, churn=%d samples)\n", flood_final, churn_received.load()); + return 0; +} diff --git a/pc/tests/socket_reactor.cpp b/pc/tests/socket_reactor.cpp index 9aadd9f898..62322df1dd 100644 --- a/pc/tests/socket_reactor.cpp +++ b/pc/tests/socket_reactor.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -456,6 +457,104 @@ int main() { } } + // ------------------------------------------------------------------------- + // 7. Removal-completion callbacks: a throwing callback must be contained + // (it runs on a pool worker OUTSIDE the handler try/catch) and a chained + // callback must still run (exactly-once) even when the one before throws. + // ------------------------------------------------------------------------- +#if defined(__cpp_exceptions) && __cpp_exceptions + logger.info("--- removal-callback exception containment ---"); + { + constexpr size_t idle_port = 6160; + constexpr size_t alive_port = 6161; + constexpr size_t gated_port = 6162; + + // sockets declared before the reactor so the reactor is destroyed first + // (one socket per registration - add_udp_receiver binds the socket, and a + // second registration of the same socket would fail the re-bind) + espp::UdpSocket idle_server({.log_level = WARN}); + espp::UdpSocket gated_server({.log_level = WARN}); + espp::UdpSocket alive_server({.log_level = WARN}); + { + std::mutex gate_mtx; + std::condition_variable gate_cv; + bool release = false; + std::atomic handler_running{false}; + + espp::SocketReactor reactor({.log_level = WARN}); + + // Idle-path removal: the completion callback runs synchronously on the + // caller's thread; a throw must be contained there too. + std::atomic idle_cb_ran{false}; + auto idle_id = reactor.add_udp_receiver( + idle_server, {.port = idle_port, + .buffer_size = kBufferSize, + .on_receive_callback = [](const ByteVector &, const espp::Socket::Info &) + -> std::optional { return std::nullopt; }}); + check(idle_id != espp::SocketReactor::INVALID_ID, "receiver registered for idle removal"); + check(reactor.remove(idle_id, + [&]() { + idle_cb_ran = true; + throw std::runtime_error("idle removal callback throws"); + }), + "idle remove() with a throwing callback returns true (throw contained)"); + check(idle_cb_ran.load(), "idle removal callback ran"); + + // In-flight chained removal: re-register, gate the handler, then chain + // two removal callbacks while it is in flight - the FIRST throws. + auto id = reactor.add_udp_receiver( + gated_server, + {.port = gated_port, + .buffer_size = kBufferSize, + .on_receive_callback = [&](const ByteVector &, + const espp::Socket::Info &) -> std::optional { + handler_running = true; + std::unique_lock lk(gate_mtx); + gate_cv.wait(lk, [&] { return release; }); + return std::nullopt; + }}); + auto alive_id = reactor.add_udp_receiver( + alive_server, + {.port = alive_port, .buffer_size = kBufferSize, .on_receive_callback = echo_reversed}); + check(id != espp::SocketReactor::INVALID_ID && alive_id != espp::SocketReactor::INVALID_ID, + "gated + liveness receivers registered"); + + espp::UdpSocket client({.log_level = WARN}); + client.send(make_payload(8, 0x01), {.ip_address = kLoopback, .port = gated_port}); + check(wait_until([&] { return handler_running.load(); }, 5s), "gated handler is in flight"); + + std::atomic first_ran{false}; + std::atomic second_ran{false}; + check(reactor.remove(id, + [&]() { + first_ran = true; + throw std::runtime_error("first removal callback throws"); + }), + "in-flight remove() with a throwing callback accepted"); + check(reactor.remove(id, [&]() { second_ran = true; }), + "second remove() chains onto the pending removal"); + + { + std::lock_guard lk(gate_mtx); + release = true; + } + gate_cv.notify_all(); + + // The throw from the first callback must not kill the pool worker, and + // the chained second callback must still run (exactly-once contract). + check(wait_until([&] { return second_ran.load(); }, 5s), + "chained callback ran despite the earlier callback throwing"); + check(first_ran.load(), "throwing callback itself was invoked"); + check(wait_until([&] { return reactor.num_registered() == 1; }, 5s), + "gated registration fully removed"); + // The reactor (and its pool worker) must still be fully functional. + check(udp_echo_roundtrip(alive_port, make_payload(24, 0x77)), + "reactor still dispatches after contained throws"); + reactor.stop(); + } + } +#endif // __cpp_exceptions + // ------------------------------------------------------------------------- // Summary // ------------------------------------------------------------------------- diff --git a/pc/tests/thread_pool.cpp b/pc/tests/thread_pool.cpp index bbaf32b409..dbcc5697f1 100644 --- a/pc/tests/thread_pool.cpp +++ b/pc/tests/thread_pool.cpp @@ -86,6 +86,10 @@ int main() { for (int i = 0; i < N; ++i) { pool.submit(espp::ThreadPool::Job([&]() { std::this_thread::sleep_for(20ms); + // Notify UNDER the lock: main destroys the cv right after its wait() + // returns; wait() re-acquires mtx to return, ordering the destruction + // after this notify (an unlocked notify races the destruction). + std::lock_guard lk(mtx); ++done; cv.notify_one(); })); @@ -217,6 +221,9 @@ int main() { for (int i = 0; i < jobs_per_thread; ++i) { pool.submit(espp::ThreadPool::Job([&]() { std::this_thread::sleep_for(5ms); + // under the wait mutex: prevents the lost-wakeup where the final + // notify lands between the waiter's predicate check and its block + std::lock_guard lk(mtx); ++done; cv.notify_one(); })); @@ -263,6 +270,8 @@ int main() { for (int i = 0; i < total; ++i) { if (pool.submit(espp::ThreadPool::Job([&]() { std::this_thread::sleep_for(30ms); + // under the wait mutex (lost-wakeup guard, see above) + std::lock_guard lk(mtx); ++done; cv.notify_one(); }))) { @@ -349,10 +358,12 @@ int main() { }); for (int i = 0; i < num_a_jobs; ++i) { - pool_a.submit(espp::ThreadPool::Job([&pool_b, &done_b, &cv]() { + pool_a.submit(espp::ThreadPool::Job([&pool_b, &done_b, &cv, &mtx]() { for (int j = 0; j < b_jobs_per_a; ++j) { - pool_b.submit(espp::ThreadPool::Job([&done_b, &cv]() { + pool_b.submit(espp::ThreadPool::Job([&done_b, &cv, &mtx]() { std::this_thread::sleep_for(20ms); + // under the wait mutex (lost-wakeup guard, see above) + std::lock_guard lk(mtx); ++done_b; cv.notify_one(); })); @@ -397,11 +408,16 @@ int main() { }); for (int i = 0; i < num_initial; ++i) { - pool.submit(espp::ThreadPool::Job([&pool, &done, &cv]() { - ++done; - cv.notify_one(); - pool.submit(espp::ThreadPool::Job([&done, &cv]() { + pool.submit(espp::ThreadPool::Job([&pool, &done, &cv, &mtx]() { + { + // under the wait mutex (lost-wakeup guard, see above) + std::lock_guard lk(mtx); + ++done; + cv.notify_one(); + } + pool.submit(espp::ThreadPool::Job([&done, &cv, &mtx]() { std::this_thread::sleep_for(10ms); + std::lock_guard lk(mtx); ++done; cv.notify_one(); }));